// Update Sensor Readings into _latest_sensor_data
bool FC_IMU::update_sensors() {
uint8_t data[14];
_tw.read_bytes(_i2c_address, ACCEL_XOUT_H, 14, data);
_accel_data[0] = ((((int16_t)data[0]) << 8) | data[1]) - _accel_baseline[0];
_accel_data[1] = ((((int16_t)data[2]) << 8) | data[3]) - _accel_baseline[1];
_accel_data[2] = ((((int16_t)data[4]) << 8) | data[5]) - _accel_baseline[2] + (int16_t)16384;
_gyro_data[0] = ((((int16_t)data[8]) << 8) | data[9]) - _gyro_baseline[0];
_gyro_data[1] = ((((int16_t)data[10]) << 8) | data[11]) - _gyro_baseline[1];
_gyro_data[2] = ((((int16_t)data[12]) << 8) | data[13]) - _gyro_baseline[2];
if (abs(_gyro_data[0]) < 30 ) {
_gyro_data[0] = (int16_t)0;
}
if (abs(_gyro_data[1]) < 30 ) {
_gyro_data[1] = (int16_t)0;
}
if (abs(_gyro_data[2]) < 100 ) {
_gyro_data[2] = (int16_t)0;
}
gyro_rate();
accel_angle();
update_quaternion();
}
// Whole function to get angles from accelerometer + compass
vectorMe accelcompass_angle_acquisition(vectorMe a, vectorMe m)
{
// Vector p should be defined as pointing forward, parallel to the ground, with coordinates {X, Y, Z}.
vectorMe p = {0, -1, 0};
vectorMe angles;
float heading;
//compass_read_data(&a, &m);
heading = get_heading(&a, &m, &p);
accel_g(&a);
angles = accel_angle(a);
angles.z= heading;
return angles;
}
// Whole function to get angles from accelerometer
vectorMe accel_angle_acquisition(void)
{
vectorMe a;
vectorMe angles;
//compass_read_data(&a, &m);
accel_g(&a);
angles = accel_angle(a);
// Ensures that Z gets negative if pointing downward
if (angles.x<0)
{
angles.z = - angles.z;
}
return angles;
}
// Calculate the accelerometer measurement noise
vectorMe accel_measurement_noise (void)
{
// Calculated variance: {0.02;0.02;0.02}
vectorMe a;
vectorMe store[30];
vectorMe variance= {0,0,0};
vectorMe sum ={0,0,0};
vectorMe sum2 ={0,0,0};
// Data acquisition and storage
for (int i =0; i<20; i++)
{
//compass_read_data(&a, &m);
accel_g(&a);
store[i] = accel_angle(a);
}
// Calculate variance // formula: variance = (sum2 - (sum^2/ number of samples))/(number of samples -1)
// Calculate the sum of values
for (int k=0; k<20; k++)
{
sum.x = sum.x+store[k].x;
sum.y = sum.y+store[k].y;
sum.z = sum.z+store[k].z;
}
// Calculate the sum of values^2
for (int k=0; k<20; k++)
{
sum2.x = sum2.x+(store[k].x)*(store[k].x);
sum2.y = sum2.y+(store[k].y)*(store[k].y);
sum2.z = sum2.z+(store[k].z)*(store[k].z);
}
// Calculate the variance
variance.x += (sum2.x - ((sum.x)*(sum.x))/20)/19;
variance.y += (sum2.y - ((sum.y)*(sum.y))/20)/19;
variance.z += (sum2.z - ((sum.z)*(sum.z))/20)/19;
return variance;
}
