Plane: update ArduPlane for new ins interface

ArduPlane/ArduPlane.pde

@@ -106,6 +106,11 @@ AP_Param param_loader(var_info, WP_START_BYTE);
 APM_OBC obc;
 #endif
 
+////////////////////////////////////////////////////////////////////////////////
+// the rate we run the main loop at
+////////////////////////////////////////////////////////////////////////////////
+static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_50HZ;
+
 
 ////////////////////////////////////////////////////////////////////////////////
 // ISR Registry
@@ -690,7 +695,7 @@ void loop()
 {
     // We want this to execute at 50Hz, but synchronised with the gyro/accel
     uint16_t num_samples = ins.num_samples_available();
-    if (num_samples >= NUM_INS_SAMPLES_FOR_50HZ) {
+    if (num_samples >= 1) {
         delta_ms_fast_loop      = millis() - fast_loopTimer_ms;
         load                = (float)(fast_loopTimeStamp_ms - fast_loopTimer_ms)/delta_ms_fast_loop;
         G_Dt                = (float)delta_ms_fast_loop / 1000.f;
@@ -724,8 +729,8 @@ void loop()
         }
 
         fast_loopTimeStamp_ms = millis();
-    } else if (num_samples < NUM_INS_SAMPLES_FOR_50HZ-1) {
-        // less than 20ms has passed. We have at least one millisecond
+    } else if (millis() - fast_loopTimeStamp_ms < 19) {
+        // less than 19ms has passed. We have at least one millisecond
         // of free time. The most useful thing to do with that time is
         // to accumulate some sensor readings, specifically the
         // compass, which is often very noisy but is not interrupt

ArduPlane/config.h

@@ -816,14 +816,3 @@
 # define SERIAL_BUFSIZE 256
 #endif
 
-#if CONFIG_INS_TYPE == CONFIG_INS_OILPAN
- # define NUM_INS_SAMPLES_FOR_200HZ 5
- # define NUM_INS_SAMPLES_FOR_100HZ 10
- # define NUM_INS_SAMPLES_FOR_50HZ  20
-#endif
-
-#if CONFIG_INS_TYPE == CONFIG_INS_MPU6000
- # define NUM_INS_SAMPLES_FOR_200HZ 1
- # define NUM_INS_SAMPLES_FOR_100HZ 2
- # define NUM_INS_SAMPLES_FOR_50HZ  4
-#endif

ArduPlane/setup.pde

@@ -330,7 +330,9 @@ static int8_t
 setup_accel_scale(uint8_t argc, const Menu::arg *argv)
 {
     cliSerial->println_P(PSTR("Initialising gyros"));
-    ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
+    ins.init(AP_InertialSensor::COLD_START, 
+             ins_sample_rate,
+             delay, flash_leds, &timer_scheduler);
     if (ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key)) {
         if (g.manual_level == 0) {
             cliSerial->println_P(PSTR("Setting MANUAL_LEVEL to 1"));

ArduPlane/system.pde

@@ -245,7 +245,9 @@ static void init_ardupilot()
         //----------------
         //read_EEPROM_airstart_critical();
 #if HIL_MODE != HIL_MODE_ATTITUDE
-        ins.init(AP_InertialSensor::WARM_START, mavlink_delay, flash_leds, &timer_scheduler);
+        ins.init(AP_InertialSensor::WARM_START, 
+                 ins_sample_rate,
+                 mavlink_delay, flash_leds, &timer_scheduler);
 
         ahrs.init(&timer_scheduler);
         ahrs.set_fly_forward(true);
@@ -434,7 +436,9 @@ static void startup_INS_ground(bool force_accel_level)
     gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move plane"));
     mavlink_delay(1000);
 
-    ins.init(AP_InertialSensor::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
+    ins.init(AP_InertialSensor::COLD_START, 
+             ins_sample_rate,
+             mavlink_delay, flash_leds, &timer_scheduler);
 #if HIL_MODE == HIL_MODE_DISABLED
     if (force_accel_level || g.manual_level == 0) {
         // when MANUAL_LEVEL is set to 1 we don't do accelerometer

ArduPlane/test.pde

@@ -467,7 +467,9 @@ static int8_t
 test_ins(uint8_t argc, const Menu::arg *argv)
 {
     //cliSerial->printf_P(PSTR("Calibrating."));
-    ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
+    ins.init(AP_InertialSensor::COLD_START, 
+             ins_sample_rate,
+             delay, flash_leds, &timer_scheduler);
     ahrs.reset();
 
     print_hit_enter();
@@ -528,7 +530,9 @@ test_mag(uint8_t argc, const Menu::arg *argv)
     report_compass();
 
     // we need the AHRS initialised for this test
-    ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
+    ins.init(AP_InertialSensor::COLD_START, 
+             ins_sample_rate,
+             delay, flash_leds, &timer_scheduler);
     ahrs.reset();
 
     int16_t counter = 0;