Diff: Razor_AHRS.cpp

Revision:

2:5aa75c3d8cc3

Parent:

0:9a72d42c0da3

--- a/Razor_AHRS.cpp	Wed Dec 28 17:13:14 2011 +0000
+++ b/Razor_AHRS.cpp	Wed Dec 28 18:09:14 2011 +0000
@@ -26,7 +26,7 @@
 
     // 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
@@ -96,36 +96,7 @@
     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() {
-
+void IMU::readInput() {
     // Read incoming control messages
     if (pc.rxBufferGetCount() >= 2) {
         if (pc.getc() == '#') { // Start of new control message
@@ -182,55 +153,149 @@
 #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;
+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(-1) // 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;
 
-        // Update sensor readings
-        read_sensors();
+    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;
 
-        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();
+    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
+}
 
-            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();
+// 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();
 
-            // Run DCM algorithm
-            Compass_Heading(); // Calculate magnetic heading
-            Matrix_update();
-            Normalize();
-            Drift_correction();
-            Euler_angles();
+    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_angles();
-        }
+        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();
 
-        output_single_on = false;
+        // 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...");
-    }
+    pc.printf("loop time (ms) = %f" NEW_LINE, timer.read_ms() - timestamp);
 #endif
 }
+
+int main() {
+    IMU imu;
+    
+    Ticker looper;
+    looper.attach(&imu, &IMU::loop, (float)OUTPUT__DATA_INTERVAL / 1000.0f); // 50Hz update
+    
+    while (1)
+    {
+        imu.readInput();
+        wait_ms(5);
+    }
+    
+    return 0;
+}