diff options
Diffstat (limited to 'src/modules/commander')
| -rw-r--r-- | src/modules/commander/accelerometer_calibration.cpp | 44 | ||||
| -rw-r--r-- | src/modules/commander/airspeed_calibration.cpp | 39 | ||||
| -rw-r--r-- | src/modules/commander/commander.cpp | 662 | ||||
| -rw-r--r-- | src/modules/commander/commander_params.c | 66 | ||||
| -rw-r--r-- | src/modules/commander/commander_tests/commander_tests.cpp | 4 | ||||
| -rw-r--r-- | src/modules/commander/commander_tests/state_machine_helper_test.cpp | 23 | ||||
| -rw-r--r-- | src/modules/commander/commander_tests/state_machine_helper_test.h | 2 | ||||
| -rw-r--r-- | src/modules/commander/gyro_calibration.cpp | 5 | ||||
| -rw-r--r-- | src/modules/commander/mag_calibration.cpp | 2 | ||||
| -rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 118 | ||||
| -rw-r--r-- | src/modules/commander/state_machine_helper.h | 4 |
11 files changed, 719 insertions, 250 deletions
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7a4e7a766..0cb41489f 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -83,7 +83,7 @@ * | accel_T[1][i] | * [ accel_T[2][i] ] * - * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * b = [ accel_corr_ref[0][i] ] // One measurement per side is enough * | accel_corr_ref[2][i] | * [ accel_corr_ref[4][i] ] * @@ -162,6 +162,11 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "You need to put the system on all six sides"); + sleep(3); + mavlink_log_info(mavlink_fd, "Follow the instructions on the screen"); + sleep(5); + struct accel_scale accel_scale = { 0.0f, 1.0f, @@ -258,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; - const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" }; int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -287,29 +292,37 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float break; } - mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", - (!data_collected[0]) ? "x+ " : "", - (!data_collected[1]) ? "x- " : "", - (!data_collected[2]) ? "y+ " : "", - (!data_collected[3]) ? "y- " : "", - (!data_collected[4]) ? "z+ " : "", - (!data_collected[5]) ? "z- " : ""); + /* inform user which axes are still needed */ + mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s", + (!data_collected[0]) ? "front " : "", + (!data_collected[1]) ? "back " : "", + (!data_collected[2]) ? "left " : "", + (!data_collected[3]) ? "right " : "", + (!data_collected[4]) ? "up " : "", + (!data_collected[5]) ? "down " : ""); + + /* allow user enough time to read the message */ + sleep(3); int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) { - res = ERROR; - break; + mavlink_log_info(mavlink_fd, "invalid motion, hold still..."); + sleep(3); + continue; } + /* inform user about already handled side */ if (data_collected[orient]) { - mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]); + sleep(4); continue; } - mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]); + sleep(1); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], + mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], (double)accel_ref[orient][1], (double)accel_ref[orient][2]); @@ -400,7 +413,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* is still now */ if (t_still == 0) { /* first time */ - mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); + mavlink_log_info(mavlink_fd, "detected rest position, hold still..."); t_still = t; t_timeout = t + timeout; @@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* not still, reset still start time */ if (t_still != 0) { mavlink_log_info(mavlink_fd, "detected motion, hold still..."); + sleep(3); t_still = 0; } } diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 0e58c68b6..cae1d7684 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -61,6 +61,15 @@ static const int ERROR = -1; static const char *sensor_name = "dpress"; +#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd" + +static void feedback_calibration_failed(int mavlink_fd) +{ + sleep(5); + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG); +} + int do_airspeed_calibration(int mavlink_fd) { /* give directions */ @@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd) float analog_scaling = 0.0f; param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { - mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd"); close(diff_pres_sub); return ERROR; } @@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } @@ -175,16 +184,18 @@ int do_airspeed_calibration(int mavlink_fd) } } else { - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } - mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); + mavlink_log_critical(mavlink_fd, "Create airflow now"); + calibration_counter = 0; const unsigned maxcount = 3000; @@ -204,18 +215,18 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { - if (calibration_counter % 100 == 0) { - mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", - (int)diff_pres.differential_pressure_raw_pa); + if (calibration_counter % 500 == 0) { + mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)", + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", - (int)diff_pres.differential_pressure_raw_pa); - mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -233,24 +244,24 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); return ERROR; } else { - mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", + mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } } if (calibration_counter == maxcount) { - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + feedback_calibration_failed(mavlink_fd); close(diff_pres_sub); return ERROR; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a0c896178..c9c8d16b5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -59,6 +59,7 @@ #include <string.h> #include <math.h> #include <poll.h> +#include <float.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> @@ -92,6 +93,7 @@ #include <systemlib/err.h> #include <systemlib/cpuload.h> #include <systemlib/rc_check.h> +#include <geo/geo.h> #include <systemlib/state_table.h> #include <dataman/dataman.h> @@ -124,10 +126,8 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ +#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 5 * 1000* 1000 #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -163,7 +163,8 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; static int parachute_enabled = 0; -static float eph_epv_threshold = 5.0f; +static float eph_threshold = 5.0f; +static float epv_threshold = 10.0f; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -206,7 +207,9 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub); +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, + struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, + orb_advert_t *home_pub); /** * Mainloop of commander. @@ -217,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); - transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); void set_control_mode(); @@ -229,7 +230,8 @@ 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); +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 arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); @@ -259,7 +261,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 2950, + 3200, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -308,12 +310,16 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - arm_disarm(true, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(true, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } if (!strcmp(argv[1], "disarm")) { - arm_disarm(false, mavlink_fd, "command line"); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + arm_disarm(false, mavlink_fd_local, "command line"); + close(mavlink_fd_local); exit(0); } @@ -335,6 +341,7 @@ void print_status() { warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE"); warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); /* read all relevant states */ int state_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -391,7 +398,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, + true /* fRunPreArmChecks */, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -404,11 +412,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char } bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, - struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, - struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) + struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, + struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components + if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) + && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } @@ -535,13 +544,13 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else { mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", - (double)cmd->param1, - (double)cmd->param2, - (double)cmd->param3, - (double)cmd->param4, - (double)cmd->param5, - (double)cmd->param6, - (double)cmd->param7); + (double)cmd->param1, + (double)cmd->param2, + (double)cmd->param3, + (double)cmd->param4, + (double)cmd->param5, + (double)cmd->param6, + (double)cmd->param7); } } break; @@ -551,11 +560,44 @@ 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; } break; @@ -607,10 +649,44 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } break; + case VEHICLE_CMD_NAV_GUIDED_ENABLE: { + transition_result_t res = TRANSITION_DENIED; + static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + + if (status_local->main_state != MAIN_STATE_OFFBOARD) { + main_state_pre_offboard = status_local->main_state; + } + + if (cmd->param1 > 0.5f) { + res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + + if (res == TRANSITION_DENIED) { + print_reject_mode(status_local, "OFFBOARD"); + status_local->offboard_control_set_by_command = false; + + } else { + /* Set flag that offboard was set via command, main state is not overridden by rc */ + status_local->offboard_control_set_by_command = true; + } + + } else { + /* If the mavlink command is used to enable or disable offboard control: + * switch back to previous mode when disabling */ + res = main_state_transition(status_local, main_state_pre_offboard); + status_local->offboard_control_set_by_command = false; + } + } + break; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: 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; @@ -650,6 +726,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"); @@ -680,7 +762,10 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER] = "AUTO_RCRECOVER"; nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL"; + nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; @@ -736,9 +821,13 @@ 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); + if (status_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); warnx("exiting."); @@ -764,11 +853,14 @@ int commander_thread_main(int argc, char *argv[]) /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ orb_advert_t mission_pub = -1; mission_s mission; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { - warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); + warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, + mission.current_seq); mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", - mission.dataman_id, mission.count, mission.current_seq); + mission.dataman_id, mission.count, mission.current_seq); + } else { const char *missionfail = "reading mission state failed"; warnx("%s", missionfail); @@ -841,11 +933,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; } @@ -872,6 +966,8 @@ int commander_thread_main(int argc, char *argv[]) int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps_position; memset(&gps_position, 0, sizeof(gps_position)); + gps_position.eph = FLT_MAX; + gps_position.epv = FLT_MAX; /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -929,6 +1025,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; @@ -976,7 +1081,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_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; @@ -988,6 +1100,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); @@ -1008,6 +1126,7 @@ int commander_thread_main(int argc, char *argv[]) status.offboard_control_signal_lost = false; status_changed = true; } + } else { if (!status.offboard_control_signal_lost) { status.offboard_control_signal_lost = true; @@ -1026,9 +1145,9 @@ int commander_thread_main(int argc, char *argv[]) /* perform system checks when new telemetry link connected */ if (mavlink_fd && - telemetry_last_heartbeat[i] == 0 && - telemetry.heartbeat_time > 0 && - hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) { + telemetry_last_heartbeat[i] == 0 && + telemetry.heartbeat_time > 0 && + hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { (void)rc_calibration_check(mavlink_fd); } @@ -1041,6 +1160,27 @@ 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); @@ -1056,10 +1196,11 @@ int commander_thread_main(int argc, char *argv[]) if (hrt_elapsed_time(&system_power.timestamp) < 200000) { if (system_power.servo_valid && - !system_power.brick_valid && - !system_power.usb_connected) { + !system_power.brick_valid && + !system_power.usb_connected) { /* flying only on servo rail, this is unsafe */ status.condition_power_input_valid = false; + } else { status.condition_power_input_valid = true; } @@ -1079,9 +1220,11 @@ int commander_thread_main(int argc, char *argv[]) /* disarm if safety is now on and still armed */ if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : + ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, + true /* fRunPreArmChecks */, mavlink_fd)) { mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } @@ -1106,32 +1249,31 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ - bool eph_epv_good; + bool eph_good; if (status.condition_global_position_valid) { - if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { - eph_epv_good = false; + if (global_position.eph > eph_threshold * 2.5f) { + eph_good = false; } else { - eph_epv_good = true; + eph_good = true; } } else { - if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { - eph_epv_good = true; + if (global_position.eph < eph_threshold) { + eph_good = true; } else { - eph_epv_good = false; + eph_good = false; } } - check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); - - /* check if GPS fix is ok */ + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), + &status_changed); /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { home.lat = global_position.lat; home.lon = global_position.lon; @@ -1161,8 +1303,8 @@ int commander_thread_main(int argc, char *argv[]) /* hysteresis for EPH */ bool local_eph_good; - if (status.condition_global_position_valid) { - if (local_position.eph > eph_epv_threshold * 2.0f) { + if (status.condition_local_position_valid) { + if (local_position.eph > eph_threshold * 2.5f) { local_eph_good = false; } else { @@ -1170,15 +1312,18 @@ int commander_thread_main(int argc, char *argv[]) } } else { - if (local_position.eph < eph_epv_threshold) { + if (local_position.eph < eph_threshold) { local_eph_good = true; } else { local_eph_good = false; } } - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed); - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid + && local_eph_good, &(status.condition_local_position_valid), &status_changed); + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, + &(status.condition_local_altitude_valid), &status_changed); if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { @@ -1209,7 +1354,8 @@ int commander_thread_main(int argc, char *argv[]) /* get throttle (if armed), as we only care about energy negative throttle also counts */ float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f; - status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle); + status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, + throttle); } } @@ -1266,8 +1412,8 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; /* check if board is connected via USB */ - //struct stat statbuf; - //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); + struct stat statbuf; + on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } /* if battery voltage is getting lower, warn using buzzer, etc. */ @@ -1277,26 +1423,30 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + } else if (!on_usb_power && status.condition_battery_voltage_valid && status.battery_remaining < 0.09f + && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ critical_battery_voltage_actions_done = true; mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } } + status_changed = true; } @@ -1305,7 +1455,8 @@ 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) { /* TODO: check for sensors */ - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, + mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -1331,14 +1482,64 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); } + /* Initialize map projection if gps is valid */ + if (!map_projection_global_initialized() + && (gps_position.eph < eph_threshold) + && (gps_position.epv < epv_threshold) + && hrt_elapsed_time((hrt_abstime *)&gps_position.timestamp_position) < 1e6) { + /* set reference for global coordinates <--> local coordiantes conversion and map_projection */ + 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; @@ -1347,7 +1548,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC signal regained"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000); status_changed = true; } } @@ -1363,11 +1564,15 @@ int commander_thread_main(int argc, char *argv[]) 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); - arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : + ARMING_STATE_STANDBY_ERROR); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, + mavlink_fd); + if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } + stick_off_counter = 0; } else { @@ -1389,8 +1594,11 @@ int commander_thread_main(int argc, char *argv[]) */ if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + } else { - arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, + mavlink_fd); + if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; } @@ -1413,6 +1621,7 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "DISARMED by RC"); } + arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { @@ -1440,24 +1649,38 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000); status.rc_signal_lost = true; + status.rc_signal_lost_timestamp=sp_man.timestamp; status_changed = true; } } /* 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; @@ -1476,10 +1699,42 @@ 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); @@ -1493,6 +1748,57 @@ 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 */ @@ -1502,7 +1808,7 @@ int commander_thread_main(int argc, char *argv[]) /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { // TODO remove code duplication home.lat = global_position.lat; @@ -1527,6 +1833,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; } + arming_state_changed = false; } @@ -1534,7 +1841,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) { @@ -1546,7 +1854,11 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe != failsafe_old) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); + if (status.failsafe) { + mavlink_log_critical(mavlink_fd, "failsafe mode on"); + } else { + mavlink_log_critical(mavlink_fd, "failsafe mode off"); + } failsafe_old = status.failsafe; } @@ -1570,7 +1882,8 @@ int commander_thread_main(int argc, char *argv[]) } /* play arming and battery warning tunes */ - if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available + && safety.safety_off))) { /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); arm_tune_played = true; @@ -1740,9 +2053,15 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; + /* if offboard is set allready by a mavlink command, abort */ + if (status.offboard_control_set_by_command) { + return main_state_transition(status_local, MAIN_STATE_OFFBOARD); + } + /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); @@ -1764,6 +2083,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } else { res = main_state_transition(status_local, MAIN_STATE_MANUAL); } + // TRANSITION_DENIED is not possible here break; @@ -1778,7 +2098,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s print_reject_mode(status_local, "POSCTL"); } - // fallback to ALTCTL + // fallback to ALTCTL res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -1802,14 +2122,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_RTL"); + print_reject_mode(status_local, "AUTO_RTL"); - // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + // fallback to LOITER if home position not set + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } else if (sp_man->loiter_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); @@ -1818,7 +2138,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_LOITER"); + print_reject_mode(status_local, "AUTO_LOITER"); } else { res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); @@ -1827,22 +2147,22 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s break; // changed successfully or already in this state } - print_reject_mode(status_local, "AUTO_MISSION"); + print_reject_mode(status_local, "AUTO_MISSION"); - // fallback to LOITER if home position not set - res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); + // fallback to LOITER if home position not set + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } - // fallback to POSCTL - res = main_state_transition(status_local, MAIN_STATE_POSCTL); + // fallback to POSCTL + res = main_state_transition(status_local, MAIN_STATE_POSCTL); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state - } + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } // fallback to ALTCTL res = main_state_transition(status_local, MAIN_STATE_ALTCTL); @@ -1886,18 +2206,6 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ACRO: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - 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_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -1910,54 +2218,6 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_OFFBOARD: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_offboard_enabled = true; - - switch (sp_offboard.mode) { - case OFFBOARD_CONTROL_MODE_DIRECT_RATES: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: - 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 = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */ - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; /* XXX: hack for now */ - control_mode.flag_control_velocity_enabled = true; - break; - case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - break; - default: - control_mode.flag_control_rates_enabled = false; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - } - break; - case NAVIGATION_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -1973,7 +2233,9 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RCRECOVER: 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; @@ -1985,6 +2247,31 @@ 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_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + 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; @@ -1998,6 +2285,19 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_DESCEND: + /* TODO: check if this makes sense */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; @@ -2011,6 +2311,63 @@ set_control_mode() control_mode.flag_control_termination_enabled = true; break; + case NAVIGATION_STATE_OFFBOARD: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_offboard_enabled = true; + + switch (sp_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + 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 = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_force_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + //XXX: the flags could depend on sp_offboard.ignore + break; + + default: + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + } + break; + default: break; } @@ -2152,7 +2509,8 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, + true /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2215,7 +2573,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); break; } 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 2e18c4284..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(); @@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) armed.ready_to_arm = test->current_state.ready_to_arm; // Attempt transition - transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */); + transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */); // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); @@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) MTT_ALL_NOT_VALID, MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, - { "transition: MANUAL to AUTO_MISSION - global position valid", - MTT_GLOBAL_POS_VALID, + { "transition: MANUAL to AUTO_MISSION - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, - { "transition: AUTO_MISSION to MANUAL - global position valid", - MTT_GLOBAL_POS_VALID, + { "transition: AUTO_MISSION to MANUAL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, { "transition: MANUAL to AUTO_LOITER - global position valid", @@ -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/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index d89c67c2b..8ab14dd52 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -63,7 +63,10 @@ static const char *sensor_name = "gyro"; int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "don't move system"); + mavlink_log_info(mavlink_fd, "HOLD STILL"); + + /* wait for the user to respond */ + sleep(2); struct gyro_scale gyro_scale = { 0.0f, diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 23900f386..7be8de9c6 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd) uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; unsigned poll_errcount = 0; - mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); + mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); calibration_counter = 0; 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 } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bb1b87e71..61d0f29d0 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,13 +57,13 @@ typedef enum { bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); + arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd); transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_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); |