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

Rover: removed fast_loop() and use scheduler for all tasks 

this also fixes a parameter download speed issue
This commit is contained in:
Andrew Tridgell 2013-10-28 10:33:52 +11:00
parent c602f7da0c
commit 54ab96fe16
2 changed files with 70 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
APMrover2/APMrover2.pde
View File

@ -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();
}
/*

50
APMrover2/GCS_Mavlink.pde
View File

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