From c7b64e3c012eb922cdc6e65619bfaf52fa296c4d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 5 Aug 2022 10:42:37 +0200 Subject: [PATCH] commander: check for ack when calling 'commander takeoff' --- src/modules/commander/Commander.cpp | 32 ++++++++++++++++++++++++----- 1 file changed, 27 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index faf887c826..40cf41740b 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -192,6 +192,24 @@ static bool send_vehicle_command(const uint32_t cmd, const float param1 = NAN, c return vcmd_pub.publish(vcmd); } +static bool wait_for_vehicle_command_reply(const uint32_t cmd, + uORB::SubscriptionData &vehicle_command_ack_sub) +{ + hrt_abstime start = hrt_absolute_time(); + + while (hrt_absolute_time() - start < 100_ms) { + if (vehicle_command_ack_sub.update()) { + if (vehicle_command_ack_sub.get().command == cmd) { + return vehicle_command_ack_sub.get().result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + } + } + + px4_usleep(10000); + } + + return false; +} + static bool broadcast_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN, const float param3 = NAN, const float param4 = NAN, const double param5 = static_cast(NAN), const double param6 = static_cast(NAN), const float param7 = NAN) @@ -338,10 +356,14 @@ int Commander::custom_command(int argc, char *argv[]) if (!strcmp(argv[0], "takeoff")) { // switch to takeoff mode and arm + uORB::SubscriptionData vehicle_command_ack_sub{ORB_ID(vehicle_command_ack)}; send_vehicle_command(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF); - send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, - static_cast(vehicle_command_s::ARMING_ACTION_ARM), - 0.f); + + if (wait_for_vehicle_command_reply(vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF, vehicle_command_ack_sub)) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM, + static_cast(vehicle_command_s::ARMING_ACTION_ARM), + 0.f); + } return 0; } @@ -1190,12 +1212,12 @@ Commander::handle_command(const vehicle_command_s &cmd) cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_critical(&_mavlink_log_pub, "Takeoff denied! Please disarm and retry\t"); + mavlink_log_critical(&_mavlink_log_pub, "Takeoff denied!\t"); /* EVENT * @description Check for a valid position estimate */ events::send(events::ID("commander_takeoff_denied"), {events::Log::Critical, events::LogInternal::Info}, - "Takeoff denied! Please disarm and retry"); + "Takeoff denied!"); cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } }