mirror of https://github.com/ArduPilot/ardupilot
ArduCopter: move gcs updates to run only when there are 4ms of spare time before the next 100hz iteration starts
This commit is contained in:
parent
881dae1e27
commit
d926360e81
|
@ -1003,6 +1003,11 @@ void loop()
|
|||
compass.accumulate();
|
||||
}
|
||||
}
|
||||
|
||||
// process communications with the GCS
|
||||
if (timer - fast_loopTimer < 6000) {
|
||||
gcs_check();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -1010,10 +1015,6 @@ void loop()
|
|||
// Main loop - 100hz
|
||||
static void fast_loop()
|
||||
{
|
||||
// try to send any deferred messages if the serial port now has
|
||||
// some space available
|
||||
gcs_send_message(MSG_RETRY_DEFERRED);
|
||||
|
||||
// run low level rate controllers that only require IMU data
|
||||
run_rate_controllers();
|
||||
|
||||
|
@ -1232,10 +1233,6 @@ static void fifty_hz_loop()
|
|||
if (g.log_bitmask & MASK_LOG_RAW && motors.armed())
|
||||
Log_Write_Raw();
|
||||
#endif
|
||||
|
||||
// kick the GCS to process uplink data
|
||||
gcs_update();
|
||||
gcs_data_stream_send();
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -18,6 +18,36 @@ static bool mavlink_active;
|
|||
// prototype this for use inside the GCS class
|
||||
static void gcs_send_text_fmt(const prog_char_t *fmt, ...);
|
||||
|
||||
// gcs_check - throttles communication with ground station.
|
||||
// should be called regularly
|
||||
// returns true if it has sent a message to the ground station
|
||||
static bool gcs_check()
|
||||
{
|
||||
static uint32_t last_1hz, last_50hz, last_5s;
|
||||
bool sent_message = false;
|
||||
|
||||
uint32_t tnow = millis();
|
||||
if (tnow - last_1hz > 1000) {
|
||||
last_1hz = tnow;
|
||||
gcs_send_message(MSG_HEARTBEAT);
|
||||
gcs_send_message(MSG_EXTENDED_STATUS1);
|
||||
sent_message = true;
|
||||
}
|
||||
if (tnow - last_50hz > 20 && !sent_message) {
|
||||
last_50hz = tnow;
|
||||
gcs_update();
|
||||
gcs_data_stream_send();
|
||||
sent_message = true;
|
||||
}
|
||||
if (tnow - last_5s > 5000 && !sent_message) {
|
||||
last_5s = tnow;
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
|
||||
sent_message = true;
|
||||
}
|
||||
|
||||
return sent_message;
|
||||
}
|
||||
|
||||
/*
|
||||
* !!NOTE!!
|
||||
*
|
||||
|
|
Loading…
Reference in New Issue