From 2d795ac85adcb941ec0997e20ee79db381b35402 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Jun 2013 10:37:05 +1000 Subject: [PATCH] Rover: convert to use AP_Scheduler this gives us better timing information and scheduling, while also making the code easier to read --- APMrover2/APMrover2.pde | 282 +++++++++++++++++---------------------- APMrover2/Parameters.h | 1 + APMrover2/Parameters.pde | 4 + APMrover2/sensors.pde | 21 ++- APMrover2/system.pde | 4 + APMrover2/test.pde | 9 +- 6 files changed, 151 insertions(+), 170 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 4054b5dccb..6f38677493 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -75,6 +75,7 @@ version 2.1 of the License, or (at your option) any later version. #include #include // RC input mapping library #include +#include // main loop scheduler #include #include @@ -116,6 +117,9 @@ static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor: // static Parameters g; +// main loop scheduler +static AP_Scheduler scheduler; + // mapping between input channels static RCMapper rcmap; @@ -532,20 +536,6 @@ static uint8_t delta_ms_fast_loop; // Counter of main loop executions. Used for performance monitoring and failsafe processing static uint16_t mainLoop_count; -// Time in miliseconds of start of medium control loop. Milliseconds -static uint32_t medium_loopTimer; -// Counters for branching from main control loop to slower loops -static uint8_t medium_loopCounter; -// Number of milliseconds used in last medium loop cycle -static uint8_t delta_ms_medium_loop; - -// Counters for branching from medium control loop to slower loops -static uint8_t slow_loopCounter; -// Counter to trigger execution of very low rate processes -static uint8_t superslow_loopCounter; -// Counter to trigger execution of 1 Hz processes -static uint8_t counter_one_herz; - // % MCU cycles used static float load; @@ -553,6 +543,33 @@ static float load; // Top-level logic //////////////////////////////////////////////////////////////////////////////// +/* + scheduler table - all regular tasks apart from the fast_loop() + should be listed here, along with how often they should be called + (in 20ms units) and the maximum time they are expected to take (in + microseconds) + */ +static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { + { update_GPS, 5, 900 }, + { navigate, 5, 1000 }, + { update_compass, 5, 1000 }, + { update_commands, 5, 1000 }, + { update_logging, 5, 1000 }, + { read_battery, 5, 1000 }, + { read_receiver_rssi, 5, 1000 }, + { read_trim_switch, 5, 1000 }, + { read_control_switch, 15, 1000 }, + { update_events, 15, 1000 }, + { check_usb_mux, 15, 1000 }, + { mount_update, 1, 500 }, + { failsafe_check, 5, 500 }, + { one_second_loop, 50, 1000 } +}; + + +/* + setup is called when the sketch starts + */ void setup() { memcheck_init(); cliSerial = hal.console; @@ -566,10 +583,18 @@ void setup() { batt_curr_pin = hal.analogin->channel(g.battery_curr_pin); init_ardupilot(); + + // initialise the main loop scheduler + scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0])); } +/* + loop() is called rapidly while the sketch is running + */ void loop() { + uint32_t timer = millis(); + // We want this to execute at 50Hz, but synchronised with the gyro/accel uint16_t num_samples = ins.num_samples_available(); if (num_samples >= 1) { @@ -584,35 +609,14 @@ void loop() // --------------------- fast_loop(); - // Execute the medium loop - // ----------------------- - medium_loop(); - - counter_one_herz++; - if(counter_one_herz == 50){ - one_second_loop(); - counter_one_herz = 0; - } - - if (millis() - perf_mon_timer > 20000) { - if (mainLoop_count != 0) { - if (g.log_bitmask & MASK_LOG_PM) - #if HIL_MODE != HIL_MODE_ATTITUDE - Log_Write_Performance(); - #endif - resetPerfData(); - } - } - + // tell the scheduler one tick has passed + scheduler.tick(); fast_loopTimeStamp = millis(); - } else if (millis() - fast_loopTimeStamp < 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 - // driven, so it can't accumulate readings by itself - if (g.compass_enabled) { - compass.accumulate(); + } else { + uint16_t dt = timer - fast_loopTimer; + if (dt < 20) { + uint16_t time_to_next_loop = 20 - dt; + scheduler.run(time_to_next_loop * 1000U); } } } @@ -665,135 +669,60 @@ static void fast_loop() gcs_data_stream_send(); } -static void medium_loop() +/* + update camera mount - 50Hz + */ +static void mount_update(void) { #if MOUNT == ENABLED camera_mount.update_mount_position(); #endif - - // This is the start of the medium (10 Hz) loop pieces - // ----------------------------------------- - switch(medium_loopCounter) { - - // This case deals with the GPS - //------------------------------- - case 0: - failsafe_trigger(FAILSAFE_EVENT_GCS, last_heartbeat_ms != 0 && (millis() - last_heartbeat_ms) > 2000); - medium_loopCounter++; - update_GPS(); - - #if HIL_MODE != HIL_MODE_ATTITUDE - if (g.compass_enabled && compass.read()) { - ahrs.set_compass(&compass); - // Calculate heading - compass.null_offsets(); - if (g.log_bitmask & MASK_LOG_COMPASS) { - Log_Write_Compass(); - } - } else { - ahrs.set_compass(NULL); - } - #endif - break; - - // This case performs some navigation computations - //------------------------------------------------ - case 1: - medium_loopCounter++; - navigate(); - break; - - // command processing - //------------------------------ - case 2: - medium_loopCounter++; - - read_receiver_rssi(); - - // perform next command - // -------------------- - update_commands(); - break; - - // This case deals with sending high rate telemetry - //------------------------------------------------- - case 3: - medium_loopCounter++; - #if HIL_MODE != HIL_MODE_ATTITUDE - if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) - Log_Write_Attitude(); - - if (g.log_bitmask & MASK_LOG_CTUN) - Log_Write_Control_Tuning(); - #endif - - if (g.log_bitmask & MASK_LOG_NTUN) - Log_Write_Nav_Tuning(); - break; - - // This case controls the slow loop - //--------------------------------- - case 4: - medium_loopCounter = 0; - delta_ms_medium_loop = millis() - medium_loopTimer; - medium_loopTimer = millis(); - - if (g.battery_monitoring != 0){ - read_battery(); - } - - read_trim_switch(); - - slow_loop(); - break; - } } -static void slow_loop() +/* + check for GCS failsafe - 10Hz + */ +static void failsafe_check(void) { - // This is the slow (3 1/3 Hz) loop pieces - //---------------------------------------- - switch (slow_loopCounter){ - case 0: - slow_loopCounter++; - superslow_loopCounter++; - if(superslow_loopCounter >=200) { // 200 = Execute every minute - #if HIL_MODE != HIL_MODE_ATTITUDE - if(g.compass_enabled) { - compass.save_offsets(); - } - #endif - superslow_loopCounter = 0; - } - break; - - case 1: - slow_loopCounter++; - - // Read 3-position switch on radio - // ------------------------------- - read_control_switch(); - - update_aux_servo_function(&g.rc_2, &g.rc_4, &g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); - -#if MOUNT == ENABLED - camera_mount.update_mount_type(); -#endif - break; - - case 2: - slow_loopCounter = 0; - - update_events(); - - mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter - - check_usb_mux(); - break; - } + failsafe_trigger(FAILSAFE_EVENT_GCS, last_heartbeat_ms != 0 && (millis() - last_heartbeat_ms) > 2000); } -static void one_second_loop() +/* + check for new compass data - 10Hz + */ +static void update_compass(void) +{ + if (g.compass_enabled && compass.read()) { + ahrs.set_compass(&compass); + // update offsets + compass.null_offsets(); + if (g.log_bitmask & MASK_LOG_COMPASS) { + Log_Write_Compass(); + } + } else { + ahrs.set_compass(NULL); + } +} + +/* + log some key data - 10Hz + */ +static void update_logging(void) +{ + if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) + Log_Write_Attitude(); + + if (g.log_bitmask & MASK_LOG_CTUN) + Log_Write_Control_Tuning(); + + if (g.log_bitmask & MASK_LOG_NTUN) + Log_Write_Nav_Tuning(); +} + +/* + once a second events + */ +static void one_second_loop(void) { if (g.log_bitmask & MASK_LOG_CURRENT) Log_Write_Current(); @@ -804,6 +733,35 @@ static void one_second_loop() ahrs.set_orientation(); set_control_channels(); + + // cope with changes to aux functions + update_aux_servo_function(&g.rc_2, &g.rc_4, &g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); + +#if MOUNT == ENABLED + camera_mount.update_mount_type(); +#endif + + // cope with changes to mavlink system ID + mavlink_system.sysid = g.sysid_this_mav; + + static uint8_t counter; + + counter++; + + // write perf data every 20s + if (counter == 20) { + if (g.log_bitmask & MASK_LOG_PM) + Log_Write_Performance(); + resetPerfData(); + } + + // save compass offsets once a minute + if (counter >= 60) { + if (g.compass_enabled) { + compass.save_offsets(); + } + counter = 0; + } } static void update_GPS(void) diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 19d1885535..c3578fbc24 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -32,6 +32,7 @@ public: k_param_num_resets, k_param_reset_switch_chan, k_param_initial_mode, + k_param_scheduler, // IO pins k_param_rssi_pin = 20, diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index f226f3e9ac..9845b9c079 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -416,6 +416,10 @@ const AP_Param::Info var_info[] PROGMEM = { // @Path: ../libraries/AP_Compass/Compass.cpp GOBJECT(compass, "COMPASS_", Compass), + // @Group: SCHED_ + // @Path: ../libraries/AP_Scheduler/AP_Scheduler.cpp + GOBJECT(scheduler, "SCHED_", AP_Scheduler), + // @Group: RCMAP_ // @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp GOBJECT(rcmap, "RCMAP_", RCMapper), diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index 6a4043f83b..9af97f941b 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -18,6 +18,9 @@ void ReadSCP1000(void) {} #endif // HIL_MODE != HIL_MODE_ATTITUDE +/* + read and update the battery + */ static void read_battery(void) { if(g.battery_monitoring == 0) { @@ -30,11 +33,19 @@ static void read_battery(void) batt_volt_pin->set_pin(g.battery_volt_pin); battery_voltage1 = BATTERY_VOLTAGE(batt_volt_pin); } - if(g.battery_monitoring == 4) { - // this copes with changing the pin at runtime - batt_curr_pin->set_pin(g.battery_curr_pin); - current_amps1 = CURRENT_AMPS(batt_curr_pin); - current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours) + + if (g.battery_monitoring == 4) { + static uint32_t last_time_ms; + uint32_t tnow = hal.scheduler->millis(); + float dt = tnow - last_time_ms; + if (last_time_ms != 0 && dt < 2000) { + // this copes with changing the pin at runtime + batt_curr_pin->set_pin(g.battery_curr_pin); + current_amps1 = CURRENT_AMPS(batt_curr_pin); + // .0002778 is 1/3600 (conversion to hours) + current_total1 += current_amps1 * dt * 0.0002778; + } + last_time_ms = tnow; } } diff --git a/APMrover2/system.pde b/APMrover2/system.pde index 1f9987a4ae..22da95b768 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -294,6 +294,10 @@ static void startup_ground(void) // ----------------------- demo_servos(3); + hal.uartA->set_blocking_writes(false); + hal.uartB->set_blocking_writes(false); + hal.uartC->set_blocking_writes(false); + gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to drive.")); } diff --git a/APMrover2/test.pde b/APMrover2/test.pde index adcc45e9c1..fc11bfa1b5 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -233,7 +233,6 @@ test_battery(uint8_t argc, const Menu::arg *argv) { if (g.battery_monitoring == 3 || g.battery_monitoring == 4) { print_hit_enter(); - delta_ms_medium_loop = 100; while(1){ delay(100); @@ -423,6 +422,8 @@ test_ins(uint8_t argc, const Menu::arg *argv) print_hit_enter(); delay(1000); + uint8_t medium_loopCounter = 0; + while(1){ delay(20); if (millis() - fast_loopTimer > 19) { @@ -436,7 +437,7 @@ test_ins(uint8_t argc, const Menu::arg *argv) if(g.compass_enabled) { medium_loopCounter++; - if(medium_loopCounter == 5){ + if(medium_loopCounter >= 5){ compass.read(); medium_loopCounter = 0; } @@ -489,6 +490,8 @@ test_mag(uint8_t argc, const Menu::arg *argv) print_hit_enter(); + uint8_t medium_loopCounter = 0; + while(1) { delay(20); if (millis() - fast_loopTimer > 19) { @@ -501,7 +504,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) ahrs.update(); medium_loopCounter++; - if(medium_loopCounter == 5){ + if(medium_loopCounter >= 5){ if (compass.read()) { // Calculate heading Matrix3f m = ahrs.get_dcm_matrix();