Copter: update ArduCopter for new ins interface

This commit is contained in:
Andrew Tridgell 2012-11-29 22:56:54 +11:00
parent bcef536f64
commit 45e62add9f
5 changed files with 20 additions and 19 deletions

View File

@ -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

View File

@ -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.
//

View File

@ -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);

View File

@ -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

View File

@ -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);