// pitch/roll are absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
void ledringState(bool armed, int16_t pitch, int16_t roll, int16_t heading)
{
uint8_t b[10];
static uint8_t state;
if (state == 0) {
b[0] = 'z';
b[1] = (180 - heading) / 2; // 1 unit = 2 degrees;
i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 2, b);
state = 1;
} else if (state == 1) {
b[0] = 'y';
b[1] = constrain(roll / 10 + 90, 0, 180);
b[2] = constrain(pitch / 10 + 90, 0, 180);
i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b);
state = 2;
} else if (state == 2) {
b[0] = 'd'; // all unicolor GREEN
b[1] = 1;
if (armed)
b[2] = 1;
else
b[2] = 0;
i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b);
state = 0;
}
}
void ledringState(void)
{
uint8_t b[10];
static uint8_t state;
if (state == 0) {
b[0] = 'z';
b[1] = (180 - heading) / 2; // 1 unit = 2 degrees;
i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 2, b);
state = 1;
} else if (state == 1) {
b[0] = 'y';
b[1] = constrain(angle[ROLL] / 10 + 90, 0, 180);
b[2] = constrain(angle[PITCH] / 10 + 90, 0, 180);
i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b);
state = 2;
} else if (state == 2) {
b[0] = 'd'; // all unicolor GREEN
b[1] = 1;
if (f.ARMED)
b[2] = 1;
else
b[2] = 0;
i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b);
state = 0;
}
}
static void mpu6050DmpBankInit(void)
{
uint8_t i, j;
uint8_t incoming[9];
for (i = 0; i < 7; i++) {
mpu6050DmpBankSelect(i);
for (j = 0; j < 16; j++) {
uint8_t start_addy = j * 0x10;
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, start_addy);
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 16, (uint8_t *) & dmpMem[i][j][0]);
}
}
mpu6050DmpBankSelect(7);
for (j = 0; j < 8; j++) {
uint8_t start_addy = j * 0x10;
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, start_addy);
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 16, (uint8_t *) & dmpMem[7][j][0]);
}
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, 0x80);
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, 9, (uint8_t *) & dmpMem[7][8][0]);
i2cRead(MPU6050_ADDRESS, DMP_MEM_R_W, 8, incoming);
}
void mpu6050DmpLoop(void)
{
uint8_t temp;
uint8_t buf[2];
if (mpu6050DmpFifoReady()) {
LED1_ON;
mpu6050DmpGetPacket();
i2cRead(MPU6050_ADDRESS, MPU_RA_INT_STATUS, 1, &temp);
if (dmp_firstPacket) {
delay(1);
mpu6050DmpBankSelect(0x00);
mpu6050DmpBankSelect(0x00); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x04\x00\x00\x00"); // data
mpu6050DmpBankSelect(0x01);
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x62);
i2cRead(MPU6050_ADDRESS, DMP_MEM_R_W, 2, buf);
dmp_firstPacket = false;
mpu6050DmpFifoReady();
}
if (dmp_fifoCountL == 42) {
mpu6050DmpProcessQuat();
}
LED1_OFF;
}
}
void ledringBlink(void)
{
uint8_t b[3];
b[0] = 'k';
b[1] = 10;
b[2] = 10;
i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b);
}
int8_t i2cwrite(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data)
{
if(i2cWriteBuffer(addr,reg,len,data))
{
return TRUE;
}
else
{
return FALSE;
}
//return FALSE;
}
bool i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
{
return i2cWriteBuffer(addr_, reg_, 1, &data);
}
bool i2cWrite(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data)
{
return i2cWriteBuffer(device, addr_, reg_, 1, &data);
}
uint8_t i2c2_write(uint8_t DeviceAddr, uint8_t RegAddr, const uint8_t* pBuffer, uint16_t len)
{
return i2cWriteBuffer(I2C2, &i2c2_context, DeviceAddr,RegAddr,len,pBuffer);
}
uint8_t i2cWrite(I2C_TypeDef* I2Cx, I2C_context_t * context, uint8_t addr_, uint8_t reg_, uint8_t data)
{
return i2cWriteBuffer(I2Cx, context, addr_, reg_, 1, &data);
}
bool i2cWrite(I2C_TypeDef *I2C, uint8_t addr_, uint8_t reg_, uint8_t data)
{
return i2cWriteBuffer(I2C, addr_, reg_, 1, &data);
}
bool i2cBusWriteBuffer(const busDevice_t * dev, uint8_t reg, const uint8_t * data, uint8_t length)
{
return i2cWriteBuffer(dev->busdev.i2c.i2cBus, dev->busdev.i2c.address, reg, length, data);
}
bool i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data, I2CDevice bus)
{
return i2cWriteBuffer(addr_, reg_, 1, &data, bus);
}
static void mpu6050DmpMemInit(void)
{
uint8_t i;
uint8_t temp;
mpu6050DmpBankInit();
// Bank, Start Address, Update Length, Update Data...
for (i = 0; i < 22; i++) {
mpu6050DmpBankSelect(dmp_updates[i][0]); // bank
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, dmp_updates[i][1]); // address
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, dmp_updates[i][2], (uint8_t *)&dmp_updates[i][3]); // data
}
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, 0x32);
for (i = 22; i < 29; i++) {
mpu6050DmpBankSelect(dmp_updates[i][0]); // bank
i2cWrite(MPU6050_ADDRESS, DMP_MEM_START_ADDR, dmp_updates[i][1]); // address
i2cWriteBuffer(MPU6050_ADDRESS, DMP_MEM_R_W, dmp_updates[i][2], (uint8_t *)&dmp_updates[i][3]); // data
}
/*
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_PWR_MGMT_1);
dmp_temp = i2c_readReg(MPU60X0_I2CADDR, MPU_RA_PWR_MGMT_2);
*/
i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, 0x02); // ??
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); // CLKSEL = PLL w Z ref
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x04);
i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, 0x18); // full scale 2000 deg/s
i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, 0x0B); // ext_sync_set=temp_out_L, accel DLPF 44Hz, gyro DLPF 42Hz
i2cWrite(MPU6050_ADDRESS, MPU_RA_DMP_CFG_1, 0x03);
i2cWrite(MPU6050_ADDRESS, MPU_RA_DMP_CFG_2, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_XG_OFFS_TC, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_YG_OFFS_TC, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_ZG_OFFS_TC, 0x00);
// clear offsets
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_XG_OFFS_USRH, 6, "\x00\x00\x00\x00\x00\x00"); // data
mpu6050DmpBankSelect(0x01); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0xB2);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 2, "\xFF\xFF"); // data
mpu6050DmpBankSelect(0x01); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x90);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x09\x23\xA1\x35"); // data
i2cRead(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x04); // fifo reset
// Insert FIFO count read?
mpu6050DmpFifoReady();
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x00); // ?? I think this enables a lot of stuff but disables fifo
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); // CLKSEL = PLL w Z ref
delay(2);
i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_2, 0x00);
i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 0x00); // full scale range +/- 2g
delay(2);
i2cRead(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 1, &temp);
i2cWrite(MPU6050_ADDRESS, MPU_RA_MOT_THR, 0x02);
i2cWrite(MPU6050_ADDRESS, MPU_RA_ZRMOT_THR, 0x9C);
i2cWrite(MPU6050_ADDRESS, MPU_RA_MOT_DUR, 0x50);
i2cWrite(MPU6050_ADDRESS, MPU_RA_ZRMOT_DUR, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x04); // fifo reset
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0x00);
i2cWrite(MPU6050_ADDRESS, MPU_RA_USER_CTRL, 0xC8); // fifo enable
mpu6050DmpBankSelect(0x01); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x6A);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 2, "\x06\x00"); // data
mpu6050DmpBankSelect(0x01); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 8, "\x00\x00\x00\x00\x00\x00\x00\x00"); // data
mpu6050DmpBankSelect(0x00); // bank
i2cWrite(MPU6050_ADDRESS, MPU_RA_MEM_START_ADDR, 0x60);
i2cWriteBuffer(MPU6050_ADDRESS, MPU_RA_MEM_R_W, 4, "\x40\x00\x00\x00"); // data
}
