Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: update ArduPlane for new ins interface

This commit is contained in:
Andrew Tridgell 2012-11-29 22:56:40 +11:00
parent 2d8d6b2459
commit bcef536f64
5 changed files with 23 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduPlane/ArduPlane.pde
View File

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

11
ArduPlane/config.h
View File

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

4
ArduPlane/setup.pde
View File

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

8
ArduPlane/system.pde
View File

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

8
ArduPlane/test.pde
View File

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