Tyler Weaver / AHRS

AHRS library, modified version of Peter Bartz work.

Dependencies:   MODSERIAL

Dependents:   AHRS_demo

AHRS.cpp@1:da3b20b5d38a, 2012-11-08 (annotated)

Committer:
tylerjw
Date:
Thu Nov 08 20:13:35 2012 +0000
Revision:
1:da3b20b5d38a
Parent:
0:014ee3239c80 
Modified AHRS library.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tylerjw 0:014ee3239c80 1 /* This file is part of the Razor AHRS Firmware */
tylerjw 0:014ee3239c80 2 #include "AHRS.h"
tylerjw 0:014ee3239c80 3 #include <math.h>
tylerjw 0:014ee3239c80 4
tylerjw 0:014ee3239c80 5 void IMU::read_sensors()
tylerjw 0:014ee3239c80 6 {
tylerjw 0:014ee3239c80 7 Read_Gyro(); // Read gyroscope (and temperature)
tylerjw 0:014ee3239c80 8 Read_Accel(); // Read accelerometer
tylerjw 0:014ee3239c80 9 Read_Magn(); // Read magnetometer
tylerjw 0:014ee3239c80 10 }
tylerjw 0:014ee3239c80 11
tylerjw 0:014ee3239c80 12 // Read every sensor and record a time stamp
tylerjw 0:014ee3239c80 13 // Init DCM with unfiltered orientation
tylerjw 0:014ee3239c80 14 // TODO re-init global vars?
tylerjw 0:014ee3239c80 15 void IMU::reset_sensor_fusion()
tylerjw 0:014ee3239c80 16 {
tylerjw 0:014ee3239c80 17 float temp0[3] = { accel[0], accel[1], accel[2] };
tylerjw 0:014ee3239c80 18 float temp1[3];
tylerjw 0:014ee3239c80 19 float temp2[3];
tylerjw 0:014ee3239c80 20 float xAxis[] = {1.0f, 0.0f, 0.0f};
tylerjw 0:014ee3239c80 21
tylerjw 0:014ee3239c80 22 read_sensors();
tylerjw 0:014ee3239c80 23 timestamp = timer.read_ms();
tylerjw 0:014ee3239c80 24
tylerjw 0:014ee3239c80 25 // GET PITCH
tylerjw 0:014ee3239c80 26 // Using y-z-plane-component/x-component of gravity vector
tylerjw 0:014ee3239c80 27 pitch = -atan2(accel[0], sqrt((double)(accel[1] * accel[1] + accel[2] * accel[2])));
tylerjw 0:014ee3239c80 28
tylerjw 0:014ee3239c80 29 // GET ROLL
tylerjw 0:014ee3239c80 30 // Compensate pitch of gravity vector
tylerjw 0:014ee3239c80 31
tylerjw 0:014ee3239c80 32 Vector_Cross_Product(temp1, temp0, xAxis);
tylerjw 0:014ee3239c80 33 Vector_Cross_Product(temp2, xAxis, temp1);
tylerjw 0:014ee3239c80 34 // Normally using x-z-plane-component/y-component of compensated gravity vector
tylerjw 0:014ee3239c80 35 // roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
tylerjw 0:014ee3239c80 36 // Since we compensated for pitch, x-z-plane-component equals z-component:
tylerjw 0:014ee3239c80 37 roll = atan2(temp2[1], temp2[2]);
tylerjw 0:014ee3239c80 38
tylerjw 0:014ee3239c80 39 // GET YAW
tylerjw 0:014ee3239c80 40 Compass_Heading();
tylerjw 0:014ee3239c80 41 yaw = MAG_Heading;
tylerjw 0:014ee3239c80 42
tylerjw 0:014ee3239c80 43 // Init rotation matrix
tylerjw 0:014ee3239c80 44 init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
tylerjw 0:014ee3239c80 45 }
tylerjw 0:014ee3239c80 46
tylerjw 0:014ee3239c80 47 // Apply calibration to raw sensor readings
tylerjw 0:014ee3239c80 48 void IMU::compensate_sensor_errors()
tylerjw 0:014ee3239c80 49 {
tylerjw 0:014ee3239c80 50 // Compensate accelerometer error
tylerjw 0:014ee3239c80 51 accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
tylerjw 0:014ee3239c80 52 accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
tylerjw 0:014ee3239c80 53 accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
tylerjw 0:014ee3239c80 54
tylerjw 0:014ee3239c80 55 // Compensate magnetometer error
tylerjw 0:014ee3239c80 56 magnetom[0] = (magnetom[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
tylerjw 0:014ee3239c80 57 magnetom[1] = (magnetom[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
tylerjw 0:014ee3239c80 58 magnetom[2] = (magnetom[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
tylerjw 0:014ee3239c80 59
tylerjw 0:014ee3239c80 60 // Compensate gyroscope error
tylerjw 0:014ee3239c80 61 gyro[0] -= GYRO_AVERAGE_OFFSET_X;
tylerjw 0:014ee3239c80 62 gyro[1] -= GYRO_AVERAGE_OFFSET_Y;
tylerjw 0:014ee3239c80 63 gyro[2] -= GYRO_AVERAGE_OFFSET_Z;
tylerjw 0:014ee3239c80 64 }
tylerjw 0:014ee3239c80 65
tylerjw 0:014ee3239c80 66 // Reset calibration session if reset_calibration_session_flag is set
tylerjw 0:014ee3239c80 67 void IMU::check_reset_calibration_session()
tylerjw 0:014ee3239c80 68 {
tylerjw 0:014ee3239c80 69 // Raw sensor values have to be read already, but no error compensation applied
tylerjw 0:014ee3239c80 70
tylerjw 0:014ee3239c80 71 // Reset this calibration session?
tylerjw 0:014ee3239c80 72 if (!reset_calibration_session_flag) return;
tylerjw 0:014ee3239c80 73
tylerjw 0:014ee3239c80 74 // Reset acc and mag calibration variables
tylerjw 0:014ee3239c80 75 for (int i = 0; i < 3; i++) {
tylerjw 0:014ee3239c80 76 accel_min[i] = accel_max[i] = accel[i];
tylerjw 0:014ee3239c80 77 magnetom_min[i] = magnetom_max[i] = magnetom[i];
tylerjw 0:014ee3239c80 78 }
tylerjw 0:014ee3239c80 79
tylerjw 0:014ee3239c80 80 // Reset gyro calibration variables
tylerjw 0:014ee3239c80 81 gyro_num_samples = 0; // Reset gyro calibration averaging
tylerjw 0:014ee3239c80 82 gyro_average[0] = gyro_average[1] = gyro_average[2] = 0;
tylerjw 0:014ee3239c80 83
tylerjw 0:014ee3239c80 84 reset_calibration_session_flag = false;
tylerjw 0:014ee3239c80 85 }
tylerjw 0:014ee3239c80 86
tylerjw 0:014ee3239c80 87 void IMU::turn_output_stream_on()
tylerjw 0:014ee3239c80 88 {
tylerjw 0:014ee3239c80 89 output_stream_on = true;
tylerjw 0:014ee3239c80 90 statusLed = 1;
tylerjw 0:014ee3239c80 91 }
tylerjw 0:014ee3239c80 92
tylerjw 0:014ee3239c80 93 void IMU::turn_output_stream_off()
tylerjw 0:014ee3239c80 94 {
tylerjw 0:014ee3239c80 95 output_stream_on = false;
tylerjw 0:014ee3239c80 96 statusLed = 0;
tylerjw 0:014ee3239c80 97 }
tylerjw 0:014ee3239c80 98
tylerjw 0:014ee3239c80 99 // Blocks until another byte is available on serial port
tylerjw 0:014ee3239c80 100 char IMU::readChar()
tylerjw 0:014ee3239c80 101 {
tylerjw 0:014ee3239c80 102 while (pc.rxBufferGetCount() < 1) { } // Block
tylerjw 0:014ee3239c80 103 return pc.getc();
tylerjw 0:014ee3239c80 104 }
tylerjw 0:014ee3239c80 105
tylerjw 0:014ee3239c80 106 void IMU::readInput()
tylerjw 0:014ee3239c80 107 {
tylerjw 0:014ee3239c80 108 // Read incoming control messages
tylerjw 0:014ee3239c80 109 if (pc.rxBufferGetCount() >= 2) {
tylerjw 0:014ee3239c80 110 if (pc.getc() == '#') { // Start of new control message
tylerjw 0:014ee3239c80 111 int command = pc.getc(); // Commands
tylerjw 0:014ee3239c80 112 if (command == 'f') // request one output _f_rame
tylerjw 0:014ee3239c80 113 output_single_on = true;
tylerjw 0:014ee3239c80 114 else if (command == 's') { // _s_ynch request
tylerjw 0:014ee3239c80 115 // Read ID
tylerjw 0:014ee3239c80 116 char id[2];
tylerjw 0:014ee3239c80 117 id[0] = readChar();
tylerjw 0:014ee3239c80 118 id[1] = readChar();
tylerjw 0:014ee3239c80 119
tylerjw 0:014ee3239c80 120 // Reply with synch message
tylerjw 0:014ee3239c80 121 pc.printf("#SYNCH");
tylerjw 0:014ee3239c80 122 pc.putc(id[0]);
tylerjw 0:014ee3239c80 123 pc.putc(id[1]);
tylerjw 0:014ee3239c80 124 pc.printf(NEW_LINE);
tylerjw 0:014ee3239c80 125 } else if (command == 'o') { // Set _o_utput mode
tylerjw 0:014ee3239c80 126 char output_param = readChar();
tylerjw 0:014ee3239c80 127 if (output_param == 'n') { // Calibrate _n_ext sensor
tylerjw 0:014ee3239c80 128 curr_calibration_sensor = (curr_calibration_sensor + 1) % 3;
tylerjw 0:014ee3239c80 129 reset_calibration_session_flag = true;
tylerjw 0:014ee3239c80 130 } else if (output_param == 't') // Output angles as _t_ext
tylerjw 0:014ee3239c80 131 output_mode = OUTPUT__MODE_ANGLES_TEXT;
tylerjw 0:014ee3239c80 132 else if (output_param == 'b') // Output angles in _b_inary form
tylerjw 0:014ee3239c80 133 output_mode = OUTPUT__MODE_ANGLES_BINARY;
tylerjw 0:014ee3239c80 134 else if (output_param == 'c') { // Go to _c_alibration mode
tylerjw 0:014ee3239c80 135 output_mode = OUTPUT__MODE_CALIBRATE_SENSORS;
tylerjw 0:014ee3239c80 136 reset_calibration_session_flag = true;
tylerjw 0:014ee3239c80 137 } else if (output_param == 's') // Output _s_ensor values as text
tylerjw 0:014ee3239c80 138 output_mode = OUTPUT__MODE_SENSORS_TEXT;
tylerjw 0:014ee3239c80 139 else if (output_param == '0') { // Disable continuous streaming output
tylerjw 0:014ee3239c80 140 turn_output_stream_off();
tylerjw 0:014ee3239c80 141 reset_calibration_session_flag = true;
tylerjw 0:014ee3239c80 142 } else if (output_param == '1') { // Enable continuous streaming output
tylerjw 0:014ee3239c80 143 reset_calibration_session_flag = true;
tylerjw 0:014ee3239c80 144 turn_output_stream_on();
tylerjw 0:014ee3239c80 145 } else if (output_param == 'e') { // _e_rror output settings
tylerjw 0:014ee3239c80 146 char error_param = readChar();
tylerjw 0:014ee3239c80 147 if (error_param == '0') output_errors = false;
tylerjw 0:014ee3239c80 148 else if (error_param == '1') output_errors = true;
tylerjw 0:014ee3239c80 149 else if (error_param == 'c') { // get error count
tylerjw 0:014ee3239c80 150 pc.printf("#AMG-ERR:%d,%d,%d" NEW_LINE,num_accel_errors,num_magn_errors,num_gyro_errors);
tylerjw 0:014ee3239c80 151 }
tylerjw 0:014ee3239c80 152 }
tylerjw 0:014ee3239c80 153 }
tylerjw 0:014ee3239c80 154 #if OUTPUT__HAS_RN_BLUETOOTH == true
tylerjw 0:014ee3239c80 155 // Read messages from bluetooth module
tylerjw 0:014ee3239c80 156 // For this to work, the connect/disconnect message prefix of the module has to be set to "#".
tylerjw 0:014ee3239c80 157 else if (command == 'C') // Bluetooth "#CONNECT" message (does the same as "#o1")
tylerjw 0:014ee3239c80 158 turn_output_stream_on();
tylerjw 0:014ee3239c80 159 else if (command == 'D') // Bluetooth "#DISCONNECT" message (does the same as "#o0")
tylerjw 0:014ee3239c80 160 turn_output_stream_off();
tylerjw 0:014ee3239c80 161 #endif // OUTPUT__HAS_RN_BLUETOOTH == true
tylerjw 0:014ee3239c80 162 } else { } // Skip character
tylerjw 0:014ee3239c80 163 }
tylerjw 0:014ee3239c80 164 }
tylerjw 0:014ee3239c80 165
tylerjw 0:014ee3239c80 166 IMU::IMU()
tylerjw 0:014ee3239c80 167 : gyro_num_samples(0)
tylerjw 0:014ee3239c80 168 , yaw(0)
tylerjw 0:014ee3239c80 169 , pitch(0)
tylerjw 0:014ee3239c80 170 , roll(0)
tylerjw 0:014ee3239c80 171 , MAG_Heading(0)
tylerjw 0:014ee3239c80 172 , timestamp(0)
tylerjw 0:014ee3239c80 173 , timestamp_old(0)
tylerjw 0:014ee3239c80 174 , G_Dt(0)
tylerjw 0:014ee3239c80 175 , output_mode(OUTPUT__MODE_ANGLES_BINARY) // Select your startup output mode here!// Select your startup output mode here!
tylerjw 0:014ee3239c80 176 , output_stream_on(false)
tylerjw 0:014ee3239c80 177 , output_single_on(true)
tylerjw 0:014ee3239c80 178 , curr_calibration_sensor(0)
tylerjw 0:014ee3239c80 179 , reset_calibration_session_flag(true)
tylerjw 0:014ee3239c80 180 , num_accel_errors(0)
tylerjw 0:014ee3239c80 181 , num_magn_errors(0)
tylerjw 0:014ee3239c80 182 , num_gyro_errors(0)
tylerjw 0:014ee3239c80 183 , output_errors(true)
tylerjw 0:014ee3239c80 184 , statusLed(LED1)
tylerjw 0:014ee3239c80 185 , pc(USBTX, USBRX)
tylerjw 0:014ee3239c80 186 , Wire(p28, p27)
tylerjw 0:014ee3239c80 187 {
tylerjw 0:014ee3239c80 188
tylerjw 0:014ee3239c80 189 accel[0] = accel_min[0] = accel_max[0] = magnetom[0] = magnetom_min[0] = magnetom_max[0] = gyro[0] = gyro_average[0] = 0;
tylerjw 0:014ee3239c80 190 accel[1] = accel_min[1] = accel_max[1] = magnetom[1] = magnetom_min[1] = magnetom_max[1] = gyro[1] = gyro_average[1] = 0;
tylerjw 0:014ee3239c80 191 accel[2] = accel_min[2] = accel_max[2] = magnetom[2] = magnetom_min[2] = magnetom_max[2] = gyro[2] = gyro_average[2] = 0;
tylerjw 0:014ee3239c80 192
tylerjw 0:014ee3239c80 193 Accel_Vector[0] = Gyro_Vector[0] = Omega_Vector[0] = Omega_P[0] = Omega_I[0] = Omega[0] = errorRollPitch[0] = errorYaw[0] = 0;
tylerjw 0:014ee3239c80 194 Accel_Vector[1] = Gyro_Vector[1] = Omega_Vector[1] = Omega_P[1] = Omega_I[1] = Omega[1] = errorRollPitch[1] = errorYaw[1] = 0;
tylerjw 0:014ee3239c80 195 Accel_Vector[2] = Gyro_Vector[2] = Omega_Vector[2] = Omega_P[2] = Omega_I[2] = Omega[2] = errorRollPitch[2] = errorYaw[2] = 0;
tylerjw 0:014ee3239c80 196
tylerjw 0:014ee3239c80 197 DCM_Matrix[0][0] = 1;
tylerjw 0:014ee3239c80 198 DCM_Matrix[0][1] = 0;
tylerjw 0:014ee3239c80 199 DCM_Matrix[0][2] = 0;
tylerjw 0:014ee3239c80 200 DCM_Matrix[1][0] = 0;
tylerjw 0:014ee3239c80 201 DCM_Matrix[1][1] = 1;
tylerjw 0:014ee3239c80 202 DCM_Matrix[1][2] = 0;
tylerjw 0:014ee3239c80 203 DCM_Matrix[2][0] = 0;
tylerjw 0:014ee3239c80 204 DCM_Matrix[2][1] = 0;
tylerjw 0:014ee3239c80 205 DCM_Matrix[2][2] = 1;
tylerjw 0:014ee3239c80 206
tylerjw 0:014ee3239c80 207 Update_Matrix[0][0] = 0;
tylerjw 0:014ee3239c80 208 Update_Matrix[0][1] = 1;
tylerjw 0:014ee3239c80 209 Update_Matrix[0][2] = 2;
tylerjw 0:014ee3239c80 210 Update_Matrix[1][0] = 3;
tylerjw 0:014ee3239c80 211 Update_Matrix[1][1] = 4;
tylerjw 0:014ee3239c80 212 Update_Matrix[1][2] = 5;
tylerjw 0:014ee3239c80 213 Update_Matrix[2][0] = 6;
tylerjw 0:014ee3239c80 214 Update_Matrix[2][1] = 7;
tylerjw 0:014ee3239c80 215 Update_Matrix[2][2] = 8;
tylerjw 0:014ee3239c80 216
tylerjw 0:014ee3239c80 217 Temporary_Matrix[0][0] = 0;
tylerjw 0:014ee3239c80 218 Temporary_Matrix[0][1] = 0;
tylerjw 0:014ee3239c80 219 Temporary_Matrix[0][2] = 0;
tylerjw 0:014ee3239c80 220 Temporary_Matrix[1][0] = 0;
tylerjw 0:014ee3239c80 221 Temporary_Matrix[1][1] = 0;
tylerjw 0:014ee3239c80 222 Temporary_Matrix[1][2] = 0;
tylerjw 0:014ee3239c80 223 Temporary_Matrix[2][0] = 0;
tylerjw 0:014ee3239c80 224 Temporary_Matrix[2][1] = 0;
tylerjw 0:014ee3239c80 225 Temporary_Matrix[2][2] = 0;
tylerjw 0:014ee3239c80 226
tylerjw 0:014ee3239c80 227 timer.start();
tylerjw 0:014ee3239c80 228 // Init serial output
tylerjw 0:014ee3239c80 229 pc.baud(OUTPUT__BAUD_RATE);
tylerjw 0:014ee3239c80 230
tylerjw 0:014ee3239c80 231 // Init status LED
tylerjw 0:014ee3239c80 232 statusLed = 0;
tylerjw 0:014ee3239c80 233
tylerjw 0:014ee3239c80 234 // Init sensors
tylerjw 0:014ee3239c80 235 wait_ms(50); // Give sensors enough time to start
tylerjw 0:014ee3239c80 236 I2C_Init();
tylerjw 0:014ee3239c80 237 Accel_Init();
tylerjw 0:014ee3239c80 238 Magn_Init();
tylerjw 0:014ee3239c80 239 Gyro_Init();
tylerjw 0:014ee3239c80 240
tylerjw 0:014ee3239c80 241 // Read sensors, init DCM algorithm
tylerjw 0:014ee3239c80 242 wait_ms(20); // Give sensors enough time to collect data
tylerjw 0:014ee3239c80 243 reset_sensor_fusion();
tylerjw 0:014ee3239c80 244
tylerjw 0:014ee3239c80 245 // Init output
tylerjw 0:014ee3239c80 246 #if (OUTPUT__HAS_RN_BLUETOOTH == true) || (OUTPUT__STARTUP_STREAM_ON == false)
tylerjw 0:014ee3239c80 247 turn_output_stream_off();
tylerjw 0:014ee3239c80 248 #else
tylerjw 0:014ee3239c80 249 turn_output_stream_on();
tylerjw 0:014ee3239c80 250 #endif
tylerjw 0:014ee3239c80 251 }
tylerjw 0:014ee3239c80 252
tylerjw 0:014ee3239c80 253 // Main loop
tylerjw 0:014ee3239c80 254 void IMU::loop()
tylerjw 0:014ee3239c80 255 {
tylerjw 0:014ee3239c80 256 timestamp_old = timestamp;
tylerjw 0:014ee3239c80 257 timestamp = timer.read_ms();
tylerjw 0:014ee3239c80 258 if (timestamp > timestamp_old)
tylerjw 0:014ee3239c80 259 G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
tylerjw 0:014ee3239c80 260 else
tylerjw 0:014ee3239c80 261 G_Dt = 0;
tylerjw 0:014ee3239c80 262
tylerjw 0:014ee3239c80 263 // Update sensor readings
tylerjw 0:014ee3239c80 264 read_sensors();
tylerjw 0:014ee3239c80 265
tylerjw 0:014ee3239c80 266 if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS) { // We're in calibration mode
tylerjw 0:014ee3239c80 267 check_reset_calibration_session(); // Check if this session needs a reset
tylerjw 0:014ee3239c80 268 if (output_stream_on || output_single_on)
tylerjw 0:014ee3239c80 269 output_calibration(curr_calibration_sensor);
tylerjw 0:014ee3239c80 270 } else if (output_mode == OUTPUT__MODE_SENSORS_TEXT) {
tylerjw 0:014ee3239c80 271 // Apply sensor calibration
tylerjw 0:014ee3239c80 272 compensate_sensor_errors();
tylerjw 0:014ee3239c80 273
tylerjw 0:014ee3239c80 274 if (output_stream_on || output_single_on)
tylerjw 0:014ee3239c80 275 output_sensors();
tylerjw 0:014ee3239c80 276 } else if (output_mode == OUTPUT__MODE_ANGLES_TEXT || output_mode == OUTPUT__MODE_ANGLES_BINARY) {
tylerjw 0:014ee3239c80 277 // Apply sensor calibration
tylerjw 0:014ee3239c80 278 compensate_sensor_errors();
tylerjw 0:014ee3239c80 279
tylerjw 0:014ee3239c80 280 // Run DCM algorithm
tylerjw 0:014ee3239c80 281 Compass_Heading(); // Calculate magnetic heading
tylerjw 0:014ee3239c80 282 Matrix_update();
tylerjw 0:014ee3239c80 283 Normalize();
tylerjw 0:014ee3239c80 284 Drift_correction();
tylerjw 0:014ee3239c80 285 Euler_angles();
tylerjw 0:014ee3239c80 286
tylerjw 0:014ee3239c80 287 if (output_stream_on || output_single_on)
tylerjw 0:014ee3239c80 288 output_angles();
tylerjw 0:014ee3239c80 289 }
tylerjw 0:014ee3239c80 290
tylerjw 0:014ee3239c80 291 output_single_on = false;
tylerjw 0:014ee3239c80 292
tylerjw 0:014ee3239c80 293 #if DEBUG__PRINT_LOOP_TIME == true
tylerjw 0:014ee3239c80 294 pc.printf("loop time (ms) = %f" NEW_LINE, timer.read_ms() - timestamp);
tylerjw 0:014ee3239c80 295 #endif
tylerjw 0:014ee3239c80 296 }
tylerjw 0:014ee3239c80 297
tylerjw 0:014ee3239c80 298
tylerjw 0:014ee3239c80 299 // Main loop
tylerjw 0:014ee3239c80 300 void IMU::loop(float *data)
tylerjw 0:014ee3239c80 301 {
tylerjw 0:014ee3239c80 302 timestamp_old = timestamp;
tylerjw 0:014ee3239c80 303 timestamp = timer.read_ms();
tylerjw 0:014ee3239c80 304 if (timestamp > timestamp_old)
tylerjw 0:014ee3239c80 305 G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
tylerjw 0:014ee3239c80 306 else
tylerjw 0:014ee3239c80 307 G_Dt = 0;
tylerjw 0:014ee3239c80 308
tylerjw 0:014ee3239c80 309 // Update sensor readings
tylerjw 0:014ee3239c80 310 read_sensors();
tylerjw 0:014ee3239c80 311
tylerjw 0:014ee3239c80 312 // Apply sensor calibration
tylerjw 1:da3b20b5d38a 313 //compensate_sensor_errors();
tylerjw 0:014ee3239c80 314
tylerjw 0:014ee3239c80 315 // Run DCM algorithm
tylerjw 0:014ee3239c80 316 Compass_Heading(); // Calculate magnetic heading
tylerjw 0:014ee3239c80 317 Matrix_update();
tylerjw 0:014ee3239c80 318 Normalize();
tylerjw 0:014ee3239c80 319 Drift_correction();
tylerjw 0:014ee3239c80 320 Euler_angles();
tylerjw 0:014ee3239c80 321
tylerjw 0:014ee3239c80 322 data[0] = temperature;
tylerjw 0:014ee3239c80 323 data[1] = TO_DEG(yaw);
tylerjw 0:014ee3239c80 324 data[2] = TO_DEG(pitch);
tylerjw 0:014ee3239c80 325 data[3] = TO_DEG(roll);
tylerjw 0:014ee3239c80 326 }