diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e1a0f9c57c..602727938a 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -99,8 +99,9 @@ #include // ArduPilot Mega inertial navigation library #include // ArduPilot Mega Declination Helper Library #include -#include -#include +#include // memory limit checker +#include // software in the loop support +#include // main loop scheduler // AP_HAL to Arduino compatibility layer #include "compat.h" @@ -138,6 +139,8 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; // static Parameters g; +// main loop scheduler +AP_Scheduler scheduler; //////////////////////////////////////////////////////////////////////////////// // prototypes @@ -817,8 +820,6 @@ AP_InertialNav inertial_nav(&ahrs, &ins, &barometer, &g_gps); //////////////////////////////////////////////////////////////////////////////// // Performance monitoring //////////////////////////////////////////////////////////////////////////////// -// Used to manage the rate of performance logging messages -static int16_t perf_mon_counter; // The number of GPS fixes we have had static int16_t gps_fix_count; @@ -906,6 +907,31 @@ void get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t ma // setup the var_info table AP_Param param_loader(var_info, WP_START_BYTE); +/* + scheduler table - all regular tasks apart from the fast_loop() + should be listed here, along with how often they should be called + (in 10ms units) and the maximum time they are expected to take (in + microseconds) + */ +static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { + { update_GPS, 2, 900 }, + { update_navigation, 2, 500 }, + { medium_loop, 2, 700 }, + { update_altitude_est, 2, 1000 }, + { fifty_hz_loop, 2, 950 }, + { run_nav_updates, 2, 500 }, + { slow_loop, 10, 500 }, + { gcs_check_input, 2, 700 }, + { gcs_send_heartbeat, 100, 700 }, + { gcs_data_stream_send, 2, 1500 }, + { gcs_send_deferred, 2, 1200 }, + { compass_accumulate, 2, 700 }, + { barometer_accumulate, 2, 900 }, + { super_slow_loop, 100, 1100 }, + { perf_update, 1000, 500 } +}; + + void setup() { // this needs to be the first call, as it fills memory with // sentinel values @@ -935,6 +961,9 @@ void setup() { board_vcc_analog_source = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC); init_ardupilot(); + + // initialise the main loop scheduler + scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0])); } /* @@ -962,7 +991,7 @@ static void perf_update(void) { if (g.log_bitmask & MASK_LOG_PM) Log_Write_Performance(); -#if SCHEDULER_DEBUG +#if 1 cliSerial->printf_P(PSTR("PERF: %u/%u %lu\n"), (unsigned)perf_info_get_num_long_running(), (unsigned)perf_info_get_num_loops(), @@ -972,112 +1001,6 @@ static void perf_update(void) gps_fix_count = 0; } -/* - return number of micros until the main loop will want to run again - */ -static int16_t main_loop_time_available(void) -{ - uint32_t dt = (micros() - fast_loopTimer); - if (dt > 10000) { - return 0; - } - return 10000 - dt; -} - -typedef void (*event_fn_t)(void); - -struct timer_event_table { - event_fn_t func; - uint16_t time_interval_10ms; - uint16_t min_time_usec; -}; - -#define NUM_TIMER_EVENTS 15 - -/* - scheduler table - all regular events apart from the fast_loop() - should be listed here, along with how often they should be called - (in 10ms units) and the maximum time they are expected to take - */ -static const struct timer_event_table PROGMEM timer_events[NUM_TIMER_EVENTS] = { - { update_GPS, 2, 900 }, - { update_navigation, 2, 500 }, - { medium_loop, 2, 700 }, - { update_altitude_est, 2, 900 }, - { fifty_hz_loop, 2, 950 }, - { run_nav_updates, 2, 500 }, - { slow_loop, 10, 500 }, - { gcs_check_input, 2, 700 }, - { gcs_send_heartbeat, 100, 700 }, - { gcs_data_stream_send, 2, 1100 }, - { gcs_send_deferred, 2, 1000 }, - { compass_accumulate, 2, 700 }, - { barometer_accumulate, 2, 900 }, - { super_slow_loop, 100, 1100 }, - { perf_update, 1000, 500 } -}; -static uint16_t timer_counters[NUM_TIMER_EVENTS]; - -static uint16_t tick_counter; -static uint32_t event_time_started; -static uint16_t event_time_allowed; - -/* - return number of micros until the current event reaches its deadline - */ -static uint16_t event_time_available(void) -{ - uint32_t dt = micros() - event_time_started; - if (dt > event_time_allowed) { - return 0; - } - return event_time_allowed - dt; -} - -/* - run as many scheduler events as we can - */ -static void run_events(uint16_t time_available_usec) -{ - for (uint8_t i=0; i= pgm_read_word(&timer_events[i].time_interval_10ms)) { - // this event is due to run. Do we have enough time to run it? - event_time_allowed = pgm_read_word(&timer_events[i].min_time_usec); - if (event_time_allowed <= time_available_usec) { - // run it - event_time_started = micros(); - event_fn_t func = (event_fn_t)pgm_read_pointer(&timer_events[i].func); - func(); - - // record the tick counter when we ran. This drives - // when we next run the event - timer_counters[i] = tick_counter; - - // work out how long the event actually took - uint32_t time_taken = micros() - event_time_started; - - if (time_taken > event_time_allowed+500) { - // the event overran! -#if SCHEDULER_DEBUG - cliSerial->printf_P(PSTR("overrun in event %u (%u/%u)\n"), - (unsigned)i, - (unsigned)time_taken, - (unsigned)event_time_allowed); -#endif - return; - } - time_available_usec -= time_taken; - } - } - } -} - void loop() { uint32_t timer = micros(); @@ -1099,16 +1022,14 @@ void loop() // --------------------- fast_loop(); - tick_counter++; + // tell the scheduler one tick has passed + scheduler.tick(); } else { - uint16_t time_to_next_loop; uint16_t dt = timer - fast_loopTimer; - if (dt > 10000) { - time_to_next_loop = 0; - } else { - time_to_next_loop = 10000 - dt; + if (dt < 10000) { + uint16_t time_to_next_loop = 10000 - dt; + scheduler.run(time_to_next_loop); } - run_events(time_to_next_loop); } } diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 8d5de3b51a..96081f3389 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -544,7 +544,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, // if we don't have at least 1ms remaining before the main loop // wants to fire then don't send a mavlink message. We want to // prioritise the main flight control loop over communications - if (event_time_available() < 500) { + if (scheduler.time_available_usec() < 800) { gcs_out_of_time = true; return false; } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 4582c96b4d..fd5e5d184b 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -61,6 +61,9 @@ public: // barometer object (needed for SITL) k_param_barometer, + // scheduler object (for debugging) + k_param_scheduler, + // Misc // k_param_log_bitmask = 20, diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 15a8afaa96..89c1b49efd 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -1003,6 +1003,7 @@ const AP_Param::Info var_info[] PROGMEM = { #endif GOBJECT(barometer, "GND_", AP_Baro), + GOBJECT(scheduler, "SCHED_", AP_Scheduler), #if AP_LIMITS == ENABLED //@Group: LIM_