Browse Source

IMU: update ACM and APM for flash_leds change in IMU init

mission-4.1.18
Andrew Tridgell 13 years ago
parent
commit
483bef35e5
  1. 2
      ArduCopter/GCS_Mavlink.pde
  2. 2
      ArduCopter/motors.pde
  3. 4
      ArduCopter/setup.pde
  4. 12
      ArduCopter/system.pde
  5. 8
      ArduCopter/test.pde
  6. 17
      ArduPlane/system.pde
  7. 4
      ArduPlane/test.pde

2
ArduCopter/GCS_Mavlink.pde

@ -845,7 +845,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) @@ -845,7 +845,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_ACTION_CALIBRATE_ACC:
imu.init_accel(mavlink_delay);
imu.init_accel(mavlink_delay, flash_leds);
result=1;
break;

2
ArduCopter/motors.pde

@ -42,7 +42,7 @@ static void arm_motors() @@ -42,7 +42,7 @@ static void arm_motors()
//Serial.printf("\nLEV\n");
// begin manual leveling
imu.init_accel(mavlink_delay);
imu.init_accel(mavlink_delay, flash_leds);
arming_counter = 0;
}else if (arming_counter == DISARM_DELAY){

4
ArduCopter/setup.pde

@ -245,8 +245,8 @@ setup_motors(uint8_t argc, const Menu::arg *argv) @@ -245,8 +245,8 @@ 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, &timer_scheduler);
imu.init_accel();
imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
imu.init_accel(delay, flash_leds);
print_accel_offsets();
report_imu();
return(0);

12
ArduCopter/system.pde

@ -389,7 +389,7 @@ static void startup_ground(void) @@ -389,7 +389,7 @@ static void startup_ground(void)
#if HIL_MODE != HIL_MODE_ATTITUDE
// Warm up and read Gyro offsets
// -----------------------------
imu.init(IMU::COLD_START, mavlink_delay, &timer_scheduler);
imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
#if CLI_ENABLED == ENABLED
report_imu();
#endif
@ -655,3 +655,13 @@ static void check_usb_mux(void) @@ -655,3 +655,13 @@ static void check_usb_mux(void)
}
}
#endif
/*
called by gyro/accel init to flash LEDs so user
has some mesmerising lights to watch while waiting
*/
void flash_leds(bool on)
{
digitalWrite(A_LED_PIN, on?LED_OFF:LED_ON);
digitalWrite(C_LED_PIN, on?LED_ON:LED_OFF);
}

8
ArduCopter/test.pde

@ -484,10 +484,10 @@ test_imu(uint8_t argc, const Menu::arg *argv) @@ -484,10 +484,10 @@ test_imu(uint8_t argc, const Menu::arg *argv)
Vector3f gyro;
Vector3f accel;
imu.init(IMU::WARM_START, delay, &timer_scheduler);
imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler);
report_imu();
imu.init_gyro();
imu.init_gyro(delay, flash_leds);
report_imu();
print_hit_enter();
@ -524,10 +524,10 @@ test_dcm_eulers(uint8_t argc, const Menu::arg *argv) @@ -524,10 +524,10 @@ test_dcm_eulers(uint8_t argc, const Menu::arg *argv)
//dcm.kp_yaw(0.02);
//dcm.ki_yaw(0);
imu.init(IMU::WARM_START, delay, &timer_scheduler);
imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler);
report_imu();
imu.init_gyro();
imu.init_gyro(delay, flash_leds);
report_imu();
print_hit_enter();

17
ArduPlane/system.pde

@ -261,7 +261,7 @@ static void init_ardupilot() @@ -261,7 +261,7 @@ static void init_ardupilot()
//----------------
//read_EEPROM_airstart_critical();
#if HIL_MODE != HIL_MODE_ATTITUDE
imu.init(IMU::WARM_START, mavlink_delay, &timer_scheduler);
imu.init(IMU::WARM_START, mavlink_delay, flash_leds, &timer_scheduler);
dcm.set_centripetal(1);
#endif
@ -455,8 +455,8 @@ static void startup_IMU_ground(void) @@ -455,8 +455,8 @@ static void startup_IMU_ground(void)
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane"));
mavlink_delay(1000);
imu.init(IMU::COLD_START, mavlink_delay, &timer_scheduler);
imu.init_accel(mavlink_delay);
imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
imu.init_accel(mavlink_delay, flash_leds);
dcm.set_centripetal(1);
// read Baro pressure at ground
@ -556,3 +556,14 @@ static void check_usb_mux(void) @@ -556,3 +556,14 @@ static void check_usb_mux(void)
}
}
#endif
/*
called by gyro/accel init to flash LEDs so user
has some mesmerising lights to watch while waiting
*/
void flash_leds(bool on)
{
digitalWrite(A_LED_PIN, on?LED_OFF:LED_ON);
digitalWrite(C_LED_PIN, on?LED_ON:LED_OFF);
}

4
ArduPlane/test.pde

@ -505,7 +505,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) @@ -505,7 +505,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
//Serial.printf_P(PSTR("Calibrating."));
isr_registry.init();
timer_scheduler.init( &isr_registry );
imu.init(IMU::COLD_START, delay, &timer_scheduler);
imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
print_hit_enter();
delay(1000);
@ -568,7 +568,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) @@ -568,7 +568,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
// we need the DCM initialised for this test
isr_registry.init();
timer_scheduler.init( &isr_registry );
imu.init(IMU::COLD_START, delay, &timer_scheduler);
imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
int counter = 0;
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);

Loading…
Cancel
Save