ArduCopter: move to use new INS library instead of IMU library

ArduCopter/ArduCopter.pde

@@ -78,7 +78,6 @@
 #include <AP_Math.h>        // ArduPilot Mega Vector/Matrix math Library
 #include <AP_Curve.h>       // Curve used to linearlise throttle pwm to thrust
 #include <AP_InertialSensor.h> // ArduPilot Mega Inertial Sensor (accel & gyro) Library
-#include <AP_IMU.h>         // ArduPilot Mega IMU Library
 #include <AP_PeriodicProcess.h>         // Parent header of Timer
                                         // (only included for makefile libpath to work)
 #include <AP_TimerProcess.h>            // TimerProcess is the scheduler for MPU6000 reads.
@@ -98,6 +97,7 @@
 #include <AP_RangeFinder.h>     // Range finder library
 #include <AP_OpticalFlow.h> // Optical Flow library
 #include <Filter.h>                     // Filter library
+#include <AP_Buffer.h>      // APM FIFO Buffer
 #include <ModeFilter.h>         // Mode Filter from Filter library
 #include <AverageFilter.h>      // Mode Filter from Filter library
 #include <AP_LeadFilter.h>      // GPS Lead filter
@@ -268,21 +268,20 @@ AP_InertialSensor_MPU6000 ins;
  #else
 AP_InertialSensor_Oilpan ins(&adc);
  #endif
-AP_IMU_INS  imu(&ins);
 
 // we don't want to use gps for yaw correction on ArduCopter, so pass
 // a NULL GPS object pointer
 static GPS         *g_gps_null;
 
  #if DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
-AP_AHRS_MPU6000  ahrs(&imu, g_gps, &ins);               // only works with APM2
+AP_AHRS_MPU6000  ahrs(&ins, g_gps);               // only works with APM2
  #else
-AP_AHRS_DCM ahrs(&imu, g_gps);
+AP_AHRS_DCM ahrs(&ins, g_gps);
  #endif
 
 // ahrs2 object is the secondary ahrs to allow running DMP in parallel with DCM
   #if SECONDARY_DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
-AP_AHRS_MPU6000  ahrs2(&imu, g_gps, &ins);               // only works with APM2
+AP_AHRS_MPU6000  ahrs2(&ins, g_gps);               // only works with APM2
   #endif
 
 #elif HIL_MODE == HIL_MODE_SENSORS
@@ -291,20 +290,19 @@ AP_ADC_HIL adc;
 AP_Baro_BMP085_HIL barometer;
 AP_Compass_HIL compass;
 AP_GPS_HIL              g_gps_driver(NULL);
-AP_IMU_Shim imu;
-AP_AHRS_DCM                         ahrs(&imu, g_gps);
 AP_InertialSensor_Stub ins;
+AP_AHRS_DCM                         ahrs(&ins, g_gps);
+
 
 static int32_t gps_base_alt;
 
 #elif HIL_MODE == HIL_MODE_ATTITUDE
 AP_ADC_HIL adc;
-AP_IMU_Shim imu;
+AP_InertialSensor_Stub ins;
-AP_AHRS_HIL             ahrs(&imu, g_gps);
+AP_AHRS_HIL             ahrs(&ins, g_gps);
 AP_GPS_HIL              g_gps_driver(NULL);
 AP_Compass_HIL compass;                  // never used
 AP_Baro_BMP085_HIL barometer;
-AP_InertialSensor_Stub ins;
 #ifdef OPTFLOW_ENABLED
 #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
 AP_OpticalFlow_ADNS3080 optflow(&spi3_semaphore,OPTFLOW_CS_PIN);
@@ -919,8 +917,8 @@ static uint32_t last_gps_time;
 
 // Set true if we have new PWM data to act on from the Radio
 static bool new_radio_frame;
-// Used to auto exit the in-flight leveler
+// Used to auto exit the roll_pitch_trim saving function
-static int16_t auto_level_counter;
+static uint8_t save_trim_counter;
 
 // Reference to the AP relay object - APM1 only
 AP_Relay relay;
@@ -984,7 +982,7 @@ void loop()
 
     // We want this to execute fast
     // ----------------------------
-    num_samples = imu.num_samples_available();
+    num_samples = ins.num_samples_available();
     if (num_samples >= NUM_IMU_SAMPLES_FOR_100HZ) {
 
 		#if DEBUG_FAST_LOOP == ENABLED
@@ -1148,8 +1146,8 @@ static void medium_loop()
         }
 #endif
 
-        // auto_trim, uses an auto_level algorithm
+        // save_trim - stores roll and pitch radio inputs to ahrs
-        auto_trim();
+        save_trim();
 
         // record throttle output
         // ------------------------------
@@ -2067,7 +2065,7 @@ static void read_AHRS(void)
 #endif
 
     ahrs.update();
-    omega = imu.get_gyro();
+    omega = ins.get_gyro();
 
 #if SECONDARY_DMP_ENABLED == ENABLED
     ahrs2.update();

ArduCopter/Attitude.pde

@@ -647,7 +647,7 @@ static int16_t get_z_damping()
 
 float get_world_Z_accel()
 {
-    accels_rot = ahrs.get_dcm_matrix() * imu.get_accel();
+    accels_rot = ahrs.get_dcm_matrix() * ins.get_accel();
     //Serial.printf("z %1.4f\n", accels_rot.z);
     return accels_rot.z;
 }
@@ -695,7 +695,7 @@ static float fullDampP = 0.100;
 
 float get_world_Z_accel()
 {
-    accels_rot = ahrs.get_dcm_matrix() * imu.get_accel();
+    accels_rot = ahrs.get_dcm_matrix() * ins.get_accel();
     return accels_rot.z;
 }
 

ArduCopter/GCS_Mavlink.pde

@@ -395,8 +395,8 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
 
 static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
 {
-    Vector3f accel = imu.get_accel();
+    Vector3f accel = ins.get_accel();
-    Vector3f gyro = imu.get_gyro();
+    Vector3f gyro = ins.get_gyro();
     mavlink_msg_raw_imu_send(
         chan,
         micros(),
@@ -432,8 +432,8 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
                                     compass.get_declination(),
                                     barometer.get_raw_pressure(),
                                     barometer.get_raw_temp(),
-                                    imu.gx(), imu.gy(), imu.gz(),
+                                    ins.gx(), ins.gy(), ins.gz(),
-                                    imu.ax(), imu.ay(), imu.az());
+                                    ins.ax(), ins.ay(), ins.az());
 }
 
 static void NOINLINE send_gps_status(mavlink_channel_t chan)
@@ -1065,7 +1065,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             if (packet.param1 == 1 ||
                 packet.param2 == 1 ||
                 packet.param3 == 1) {
-                imu.init_accel(mavlink_delay, flash_leds);
+                ins.init_accel(mavlink_delay, flash_leds);
             }
             if (packet.param4 == 1) {
                 trim_radio();
@@ -1748,9 +1748,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         accels.y = (float)packet.yacc / 1000.0;
         accels.z = (float)packet.zacc / 1000.0;
 
-        imu.set_gyro(gyros);
+        ins.set_gyro_offsets(gyros);
 
-        imu.set_accel(accels);
+        ins.set_accel_offsets(accels);
 
 
         // set AHRS hil sensor
@@ -1814,9 +1814,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         accels.y = (float)packet.yacc / 1000.0;
         accels.z = (float)packet.zacc / 1000.0;
 
-        imu.set_gyro(gyros);
+        ins.set_gyro_offsets(gyros);
 
-        imu.set_accel(accels);
+        ins.set_accel_offsets(accels);
 
         compass.setHIL(packet.xmag,packet.ymag,packet.zmag);
         break;

ArduCopter/Log.pde

@@ -292,7 +292,8 @@ static void Log_Read_GPS()
  #if INERTIAL_NAV == ENABLED
 static void Log_Write_Raw()
 {
-    Vector3f accel = imu.get_accel();
+    Vector3f gyro = ins.get_gyro();
+    Vector3f accel = ins.get_accel();
 
     DataFlash.WriteByte(HEAD_BYTE1);
     DataFlash.WriteByte(HEAD_BYTE2);
@@ -345,8 +346,8 @@ static void Log_Read_Raw()
  #else
 static void Log_Write_Raw()
 {
-    Vector3f gyro = imu.get_gyro();
+    Vector3f gyro = ins.get_gyro();
-    Vector3f accel = imu.get_accel();
+    Vector3f accel = ins.get_accel();
 
     DataFlash.WriteByte(HEAD_BYTE1);
     DataFlash.WriteByte(HEAD_BYTE2);
@@ -724,7 +725,7 @@ static void Log_Write_Performance()
     DataFlash.WriteByte(HEAD_BYTE1);
     DataFlash.WriteByte(HEAD_BYTE2);
     DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
-    DataFlash.WriteByte(    imu.adc_constraints);                               //1
+    DataFlash.WriteByte(    0);                                                 //1 - was adc_constraints
     DataFlash.WriteByte(    ahrs.renorm_range_count);                           //2
     DataFlash.WriteByte(    ahrs.renorm_blowup_count);                          //3
     DataFlash.WriteByte(    gps_fix_count);                                             //4

ArduCopter/Parameters.h

@@ -99,8 +99,8 @@ public:
         //
         // 140: Sensor parameters
         //
-        k_param_imu = 140, // sensor calibration
+        k_param_imu = 140, // deprecated - can be deleted
-        k_param_battery_monitoring,
+        k_param_battery_monitoring = 141,
         k_param_volt_div_ratio,
         k_param_curr_amp_per_volt,
         k_param_input_voltage,

ArduCopter/Parameters.pde

@@ -363,18 +363,14 @@ const AP_Param::Info var_info[] PROGMEM = {
     GOBJECT(compass,        "COMPASS_", Compass),
 
     // @Group: INS_
-    // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp
+    // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
-#if HIL_MODE == HIL_MODE_DISABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
+#if HIL_MODE == HIL_MODE_DISABLED
-    GOBJECT(ins,            "INS_", AP_InertialSensor_Oilpan),
+    GOBJECT(ins,            "INS_", AP_InertialSensor),
 #endif
 
     GOBJECT(gcs0,                   "SR0_",     GCS_MAVLINK),
     GOBJECT(gcs3,                   "SR3_",     GCS_MAVLINK),
 
-    // @Group: IMU_
-    // @Path: ../libraries/AP_IMU/IMU.cpp
-    GOBJECT(imu,                    "IMU_",     IMU),
-
     // @Group: AHRS_
     // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp
     GOBJECT(ahrs,                   "AHRS_",    AP_AHRS),

ArduCopter/control_modes.pde

@@ -148,102 +148,38 @@ static void read_trim_switch()
             }
         }
     }else if (option == CH7_AUTO_TRIM) {
-        if (g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
+        if(g.rc_7.radio_in > CH_7_PWM_TRIGGER && control_mode <= ACRO && g.rc_3.control_in == 0) {
-            auto_level_counter = 10;
+            save_trim_counter = 15;
         }
     }
 }
 
-static void auto_trim()
+// save_trim - adds roll and pitch trims from the radio to ahrs
+static void save_trim()
 {
-    if(auto_level_counter > 0) {
+    float roll_trim, pitch_trim;
-        //g.rc_1.dead_zone = 60;		// 60 = .6 degrees
-        //g.rc_2.dead_zone = 60;
 
-        auto_level_counter--;
+    if(save_trim_counter > 0) {
+        save_trim_counter--;
 
-        if( motors.armed() ) {
+        // first few iterations we simply flash the leds
-            // set high AHRS gains to force accelerometers to impact attitude estimate
+        led_mode = SAVE_TRIM_LEDS;
-            ahrs.set_fast_gains(true);
-#if SECONDARY_DMP_ENABLED == ENABLED
-            ahrs2.set_fast_gains(true);
-#endif
-        }
 
-        trim_accel();
+        if(save_trim_counter == 1) {
-        led_mode = AUTO_TRIM_LEDS;
+            // save roll trim
-        do_simple = false;
+            roll_trim = ToRad((float)g.rc_1.control_in/100.0);
-
+            pitch_trim = ToRad((float)g.rc_2.control_in/100.0);
-        if(auto_level_counter == 1) {
+            ahrs.add_trim(roll_trim, pitch_trim);
-            //g.rc_1.dead_zone = 0;		// 60 = .6 degrees
+            Serial.printf_P(PSTR("\nTrim Roll:%4.2f Pitch:%4.2f\n"),ToDeg(roll_trim), ToDeg(pitch_trim));
-            //g.rc_2.dead_zone = 0;
-            led_mode = NORMAL_LEDS;
-            clear_leds();
-            imu.save();
 
             reset_control_switch();
 
-            // go back to normal AHRS gains
+            // switch back leds to normal
-            if( motors.armed() ) {
+            led_mode = NORMAL_LEDS;
-                ahrs.set_fast_gains(false);
-#if SECONDARY_DMP_ENABLED == ENABLED
-                ahrs2.set_fast_gains(false);
-#endif
-            }
 
-            //Serial.println("Done");
+            save_trim_counter = 0;
-            auto_level_counter = 0;
         }
     }
 }
 
 
-/*
- *  How this works:
- *  Level Example:
- *  A_off: -14.00, -20.59, -30.80
- *
- *  Right roll Example:
- *  A_off: -6.73, 89.05, -46.02
- *
- *  Left Roll Example:
- *  A_off: -18.11, -160.31, -56.42
- *
- *  Pitch Forward:
- *  A_off: -127.00, -22.16, -50.09
- *
- *  Pitch Back:
- *  A_off: 201.95, -24.00, -88.56
- */
-
-static void trim_accel()
-{
-    reset_stability_I();
-
-    float trim_roll  = (float)g.rc_1.control_in / 30000.0;
-    float trim_pitch = (float)g.rc_2.control_in / 30000.0;
-
-    trim_roll       = constrain(trim_roll, -1.5, 1.5);
-    trim_pitch      = constrain(trim_pitch, -1.5, 1.5);
-
-    if(g.rc_1.control_in > 200) {    // Roll Right
-        imu.ay(imu.ay() - trim_roll);
-    }else if (g.rc_1.control_in < -200) {
-        imu.ay(imu.ay() - trim_roll);
-    }
-
-    if(g.rc_2.control_in > 200) {    // Pitch Back
-        imu.ax(imu.ax() + trim_pitch);
-    }else if (g.rc_2.control_in < -200) {
-        imu.ax(imu.ax() + trim_pitch);
-    }
-
-    /*
-     *  Serial.printf_P(PSTR("r:%1.2f  %1.2f \t| p:%1.2f  %1.2f\n"),
-     *                                               trim_roll,
-     *                                               (float)imu.ay(),
-     *                                               trim_pitch,
-     *                                               (float)imu.ax());
-     *  //*/
-}
-

ArduCopter/defines.h

@@ -67,7 +67,7 @@
 
 // LED output
 #define NORMAL_LEDS 0
-#define AUTO_TRIM_LEDS 1
+#define SAVE_TRIM_LEDS 1
 
 
 #define CH_7_PWM_TRIGGER 1800

ArduCopter/inertia.pde

@@ -6,7 +6,7 @@ void calc_inertia()
 {
     // rotate accels based on DCM
     // --------------------------
-    accels_rotated          = ahrs.get_dcm_matrix() * imu.get_accel();
+    accels_rotated          = ahrs.get_dcm_matrix() * ins.get_accel();
     //accels_rotated		+= accels_offset;						// skew accels to account for long term error using calibration
     accels_rotated.z        += 9.805;                                                                   // remove influence of gravity
 

ArduCopter/leds.pde

@@ -6,7 +6,7 @@ static void update_lights()
         update_GPS_light();
         break;
 
-    case AUTO_TRIM_LEDS:
+    case SAVE_TRIM_LEDS:
         dancing_light();
         break;
     }

ArduCopter/motors.pde

@@ -3,7 +3,6 @@
 // 10 = 1 second
 #define ARM_DELAY 20
 #define DISARM_DELAY 20
-#define LEVEL_DELAY 100
 
 
 // called at 10hz
@@ -12,7 +11,7 @@ static void arm_motors()
     static int16_t arming_counter;
 
     // don't allow arming/disarming in anything but manual
-    if ((g.rc_3.control_in > 0) || (arming_counter > LEVEL_DELAY)) {
+    if (g.rc_3.control_in > 0) {
         arming_counter = 0;
         return;
     }
@@ -30,13 +29,7 @@ static void arm_motors()
 
     // full right
     if (tmp > 4000) {
-        if (arming_counter == LEVEL_DELAY) {
+        if (arming_counter == ARM_DELAY) {
-            //Serial.printf("\nAL\n");
-            // begin auto leveling
-            auto_level_counter = 250;
-            arming_counter = 0;
-
-        }else if (arming_counter == ARM_DELAY) {
             if(motors.armed() == false) {
                 // arm the motors and configure for flight
 
@@ -74,14 +67,7 @@ static void arm_motors()
 
         // full left
     }else if (tmp < -4000) {
-        if (arming_counter == LEVEL_DELAY) {
+        if (arming_counter == DISARM_DELAY) {
-            //Serial.printf("\nLEV\n");
-
-            // begin manual leveling
-            imu.init_accel(mavlink_delay, flash_leds);
-            arming_counter = 0;
-
-        }else if (arming_counter == DISARM_DELAY) {
             if(motors.armed()) {
                 // arm the motors and configure for flight
                 init_disarm_motors();

ArduCopter/setup.pde

@@ -97,7 +97,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
     //report_xtrack();
     //report_throttle();
     report_flight_modes();
-    report_imu();
+    report_ins();
     report_compass();
     report_optflow();
 
@@ -247,10 +247,9 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
 static int8_t
 setup_accel(uint8_t argc, const Menu::arg *argv)
 {
-    imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
+    ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
-    imu.init_accel(delay, flash_leds);
+    ins.init_accel(delay, flash_leds);
-    print_accel_offsets();
+    report_ins();
-    report_imu();
     return(0);
 }
 
@@ -258,69 +257,10 @@ setup_accel(uint8_t argc, const Menu::arg *argv)
 static int8_t
 setup_accel_scale(uint8_t argc, const Menu::arg *argv)
 {
- #if CONFIG_ADC == ENABLED && HIL_MODE == HIL_MODE_DISABLED
+    ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
-    int8_t accel_num;
+    ins.calibrate_accel(delay, flash_leds);
-    float accel_avg = 0;
+    report_ins();
-
+    return(0);
-    if (!strcmp_P(argv[1].str, PSTR("x"))) {
-        accel_num = 4;
-    }else if (!strcmp_P(argv[1].str, PSTR("y"))) {
-        accel_num = 5;
-    }else if (!strcmp_P(argv[1].str, PSTR("z"))) {
-        accel_num = 6;
-    }else{
-        Serial.printf_P(PSTR("x, y, or z\n"));
-        return 0;
-    }
-    print_hit_enter();
-    Serial.printf_P(PSTR("ADC\n"));
-
-    adc.Init(&timer_scheduler);                           // APM ADC library initialization
-
-    int16_t low, high;
-
-    delay(1000);
-    accel_avg = adc.Ch(accel_num);
-    low = high = accel_avg;
-
-    while(1) {
-        delay(50);
-        accel_avg = accel_avg * .95 + adc.Ch(accel_num) * .05;
-
-        if(accel_avg > high)
-            high = ceil(accel_avg);
-
-        if(accel_avg < low)
-            low = floor(accel_avg);
-
-        Serial.printf_P(PSTR("%1.2f, %d, %d\n"), accel_avg, low, high);
-
-        if(Serial.available() > 0) {
-            if(wait_for_yes()) {
-                if(accel_num == 4) {
-                    ins._x_high     = high;
-                    ins._x_low              = low;
-                    ins._x_high.save();
-                    ins._x_low.save();
-                }else if(accel_num == 5) {
-                    ins._y_high     = high;
-                    ins._y_low              = low;
-                    ins._y_high.save();
-                    ins._y_low.save();
-                }else{
-                    ins._z_high     = high;
-                    ins._z_low              = low;
-                    ins._z_high.save();
-                    ins._z_low.save();
-                }
-                print_done();
-            }
-            return (0);
-        }
-    }
- #else
-    return 0;
- #endif        // CONFIG_ADC
 }
 
 static int8_t
@@ -873,13 +813,13 @@ static void report_radio()
     print_blanks(2);
 }
 
-static void report_imu()
+static void report_ins()
 {
-    Serial.printf_P(PSTR("IMU\n"));
+    Serial.printf_P(PSTR("INS\n"));
     print_divider();
 
     print_gyro_offsets();
-    print_accel_offsets();
+    print_accel_offsets_and_scaling();
     print_blanks(2);
 }
 
@@ -1029,21 +969,27 @@ static void zero_eeprom(void)
 }
 
 static void
-print_accel_offsets(void)
+print_accel_offsets_and_scaling(void)
 {
-    Serial.printf_P(PSTR("A_off: %4.2f, %4.2f, %4.2f\n"),
+    Vector3f accel_offsets = ins.get_accel_offsets();
-                    (float)imu.ax(),                                    // Pitch
+    Vector3f accel_scale = ins.get_accel_scale();
-                    (float)imu.ay(),                                    // Roll
+    Serial.printf_P(PSTR("A_off: %4.2f, %4.2f, %4.2f\tA_scale: %4.2f, %4.2f, %4.2f\n"),
-                    (float)imu.az());                                   // YAW
+                    (float)accel_offsets.x,                           // Pitch
+                    (float)accel_offsets.y,                           // Roll
+                    (float)accel_offsets.z,                           // YAW
+                    (float)accel_scale.x,                             // Pitch
+                    (float)accel_scale.y,                             // Roll
+                    (float)accel_scale.z);                            // YAW
 }
 
 static void
 print_gyro_offsets(void)
 {
+    Vector3f gyro_offsets = ins.get_gyro_offsets();
     Serial.printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"),
-                    (float)imu.gx(),
+                    (float)gyro_offsets.x,
-                    (float)imu.gy(),
+                    (float)gyro_offsets.y,
-                    (float)imu.gz());
+                    (float)gyro_offsets.z);
 }
 
  #if FRAME_CONFIG == HELI_FRAME

ArduCopter/system.pde

@@ -367,9 +367,9 @@ static void startup_ground(void)
 
     // Warm up and read Gyro offsets
     // -----------------------------
-    imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
+    ins.init(AP_InertialSensor::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
  #if CLI_ENABLED == ENABLED
-    report_imu();
+    report_ins();
  #endif
 
     // initialise ahrs (may push imu calibration into the mpu6000 if using that device).

ArduCopter/test.pde

@@ -66,7 +66,6 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
     {"gps",                 test_gps},
 //	{"adc",         test_adc},
     {"ins",                 test_ins},
-//	{"imu",			test_imu},
 //	{"dcm",			test_dcm_eulers},
     //{"omega",		test_omega},
 //	{"stab_d",		test_stab_d},
@@ -440,69 +439,6 @@ test_radio(uint8_t argc, const Menu::arg *argv)
  * #endif
  */
 
-/*
- *  static int8_t
- *  test_adc(uint8_t argc, const Menu::arg *argv)
- *  {
- *       ins.init(&timer_scheduler);
- *
- *       int8_t mytimer = 0;
- *       startup_ground();
- *       Serial.println("OK");
- *
- *       while(1){
- *               // We want this to execute fast
- *               // ----------------------------
- *               uint32_t timer             = micros();
- *
- *               if ((timer - fast_loopTimer) >= 10000 && imu.new_data_available()) {
- *                       G_Dt               = (float)(timer - fast_loopTimer) / 1000000.f;		// used by PI Loops
- *                       fast_loopTimer         = timer;
- *
- *                       read_AHRS();
- *
- *                       //calc_inertia();
- *                       accels_rotated		= ahrs.get_dcm_matrix() * imu.get_accel();
- *                       //accels_rotated		+= accels_offset;						// skew accels to account for long term error using calibration
- *
- *                       mytimer++;
- *
- *                       if ((timer - fiftyhz_loopTimer) >= 20000) {
- *                               fiftyhz_loopTimer		= timer;
- *                               //sensed_loc.lng = sensed_loc.lat = sensed_loc.alt = 0;
- *
- *                               // position fix
- *                               //inertial_error_correction();
- *                       }
- *
- *                       if (mytimer >= 10){
- *                               float test = sqrt(sq(accels_rotated.x) + sq(accels_rotated.y) + sq(accels_rotated.z)) / 9.80665;
- *
- *                               Vector3f _accels = imu.get_accel();
- *                               mytimer = 0;
- *
- *
- *                               Serial.printf("%1.4f, %1.4f, %1.4f  |   %1.4f, %1.4f, %1.4f   |  %d, %1.4f, %d, %1.4f \n",
- *                                                                       _accels.x,
- *                                                                       _accels.y,
- *                                                                       _accels.z,
- *                                                                       accels_rotated.x,
- *                                                                       accels_rotated.y,
- *                                                                       accels_rotated.z,
- *                                                                       test);
- *
- *
- *                       }
- *
- *                       if(Serial.available() > 0){
- *                               return (0);
- *                       }
- *               }
- *       }
- *       return (0);
- *  }
- */
-
 static int8_t
 test_ins(uint8_t argc, const Menu::arg *argv)
 {
@@ -510,37 +446,28 @@ test_ins(uint8_t argc, const Menu::arg *argv)
     print_test_disabled();
     return (0);
 #else
-    float gyro[3], accel[3], temp;
+    Vector3f gyro, accel;
+    float temp;
     print_hit_enter();
-    Serial.printf_P(PSTR("InertialSensor\n"));
+    Serial.printf_P(PSTR("INS\n"));
     delay(1000);
 
-    ins.init(&timer_scheduler);
+    ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
 
     delay(50);
 
     while(1) {
         ins.update();
-        ins.get_gyros(gyro);
+        gyro = ins.get_gyro();
-        ins.get_accels(accel);
+        accel = ins.get_accel();
         temp = ins.temperature();
 
-        float test = sqrt(sq(accel[0]) + sq(accel[1]) + sq(accel[2])) / 9.80665;
+        float test = sqrt(sq(accel.x) + sq(accel.y) + sq(accel.z)) / 9.80665;
 
-        Serial.printf_P(PSTR("g"));
+        Serial.printf_P(PSTR("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %74f | %7.4f\n"),
-
+            accel.x, accel.y, accel.z,
-        for (int16_t i = 0; i < 3; i++) {
+            gyro.x, gyro.y, gyro.z,
-            Serial.printf_P(PSTR(" %7.4f"), gyro[i]);
+            temp, test);
-        }
-
-        Serial.printf_P(PSTR(" a"));
-
-        for (int16_t i = 0; i < 3; i++) {
-            Serial.printf_P(PSTR(" %7.4f"),accel[i]);
-        }
-
-        Serial.printf_P(PSTR(" t %7.4f "), temp);
-        Serial.printf_P(PSTR(" | %7.4f \n"), test);
 
         delay(40);
         if(Serial.available() > 0) {
@@ -550,190 +477,6 @@ test_ins(uint8_t argc, const Menu::arg *argv)
 #endif
 }
 
-
-/*
- *  test the IMU interface
- */
-/*
- *  static int8_t
- *  test_imu(uint8_t argc, const Menu::arg *argv)
- *  {
- * #if defined( __AVR_ATmega1280__ )  // test disabled to save code size for 1280
- *               print_test_disabled();
- *               return (0);
- * #else
- *               Vector3f gyro;
- *               Vector3f accel;
- *
- *               imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler);
- *
- *               report_imu();
- *               imu.init_gyro(delay, flash_leds);
- *               report_imu();
- *
- *               print_hit_enter();
- *               delay(1000);
- *
- *               while(1){
- *                       delay(40);
- *
- *                       imu.update();
- *                       gyro = imu.get_gyro();
- *                       accel = imu.get_accel();
- *
- *                       Serial.printf_P(PSTR("g %8.4f %8.4f %8.4f"), gyro.x, gyro.y, gyro.z);
- *                       Serial.printf_P(PSTR("  a %8.4f %8.4f %8.4f\n"), accel.x, accel.y, accel.z);
- *
- *                       if(Serial.available() > 0){
- *                               return (0);
- *                       }
- *               }
- * #endif
- *  }
- */
-
-/*
- *  static int8_t
- *  test_imu(uint8_t argc, const Menu::arg *argv)
- *  {
- *       print_hit_enter();
- *       Serial.printf_P(PSTR("ADC\n"));
- *       adc.Init(&timer_scheduler);
- *
- *       delay(1000);
- *       Vector3f avg;
- *       avg.x = adc.Ch(4);
- *       avg.y = adc.Ch(5);
- *       avg.z = adc.Ch(6);
- *
- *       //Serial.printf_P(PSTR("init %.2f, %.2f, %.2f\n"), avg.x, avg.y, avg.z);
- *       Vector3f low = avg;
- *       Vector3f high = avg;
- *
- *       while(1){
- *               delay(100);
- *               avg.x = avg.x * .95 + adc.Ch(4) * .05;
- *               avg.y = avg.y * .95 + adc.Ch(5) * .05;
- *               avg.z = avg.z * .95 + adc.Ch(6) * .05;
- *
- *               if(avg.x > high.x)
- *                       high.x = ceil(high.x *.9 + avg.x * .1);
- *
- *               if(avg.y > high.y)
- *                       high.y = ceil(high.y *.9 + avg.y * .1);
- *
- *               if(avg.z > high.z)
- *                       high.z = ceil(high.z *.9 + avg.z * .1);
- *
- *               //
- *               if(avg.x < low.x)
- *                       low.x = floor(low.x *.9 + avg.x * .1);
- *
- *               if(avg.y < low.y)
- *                       low.y = floor(low.y *.9 + avg.y * .1);
- *
- *               if(avg.z < low.z)
- *                       low.z = floor(low.z *.9 + avg.z * .1);
- *
- *               Serial.printf_P(PSTR("%.2f, %.2f, %.2f \t| %.2f, %.2f, %.2f \t| %.2f, %.2f, %.2f\n"), avg.x, avg.y, avg.z, low.x, low.y, low.z, high.x, high.y, high.z);
- *
- *               //Serial.printf_P(PSTR("%.2f, %.2f, %.2f \t| %d, %d\n"), avg.x, avg.y, avg.z, _count[0], _sum[0]);
- *
- *               //Serial.println();
- *               if(Serial.available() > 0){
- *                       Serial.printf_P(PSTR("Y to save\n"));
- *                       int16_t c;
- *                       c = Serial.read();
- *
- *                       do {
- *                               c = Serial.read();
- *                       } while (-1 == c);
- *
- *                       if (('y' == c) || ('Y' == c)){
- *                               ins._x_high    = high.x;
- *                               ins._x_low         = low.x;
- *                               ins._y_high    = high.y;
- *                               ins._y_low         = low.y;
- *                               ins._z_high    = high.z;
- *                               ins._z_low         = low.z;
- *                               ins._x_high.save();
- *                               ins._x_low.save();
- *                               ins._y_high.save();
- *                               ins._y_low.save();
- *                               ins._z_high.save();
- *                               ins._z_low.save();
- *                               Serial.printf_P(PSTR("saved\n"));
- *                       }
- *
- *                       return (0);
- *               }
- *       }
- *  }
- */
-
-
-
-/*
- *  test the DCM code, printing Euler angles
- */
-/*static int8_t
- *  //test_dcm_eulers(uint8_t argc, const Menu::arg *argv)
- *  {
- *
- *       //Serial.printf_P(PSTR("Calibrating."));
- *
- *       //dcm.kp_yaw(0.02);
- *       //dcm.ki_yaw(0);
- *
- *       imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler);
- *
- *       report_imu();
- *       imu.init_gyro(delay, flash_leds);
- *       report_imu();
- *
- *       print_hit_enter();
- *       delay(1000);
- *
- *       //float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
- *       fast_loopTimer = millis();
- *
- *       while(1){
- *               //delay(20);
- *               if (millis() - fast_loopTimer >=20) {
- *
- *                       // IMU
- *                       // ---
- *                       read_AHRS();
- *                       medium_loopCounter++;
- *
- *                       if(medium_loopCounter == 4){
- *                               update_trig();
- *                       }
- *
- *                       if(medium_loopCounter == 1){
- *                               medium_loopCounter = 0;
- *                               Serial.printf_P(PSTR("dcm: %6.1f, %6.1f, %6.1f   omega: %6.1f, %6.1f, %6.1f\n"),
- *                                                               dcm.roll_sensor/100.0,
- *                                                               dcm.pitch_sensor/100.0,
- *                                                               dcm.yaw_sensor/100.0,
- *                                                               degrees(omega.x),
- *                                                               degrees(omega.y),
- *                                                               degrees(omega.z));
- *
- *                               if(g.compass_enabled){
- *                                       compass.read();		                // Read magnetometer
- *                   compass.null_offsets();
- *                               }
- *                       }
- *                       fast_loopTimer = millis();
- *               }
- *               if(Serial.available() > 0){
- *                       return (0);
- *               }
- *       }
- *       return (0);
- *  }*/
-
 static int8_t
 test_gps(uint8_t argc, const Menu::arg *argv)
 {