From d1343b0ccbc2ab1f676f36868250cb4e7b27f277 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 18 Jun 2018 11:40:55 +0200 Subject: [PATCH] calibration_routines: fix 'Command denied during calibration' error message The uorb subscribe logic got changed for queued topics with https://github.com/PX4/Firmware/pull/9436, meaning an orb_subscribe will return past messages as well now. This lead to an error 'Command denied during calibration' for the previously received calibration start command. --- src/modules/commander/calibration_routines.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index ab1ab1b2f8..fcdddeb544 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -799,7 +799,16 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, int calibrate_cancel_subscribe() { - return orb_subscribe(ORB_ID(vehicle_command)); + int vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); + if (vehicle_command_sub >= 0) { + // make sure we won't read any old messages + struct vehicle_command_s cmd; + bool update; + while (orb_check(vehicle_command_sub, &update) == 0 && update) { + orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &cmd); + } + } + return vehicle_command_sub; } void calibrate_cancel_unsubscribe(int cmd_sub)