From a242c0ff6d7d3c492946a1d41c5160a920af6893 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Nov 2015 11:15:17 +0100 Subject: [PATCH] Commander: Add takeoff command handler. Do not check RC config in SITL RC mode --- src/modules/commander/commander.cpp | 51 +++++++++++++++++-- .../commander/state_machine_helper.cpp | 24 ++++++++- 2 files changed, 70 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 49e6459b03..6251e66168 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e7dc322206..ba8720f9ee 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -351,6 +351,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case vehicle_status_s::MAIN_STATE_AUTO_MISSION: case vehicle_status_s::MAIN_STATE_AUTO_RTL: + case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { ret = TRANSITION_CHANGED; @@ -744,6 +745,27 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; + case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF: + /* require global position and home */ + + if (status->engine_failure) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((!status->condition_global_position_valid || + !status->condition_home_position_valid)) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF; + } + break; + case vehicle_status_s::MAIN_STATE_OFFBOARD: /* require offboard control, otherwise stay where you are */ if (status->offboard_control_signal_lost && !status->rc_signal_lost) { @@ -785,7 +807,7 @@ int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool } bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, - checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), + checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures); if (!status->cb_usb && status->usb_connected && prearm) {