diff options
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/commander.cpp | 165 | ||||
-rw-r--r-- | src/modules/commander/commander_params.c | 23 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 55 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.h | 2 |
4 files changed, 212 insertions, 33 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a5a772825..9885176b7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -129,7 +129,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 +554,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; } @@ -659,6 +683,8 @@ 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_datalink_regain_timeout = param_find("COM_DL_REG_T"); /* welcome user */ warnx("starting"); @@ -851,11 +877,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; } @@ -941,6 +969,8 @@ 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; + int32_t datalink_regain_timeout = 0; /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1001,6 +1031,8 @@ 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_datalink_regain_timeout, &datalink_regain_timeout); } orb_check(sp_man_sub, &updated); @@ -1041,7 +1073,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); } @@ -1054,6 +1086,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); @@ -1141,6 +1192,22 @@ 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 */ + if (gps_position.fix_type >= 3 && //XXX check eph and epv ? + 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"); + } + } + /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && @@ -1358,6 +1425,19 @@ int commander_thread_main(int argc, char *argv[]) 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"); + flight_termination_printed = true; + } + } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } /* RC input check */ @@ -1472,15 +1552,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; @@ -1499,6 +1589,7 @@ 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; } } @@ -1516,6 +1607,48 @@ 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"); + flight_termination_printed = true; + } + 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; + } + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } + } + + hrt_abstime t1 = hrt_absolute_time(); /* print new state */ @@ -1557,7 +1690,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) { @@ -1997,6 +2131,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; @@ -2008,6 +2143,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..30159dad9 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -105,3 +105,26 @@ 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); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 684c61a17..e3b5d30e4 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->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (status->gps_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } 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); |