aboutsummaryrefslogtreecommitdiff
path: root/src/modules/commander
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/commander')
-rw-r--r--src/modules/commander/commander.cpp356
-rw-r--r--src/modules/commander/commander_params.c2
-rw-r--r--src/modules/commander/state_machine_helper.cpp223
-rw-r--r--src/modules/commander/state_machine_helper.h9
4 files changed, 163 insertions, 427 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index add7312de..dbfca38b1 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -69,7 +69,6 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
@@ -189,7 +188,7 @@ void usage(const char *reason);
/**
* React to commands that are sent e.g. from the mavlink module.
*/
-void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
+bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
/**
* Mainloop of commander.
@@ -210,8 +209,6 @@ void print_reject_arm(const char *msg);
void print_status();
-transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
-
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
*/
@@ -341,13 +338,13 @@ void print_status()
warnx("arming: %s", armed_str);
}
-static orb_advert_t control_mode_pub;
static orb_advert_t status_pub;
-void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
+bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
{
/* result of the command */
- uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+ enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+ bool ret = false;
/* only handle high-priority commands here */
@@ -360,12 +357,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
- int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
+ int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
/* if HIL got enabled, reset battery status state */
- if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
+ if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
/* reset the arming mode to disarmed */
- arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
+ arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
if (arming_res != TRANSITION_DENIED) {
mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
@@ -374,6 +371,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
}
}
+ if (hil_ret == OK)
+ ret = true;
// TODO remove debug code
//mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode);
@@ -381,12 +380,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_NOT_CHANGED;
if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
- if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) {
+ if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED;
} else {
- arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
+ arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
}
if (arming_res == TRANSITION_CHANGED) {
@@ -396,7 +395,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} else {
if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
- arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed);
+ arming_res = arming_state_transition(status, safety, new_arming_state, armed);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
@@ -406,6 +405,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_NOT_CHANGED;
}
}
+ if (arming_res == TRANSITION_CHANGED)
+ ret = true;
/* set main state */
transition_result_t main_res = TRANSITION_DENIED;
@@ -446,6 +447,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
}
+ if (main_res == TRANSITION_CHANGED)
+ ret = true;
if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -457,29 +460,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break;
}
- case VEHICLE_CMD_NAV_TAKEOFF: {
- if (armed->armed) {
- transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
-
- if (nav_res == TRANSITION_CHANGED) {
- mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
- }
-
- if (nav_res != TRANSITION_DENIED) {
- result = VEHICLE_CMD_RESULT_ACCEPTED;
-
- } else {
- result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
- }
-
- } else {
- /* reject TAKEOFF not armed */
- result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
- }
-
- break;
- }
-
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
@@ -489,12 +469,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_DENIED;
} else {
- arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
+ arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
}
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
+ ret = true;
} else {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
@@ -504,29 +485,61 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
- default:
- break;
- }
+ case VEHICLE_CMD_OVERRIDE_GOTO: {
+ // TODO listen vehicle_command topic directly from navigator (?)
+ unsigned int mav_goto = cmd->param1;
+ if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
+ status->set_nav_state = NAV_STATE_LOITER;
+ status->set_nav_state_timestamp = hrt_absolute_time();
+ mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ ret = true;
- /* supported command handling stop */
- if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
- tune_positive();
+ } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
+ status->set_nav_state = NAV_STATE_MISSION;
+ status->set_nav_state_timestamp = hrt_absolute_time();
+ mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ ret = true;
- } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
- /* we do not care in the high prio loop about commands we don't know */
- } else {
- tune_negative();
+ } else {
+ mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
+ }
+ }
+ break;
- if (result == VEHICLE_CMD_RESULT_DENIED) {
- mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command);
+ /* Flight termination */
+ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
- } else if (result == VEHICLE_CMD_RESULT_FAILED) {
- mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command);
+ if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
+ transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ ret = true;
- } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
- mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
+ } else {
+ /* reject parachute depoyment not armed */
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ }
}
+ break;
+
+ case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+ case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
+ case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
+ case VEHICLE_CMD_PREFLIGHT_STORAGE:
+ /* ignore commands that handled in low prio loop */
+ break;
+
+ default:
+ /* warn about unsupported commands */
+ answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
+ break;
+ }
+
+ if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+ /* already warned about unsupported commands in "default" case */
+ answer_command(*cmd, result);
}
/* send any requested ACKs */
@@ -544,9 +557,6 @@ static struct actuator_armed_s armed;
static struct safety_s safety;
-/* flags for control apps */
-struct vehicle_control_mode_s control_mode;
-
int commander_thread_main(int argc, char *argv[])
{
/* not yet initialized */
@@ -589,11 +599,9 @@ int commander_thread_main(int argc, char *argv[])
/* Initialize armed with all false */
memset(&armed, 0, sizeof(armed));
- /* Initialize all flags to false */
- memset(&control_mode, 0, sizeof(control_mode));
-
status.main_state = MAIN_STATE_MANUAL;
- status.navigation_state = NAVIGATION_STATE_DIRECT;
+ status.set_nav_state = NAV_STATE_NONE;
+ status.set_nav_state_timestamp = 0;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;
@@ -605,9 +613,6 @@ int commander_thread_main(int argc, char *argv[])
status.rc_signal_lost = true;
status.offboard_control_signal_lost = true;
- /* allow manual override initially */
- control_mode.flag_external_manual_override_ok = true;
-
/* set battery warning flag */
status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
status.condition_battery_voltage_valid = false;
@@ -615,9 +620,6 @@ int commander_thread_main(int argc, char *argv[])
// XXX for now just set sensors as initialized
status.condition_system_sensors_initialized = true;
- // XXX just disable offboard control for now
- control_mode.flag_control_offboard_enabled = false;
-
/* advertise to ORB */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
/* publish current state machine */
@@ -629,8 +631,6 @@ int commander_thread_main(int argc, char *argv[])
armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
- control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
-
/* home position */
orb_advert_t home_pub = -1;
struct home_position_s home;
@@ -780,11 +780,9 @@ int commander_thread_main(int argc, char *argv[])
status.system_type == VEHICLE_TYPE_QUADROTOR ||
status.system_type == VEHICLE_TYPE_HEXAROTOR ||
status.system_type == VEHICLE_TYPE_OCTOROTOR) {
- control_mode.flag_external_manual_override_ok = false;
status.is_rotary_wing = true;
} else {
- control_mode.flag_external_manual_override_ok = true;
status.is_rotary_wing = false;
}
@@ -836,7 +834,7 @@ int commander_thread_main(int argc, char *argv[])
// XXX this would be the right approach to do it, but do we *WANT* this?
// /* disarm if safety is now on and still armed */
// if (safety.safety_switch_available && !safety.safety_off) {
- // (void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+ // (void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
// }
}
@@ -956,10 +954,10 @@ int commander_thread_main(int argc, char *argv[])
battery_tune_played = false;
if (armed.armed) {
- arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
+ arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
} else {
- arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
+ arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
}
status_changed = true;
@@ -970,7 +968,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
// XXX check for sensors
- arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+ arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
} else {
// XXX: Add emergency stuff if sensors are lost
@@ -1006,23 +1004,17 @@ int commander_thread_main(int argc, char *argv[])
if (!home_position_set && gps_position.fix_type >= 3 &&
(gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk
- (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
+ (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
+ && global_position.valid) {
/* copy position data to uORB home message, store it locally as well */
- // TODO use global position estimate
- home.lat = gps_position.lat;
- home.lon = gps_position.lon;
- home.alt = gps_position.alt;
- home.eph_m = gps_position.eph_m;
- home.epv_m = gps_position.epv_m;
- home.s_variance_m_s = gps_position.s_variance_m_s;
- home.p_variance_m = gps_position.p_variance_m;
+ home.lat = (double)global_position.lat / 1e7d;
+ home.lon = (double)global_position.lon / 1e7d;
+ home.altitude = (float)global_position.alt;
- double home_lat_d = home.lat * 1e-7;
- double home_lon_d = home.lon * 1e-7;
- warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d);
- mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d);
+ warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude);
+ mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude);
/* announce new home position */
if (home_pub > 0) {
@@ -1066,15 +1058,13 @@ int commander_thread_main(int argc, char *argv[])
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
- (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
- (status.condition_landed && (
- status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
- status.navigation_state == NAVIGATION_STATE_VECTOR
- ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+ (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
+ sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
- res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed);
+ res = arming_state_transition(&status, &safety, new_arming_state, &armed);
stick_off_counter = 0;
} else {
@@ -1096,7 +1086,7 @@ int commander_thread_main(int argc, char *argv[])
print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
} else {
- res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
+ res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
}
stick_on_counter = 0;
@@ -1147,6 +1137,16 @@ int commander_thread_main(int argc, char *argv[])
}
}
+ /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */
+ if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
+ transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON);
+ if (flighttermination_res == TRANSITION_CHANGED) {
+ tune_positive();
+ }
+ } else {
+ flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF);
+ }
+
/* handle commands last, as the system needs to be updated to handle them */
orb_check(cmd_sub, &updated);
@@ -1156,31 +1156,19 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
- handle_command(&status, &safety, &control_mode, &cmd, &armed);
- }
-
- /* evaluate the navigation state machine */
- transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
-
- if (res == TRANSITION_DENIED) {
- /* DENIED here indicates bug in the commander */
- warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
- mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+ if (handle_command(&status, &safety, &cmd, &armed))
+ status_changed = true;
}
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = check_arming_state_changed();
bool main_state_changed = check_main_state_changed();
- bool navigation_state_changed = check_navigation_state_changed();
+ bool flighttermination_state_changed = check_flighttermination_state_changed();
hrt_abstime t1 = hrt_absolute_time();
- if (navigation_state_changed || arming_state_changed) {
- control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic
- }
-
- if (arming_state_changed || main_state_changed || navigation_state_changed) {
- mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
+ if (arming_state_changed || main_state_changed) {
+ mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state);
status_changed = true;
}
@@ -1188,8 +1176,6 @@ int commander_thread_main(int argc, char *argv[])
if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
status.timestamp = t1;
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
- control_mode.timestamp = t1;
- orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
armed.timestamp = t1;
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
}
@@ -1379,7 +1365,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
current_status->mode_switch = MODE_SWITCH_ASSISTED;
}
- /* land switch */
+ /* return switch */
if (!isfinite(sp_man->return_switch)) {
current_status->return_switch = RETURN_SWITCH_NONE;
@@ -1387,7 +1373,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
current_status->return_switch = RETURN_SWITCH_RETURN;
} else {
- current_status->return_switch = RETURN_SWITCH_NONE;
+ current_status->return_switch = RETURN_SWITCH_NORMAL;
}
/* assisted switch */
@@ -1403,10 +1389,10 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
/* mission switch */
if (!isfinite(sp_man->mission_switch)) {
- current_status->mission_switch = MISSION_SWITCH_MISSION;
+ current_status->mission_switch = MISSION_SWITCH_NONE;
} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
- current_status->mission_switch = MISSION_SWITCH_NONE;
+ current_status->mission_switch = MISSION_SWITCH_LOITER;
} else {
current_status->mission_switch = MISSION_SWITCH_MISSION;
@@ -1502,133 +1488,6 @@ print_reject_arm(const char *msg)
}
}
-transition_result_t
-check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
-{
- transition_result_t res = TRANSITION_DENIED;
-
- if (status->main_state == MAIN_STATE_AUTO) {
- if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
- // TODO AUTO_LAND handling
- if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
- /* don't switch to other states until takeoff not completed */
- // XXX: only respect the condition_landed when the local position is actually valid
- if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
- return TRANSITION_NOT_CHANGED;
- }
- }
-
- if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
- status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
- status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
- status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
- /* possibly on ground, switch to TAKEOFF if needed */
- if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
- res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
- return res;
- }
- }
-
- /* switch to AUTO mode */
- if (status->rc_signal_found_once && !status->rc_signal_lost) {
- /* act depending on switches when manual control enabled */
- if (status->return_switch == RETURN_SWITCH_RETURN) {
- /* RTL */
- res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
-
- } else {
- if (status->mission_switch == MISSION_SWITCH_MISSION) {
- /* MISSION */
- res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
-
- } else {
- /* LOITER */
- res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
- }
- }
-
- } else {
- /* switch to MISSION when no RC control and first time in some AUTO mode */
- if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
- status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
- status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
- status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
- res = TRANSITION_NOT_CHANGED;
-
- } else {
- res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
- }
- }
-
- } else {
- /* disarmed, always switch to AUTO_READY */
- res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
- }
-
- } else {
- /* manual control modes */
- if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) {
- /* switch to failsafe mode */
- bool manual_control_old = control_mode->flag_control_manual_enabled;
-
- if (!status->condition_landed && status->condition_local_position_valid) {
- /* in air: try to hold position if possible */
- res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
-
- } else {
- /* landed: don't try to hold position but land (if taking off) */
- res = TRANSITION_DENIED;
- }
-
- if (res == TRANSITION_DENIED) {
- res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
- }
-
- control_mode->flag_control_manual_enabled = false;
-
- if (res == TRANSITION_NOT_CHANGED && manual_control_old) {
- /* mark navigation state as changed to force immediate flag publishing */
- set_navigation_state_changed();
- res = TRANSITION_CHANGED;
- }
-
- if (res == TRANSITION_CHANGED) {
- if (control_mode->flag_control_position_enabled) {
- mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD");
-
- } else {
- if (status->condition_landed) {
- mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)");
-
- } else {
- mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD");
- }
- }
- }
-
- } else {
- switch (status->main_state) {
- case MAIN_STATE_MANUAL:
- res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
- break;
-
- case MAIN_STATE_SEATBELT:
- res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
- break;
-
- case MAIN_STATE_EASY:
- res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
- break;
-
- default:
- break;
- }
- }
- }
-
- return res;
-}
-
void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
{
switch (result) {
@@ -1698,7 +1557,8 @@ void *commander_low_prio_loop(void *arg)
/* ignore commands the high-prio loop handles */
if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
- cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
+ cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
+ cmd.command == VEHICLE_CMD_DO_SET_SERVO)
continue;
/* only handle low-priority commands here */
@@ -1736,7 +1596,7 @@ void *commander_low_prio_loop(void *arg)
/* try to go to INIT/PREFLIGHT arming state */
// XXX disable interrupts in arming_state_transition
- if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) {
+ if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@@ -1776,7 +1636,7 @@ void *commander_low_prio_loop(void *arg)
else
tune_negative();
- arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+ arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
break;
}
@@ -1828,7 +1688,7 @@ void *commander_low_prio_loop(void *arg)
}
default:
- answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
+ /* don't answer on unsupported commands, it will be done in main loop */
break;
}
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index e10b7f18d..d3155f7bf 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -45,8 +45,6 @@
#include <nuttx/config.h>
#include <systemlib/param/param.h>
-PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f);
-PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f);
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 490fc8fc6..731e0e3ff 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -46,7 +46,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_control_mode.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@@ -64,11 +63,10 @@ static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
-static bool navigation_state_changed = true;
+static bool flighttermination_state_changed = true;
transition_result_t
arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
- const struct vehicle_control_mode_s *control_mode,
arming_state_t new_arming_state, struct actuator_armed_s *armed)
{
/*
@@ -85,7 +83,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
} else {
/* enforce lockdown in HIL */
- if (control_mode->flag_system_hil_enabled) {
+ if (status->hil_state == HIL_STATE_ON) {
armed->lockdown = true;
} else {
armed->lockdown = false;
@@ -108,7 +106,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* allow coming from INIT and disarming from ARMED */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_ARMED
- || control_mode->flag_system_hil_enabled) {
+ || status->hil_state == HIL_STATE_ON) {
/* sensors need to be initialized for STANDBY state */
if (status->condition_system_sensors_initialized) {
@@ -125,7 +123,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
/* allow arming from STANDBY and IN-AIR-RESTORE */
if ((status->arming_state == ARMING_STATE_STANDBY
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
- && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */
+ && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */
ret = TRANSITION_CHANGED;
armed->armed = true;
armed->ready_to_arm = true;
@@ -239,8 +237,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
case MAIN_STATE_SEATBELT:
/* need at minimum altitude estimate */
- if (current_state->condition_local_altitude_valid ||
- current_state->condition_global_position_valid) {
+ if (!current_state->is_rotary_wing ||
+ (current_state->condition_local_altitude_valid ||
+ current_state->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
}
@@ -287,162 +286,11 @@ check_main_state_changed()
}
}
-transition_result_t
-navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
-{
- transition_result_t ret = TRANSITION_DENIED;
-
- /* only check transition if the new state is actually different from the current one */
- if (new_navigation_state == status->navigation_state) {
- ret = TRANSITION_NOT_CHANGED;
-
- } else {
-
- switch (new_navigation_state) {
- case NAVIGATION_STATE_DIRECT:
- ret = TRANSITION_CHANGED;
- control_mode->flag_control_rates_enabled = true;
- control_mode->flag_control_attitude_enabled = false;
- control_mode->flag_control_velocity_enabled = false;
- control_mode->flag_control_position_enabled = false;
- control_mode->flag_control_altitude_enabled = false;
- control_mode->flag_control_climb_rate_enabled = false;
- control_mode->flag_control_manual_enabled = true;
- control_mode->flag_control_auto_enabled = false;
- break;
-
- case NAVIGATION_STATE_STABILIZE:
- ret = TRANSITION_CHANGED;
- control_mode->flag_control_rates_enabled = true;
- control_mode->flag_control_attitude_enabled = true;
- control_mode->flag_control_velocity_enabled = false;
- control_mode->flag_control_position_enabled = false;
- control_mode->flag_control_altitude_enabled = false;
- control_mode->flag_control_climb_rate_enabled = false;
- control_mode->flag_control_manual_enabled = true;
- control_mode->flag_control_auto_enabled = false;
- break;
-
- case NAVIGATION_STATE_ALTHOLD:
- ret = TRANSITION_CHANGED;
- control_mode->flag_control_rates_enabled = true;
- control_mode->flag_control_attitude_enabled = true;
- control_mode->flag_control_velocity_enabled = false;
- control_mode->flag_control_position_enabled = false;
- control_mode->flag_control_altitude_enabled = true;
- control_mode->flag_control_climb_rate_enabled = true;
- control_mode->flag_control_manual_enabled = true;
- control_mode->flag_control_auto_enabled = false;
- break;
-
- case NAVIGATION_STATE_VECTOR:
- ret = TRANSITION_CHANGED;
- control_mode->flag_control_rates_enabled = true;
- control_mode->flag_control_attitude_enabled = true;
- control_mode->flag_control_velocity_enabled = true;
- control_mode->flag_control_position_enabled = true;
- control_mode->flag_control_altitude_enabled = true;
- control_mode->flag_control_climb_rate_enabled = true;
- control_mode->flag_control_manual_enabled = true;
- control_mode->flag_control_auto_enabled = false;
- break;
-
- case NAVIGATION_STATE_AUTO_READY:
- ret = TRANSITION_CHANGED;
- control_mode->flag_control_rates_enabled = false;
- control_mode->flag_control_attitude_enabled = false;
- control_mode->flag_control_velocity_enabled = false;
- control_mode->flag_control_position_enabled = false;
- control_mode->flag_control_altitude_enabled = false;
- control_mode->flag_control_climb_rate_enabled = false;
- control_mode->flag_control_manual_enabled = false;
- control_mode->flag_control_auto_enabled = true;
- break;
-
- case NAVIGATION_STATE_AUTO_TAKEOFF:
- ret = TRANSITION_CHANGED;
- control_mode->flag_control_rates_enabled = true;
- control_mode->flag_control_attitude_enabled = true;
- control_mode->flag_control_velocity_enabled = true;
- control_mode->flag_control_position_enabled = true;
- control_mode->flag_control_altitude_enabled = true;
- control_mode->flag_control_climb_rate_enabled = true;
- control_mode->flag_control_manual_enabled = false;
- control_mode->flag_control_auto_enabled = true;
- break;
-
- case NAVIGATION_STATE_AUTO_LOITER:
- ret = TRANSITION_CHANGED;
- control_mode->flag_control_rates_enabled = true;
- control_mode->flag_control_attitude_enabled = true;
- control_mode->flag_control_velocity_enabled = true;
- control_mode->flag_control_position_enabled = true;
- control_mode->flag_control_altitude_enabled = true;
- control_mode->flag_control_climb_rate_enabled = true;
- control_mode->flag_control_manual_enabled = false;
- control_mode->flag_control_auto_enabled = false;
- break;
-
- case NAVIGATION_STATE_AUTO_MISSION:
- ret = TRANSITION_CHANGED;
- control_mode->flag_control_rates_enabled = true;
- control_mode->flag_control_attitude_enabled = true;
- control_mode->flag_control_velocity_enabled = true;
- control_mode->flag_control_position_enabled = true;
- control_mode->flag_control_altitude_enabled = true;
- control_mode->flag_control_climb_rate_enabled = true;
- control_mode->flag_control_manual_enabled = false;
- control_mode->flag_control_auto_enabled = true;
- break;
-
- case NAVIGATION_STATE_AUTO_RTL:
- ret = TRANSITION_CHANGED;
- control_mode->flag_control_rates_enabled = true;
- control_mode->flag_control_attitude_enabled = true;
- control_mode->flag_control_velocity_enabled = true;
- control_mode->flag_control_position_enabled = true;
- control_mode->flag_control_altitude_enabled = true;
- control_mode->flag_control_climb_rate_enabled = true;
- control_mode->flag_control_manual_enabled = false;
- control_mode->flag_control_auto_enabled = true;
- break;
-
- case NAVIGATION_STATE_AUTO_LAND:
-
- /* deny transitions from landed state */
- if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
- ret = TRANSITION_CHANGED;
- control_mode->flag_control_rates_enabled = true;
- control_mode->flag_control_attitude_enabled = true;
- control_mode->flag_control_velocity_enabled = true;
- control_mode->flag_control_position_enabled = true;
- control_mode->flag_control_altitude_enabled = true;
- control_mode->flag_control_climb_rate_enabled = true;
- control_mode->flag_control_manual_enabled = false;
- control_mode->flag_control_auto_enabled = true;
- }
-
- break;
-
- default:
- break;
- }
-
- if (ret == TRANSITION_CHANGED) {
- status->navigation_state = new_navigation_state;
- control_mode->auto_state = status->navigation_state;
- navigation_state_changed = true;
- }
- }
-
- return ret;
-}
-
bool
-check_navigation_state_changed()
+check_flighttermination_state_changed()
{
- if (navigation_state_changed) {
- navigation_state_changed = false;
+ if (flighttermination_state_changed) {
+ flighttermination_state_changed = false;
return true;
} else {
@@ -450,16 +298,10 @@ check_navigation_state_changed()
}
}
-void
-set_navigation_state_changed()
-{
- navigation_state_changed = true;
-}
-
/**
* Transition from one hil state to another
*/
-int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd)
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
bool valid_transition = false;
int ret = ERROR;
@@ -488,7 +330,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
|| current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
- current_control_mode->flag_system_hil_enabled = true;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
}
@@ -507,9 +348,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
current_status->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
- current_control_mode->timestamp = hrt_absolute_time();
- orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
-
// XXX also set lockdown here
ret = OK;
@@ -522,6 +360,45 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
}
+/**
+* Transition from one flightermination state to another
+*/
+transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state)
+{
+ transition_result_t ret = TRANSITION_DENIED;
+
+ /* only check transition if the new state is actually different from the current one */
+ if (new_flighttermination_state == status->flighttermination_state) {
+ ret = TRANSITION_NOT_CHANGED;
+
+ } else {
+
+ switch (new_flighttermination_state) {
+ case FLIGHTTERMINATION_STATE_ON:
+ ret = TRANSITION_CHANGED;
+ status->flighttermination_state = FLIGHTTERMINATION_STATE_ON;
+ warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON");
+ break;
+ case FLIGHTTERMINATION_STATE_OFF:
+ ret = TRANSITION_CHANGED;
+ status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF;
+ break;
+
+ default:
+ break;
+ }
+
+ if (ret == TRANSITION_CHANGED) {
+ flighttermination_state_changed = true;
+ // TODO
+ //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON;
+ }
+ }
+
+ return ret;
+}
+
+
// /*
// * Wrapper functions (to be used in the commander), all functions assume lock on current_status
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 0bfdf36a8..e569fb4f3 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -48,7 +48,6 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
-#include <uORB/topics/vehicle_control_mode.h>
typedef enum {
TRANSITION_DENIED = -1,
@@ -58,7 +57,7 @@ typedef enum {
} transition_result_t;
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
- const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed);
+ arming_state_t new_arming_state, struct actuator_armed_s *armed);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
@@ -68,12 +67,14 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
bool check_main_state_changed();
-transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
+transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state);
bool check_navigation_state_changed();
+bool check_flighttermination_state_changed();
+
void set_navigation_state_changed();
-int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
#endif /* STATE_MACHINE_HELPER_H_ */
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-04 08:21:52 +0000