From 6fbf5ec8f2df3f22e60dbd3c4d1431e2ff92f35f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 29 Nov 2012 22:56:54 +1100 Subject: [PATCH] Copter: update ArduCopter for new ins interface --- ArduCopter/ArduCopter.pde | 11 ++++++++--- ArduCopter/config.h | 12 ------------ ArduCopter/setup.pde | 8 ++++++-- ArduCopter/system.pde | 4 +++- ArduCopter/test.pde | 4 +++- 5 files changed, 20 insertions(+), 19 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 287801ed7b6..df2a177e48b 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -190,6 +190,11 @@ DataFlash_APM1 DataFlash(&spi_semaphore); #endif +//////////////////////////////////////////////////////////////////////////////// +// the rate we run the main loop at +//////////////////////////////////////////////////////////////////////////////// +static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_50HZ; + //////////////////////////////////////////////////////////////////////////////// // Sensors //////////////////////////////////////////////////////////////////////////////// @@ -955,7 +960,7 @@ void loop() // We want this to execute fast // ---------------------------- num_samples = ins.num_samples_available(); - if (num_samples >= NUM_IMU_SAMPLES_FOR_100HZ) { + if (num_samples >= 1) { #if DEBUG_FAST_LOOP == ENABLED Log_Write_Data(DATA_FAST_LOOP, (int32_t)(timer - fast_loopTimer)); @@ -1021,9 +1026,9 @@ void loop() #ifdef DESKTOP_BUILD usleep(1000); #endif - if (num_samples < NUM_IMU_SAMPLES_FOR_100HZ-1) { + if (timer - fast_loopTimer < 9) { // we have some spare cycles available - // less than 20ms has passed. We have at least one millisecond + // less than 10ms 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 diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 277d37f42f7..2033b370bc7 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -96,18 +96,6 @@ # define CONFIG_IMU_TYPE CONFIG_IMU_OILPAN #endif -#if CONFIG_IMU_TYPE == CONFIG_IMU_OILPAN - # define NUM_IMU_SAMPLES_FOR_200HZ 5 - # define NUM_IMU_SAMPLES_FOR_100HZ 10 - # define NUM_IMU_SAMPLES_FOR_50HZ 20 -#endif - -#if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000 - # define NUM_IMU_SAMPLES_FOR_200HZ 1 - # define NUM_IMU_SAMPLES_FOR_100HZ 2 - # define NUM_IMU_SAMPLES_FOR_50HZ 4 -#endif - ////////////////////////////////////////////////////////////////////////////// // ADC Enable - used to eliminate for systems which don't have ADC. // diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 4b065a85e67..bdf14e46bc8 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -247,7 +247,9 @@ setup_motors(uint8_t argc, const Menu::arg *argv) static int8_t setup_accel(uint8_t argc, const Menu::arg *argv) { - 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); ins.init_accel(delay, flash_leds); report_ins(); return(0); @@ -282,7 +284,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); ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key); report_ins(); return(0); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 86e97b37235..ba391d71c0b 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -360,7 +360,9 @@ static void startup_ground(void) // Warm up and read Gyro offsets // ----------------------------- - 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 CLI_ENABLED == ENABLED report_ins(); #endif diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index d129d140172..1c633bb047b 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -460,7 +460,9 @@ test_ins(uint8_t argc, const Menu::arg *argv) cliSerial->printf_P(PSTR("INS\n")); delay(1000); - 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); delay(50);