Tyler Weaver
AHRS library, modified version of Peter Bartz work.
Diff: AHRS.cpp
- Revision:
- 0:014ee3239c80
- Child:
- 1:da3b20b5d38a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AHRS.cpp Thu Nov 08 18:57:18 2012 +0000
@@ -0,0 +1,326 @@
+/* This file is part of the Razor AHRS Firmware */
+#include "AHRS.h"
+#include <math.h>
+
+void IMU::read_sensors()
+{
+ Read_Gyro(); // Read gyroscope (and temperature)
+ Read_Accel(); // Read accelerometer
+ Read_Magn(); // Read magnetometer
+}
+
+// Read every sensor and record a time stamp
+// Init DCM with unfiltered orientation
+// TODO re-init global vars?
+void IMU::reset_sensor_fusion()
+{
+ float temp0[3] = { accel[0], accel[1], accel[2] };
+ float temp1[3];
+ float temp2[3];
+ float xAxis[] = {1.0f, 0.0f, 0.0f};
+
+ read_sensors();
+ timestamp = timer.read_ms();
+
+ // GET PITCH
+ // Using y-z-plane-component/x-component of gravity vector
+ pitch = -atan2(accel[0], sqrt((double)(accel[1] * accel[1] + accel[2] * accel[2])));
+
+ // GET ROLL
+ // Compensate pitch of gravity vector
+
+ Vector_Cross_Product(temp1, temp0, xAxis);
+ Vector_Cross_Product(temp2, xAxis, temp1);
+ // Normally using x-z-plane-component/y-component of compensated gravity vector
+ // roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
+ // Since we compensated for pitch, x-z-plane-component equals z-component:
+ roll = atan2(temp2[1], temp2[2]);
+
+ // GET YAW
+ Compass_Heading();
+ yaw = MAG_Heading;
+
+ // Init rotation matrix
+ init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
+}
+
+// Apply calibration to raw sensor readings
+void IMU::compensate_sensor_errors()
+{
+ // Compensate accelerometer error
+ accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
+ accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
+ accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
+
+ // Compensate magnetometer error
+ magnetom[0] = (magnetom[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
+ magnetom[1] = (magnetom[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
+ magnetom[2] = (magnetom[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
+
+ // Compensate gyroscope error
+ gyro[0] -= GYRO_AVERAGE_OFFSET_X;
+ gyro[1] -= GYRO_AVERAGE_OFFSET_Y;
+ gyro[2] -= GYRO_AVERAGE_OFFSET_Z;
+}
+
+// Reset calibration session if reset_calibration_session_flag is set
+void IMU::check_reset_calibration_session()
+{
+ // Raw sensor values have to be read already, but no error compensation applied
+
+ // Reset this calibration session?
+ if (!reset_calibration_session_flag) return;
+
+ // Reset acc and mag calibration variables
+ for (int i = 0; i < 3; i++) {
+ accel_min[i] = accel_max[i] = accel[i];
+ magnetom_min[i] = magnetom_max[i] = magnetom[i];
+ }
+
+ // Reset gyro calibration variables
+ gyro_num_samples = 0; // Reset gyro calibration averaging
+ gyro_average[0] = gyro_average[1] = gyro_average[2] = 0;
+
+ reset_calibration_session_flag = false;
+}
+
+void IMU::turn_output_stream_on()
+{
+ output_stream_on = true;
+ statusLed = 1;
+}
+
+void IMU::turn_output_stream_off()
+{
+ output_stream_on = false;
+ statusLed = 0;
+}
+
+// Blocks until another byte is available on serial port
+char IMU::readChar()
+{
+ while (pc.rxBufferGetCount() < 1) { } // Block
+ return pc.getc();
+}
+
+void IMU::readInput()
+{
+ // Read incoming control messages
+ if (pc.rxBufferGetCount() >= 2) {
+ if (pc.getc() == '#') { // Start of new control message
+ int command = pc.getc(); // Commands
+ if (command == 'f') // request one output _f_rame
+ output_single_on = true;
+ else if (command == 's') { // _s_ynch request
+ // Read ID
+ char id[2];
+ id[0] = readChar();
+ id[1] = readChar();
+
+ // Reply with synch message
+ pc.printf("#SYNCH");
+ pc.putc(id[0]);
+ pc.putc(id[1]);
+ pc.printf(NEW_LINE);
+ } else if (command == 'o') { // Set _o_utput mode
+ char output_param = readChar();
+ if (output_param == 'n') { // Calibrate _n_ext sensor
+ curr_calibration_sensor = (curr_calibration_sensor + 1) % 3;
+ reset_calibration_session_flag = true;
+ } else if (output_param == 't') // Output angles as _t_ext
+ output_mode = OUTPUT__MODE_ANGLES_TEXT;
+ else if (output_param == 'b') // Output angles in _b_inary form
+ output_mode = OUTPUT__MODE_ANGLES_BINARY;
+ else if (output_param == 'c') { // Go to _c_alibration mode
+ output_mode = OUTPUT__MODE_CALIBRATE_SENSORS;
+ reset_calibration_session_flag = true;
+ } else if (output_param == 's') // Output _s_ensor values as text
+ output_mode = OUTPUT__MODE_SENSORS_TEXT;
+ else if (output_param == '0') { // Disable continuous streaming output
+ turn_output_stream_off();
+ reset_calibration_session_flag = true;
+ } else if (output_param == '1') { // Enable continuous streaming output
+ reset_calibration_session_flag = true;
+ turn_output_stream_on();
+ } else if (output_param == 'e') { // _e_rror output settings
+ char error_param = readChar();
+ if (error_param == '0') output_errors = false;
+ else if (error_param == '1') output_errors = true;
+ else if (error_param == 'c') { // get error count
+ pc.printf("#AMG-ERR:%d,%d,%d" NEW_LINE,num_accel_errors,num_magn_errors,num_gyro_errors);
+ }
+ }
+ }
+#if OUTPUT__HAS_RN_BLUETOOTH == true
+ // Read messages from bluetooth module
+ // For this to work, the connect/disconnect message prefix of the module has to be set to "#".
+ else if (command == 'C') // Bluetooth "#CONNECT" message (does the same as "#o1")
+ turn_output_stream_on();
+ else if (command == 'D') // Bluetooth "#DISCONNECT" message (does the same as "#o0")
+ turn_output_stream_off();
+#endif // OUTPUT__HAS_RN_BLUETOOTH == true
+ } else { } // Skip character
+ }
+}
+
+IMU::IMU()
+ : gyro_num_samples(0)
+ , yaw(0)
+ , pitch(0)
+ , roll(0)
+ , MAG_Heading(0)
+ , timestamp(0)
+ , timestamp_old(0)
+ , G_Dt(0)
+ , output_mode(OUTPUT__MODE_ANGLES_BINARY) // Select your startup output mode here!// Select your startup output mode here!
+ , output_stream_on(false)
+ , output_single_on(true)
+ , curr_calibration_sensor(0)
+ , reset_calibration_session_flag(true)
+ , num_accel_errors(0)
+ , num_magn_errors(0)
+ , num_gyro_errors(0)
+ , output_errors(true)
+ , statusLed(LED1)
+ , pc(USBTX, USBRX)
+ , Wire(p28, p27)
+{
+
+ accel[0] = accel_min[0] = accel_max[0] = magnetom[0] = magnetom_min[0] = magnetom_max[0] = gyro[0] = gyro_average[0] = 0;
+ accel[1] = accel_min[1] = accel_max[1] = magnetom[1] = magnetom_min[1] = magnetom_max[1] = gyro[1] = gyro_average[1] = 0;
+ accel[2] = accel_min[2] = accel_max[2] = magnetom[2] = magnetom_min[2] = magnetom_max[2] = gyro[2] = gyro_average[2] = 0;
+
+ Accel_Vector[0] = Gyro_Vector[0] = Omega_Vector[0] = Omega_P[0] = Omega_I[0] = Omega[0] = errorRollPitch[0] = errorYaw[0] = 0;
+ Accel_Vector[1] = Gyro_Vector[1] = Omega_Vector[1] = Omega_P[1] = Omega_I[1] = Omega[1] = errorRollPitch[1] = errorYaw[1] = 0;
+ Accel_Vector[2] = Gyro_Vector[2] = Omega_Vector[2] = Omega_P[2] = Omega_I[2] = Omega[2] = errorRollPitch[2] = errorYaw[2] = 0;
+
+ DCM_Matrix[0][0] = 1;
+ DCM_Matrix[0][1] = 0;
+ DCM_Matrix[0][2] = 0;
+ DCM_Matrix[1][0] = 0;
+ DCM_Matrix[1][1] = 1;
+ DCM_Matrix[1][2] = 0;
+ DCM_Matrix[2][0] = 0;
+ DCM_Matrix[2][1] = 0;
+ DCM_Matrix[2][2] = 1;
+
+ Update_Matrix[0][0] = 0;
+ Update_Matrix[0][1] = 1;
+ Update_Matrix[0][2] = 2;
+ Update_Matrix[1][0] = 3;
+ Update_Matrix[1][1] = 4;
+ Update_Matrix[1][2] = 5;
+ Update_Matrix[2][0] = 6;
+ Update_Matrix[2][1] = 7;
+ Update_Matrix[2][2] = 8;
+
+ Temporary_Matrix[0][0] = 0;
+ Temporary_Matrix[0][1] = 0;
+ Temporary_Matrix[0][2] = 0;
+ Temporary_Matrix[1][0] = 0;
+ Temporary_Matrix[1][1] = 0;
+ Temporary_Matrix[1][2] = 0;
+ Temporary_Matrix[2][0] = 0;
+ Temporary_Matrix[2][1] = 0;
+ Temporary_Matrix[2][2] = 0;
+
+ timer.start();
+ // Init serial output
+ pc.baud(OUTPUT__BAUD_RATE);
+
+ // Init status LED
+ statusLed = 0;
+
+ // Init sensors
+ wait_ms(50); // Give sensors enough time to start
+ I2C_Init();
+ Accel_Init();
+ Magn_Init();
+ Gyro_Init();
+
+ // Read sensors, init DCM algorithm
+ wait_ms(20); // Give sensors enough time to collect data
+ reset_sensor_fusion();
+
+ // Init output
+#if (OUTPUT__HAS_RN_BLUETOOTH == true) || (OUTPUT__STARTUP_STREAM_ON == false)
+ turn_output_stream_off();
+#else
+ turn_output_stream_on();
+#endif
+}
+
+// Main loop
+void IMU::loop()
+{
+ timestamp_old = timestamp;
+ timestamp = timer.read_ms();
+ if (timestamp > timestamp_old)
+ G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
+ else
+ G_Dt = 0;
+
+ // Update sensor readings
+ read_sensors();
+
+ if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS) { // We're in calibration mode
+ check_reset_calibration_session(); // Check if this session needs a reset
+ if (output_stream_on || output_single_on)
+ output_calibration(curr_calibration_sensor);
+ } else if (output_mode == OUTPUT__MODE_SENSORS_TEXT) {
+ // Apply sensor calibration
+ compensate_sensor_errors();
+
+ if (output_stream_on || output_single_on)
+ output_sensors();
+ } else if (output_mode == OUTPUT__MODE_ANGLES_TEXT || output_mode == OUTPUT__MODE_ANGLES_BINARY) {
+ // Apply sensor calibration
+ compensate_sensor_errors();
+
+ // Run DCM algorithm
+ Compass_Heading(); // Calculate magnetic heading
+ Matrix_update();
+ Normalize();
+ Drift_correction();
+ Euler_angles();
+
+ if (output_stream_on || output_single_on)
+ output_angles();
+ }
+
+ output_single_on = false;
+
+#if DEBUG__PRINT_LOOP_TIME == true
+ pc.printf("loop time (ms) = %f" NEW_LINE, timer.read_ms() - timestamp);
+#endif
+}
+
+
+// Main loop
+void IMU::loop(float *data)
+{
+ timestamp_old = timestamp;
+ timestamp = timer.read_ms();
+ if (timestamp > timestamp_old)
+ G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
+ else
+ G_Dt = 0;
+
+ // Update sensor readings
+ read_sensors();
+
+ // Apply sensor calibration
+ compensate_sensor_errors();
+
+ // Run DCM algorithm
+ Compass_Heading(); // Calculate magnetic heading
+ Matrix_update();
+ Normalize();
+ Drift_correction();
+ Euler_angles();
+
+ data[0] = temperature;
+ data[1] = TO_DEG(yaw);
+ data[2] = TO_DEG(pitch);
+ data[3] = TO_DEG(roll);
+}
\ No newline at end of file