diff options
Diffstat (limited to 'src/modules/commander')
7 files changed, 334 insertions, 51 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 183621c57..616b855df 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -128,8 +128,6 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ -#define RC_TIMEOUT 500000 -#define DL_TIMEOUT (10 * 1000 * 1000) #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -555,10 +553,35 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (cmd->param1 > 0.5f) { //XXX update state machine? armed_local->force_failsafe = true; - warnx("forcing failsafe"); + warnx("forcing failsafe (termination)"); } else { armed_local->force_failsafe = false; - warnx("disabling failsafe"); + warnx("disabling failsafe (termination)"); + } + /* param2 is currently used for other failsafe modes */ + status_local->engine_failure_cmd = false; + status_local->data_link_lost_cmd = false; + status_local->gps_failure_cmd = false; + status_local->rc_signal_lost_cmd = false; + if ((int)cmd->param2 <= 0) { + /* reset all commanded failure modes */ + warnx("reset all non-flighttermination failsafe commands"); + } else if ((int)cmd->param2 == 1) { + /* trigger engine failure mode */ + status_local->engine_failure_cmd = true; + warnx("engine failure mode commanded"); + } else if ((int)cmd->param2 == 2) { + /* trigger data link loss mode */ + status_local->data_link_lost_cmd = true; + warnx("data link loss mode commanded"); + } else if ((int)cmd->param2 == 3) { + /* trigger gps loss mode */ + status_local->gps_failure_cmd = true; + warnx("gps loss mode commanded"); + } else if ((int)cmd->param2 == 4) { + /* trigger rc loss mode */ + status_local->rc_signal_lost_cmd = true; + warnx("rc loss mode commanded"); } cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } @@ -637,6 +660,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case VEHICLE_CMD_PREFLIGHT_STORAGE: + case VEHICLE_CMD_CUSTOM_0: + case VEHICLE_CMD_CUSTOM_1: + case VEHICLE_CMD_CUSTOM_2: + case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: /* ignore commands that handled in low prio loop */ break; @@ -676,6 +704,12 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); + param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); + param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); + param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); + param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); + param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); + param_t _param_ef_time_thres = param_find("COM_EF_TIME"); /* welcome user */ warnx("starting"); @@ -763,6 +797,8 @@ int commander_thread_main(int argc, char *argv[]) // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; status.circuit_breaker_engaged_airspd_check = false; + status.circuit_breaker_engaged_enginefailure_check = false; + status.circuit_breaker_engaged_gpsfailure_check = false; /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -868,11 +904,13 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to telemetry status topics */ int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; + uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); telemetry_last_heartbeat[i] = 0; + telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; } @@ -958,6 +996,15 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t arming_ret; int32_t datalink_loss_enabled = false; + int32_t datalink_loss_timeout = 10; + float rc_loss_timeout = 0.5; + int32_t datalink_regain_timeout = 0; + + /* Thresholds for engine failure detection */ + int32_t ef_throttle_thres = 1.0f; + int32_t ef_current2throttle_thres = 0.0f; + int32_t ef_time_thres = 1000.0f; + uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */ /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1005,8 +1052,14 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); - status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); - status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); + status.circuit_breaker_engaged_power_check = + circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.circuit_breaker_engaged_airspd_check = + circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); + status.circuit_breaker_engaged_enginefailure_check = + circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY); + status.circuit_breaker_engaged_gpsfailure_check = + circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY); status_changed = true; @@ -1018,6 +1071,12 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); + param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); + param_get(_param_rc_loss_timeout, &rc_loss_timeout); + param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); + param_get(_param_ef_throttle_thres, &ef_throttle_thres); + param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); + param_get(_param_ef_time_thres, &ef_time_thres); } orb_check(sp_man_sub, &updated); @@ -1058,7 +1117,7 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd && telemetry_last_heartbeat[i] == 0 && telemetry.heartbeat_time > 0 && - hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) { + hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { (void)rc_calibration_check(mavlink_fd); } @@ -1071,6 +1130,25 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + /* Check if the barometer is healthy and issue a warning in the GCS if not so. + * Because the barometer is used for calculating AMSL altitude which is used to ensure + * vertical separation from other airtraffic the operator has to know when the + * barometer is inoperational. + * */ + if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where baro was regained */ + if (status.barometer_failure) { + status.barometer_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "baro healthy"); + } + } else { + if (!status.barometer_failure) { + status.barometer_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "baro failed"); + } + } } orb_check(diff_pres_sub, &updated); @@ -1157,8 +1235,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); - /* check if GPS fix is ok */ - /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { @@ -1370,15 +1446,51 @@ int commander_thread_main(int argc, char *argv[]) globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } + /* check if GPS fix is ok */ + if (status.circuit_breaker_engaged_gpsfailure_check || + (gps_position.fix_type >= 3 && + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { + /* handle the case where gps was regained */ + if (status.gps_failure) { + status.gps_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps regained"); + } + } else { + if (!status.gps_failure) { + status.gps_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps fix lost"); + } + } + /* start mission result check */ orb_check(mission_result_sub, &updated); if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + + /* Check for geofence violation */ + if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) { + //XXX: make this configurable to select different actions (e.g. navigation modes) + /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ + armed.force_failsafe = true; + status_changed = true; + static bool flight_termination_printed = false; + if (!flight_termination_printed) { + warnx("Flight termination because of navigator request or geofence"); + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + flight_termination_printed = true; + } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + } + } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } /* RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && + hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1489,15 +1601,25 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { - /* handle the case where data link was regained */ - if (telemetry_lost[i]) { + if (telemetry_last_heartbeat[i] != 0 && + hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { + /* handle the case where data link was regained, + * accept datalink as healthy only after datalink_regain_timeout seconds + * */ + if (telemetry_lost[i] && + hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { + mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; + have_link = true; + } else if (!telemetry_lost[i]) { + /* telemetry was healthy also in last iteration + * we don't have to check a timeout */ + have_link = true; } - have_link = true; } else { + telemetry_last_dl_loss[i] = hrt_absolute_time(); if (!telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; @@ -1516,10 +1638,40 @@ int commander_thread_main(int argc, char *argv[]) if (!status.data_link_lost) { mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; + status.data_link_lost_counter++; status_changed = true; } } + /* Check engine failure + * only for fixed wing for now + */ + if (!status.circuit_breaker_engaged_enginefailure_check && + status.is_rotary_wing == false && + armed.armed && + ((actuator_controls.control[3] > ef_throttle_thres && + battery.current_a/actuator_controls.control[3] < + ef_current2throttle_thres) || + (status.engine_failure))) { + /* potential failure, measure time */ + if (timestamp_engine_healthy > 0 && + hrt_elapsed_time(×tamp_engine_healthy) > + ef_time_thres * 1e6 && + !status.engine_failure) { + status.engine_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "Engine Failure"); + } + } else { + /* no failure reset flag */ + timestamp_engine_healthy = hrt_absolute_time(); + if (status.engine_failure) { + status.engine_failure = false; + status_changed = true; + } + } + + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -1533,6 +1685,53 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Check for failure combinations which lead to flight termination */ + if (armed.armed) { + /* At this point the data link and the gps system have been checked + * If we are not in a manual (RC stick controlled mode) + * and both failed we want to terminate the flight */ + if (status.main_state != MAIN_STATE_MANUAL && + status.main_state != MAIN_STATE_ACRO && + status.main_state != MAIN_STATE_ALTCTL && + status.main_state != MAIN_STATE_POSCTL && + ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd))) { + armed.force_failsafe = true; + status_changed = true; + static bool flight_termination_printed = false; + if (!flight_termination_printed) { + warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + flight_termination_printed = true; + } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + } + } + + /* At this point the rc signal and the gps system have been checked + * If we are in manual (controlled with RC): + * if both failed we want to terminate the flight */ + if ((status.main_state == MAIN_STATE_ACRO || + status.main_state == MAIN_STATE_MANUAL || + status.main_state == MAIN_STATE_ALTCTL || + status.main_state == MAIN_STATE_POSCTL) && + ((status.rc_signal_lost && status.gps_failure) || + (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { + armed.force_failsafe = true; + status_changed = true; + static bool flight_termination_printed = false; + if (!flight_termination_printed) { + warnx("Flight termination because of RC signal loss && gps failure"); + flight_termination_printed = true; + } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } + } + } + + hrt_abstime t1 = hrt_absolute_time(); /* print new state */ @@ -1574,7 +1773,8 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, - mission_result.finished); + mission_result.finished, + mission_result.stay_in_failsafe); // TODO handle mode changes by commands if (main_state_changed) { @@ -2024,6 +2224,7 @@ set_control_mode() case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTGS: + case NAVIGATION_STATE_AUTO_LANDENGFAIL: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -2035,6 +2236,18 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_LAND: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index dba68700b..1b0c4258b 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -105,3 +105,69 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * @max 1 */ PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); + + /** Datalink loss time threshold + * + * After this amount of seconds without datalink the data link lost mode triggers + * + * @group commander + * @unit second + * @min 0 + * @max 30 + */ +PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); + +/** Datalink regain time threshold + * + * After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss' + * flag is set back to false + * + * @group commander + * @unit second + * @min 0 + * @max 30 + */ +PARAM_DEFINE_INT32(COM_DL_REG_T, 0); + +/** Engine Failure Throttle Threshold + * + * Engine failure triggers only above this throttle value + * + * @group commander + * @min 0.0f + * @max 1.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); + +/** Engine Failure Current/Throttle Threshold + * + * Engine failure triggers only below this current/throttle value + * + * @group commander + * @min 0.0f + * @max 7.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); + +/** Engine Failure Time Threshold + * + * Engine failure triggers only if the throttle threshold and the + * current to throttle threshold are violated for this time + * + * @group commander + * @unit second + * @min 0.0f + * @max 7.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); + +/** RC loss time threshold + * + * After this amount of seconds without RC connection the rc lost flag is set to true + * + * @group commander + * @unit second + * @min 0 + * @max 35 + */ +PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp index 0abb84a82..2bfa5d0bb 100644 --- a/src/modules/commander/commander_tests/commander_tests.cpp +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -48,7 +48,5 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); int commander_tests_main(int argc, char *argv[]) { - stateMachineHelperTest(); - - return 0; + return stateMachineHelperTest() ? 0 : -1; } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 08dda2fab..874090e93 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -49,7 +49,7 @@ public: StateMachineHelperTest(); virtual ~StateMachineHelperTest(); - virtual void runTests(void); + virtual bool run_tests(void); private: bool armingStateTransitionTest(); @@ -488,16 +488,13 @@ bool StateMachineHelperTest::isSafeTest(void) return true; } -void StateMachineHelperTest::runTests(void) +bool StateMachineHelperTest::run_tests(void) { ut_run_test(armingStateTransitionTest); ut_run_test(mainStateTransitionTest); ut_run_test(isSafeTest); + + return (_tests_failed == 0); } -void stateMachineHelperTest(void) -{ - StateMachineHelperTest* test = new StateMachineHelperTest(); - test->runTests(); - test->printResults(); -} +ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
\ No newline at end of file diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h index bbf66255e..cf6719095 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.h +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -39,6 +39,6 @@ #ifndef STATE_MACHINE_HELPER_TEST_H_ #define STATE_MACHINE_HELPER_TEST_ -void stateMachineHelperTest(void); +bool stateMachineHelperTest(void); #endif /* STATE_MACHINE_HELPER_TEST_H_ */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 684c61a17..9568752ae 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -443,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; @@ -457,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) { @@ -497,14 +498,29 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en case MAIN_STATE_AUTO_MISSION: /* go into failsafe + * - if commanded to do so + * - if we have an engine failure * - 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) || + 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_RTGS; //XXX + /* 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; + } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && 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_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) { @@ -528,31 +544,20 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } /* don't bother if RC is lost and mission is not yet finished */ - } else if (status->rc_signal_lost) { + } else if (status->rc_signal_lost && !stay_in_failsafe) { /* this mode is ok, we don't need RC for missions */ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; - } else { + } else if (!stay_in_failsafe){ /* everything is perfect */ 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; @@ -593,8 +598,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) { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 69ce8bbce..61d0f29d0 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -63,7 +63,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -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); int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); |