static void mwii_calibrate_mpu6050(void){
// calibrate gyro offset
int16_t ax, ay, az, gx, gy, gz;
mpu6050_setTCXGyroOffset(&_brd.mpu, 0); //14
mpu6050_setTCYGyroOffset(&_brd.mpu, 0); //20
mpu6050_setTCZGyroOffset(&_brd.mpu, 0); //-49
mpu6050_setXGyroOffset(&_brd.mpu, 0); //14
mpu6050_setYGyroOffset(&_brd.mpu, 0); //20
mpu6050_setZGyroOffset(&_brd.mpu, 0); //-49
int32_t aax = 0, aay= 0, aaz = 0, ggx = 0, ggy = 0, ggz = 0;
static const int iterations = 100;
for(int c = 0; c < iterations; c++){
mpu6050_readRawAcc(&_brd.mpu, &ax, &ay, &az);
mpu6050_readRawGyr(&_brd.mpu, &gx, &gy, &gz);
aax += ax; aay += ay; aaz += az;
ggx += gx; ggy += gy; ggz += gz;
delay_us(10);
}
brd->acc_bias_x = aax / iterations;
brd->acc_bias_y = aay / iterations;
brd->acc_bias_z = (aaz / iterations) + 16384;
mpu6050_setXGyroOffset(&_brd.mpu, -(int16_t)(ggx / iterations * 2) | 1); //14
mpu6050_setYGyroOffset(&_brd.mpu, -(int16_t)(ggy / iterations * 2) | 1); //20
mpu6050_setZGyroOffset(&_brd.mpu, -(int16_t)(ggz / iterations * 2) | 1); //-49
// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
// the accelerometer biases calculated above must be divided by 8.
// Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
// preserve temperature compensation bit when writing back to accelerometer bias registers
//bias_x = (int16_t)(bias_x - (aax / iterations / 16384.0 * 2048)) | 1;
//bias_y = (int16_t)(bias_y - (aay / iterations / 16384.0 * 2048)) | 1;
//bias_z = (int16_t)(bias_z - (aaz / iterations / 16384.0 * 2048)) | 1;
//mpu6050_setXAccOffset(&_brd.mpu, -(int16_t)(bias_x));
//mpu6050_setYAccOffset(&_brd.mpu, -(int16_t)(bias_y));
//mpu6050_setZAccOffset(&_brd.mpu, -(int16_t)(aaz));
/*mpu6050_setXAccOffset(&_brd.mpu, 0x0100);
mpu6050_setYAccOffset(&_brd.mpu, 0x0100);
mpu6050_setZAccOffset(&_brd.mpu, 0x0100); */
//mpu6050_setXGyroOffset(&_brd.mpu, -15 * 2); //14
//mpu6050_setYGyroOffset(&_brd.mpu, -20 * 2); //20
//mpu6050_setZGyroOffset(&_brd.mpu, 54 * 2); //-49
//mpu6050_setZAccOffset(&_brd.mpu, -20); // 15818
/*
mpu6050_getRawData(&_brd.mpu, &ax, &ay, &az, &gx, &gy, &gz);
mpu6050_setXGyroOffset(&_brd.mpu, gx);
mpu6050_setYGyroOffset(&_brd.mpu, gy);
mpu6050_setZGyroOffset(&_brd.mpu, gz); */
}
void mwii_read_sensors(struct fc_data *data){
data->flags = 0xffff; // all
mpu6050_readRawAcc(&_brd.mpu,
&data->raw_acc.x,
&data->raw_acc.y,
&data->raw_acc.z);
mpu6050_convertAcc(&_brd.mpu,
data->raw_acc.x,
data->raw_acc.y,
data->raw_acc.z,
&data->acc_g.x,
&data->acc_g.y,
&data->acc_g.z
);
mpu6050_readRawGyr(&_brd.mpu,
&data->raw_gyr.x,
&data->raw_gyr.y,
&data->raw_gyr.z
);
mpu6050_convertGyr(&_brd.mpu,
data->raw_gyr.x,
data->raw_gyr.y,
data->raw_gyr.z,
&data->gyr_deg.x,
&data->gyr_deg.y,
&data->gyr_deg.z
);
/*
hmc5883l_readRawMag(&_brd.hmc,
&data->raw_mag.x,
&data->raw_mag.y,
&data->raw_mag.z
);
hmc5883l_convertMag(&_brd.hmc,
data->raw_mag.x,
data->raw_mag.y,
data->raw_mag.z,
&data->mag.x,
&data->mag.y,
&data->mag.z
);
*/
int16_t sonar = hcsr04_read_distance_in_cm(&brd->hcsr);
data->atmospheric_altitude = bmp085_read_altitude(&brd->bmp);
data->sonar_altitude = (sonar > 0)?((float)sonar / 100.0):-1;
data->temperature = bmp085_read_temperature(&brd->bmp);
data->pressure = bmp085_read_pressure(&brd->bmp);
//data->vbat = (adc0_read_cached(2) / 65535.0);
}
//**********************
// READING SENSOR DATA
//**********************
/// reads last measured acceleration in G
void mwii_read_acceleration_g(float *ax, float *ay, float *az){
int16_t x, y, z;
mpu6050_readRawAcc(&_brd.mpu, &x, &y, &z);
mpu6050_convertAcc(&_brd.mpu, x, y, z, ax, ay, az);
}
