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

Add a test command from commander

This commit is contained in:
Lorenz Meier 2016-02-22 22:15:12 +01:00
parent 3854eae3ec
commit 5dfd9dd68b
1 changed files with 23 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/modules/commander/commander.cpp
View File

@ -470,6 +470,23 @@ int commander_main(int argc, char *argv[])
return 0;
}
if (!strcmp(argv[1], "lockdown")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
vehicle_command_s cmd = {};
cmd.target_system = status.system_id;
cmd.target_component = status.component_id;
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION;
cmd.param1 = 2.0f; /* lockdown */
// XXX inspect use of publication handle
(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
px4_close(mavlink_fd_local);
return 0;
}
usage("unrecognized command");
return 1;
}
@ -783,13 +800,18 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* Flight termination */
case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
if (cmd->param1 > 0.5f) {
if (cmd->param1 > 1.5f) {
armed_local->lockdown = true;
warnx("forcing lockdown (motors off)");
} else if (cmd->param1 > 0.5f) {
//XXX update state machine?
armed_local->force_failsafe = true;
warnx("forcing failsafe (termination)");
} else {
armed_local->force_failsafe = false;
armed_local->lockdown = false;
warnx("disabling failsafe (termination)");
}