Skip to content

Commit

Permalink
Finished orientation system
Browse files Browse the repository at this point in the history
  • Loading branch information
@luizgabriel
luizgabriel committed Dec 11, 2018
1 parent d50ff1a commit 848b534
Show file tree
Hide file tree
Showing 10 changed files with 222 additions and 127 deletions.
27 changes: 14 additions & 13 deletions main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,23 +63,24 @@
#include "system.h"
#include "comm.h"
#include "mpu6050.h"
#include "orientation.h"

void App_RunTasks();
orientation_t orient;

void main() {
System_Init();
MPU_Init();
//Comm_Init();

OrientationSystem_Init(&orient);

while (1) {
App_RunTasks();
OrientationSystem_Update(&orient);

GREEN_LED = orient.rot.x > 40;
WHITE_LED = orient.rot.x < -40;
RED_LED = orient.rot.y > 40;
YELLOW_LED = orient.rot.y < -40;

__delay_us(50);

//Comm_Send();
}
}

void App_RunTasks()
{
vec3 gyro;
MPU_GetGyro(&gyro);

__delay_us(300);
}
123 changes: 78 additions & 45 deletions mpu6050.c
View file
Original file line number Diff line number Diff line change
@@ -1,57 +1,112 @@
#include "mpu6050.h"

int MPU_Init()
int MPU_Init(mpu6050_range_t range, mpu6050_dps_t scale)
{
// Check MPU6050 Who Am I Register
if (MPU_ReadRegister(MPU6050_REG_WHO_AM_I) != 0x68)
return 0;

// Set Clock Source
MPU_SetClockSource(MPU6050_CLOCK_PLL_XGYRO);

MPU_SetScale(range);
MPU_SetRange(scale);

// Disable Sleep Mode
MPU_SetSleepEnabled(0);

return 1;
}

void MPU_SetClockSource(mpu6050_clockSource_t source)
void MPU_SetScale(mpu6050_dps_t scale)
{
byte value;
value = MPU_ReadRegister(MPU6050_REG_PWR_MGMT_1);
value &= 0b11111000;
value |= source;
MPU_WriteRegister(MPU6050_REG_PWR_MGMT_1, value);

switch (scale)
{
case MPU6050_SCALE_250DPS:
MPU.dpsPerDigit = .007633f;
break;
case MPU6050_SCALE_500DPS:
MPU.dpsPerDigit = .015267f;
break;
case MPU6050_SCALE_1000DPS:
MPU.dpsPerDigit = .030487f;
break;
case MPU6050_SCALE_2000DPS:
MPU.dpsPerDigit = .060975f;
break;
default:
break;
}

value = MPU_ReadRegister(MPU6050_REG_GYRO_CONFIG);
value &= 0b11100111;
value |= (scale << 3);
MPU_WriteRegister(MPU6050_REG_GYRO_CONFIG, value);
}

void MPU_SetSleepEnabled(byte state)
void MPU_SetRange(mpu6050_range_t range)
{
MPU_WriteRegisterBit(MPU6050_REG_PWR_MGMT_1, 6, state);
switch (range)
{
case MPU6050_RANGE_2G:
MPU.rangePerDigit = .000061f;
break;
case MPU6050_RANGE_4G:
MPU.rangePerDigit = .000122f;
break;
case MPU6050_RANGE_8G:
MPU.rangePerDigit = .000244f;
break;
case MPU6050_RANGE_16G:
MPU.rangePerDigit = .0004882f;
break;
default:
break;
}

byte value = MPU_ReadRegister(MPU6050_REG_ACCEL_CONFIG);
value &= 0b11100111;
value |= (range << 3);
MPU_WriteRegister(MPU6050_REG_ACCEL_CONFIG, value);
}

void MPU_SetGyroOffsetX(int offset)
void MPU_SetClockSource(mpu6050_clockSource_t source)
{
MPU_WriteRegister16(MPU6050_REG_GYRO_XOFFS_H, offset);
byte value = MPU_ReadRegister(MPU6050_REG_PWR_MGMT_1);
value &= 0b11111000;
value |= source;
MPU_WriteRegister(MPU6050_REG_PWR_MGMT_1, value);
}

void MPU_SetGyroOffsetY(int offset)
void MPU_SetSleepEnabled(byte state)
{
MPU_WriteRegister16(MPU6050_REG_GYRO_YOFFS_H, offset);
MPU_WriteRegisterBit(MPU6050_REG_PWR_MGMT_1, 6, state);
}

void MPU_SetGyroOffsetZ(int offset)
void MPU_GetGyro(vec3f* out)
{
MPU_WriteRegister16(MPU6050_REG_GYRO_ZOFFS_H, offset);
byte raw[6];
EESequentialRead(MPU6050_ADDRESS, MPU6050_REG_GYRO_XOUT_H, raw, 6);

float* f_out = (float*) out;
for(int i = 0; i < 3; i++)
{
int dps = ((raw[i*2] << 8) | raw[(i*2)+1]);
f_out[i] = dps * MPU.dpsPerDigit;
}
}

void MPU_GetGyro(vec3* out)
void MPU_GetAccel(vec3f* out)
{
byte values[6];
EESequentialRead(MPU6050_ADDRESS, MPU6050_REG_GYRO_XOUT_H, values, 6);

out->x = values[0] << 8 | values[1];
out->y = values[2] << 8 | values[3];
out->z = values[4] << 8 | values[5];
byte raw[6];
EESequentialRead(MPU6050_ADDRESS, MPU6050_REG_ACCEL_XOUT_H, raw, 6);

float* f_out = (float*) out;
for(int i = 0; i < 3; i++)
{
int g = ((raw[i*2] << 8) | raw[(i*2)+1]);
f_out[i] = g * MPU.rangePerDigit;
}
}

//<editor-fold desc="I2C Helper Functions" defaultstate="collapsed">
Expand All @@ -66,28 +121,6 @@ void MPU_WriteRegister(byte reg, byte value)
EEAckPolling(MPU6050_ADDRESS);
}

int MPU_ReadRegister16(byte reg)
{
byte values[2];
EESequentialRead(MPU6050_ADDRESS, reg, values, 2);

int result = values[0] << 8 | values[1];
return result;
}

void MPU_WriteRegister16(byte reg, int value)
{
byte values[2] = {(byte) value >> 8, (byte) value};
EEPageWrite(MPU6050_ADDRESS, reg, values, 2);
EEAckPolling(MPU6050_ADDRESS);
}

byte MPU_ReadRegisterBit(byte reg, byte pos)
{
byte value = MPU_ReadRegister(reg);
return ((value >> pos) & 1);
}

void MPU_WriteRegisterBit(byte reg, byte pos, byte state)
{
byte value = MPU_ReadRegister(reg);
Expand Down
55 changes: 29 additions & 26 deletions mpu6050.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,29 +11,10 @@
#include "system.h"

#define MPU6050_ADDRESS (0xD0)
//#define MPU6050_ADDRESS (0xD2)// when AD0 pin to Vcc

#define MPU6050_REG_ACCEL_XOFFS_H (0x06)
#define MPU6050_REG_ACCEL_XOFFS_L (0x07)
#define MPU6050_REG_ACCEL_YOFFS_H (0x08)
#define MPU6050_REG_ACCEL_YOFFS_L (0x09)
#define MPU6050_REG_ACCEL_ZOFFS_H (0x0A)
#define MPU6050_REG_ACCEL_ZOFFS_L (0x0B)
#define MPU6050_REG_GYRO_XOFFS_H (0x13)
#define MPU6050_REG_GYRO_XOFFS_L (0x14)
#define MPU6050_REG_GYRO_YOFFS_H (0x15)
#define MPU6050_REG_GYRO_YOFFS_L (0x16)
#define MPU6050_REG_GYRO_ZOFFS_H (0x17)
#define MPU6050_REG_GYRO_ZOFFS_L (0x18)
#define MPU6050_REG_CONFIG (0x1A)
#define MPU6050_REG_GYRO_CONFIG (0x1B) // Gyroscope Configuration
#define MPU6050_REG_ACCEL_CONFIG (0x1C) // Accelerometer Configuration
#define MPU6050_REG_FF_THRESHOLD (0x1D)
#define MPU6050_REG_FF_DURATION (0x1E)
#define MPU6050_REG_MOT_THRESHOLD (0x1F)
#define MPU6050_REG_MOT_DURATION (0x20)
#define MPU6050_REG_ZMOT_THRESHOLD (0x21)
#define MPU6050_REG_ZMOT_DURATION (0x22)
#define MPU6050_REG_INT_PIN_CFG (0x37) // INT Pin. Bypass Enable Configuration
#define MPU6050_REG_INT_ENABLE (0x38) // INT Enable
#define MPU6050_REG_INT_STATUS (0x3A)
Expand All @@ -57,7 +38,12 @@
#define MPU6050_REG_PWR_MGMT_1 (0x6B) // Power Management 1
#define MPU6050_REG_WHO_AM_I (0x75) // Who Am I

struct vec3 { float x, y, z; };
typedef struct { float x, y, z; } vec3f;

typedef struct {
float dpsPerDigit;
float rangePerDigit;
} mpu_data_t;

typedef enum
{
Expand All @@ -70,20 +56,37 @@ typedef enum
MPU6050_CLOCK_INTERNAL_8MHZ = 0b000
} mpu6050_clockSource_t;

int MPU_Init();
typedef enum
{
MPU6050_SCALE_2000DPS = 0b11,
MPU6050_SCALE_1000DPS = 0b10,
MPU6050_SCALE_500DPS = 0b01,
MPU6050_SCALE_250DPS = 0b00
} mpu6050_dps_t;

typedef enum
{
MPU6050_RANGE_16G = 0b11,
MPU6050_RANGE_8G = 0b10,
MPU6050_RANGE_4G = 0b01,
MPU6050_RANGE_2G = 0b00,
} mpu6050_range_t;

mpu_data_t MPU;

int MPU_Init(mpu6050_range_t range, mpu6050_dps_t scale);
void MPU_GetGyro(vec3f* out);
void MPU_GetAccel(vec3f* out);
void MPU_SetClockSource(mpu6050_clockSource_t source);
void MPU_SetScale(mpu6050_dps_t scale);
void MPU_SetRange(mpu6050_range_t range);
void MPU_SetSleepEnabled(byte state);
void MPU_SetGyroOffsetX(int offset);
void MPU_SetGyroOffsetY(int offset);
void MPU_SetGyroOffsetZ(int offset);
void MPU_GetGyro(vec3* out);

//<editor-fold desc="I2C Helpers" defaultstate="collapsed">
byte MPU_ReadRegister(byte reg);
void MPU_WriteRegister(byte reg, byte value);
int MPU_ReadRegister16(byte reg);
void MPU_WriteRegister16(byte reg, int value);
byte MPU_ReadRegisterBit(byte reg, byte pos);
void MPU_WriteRegisterBit(byte reg, byte pos, byte state);
//</editor-fold>

Expand Down
Loading

0 comments on commit 848b534

Please sign in to comment.