AP_Param: cope better with flooding the save queue with one param

This commit is contained in:
Andrew Tridgell 2020-12-07 11:03:26 +11:00
parent 5a8acea0dd
commit 8b8029fdfe

View File

@ -1160,9 +1160,18 @@ void AP_Param::save_sync(bool force_save)
*/
void AP_Param::save(bool force_save)
{
struct param_save p;
struct param_save p, p2;
p.param = this;
p.force_save = force_save;
if (save_queue.peek(p2) &&
p2.param == this &&
p2.force_save == force_save) {
// this one is already at the head of the list to be
// saved. This check is cheap and catches the case where we
// are flooding the save queue with one parameter (eg. mission
// creation, changing MIS_TOTAL)
return;
}
while (!save_queue.push(p)) {
// if we can't save to the queue
if (hal.util->get_soft_armed() || !hal.scheduler->in_main_thread()) {