fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 01:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Add takeoff command handler. Do not check RC config in SITL RC mode

Browse Source
This commit is contained in:
Lorenz Meier
2015-11-20 11:15:17 +01:00
parent e96fd83565
commit a242c0ff6d
2 changed files with 70 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+47 -4
View File
@@ -410,6 +410,37 @@ int commander_main(int argc, char *argv[])
return 0;
}
if (!strcmp(argv[1], "takeoff")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
if (TRANSITION_CHANGED == arm_disarm(true, mavlink_fd_local, "command line")) {
warnx("note: not updating home position on commandline arming!");
/* see if we got a home position */
if (status.condition_home_position_valid) {
vehicle_command_s cmd = {};
cmd.command = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;
// cmd.param1 = 0.25f; /* minimum pitch */
// /* param 2-3 unused */
// cmd.param4 = home_position.yaw;
// cmd.param5 = home_position.lat;
// cmd.param6 = home_position.lon;
// cmd.param7 = home_position.alt;
// XXX inspect use of publication handle
(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
} else {
warnx("rejecting takeoff, no position lock yet");
}
} else {
warnx("arming failed");
}
px4_close(mavlink_fd_local);
return 0;
}
usage("unrecognized command");
return 1;
}
@@ -809,6 +840,17 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
break;
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
/* ok, home set, use it to take off */
if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF)) {
warnx("taking off!");
} else {
warnx("takeoff denied");
}
}
break;
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@@ -974,6 +1016,7 @@ int commander_thread_main(int argc, char *argv[])
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF] = "AUTO_TAKEOFF";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS";
nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL";
@@ -1367,7 +1410,7 @@ int commander_thread_main(int argc, char *argv[])
if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) {
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
}
/* re-check RC calibration */
@@ -1471,11 +1514,11 @@ int commander_thread_main(int argc, char *argv[])
if (is_hil_setup(autostart_id)) {
/* HIL configuration: check only RC input */
(void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false, true);
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false, true);
} else {
/* check sensors also */
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
}
}
@@ -3248,7 +3291,7 @@ void *commander_low_prio_loop(void *arg)
}
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd);