aboutsummaryrefslogtreecommitdiff
path: root/apps/commander
diff options
context:
space:
mode:
Diffstat (limited to 'apps/commander')
-rw-r--r--apps/commander/commander.c131
-rw-r--r--apps/commander/state_machine_helper.c614
-rw-r--r--apps/commander/state_machine_helper.h4
3 files changed, 323 insertions, 426 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 6b026f287..8b9e7c49c 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1785,103 +1785,90 @@ int commander_thread_main(int argc, char *argv[])
if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
- /*
- * Check if manual control modes have to be switched
- */
- if (!isfinite(sp_man.mode_switch)) {
- warnx("mode sw not finite");
-
- /* this switch is not properly mapped, set attitude stabilized */
- if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
- (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
- (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
- current_status.flag_control_attitude_enabled = true;
- current_status.flag_control_rates_enabled = true;
- } else {
-
- /* assuming a fixed wing, fall back to direct pass-through */
- current_status.flag_control_attitude_enabled = false;
- current_status.flag_control_rates_enabled = false;
- }
-
- } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) {
-
- /* this switch is not properly mapped, set attitude stabilized */
- if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
- (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
- (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
- /* assuming a rotary wing, fall back to m */
- current_status.flag_control_attitude_enabled = true;
- current_status.flag_control_rates_enabled = true;
- } else {
-
- /* assuming a fixed wing, set to direct pass-through as requested */
- current_status.flag_control_attitude_enabled = false;
- current_status.flag_control_rates_enabled = false;
- }
-
- } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) {
-
- /* top stick position, set auto/mission for all vehicle types */
- current_status.flag_control_position_enabled = true;
- current_status.flag_control_velocity_enabled = true;
- current_status.flag_control_attitude_enabled = true;
- current_status.flag_control_rates_enabled = true;
-
- } else {
- /* center stick position, set seatbelt/simple control */
- current_status.flag_control_velocity_enabled = true;
- current_status.flag_control_attitude_enabled = true;
- current_status.flag_control_rates_enabled = true;
- }
+ /*
+ * Check if manual control modes have to be switched
+ */
+ if (!isfinite(sp_man.mode_switch)) {
+ warnx("mode sw not finite");
+
+ /* no valid stick position, go to default */
+
+
+ } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) {
+
+ /* bottom stick position, go to manual mode */
+ current_status.mode_switch = MODE_SWITCH_MANUAL;
+
+ } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) {
+
+ /* top stick position, set auto/mission for all vehicle types */
+ current_status.mode_switch = MODE_SWITCH_AUTO;
+
+ } else {
+ /* center stick position, set seatbelt/simple control */
+ current_status.mode_switch = MODE_SWITCH_SEATBELT;
+ }
// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
/*
- * Check if land/RTL is requested
- */
+ * Check if land/RTL is requested
+ */
if (!isfinite(sp_man.return_switch)) {
/* this switch is not properly mapped, set default */
- current_status.flag_land_requested = false; // XXX default?
+ current_status.return_switch = RETURN_SWITCH_NONE;
} else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) {
/* bottom stick position, set altitude hold */
- current_status.flag_land_requested = false;
+ current_status.return_switch = RETURN_SWITCH_NONE;
} else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) {
/* top stick position */
- current_status.flag_land_requested = true;
+ current_status.return_switch = RETURN_SWITCH_RETURN;
} else {
/* center stick position, set default */
- current_status.flag_land_requested = false; // XXX default?
+ current_status.return_switch = RETURN_SWITCH_NONE;
}
/* check mission switch */
if (!isfinite(sp_man.mission_switch)) {
/* this switch is not properly mapped, set default */
- current_status.flag_mission_activated = false;
+ current_status.mission_switch = MISSION_SWITCH_NONE;
} else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) {
/* top switch position */
- current_status.flag_mission_activated = true;
+ current_status.mission_switch = MISSION_SWITCH_MISSION;
} else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) {
/* bottom switch position */
- current_status.flag_mission_activated = false;
+ current_status.mission_switch = MISSION_SWITCH_NONE;
} else {
/* center switch position, set default */
- current_status.flag_mission_activated = false; // XXX default?
+ current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default?
+ }
+
+ /* Now it's time to handle the stick inputs */
+
+ if (current_status.arming_state == ARMING_STATE_ARMED) {
+
+ if (current_status.mode_switch == MODE_SWITCH_MANUAL) {
+ do_navigation_state_update(stat_pub, &current_status, mavlink_fd, NAVIGATION_STATE_MANUAL );
+ } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) {
+ do_navigation_state_update(stat_pub, &current_status, mavlink_fd, NAVIGATION_STATE_SEATBELT );
+ } else if (current_status.mode_switch == MODE_SWITCH_AUTO) {
+ if (current_status.navigation_state == NAVIGATION_STATE_MANUAL) {
+ do_navigation_state_update(stat_pub, &current_status, mavlink_fd, NAVIGATION_STATE_MISSION );
+ }
+ }
}
/* handle the case where RC signal was regained */
@@ -1890,17 +1877,19 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME.");
} else {
- if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
+ if (current_status.rc_signal_lost) {
+ mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
+ }
}
/*
- * Check if left stick is in lower left position --> switch to standby state.
- * Do this only for multirotors, not for fixed wing aircraft.
- */
-// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
-// ) &&
+ * Check if left stick is in lower left position --> switch to standby state.
+ * Do this only for multirotors, not for fixed wing aircraft.
+ */
+ // if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
+ // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
+ // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
+ // ) &&
if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
do_arming_state_update(stat_pub, &current_status, mavlink_fd, ARMING_STATE_STANDBY);
@@ -1969,8 +1958,8 @@ int commander_thread_main(int argc, char *argv[])
/* decide about attitude control flag, enable in att/pos/vel */
bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
- sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
- sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
+ sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
+ sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
/* decide about rate control flag, enable it always XXX (for now) */
bool rates_ctrl_enabled = true;
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index aae119d35..2e4935471 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -54,422 +54,330 @@
/**
- * Transition from one navigation state to another
+ * Transition from one sytem state to another
*/
-int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state)
+void state_machine_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
- bool valid_path = false;
- bool valid_transition = false;
int ret = ERROR;
+ system_state_t new_system_state;
- if (current_status->navigation_state == new_state) {
- warnx("Navigation state not changed");
- } else {
-
- switch (new_state) {
- case NAVIGATION_STATE_STANDBY:
-
- if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
- || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
-
- if (!current_status->flag_system_armed) {
- mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
- valid_transition = true;
- } else {
- mavlink_log_critical(mavlink_fd, "Rejected STANDBY state: armed");
- }
- valid_path = true;
- }
- break;
-
- case NAVIGATION_STATE_MANUAL:
-
- if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) {
-
- /* only check for armed flag when coming from STANDBY XXX does that make sense? */
- if (current_status->flag_system_armed) {
- mavlink_log_critical(mavlink_fd, "Switched to MANUAL state");
- valid_transition = true;
- } else {
- mavlink_log_critical(mavlink_fd, "Rejected MANUAL state: disarmed");
- }
- valid_path = true;
- } else if (current_status->navigation_state == NAVIGATION_STATE_SEATBELT
- || current_status->navigation_state == NAVIGATION_STATE_LOITER
- || current_status->navigation_state == NAVIGATION_STATE_MISSION
- || current_status->navigation_state == NAVIGATION_STATE_RTL
- || current_status->navigation_state == NAVIGATION_STATE_LAND
- || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
- || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
- mavlink_log_critical(mavlink_fd, "Switched to MANUAL state");
- valid_transition = true;
- valid_path = true;
- }
- break;
-
- case NAVIGATION_STATE_SEATBELT:
-
- if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
- || current_status->navigation_state == NAVIGATION_STATE_LOITER
- || current_status->navigation_state == NAVIGATION_STATE_MISSION) {
-
- mavlink_log_critical(mavlink_fd, "Switched to SEATBELT state");
- valid_transition = true;
- valid_path = true;
- }
- break;
- case NAVIGATION_STATE_DESCENT:
-
- if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
- || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
-
- mavlink_log_critical(mavlink_fd, "Switched to DESCENT state");
- valid_transition = true;
- valid_path = true;
- }
- break;
-
- case NAVIGATION_STATE_LOITER:
-
- /* Check for position lock when coming from MANUAL or SEATBELT */
- if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
- mavlink_log_critical(mavlink_fd, "Switched to LOITER state");
- valid_transition = true;
- if (current_status->flag_global_position_valid) {
- mavlink_log_critical(mavlink_fd, "Switched to LOITER state");
- valid_transition = true;
-
- } else {
- mavlink_log_critical(mavlink_fd, "Rejected LOITER state: no pos lock");
- }
- valid_path = true;
- } else if (current_status->navigation_state == NAVIGATION_STATE_MISSION) {
- mavlink_log_critical(mavlink_fd, "Switched to LOITER state");
- valid_transition = true;
- valid_path = true;
- }
- break;
-
- case NAVIGATION_STATE_AUTO_READY:
-
- /* coming from STANDBY pos and home lock are needed */
- if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) {
-
- if (!current_status->flag_system_armed) {
- mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: disarmed");
+ /*
+ * Firstly evaluate mode switch position
+ * */
- } else if (!current_status->flag_global_position_valid) {
- mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock");
+ /* Always accept manual mode */
+ if (current_status->mode_switch == MODE_SWITCH_MANUAL) {
+ new_system_state = SYSTEM_STATE_MANUAL;
- } else if (!current_status->flag_valid_launch_position) {
- mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock");
+ /* Accept SEATBELT if there is a local position estimate */
+ } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) {
- } else {
- mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state");
- valid_transition = true;
- }
- valid_path = true;
- /* coming from LAND home lock is needed */
- } else if (current_status->navigation_state == NAVIGATION_STATE_LAND) {
-
- if (!current_status->flag_valid_launch_position) {
- mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock");
- } else {
- mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state");
- valid_transition = true;
- }
- valid_path = true;
- }
- break;
-
- case NAVIGATION_STATE_MISSION:
-
- /* coming from TAKEOFF or RTL is always possible */
- if (current_status->navigation_state == NAVIGATION_STATE_TAKEOFF
- || current_status->navigation_state == NAVIGATION_STATE_RTL) {
- mavlink_log_critical(mavlink_fd, "Switched to MISSION state");
- valid_transition = true;
- valid_path = true;
-
- /* coming from MANUAL or SEATBELT requires home and pos lock */
- } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
- || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
-
- if (!current_status->flag_global_position_valid) {
- mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock");
-
- } else if (!current_status->flag_valid_launch_position) {
- mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock");
- mavlink_log_critical(mavlink_fd, "Switched to MISSION state");
- valid_transition = true;
- } else {
-
- }
- valid_path = true;
-
- /* coming from loiter a home lock is needed */
- } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) {
- if (current_status->flag_valid_launch_position) {
- mavlink_log_critical(mavlink_fd, "Switched to MISSION state");
- valid_transition = true;
- } else {
- mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock");
- }
- valid_path = true;
- }
- break;
-
- case NAVIGATION_STATE_RTL:
-
- /* coming from MISSION is always possible */
- if (current_status->navigation_state == NAVIGATION_STATE_MISSION) {
- mavlink_log_critical(mavlink_fd, "Switched to RTL state");
- valid_transition = true;
- valid_path = true;
-
- /* coming from MANUAL or SEATBELT requires home and pos lock */
- } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL
- || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) {
-
- if (!current_status->flag_global_position_valid) {
- mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock");
- } else if (!current_status->flag_valid_launch_position) {
- mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock");
- } else {
- mavlink_log_critical(mavlink_fd, "Switched to RTL state");
- valid_transition = true;
- }
- valid_path = true;
-
- /* coming from loiter a home lock is needed */
- } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) {
- if (current_status->flag_valid_launch_position) {
- mavlink_log_critical(mavlink_fd, "Switched to RTL state");
- valid_transition = true;
- } else {
- mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock");
- }
- valid_path = true;
- }
- break;
+ if (current_status->flag_local_position_valid) {
+ new_system_state = SYSTEM_STATE_SEATBELT;
+ } else {
+ /* or just fall back to manual */
+ mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position");
+ new_system_state = SYSTEM_STATE_MANUAL);
+ }
- case NAVIGATION_STATE_TAKEOFF:
+ /* Accept AUTO if there is a global position estimate */
+ } else if (current_status->mode_switch == MODE_SWITCH_AUTO) {
+ if (current_status->flag_local_position_valid) {
+ new_system_state = SYSTEM_STATE_SEATBELT);
+ } else {
+ mavlink_log_critical(mavlink_fd, "REJ AUTO: no global position");
+
+ /* otherwise fallback to seatbelt or even manual */
+ if (current_status->flag_local_position_valid) {
+ new_system_state = SYSTEM_STATE_SEATBELT;
+ } else {
+ mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position");
+ new_system_state = SYSTEM_STATE_MANUAL;
+ }
+ }
+ }
- if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
+ /*
+ * Next up, check for
+ */
- /* TAKEOFF is straight forward from AUTO READY */
- mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state");
- valid_transition = true;
- valid_path = true;
- }
- break;
- case NAVIGATION_STATE_LAND:
- if (current_status->navigation_state == NAVIGATION_STATE_RTL
- || current_status->navigation_state == NAVIGATION_STATE_LOITER
- || current_status->navigation_state == NAVIGATION_STATE_MISSION) {
- mavlink_log_critical(mavlink_fd, "Switched to LAND state");
- valid_transition = true;
- valid_path = true;
- }
- break;
+ /* Update the system state in case it has changed */
+ if (current_status->system_state != new_system_state) {
- default:
- warnx("Unknown navigation state");
- break;
+ /* Check if the transition is valid */
+ if (system_state_update(current_status->system_state, new_system_state) == OK) {
+ current_status->system_state = new_system_state;
+ state_machine_publish(status_pub, current_status, mavlink_fd);
+ } else {
+ mavlink_log_critical(mavlink_fd, "System state transition not valid");
}
}
- if (valid_transition) {
- current_status->navigation_state = new_state;
- state_machine_publish(status_pub, current_status, mavlink_fd);
-// publish_armed_status(current_status);
- ret = OK;
- }
-
- if (!valid_path){
- mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition");
- }
-
- return ret;
+ return;
}
-/**
- * Transition from one arming state to another
+/*
+ * This functions does not evaluate any input flags but only checks if the transitions
+ * are valid.
*/
-int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state)
-{
- bool valid_transition = false;
- int ret = ERROR;
-
- warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state);
+int system_state_update(system_state_t current_system_state, system_state_t new_system_state) {
- if (current_status->arming_state == new_state) {
- warnx("Arming state not changed");
- valid_transition = true;
-
- } else {
+ int ret = ERROR;
- switch (new_state) {
+ /* only check transition if the new state is actually different from the current one */
+ if (new_system_state != current_system_state) {
- case ARMING_STATE_INIT:
+ switch (new_system_state) {
+ case SYSTEM_STATE_INIT:
- if (current_status->arming_state == ARMING_STATE_STANDBY) {
+ /* transitions back to INIT are possible for calibration */
+ if (current_system_state == SYSTEM_STATE_MANUAL
+ || current_system_state == SYSTEM_STATE_SEATBELT
+ || current_system_state == SYSTEM_STATE_AUTO) {
- mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE");
- valid_transition = true;
+ ret = OK;
}
- break;
- case ARMING_STATE_STANDBY:
-
- // TODO check for sensors
- // XXX check if coming from reboot?
- if (current_status->arming_state == ARMING_STATE_INIT) {
+ break;
- if (current_status->flag_system_sensors_initialized) {
- current_status->flag_system_armed = false;
- mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state");
- valid_transition = true;
- } else {
- mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init.");
- }
+ case SYSTEM_STATE_MANUAL:
- } else if (current_status->arming_state == ARMING_STATE_ARMED) {
+ /* transitions from INIT or from other modes */
+ if (current_system_state == SYSTEM_STATE_INIT
+ || current_system_state == SYSTEM_STATE_SEATBELT
+ || current_system_state == SYSTEM_STATE_AUTO) {
- current_status->flag_system_armed = false;
- mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state");
- valid_transition = true;
+ ret = OK;
}
- break;
- case ARMING_STATE_ARMED:
-
- if (current_status->arming_state == ARMING_STATE_STANDBY
- || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) {
- current_status->flag_system_armed = true;
- mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state");
- valid_transition = true;
- }
break;
- case ARMING_STATE_ABORT:
+ case SYSTEM_STATE_SEATBELT:
- if (current_status->arming_state == ARMING_STATE_ARMED) {
+ /* transitions from INIT or from other modes */
+ if (current_system_state == SYSTEM_STATE_INIT
+ || current_system_state == SYSTEM_STATE_MANUAL
+ || current_system_state == SYSTEM_STATE_AUTO) {
- mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state");
- valid_transition = true;
+ ret = OK;
}
- break;
-
- case ARMING_STATE_ERROR:
-
- if (current_status->arming_state == ARMING_STATE_ARMED
- || current_status->arming_state == ARMING_STATE_ABORT
- || current_status->arming_state == ARMING_STATE_INIT) {
- mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state");
- valid_transition = true;
- }
break;
- case ARMING_STATE_REBOOT:
+ case SYSTEM_STATE_AUTO:
- if (current_status->arming_state == ARMING_STATE_STANDBY
- || current_status->arming_state == ARMING_STATE_ERROR) {
+ /* transitions from INIT or from other modes */
+ if (current_system_state == SYSTEM_STATE_INIT
+ || current_system_state == SYSTEM_STATE_MANUAL
+ || current_system_state == SYSTEM_STATE_SEATBELT) {
- valid_transition = true;
- mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
- usleep(500000);
- up_systemreset();
- /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
+ ret = OK;
}
break;
- case ARMING_STATE_IN_AIR_RESTORE:
+ case SYSTEM_STATE_REBOOT:
- if (current_status->arming_state == ARMING_STATE_INIT) {
- mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state");
- valid_transition = true;
- }
- break;
- default:
- warnx("Unknown arming state");
- break;
- }
- }
+ /* from INIT you can go straight to REBOOT */
+ if (current_system_state == SYSTEM_STATE_INIT) {
- if (valid_transition) {
- current_status->arming_state = new_state;
- state_machine_publish(status_pub, current_status, mavlink_fd);
-// publish_armed_status(current_status);
- ret = OK;
- } else {
- mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition");
- }
-
- return ret;
-}
-
-/**
- * Transition from one hil state to another
- */
-int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state)
-{
- bool valid_transition = false;
- int ret = ERROR;
-
- warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
-
- if (current_status->hil_state == new_state) {
- warnx("Hil state not changed");
- valid_transition = true;
-
- } else {
-
- switch (new_state) {
-
- case HIL_STATE_OFF:
-
- if (current_status->arming_state == ARMING_STATE_INIT
- || current_status->arming_state == ARMING_STATE_STANDBY) {
-
- current_status->flag_hil_enabled = false;
- mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
- valid_transition = true;
+ ret = OK;
}
break;
- case HIL_STATE_ON:
+ case SYSTEM_STATE_IN_AIR_RESTORE:
- if (current_status->arming_state == ARMING_STATE_INIT
- || current_status->arming_state == ARMING_STATE_STANDBY) {
+ /* from INIT you can go straight to an IN AIR RESTORE */
+ if (current_system_state == SYSTEM_STATE_INIT) {
- current_status->flag_hil_enabled = true;
- mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
- valid_transition = true;
+ ret = OK;
}
break;
default:
- warnx("Unknown hil state");
break;
}
}
- if (valid_transition) {
- current_status->hil_state = new_state;
- state_machine_publish(status_pub, current_status, mavlink_fd);
- ret = OK;
- } else {
- mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
- }
-
return ret;
}
+///**
+// * Transition from one arming state to another
+// */
+//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state)
+//{
+// bool valid_transition = false;
+// int ret = ERROR;
+//
+// warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state);
+//
+// if (current_status->arming_state == new_state) {
+// warnx("Arming state not changed");
+// valid_transition = true;
+//
+// } else {
+//
+// switch (new_state) {
+//
+// case ARMING_STATE_INIT:
+//
+// if (current_status->arming_state == ARMING_STATE_STANDBY) {
+//
+// mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE");
+// valid_transition = true;
+// }
+// break;
+//
+// case ARMING_STATE_STANDBY:
+//
+// // TODO check for sensors
+// // XXX check if coming from reboot?
+// if (current_status->arming_state == ARMING_STATE_INIT) {
+//
+// if (current_status->flag_system_sensors_initialized) {
+// current_status->flag_system_armed = false;
+// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state");
+// valid_transition = true;
+// } else {
+// mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init.");
+// }
+//
+// } else if (current_status->arming_state == ARMING_STATE_ARMED) {
+//
+// current_status->flag_system_armed = false;
+// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state");
+// valid_transition = true;
+// }
+// break;
+//
+// case ARMING_STATE_ARMED:
+//
+// if (current_status->arming_state == ARMING_STATE_STANDBY
+// || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) {
+// current_status->flag_system_armed = true;
+// mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state");
+// valid_transition = true;
+// }
+// break;
+//
+// case ARMING_STATE_ABORT:
+//
+// if (current_status->arming_state == ARMING_STATE_ARMED) {
+//
+// mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state");
+// valid_transition = true;
+// }
+// break;
+//
+// case ARMING_STATE_ERROR:
+//
+// if (current_status->arming_state == ARMING_STATE_ARMED
+// || current_status->arming_state == ARMING_STATE_ABORT
+// || current_status->arming_state == ARMING_STATE_INIT) {
+//
+// mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state");
+// valid_transition = true;
+// }
+// break;
+//
+// case ARMING_STATE_REBOOT:
+//
+// if (current_status->arming_state == ARMING_STATE_STANDBY
+// || current_status->arming_state == ARMING_STATE_ERROR) {
+//
+// valid_transition = true;
+// mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
+// usleep(500000);
+// up_systemreset();
+// /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
+// }
+// break;
+//
+// case ARMING_STATE_IN_AIR_RESTORE:
+//
+// if (current_status->arming_state == ARMING_STATE_INIT) {
+// mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state");
+// valid_transition = true;
+// }
+// break;
+// default:
+// warnx("Unknown arming state");
+// break;
+// }
+// }
+//
+// if (valid_transition) {
+// current_status->arming_state = new_state;
+// state_machine_publish(status_pub, current_status, mavlink_fd);
+//// publish_armed_status(current_status);
+// ret = OK;
+// } else {
+// mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition");
+// }
+//
+// return ret;
+//}
+
+///**
+// * Transition from one hil state to another
+// */
+//int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state)
+//{
+// bool valid_transition = false;
+// int ret = ERROR;
+//
+// warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
+//
+// if (current_status->hil_state == new_state) {
+// warnx("Hil state not changed");
+// valid_transition = true;
+//
+// } else {
+//
+// switch (new_state) {
+//
+// case HIL_STATE_OFF:
+//
+// if (current_status->arming_state == ARMING_STATE_INIT
+// || current_status->arming_state == ARMING_STATE_STANDBY) {
+//
+// current_status->flag_hil_enabled = false;
+// mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
+// valid_transition = true;
+// }
+// break;
+//
+// case HIL_STATE_ON:
+//
+// if (current_status->arming_state == ARMING_STATE_INIT
+// || current_status->arming_state == ARMING_STATE_STANDBY) {
+//
+// current_status->flag_hil_enabled = true;
+// mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+// valid_transition = true;
+// }
+// break;
+//
+// default:
+// warnx("Unknown hil state");
+// break;
+// }
+// }
+//
+// if (valid_transition) {
+// current_status->hil_state = new_state;
+// state_machine_publish(status_pub, current_status, mavlink_fd);
+// ret = OK;
+// } else {
+// mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
+// }
+//
+// return ret;
+//}
+
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
index bf9296caf..57b3db8f1 100644
--- a/apps/commander/state_machine_helper.h
+++ b/apps/commander/state_machine_helper.h
@@ -47,9 +47,9 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
-int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state);
+int navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state);
-int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);
+//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-08 02:07:27 +0000