Diff: Razor_AHRS.cpp

Revision:

0:9a72d42c0da3

Child:

2:5aa75c3d8cc3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Razor_AHRS.cpp	Tue Dec 27 17:20:06 2011 +0000
@@ -0,0 +1,236 @@
+/* This file is part of the Razor AHRS Firmware */
+#include "Razor_AHRS.h"
+#include <math.h>
+
+void IMU::read_sensors() {
+    Read_Gyro(); // Read gyroscope
+    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::setup() {
+    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() {
+
+    // 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
+    }
+
+    // Time to read the sensors again?
+    int now = timer.read_ms();
+    if ((now - timestamp) >= OUTPUT__DATA_INTERVAL) {
+        timestamp_old = timestamp;
+        timestamp = now;
+        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
+        Serial.print("loop time (ms) = ");
+        Serial.println(millis() - timestamp);
+#endif
+    }
+#if DEBUG__PRINT_LOOP_TIME == true
+    else {
+        Serial.println("waiting...");
+    }
+#endif
+}