// 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; }