diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c9682e389c..e4e671e35d 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -137,27 +137,48 @@ static bool send_vehicle_command(const uint32_t cmd, const float param1 = NAN, c const double param6 = static_cast(NAN), const float param7 = NAN) { vehicle_command_s vcmd{}; - + vcmd.command = cmd; vcmd.param1 = param1; vcmd.param2 = param2; vcmd.param3 = param3; vcmd.param4 = param4; - vcmd.param5 = (double)param5; - vcmd.param6 = (double)param6; + vcmd.param5 = param5; + vcmd.param6 = param6; vcmd.param7 = param7; - vcmd.command = cmd; - uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; vcmd.source_system = vehicle_status_sub.get().system_id; vcmd.target_system = vehicle_status_sub.get().system_id; vcmd.source_component = vehicle_status_sub.get().component_id; vcmd.target_component = vehicle_status_sub.get().component_id; + uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; vcmd.timestamp = hrt_absolute_time(); + return vcmd_pub.publish(vcmd); +} - uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; +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) +{ + vehicle_command_s vcmd{}; + vcmd.command = cmd; + vcmd.param1 = param1; + vcmd.param2 = param2; + vcmd.param3 = param3; + vcmd.param4 = param4; + vcmd.param5 = param5; + vcmd.param6 = param6; + vcmd.param7 = param7; + uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; + vcmd.source_system = vehicle_status_sub.get().system_id; + vcmd.target_system = 0; + vcmd.source_component = vehicle_status_sub.get().component_id; + vcmd.target_component = 0; + + uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; + vcmd.timestamp = hrt_absolute_time(); return vcmd_pub.publish(vcmd); } #endif @@ -357,6 +378,14 @@ int Commander::custom_command(int argc, char *argv[]) return (ret ? 0 : 1); } + if (!strcmp(argv[0], "pair")) { + + // GCS pairing request handled by a companion + bool ret = broadcast_vehicle_command(vehicle_command_s::VEHICLE_CMD_START_RX_PAIR, 10.f); + + return (ret ? 0 : 1); + } + if (!strcmp(argv[0], "set_ekf_origin")) { if (argc > 3) { @@ -3971,6 +4000,7 @@ The commander module contains the state machine for mode switching and failsafe PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode"); PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland", "Flight mode", false); + PRINT_MODULE_USAGE_COMMAND("pair"); PRINT_MODULE_USAGE_COMMAND("lockdown"); PRINT_MODULE_USAGE_ARG("off", "Turn lockdown off", true); PRINT_MODULE_USAGE_COMMAND("set_ekf_origin");