aboutsummaryrefslogtreecommitdiff
path: root/src/modules/commander/state_machine_helper.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/commander/state_machine_helper.cpp')
-rw-r--r--src/modules/commander/state_machine_helper.cpp118
1 files changed, 70 insertions, 48 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 7b26e3e8c..465f9cdc5 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = {
};
transition_result_t
-arming_state_transition(struct vehicle_status_s *status, /// current vehicle status
- const struct safety_s *safety, /// current safety settings
- arming_state_t new_arming_state, /// arming state requested
- struct actuator_armed_s *armed, /// current armed status
- const int mavlink_fd) /// mavlink fd for error reporting, 0 for none
+arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status
+ const struct safety_s *safety, ///< current safety settings
+ arming_state_t new_arming_state, ///< arming state requested
+ struct actuator_armed_s *armed, ///< current armed status
+ bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
+ const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none
{
// Double check that our static arrays are still valid
ASSERT(ARMING_STATE_INIT == 0);
@@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
int prearm_ret = OK;
/* only perform the check if we have to */
- if (new_arming_state == ARMING_STATE_ARMED) {
+ if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
prearm_ret = prearm_check(status, mavlink_fd);
}
@@ -181,12 +182,19 @@ arming_state_transition(struct vehicle_status_s *status, /// current
// Fail transition if power levels on the avionics rail
// are measured but are insufficient
- if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) &&
- (status->avionics_power_rail_voltage < 4.9f))) {
-
- mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
- feedback_provided = true;
- valid_transition = false;
+ if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
+ // Check avionics rail voltages
+ if (status->avionics_power_rail_voltage < 4.75f) {
+ mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+ feedback_provided = true;
+ valid_transition = false;
+ } else if (status->avionics_power_rail_voltage < 4.9f) {
+ mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+ feedback_provided = true;
+ } else if (status->avionics_power_rail_voltage > 5.4f) {
+ mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
+ feedback_provided = true;
+ }
}
}
@@ -435,7 +443,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
/**
* Check failsafe and main status and set navigation status for navigator accordingly
*/
-bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
+ const bool stay_in_failsafe)
{
navigation_state_t nav_state_old = status->nav_state;
@@ -449,11 +458,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
case MAIN_STATE_ALTCTL:
case MAIN_STATE_POSCTL:
/* require RC for all manual modes */
- if (status->rc_signal_lost && armed) {
+ if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
- status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+ status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
@@ -488,15 +497,35 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
break;
case MAIN_STATE_AUTO_MISSION:
+
/* go into failsafe
- * - if either the datalink is enabled and lost as well as RC is lost
- * - if there is no datalink and the mission is finished */
- if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
- (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
+ * - if commanded to do so
+ * - if we have an engine failure
+ * - depending on datalink, RC and if the mission is finished */
+
+ /* first look at the commands */
+ if (status->engine_failure_cmd) {
+ status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+ } else if (status->data_link_lost_cmd) {
+ status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+ } else if (status->gps_failure_cmd) {
+ status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+ } else if (status->rc_signal_lost_cmd) {
+ status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+
+ /* finished handling commands which have priority, now handle failures */
+ } else if (status->gps_failure) {
+ status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
+ } else if (status->engine_failure) {
+ status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+
+ /* datalink loss enabled:
+ * check for datalink lost: this should always trigger RTGS */
+ } else if (data_link_loss_enabled && status->data_link_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
- status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+ status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
@@ -505,12 +534,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
- /* also go into failsafe if just datalink is lost */
- } else if (status->data_link_lost && data_link_loss_enabled) {
+ /* datalink loss disabled:
+ * check if both, RC and datalink are lost during the mission
+ * or RC is lost after the mission is finished: this should always trigger RCRECOVER */
+ } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
+ (status->rc_signal_lost && mission_finished))) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
- status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+ status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
@@ -519,32 +551,16 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
- /* don't bother if RC is lost and mission is not yet finished */
- } else if (status->rc_signal_lost) {
-
- /* this mode is ok, we don't need RC for missions */
- status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
- } else {
- /* everything is perfect */
+ /* stay where you are if you should stay in failsafe, otherwise everything is perfect */
+ } else if (!stay_in_failsafe){
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
}
break;
case MAIN_STATE_AUTO_LOITER:
- /* go into failsafe if datalink and RC is lost */
- if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
- status->failsafe = true;
-
- if (status->condition_global_position_valid && status->condition_home_position_valid) {
- status->nav_state = NAVIGATION_STATE_AUTO_RTL;
- } else if (status->condition_local_position_valid) {
- status->nav_state = NAVIGATION_STATE_LAND;
- } else if (status->condition_local_altitude_valid) {
- status->nav_state = NAVIGATION_STATE_DESCEND;
- } else {
- status->nav_state = NAVIGATION_STATE_TERMINATION;
- }
-
+ /* go into failsafe on a engine failure */
+ if (status->engine_failure) {
+ status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
@@ -585,8 +601,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
break;
case MAIN_STATE_AUTO_RTL:
- /* require global position and home */
- if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
+ /* require global position and home, also go into failsafe on an engine failure */
+
+ if (status->engine_failure) {
+ status->nav_state = 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) {
@@ -669,7 +689,9 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
goto system_eval;
}
- if (!status->is_rotary_wing) {
+ /* Perform airspeed check only if circuit breaker is not
+ * engaged and it's not a rotary wing */
+ if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
/* accel done, close it */
close(fd);
fd = orb_subscribe(ORB_ID(airspeed));
@@ -684,7 +706,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
}
if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
- mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING");
+ mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
// XXX do not make this fatal yet
}
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-19 01:46:48 +0000