From 54ab96fe169226e13e7dea229b55f1996d43e7b4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 28 Oct 2013 10:33:52 +1100 Subject: [PATCH] Rover: removed fast_loop() and use scheduler for all tasks this also fixes a parameter download speed issue --- APMrover2/APMrover2.pde | 74 ++++++++++++++------------------------- APMrover2/GCS_Mavlink.pde | 50 ++++++++++++++++++++++---- 2 files changed, 70 insertions(+), 54 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 6c12d49527..77c08eeb92 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -548,27 +548,34 @@ static uint16_t mainLoop_count; //////////////////////////////////////////////////////////////////////////////// /* - 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) + scheduler table - all regular tasks 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 = { + { read_radio, 1, 1000 }, + { ahrs_update, 1, 6400 }, + { read_sonars, 1, 2000 }, + { update_current_mode, 1, 1000 }, + { set_servos, 1, 1000 }, { update_GPS, 5, 2500 }, { navigate, 5, 1600 }, { update_compass, 5, 2000 }, { update_commands, 5, 1000 }, { update_logging, 5, 1000 }, + { gcs_retry_deferred, 1, 1000 }, + { gcs_update, 1, 1700 }, + { gcs_data_stream_send, 1, 3000 }, + { read_control_switch, 15, 1000 }, + { read_trim_switch, 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 }, - { gcs_failsafe_check, 5, 500 }, + { mount_update, 1, 600 }, + { gcs_failsafe_check, 5, 600 }, { compass_accumulate, 1, 900 }, - { update_notify, 1, 100 }, + { update_notify, 1, 300 }, { one_second_loop, 50, 3000 } }; @@ -616,60 +623,33 @@ void loop() G_Dt = (float)delta_ms_fast_loop / 1000.f; fast_loopTimer = timer; - mainLoop_count++; + if (delta_ms_fast_loop > G_Dt_max) + G_Dt_max = delta_ms_fast_loop; - // Execute the fast loop - // --------------------- - fast_loop(); + mainLoop_count++; // tell the scheduler one tick has passed scheduler.tick(); fast_loopTimeStamp = millis(); - scheduler.run(19000U); + scheduler.run(19500U); } -// Main loop 50Hz -static void fast_loop() +// update AHRS system +static void ahrs_update() { - // This is the fast loop - we want it to execute at 50Hz if possible - // ----------------------------------------------------------------- - if (delta_ms_fast_loop > G_Dt_max) - G_Dt_max = delta_ms_fast_loop; +#if HIL_MODE != HIL_MODE_DISABLED + // update hil before AHRS update + gcs_update(); +#endif - // Read radio - // ---------- - read_radio(); - - // try to send any deferred messages if the serial port now has - // some space available - gcs_send_message(MSG_RETRY_DEFERRED); - - #if HIL_MODE != HIL_MODE_DISABLED - // update hil before dcm update - gcs_update(); - #endif - - ahrs.update(); - - read_sonars(); + ahrs.update(); if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Log_Write_Attitude(); if (g.log_bitmask & MASK_LOG_IMU) DataFlash.Log_Write_IMU(&ins); - - // custom code/exceptions for flight modes - // --------------------------------------- - update_current_mode(); - - // write out the servo PWM values - // ------------------------------ - set_servos(); - - gcs_update(); - gcs_data_stream_send(); } /* diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 67322108f6..0d24c1e249 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -9,6 +9,9 @@ static bool in_mavlink_delay; // true when we have received at least 1 MAVLink packet static bool mavlink_active; +// true if we are out of time in our event timeslice +static bool gcs_out_of_time; + // check if a message will fit in the payload space available #define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false @@ -479,6 +482,14 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, return false; } + // 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 (!in_mavlink_delay && scheduler.time_available_usec() < 1200) { + gcs_out_of_time = true; + return false; + } + switch (id) { case MSG_HEARTBEAT: CHECK_PAYLOAD_SIZE(HEARTBEAT); @@ -769,8 +780,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = { // @Range: 0 10 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 5), - + AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRates[8], 10), AP_GROUPEND }; @@ -871,7 +881,8 @@ bool GCS_MAVLINK::stream_trigger(enum streams stream_num) // send at a much lower rate while handling waypoints and // parameter sends - if (waypoint_receiving || _queued_parameter != NULL) { + if ((stream_num != STREAM_PARAMS) && + (waypoint_receiving || _queued_parameter != NULL)) { rate *= 0.25; } @@ -896,18 +907,19 @@ bool GCS_MAVLINK::stream_trigger(enum streams stream_num) void GCS_MAVLINK::data_stream_send(void) { + gcs_out_of_time = false; + if (_queued_parameter != NULL) { if (streamRates[STREAM_PARAMS].get() <= 0) { - streamRates[STREAM_PARAMS].set(5); - } - if (streamRates[STREAM_PARAMS].get() > 5) { - streamRates[STREAM_PARAMS].set(5); + streamRates[STREAM_PARAMS].set(10); } if (stream_trigger(STREAM_PARAMS)) { send_message(MSG_NEXT_PARAM); } } + if (gcs_out_of_time) return; + if (in_mavlink_delay) { #if HIL_MODE != HIL_MODE_DISABLED // in HIL we need to keep sending servo values to ensure @@ -924,11 +936,15 @@ GCS_MAVLINK::data_stream_send(void) return; } + if (gcs_out_of_time) return; + if (stream_trigger(STREAM_RAW_SENSORS)) { send_message(MSG_RAW_IMU1); send_message(MSG_RAW_IMU3); } + if (gcs_out_of_time) return; + if (stream_trigger(STREAM_EXTENDED_STATUS)) { send_message(MSG_EXTENDED_STATUS1); send_message(MSG_EXTENDED_STATUS2); @@ -937,29 +953,41 @@ GCS_MAVLINK::data_stream_send(void) send_message(MSG_NAV_CONTROLLER_OUTPUT); } + if (gcs_out_of_time) return; + if (stream_trigger(STREAM_POSITION)) { // sent with GPS read send_message(MSG_LOCATION); } + if (gcs_out_of_time) return; + if (stream_trigger(STREAM_RAW_CONTROLLER)) { send_message(MSG_SERVO_OUT); } + if (gcs_out_of_time) return; + if (stream_trigger(STREAM_RC_CHANNELS)) { send_message(MSG_RADIO_OUT); send_message(MSG_RADIO_IN); } + if (gcs_out_of_time) return; + if (stream_trigger(STREAM_EXTRA1)) { send_message(MSG_ATTITUDE); send_message(MSG_SIMSTATE); } + if (gcs_out_of_time) return; + if (stream_trigger(STREAM_EXTRA2)) { send_message(MSG_VFR_HUD); } + if (gcs_out_of_time) return; + if (stream_trigger(STREAM_EXTRA3)) { send_message(MSG_AHRS); send_message(MSG_HWSTATUS); @@ -2021,3 +2049,11 @@ void gcs_send_text_fmt(const prog_char_t *fmt, ...) } } + +/** + retry any deferred messages + */ +static void gcs_retry_deferred(void) +{ + gcs_send_message(MSG_RETRY_DEFERRED); +}