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)