dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander: Add support for pairing via commandline

This commit is contained in:
Lorenz Meier 2021-04-01 15:56:09 +02:00 committed by GitHub
parent e789124bd9
commit 82f319a84a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 36 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
src/modules/commander/Commander.cpp
View File

@ -137,27 +137,48 @@ static bool send_vehicle_command(const uint32_t cmd, const float param1 = NAN, c
const double param6 = static_cast<double>(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_s> 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<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
vcmd.timestamp = hrt_absolute_time();
return vcmd_pub.publish(vcmd);
}
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<double>(NAN),
const double param6 = static_cast<double>(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_s> 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<vehicle_command_s> 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");