diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 80ce0c45bb..96aad6f90a 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -74,26 +74,14 @@ bool is_vtol # True if the system is VTOL capable bool vtol_fw_permanent_stab # True if vtol should stabilize attitude for fw in manual mode bool in_transition_mode # True if VTOL is doing a transition -float32 avionics_power_rail_voltage # voltage of the avionics power rail - -bool rc_signal_found_once bool rc_signal_lost # true if RC reception lost -uint64 rc_signal_lost_timestamp # Time at which the RC reception was lost -bool rc_signal_lost_cmd # true if RC lost mode is commanded -bool rc_input_blocked # set if RC input should be ignored temporarily uint8 rc_input_mode # set to 1 to disable the RC input, 2 to enable manual control to RC in mapping. bool data_link_lost # datalink to GCS lost -bool data_link_lost_cmd # datalink to GCS lost mode commanded uint8 data_link_lost_counter # counts unique data link lost events bool engine_failure # Set to true if an engine failure is detected bool engine_failure_cmd # Set to true if an engine failure mode is commanded -bool vtol_transition_failure # Set to true if vtol transition failed -bool vtol_transition_failure_cmd # Set to true if vtol transition failure mode is commanded -bool gps_failure # Set to true if a gps failure is detected -bool gps_failure_cmd # Set to true if a gps failure mode is commanded bool mission_failure # Set to true if mission could not continue/finish -bool barometer_failure # Set to true if a barometer failure is detected # see SYS_STATUS mavlink message for the following uint32 onboard_control_sensors_present diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e15eb112fb..e2e4f04c3e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -224,6 +224,11 @@ static uint8_t main_state_prev = 0; static struct status_flags_s status_flags = {}; +static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was lost + +static float avionics_power_rail_voltage; // voltage of the avionics power rail + + /** * The daemon app only briefly exists to start * the background job. The stack size assigned in the @@ -555,9 +560,9 @@ void usage(const char *reason) void print_status() { warnx("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion"); - warnx("power: USB: %s, BRICK: %s", (status.usb_connected) ? "OK" : "NO", - (status.condition_power_input_valid) ? " OK" : "NO"); - warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage); + warnx("power: USB: %s, BRICK: %s", (status_flags.usb_connected) ? "[OK]" : "[NO]", + (status_flags.condition_power_input_valid) ? " [OK]" : "[NO]"); + warnx("avionics rail: %6.2f V", (double)avionics_power_rail_voltage); warnx("home: lat = %.7f, lon = %.7f, alt = %.2f, yaw: %.2f", _home.lat, _home.lon, (double)_home.alt, (double)_home.yaw); warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z); warnx("datalink: %s", (status.data_link_lost) ? "LOST" : "OK"); @@ -634,7 +639,8 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, co &armed, true /* fRunPreArmChecks */, mavlink_log_pub_local, - &status_flags); + &status_flags, + avionics_power_rail_voltage); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_log_pub_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); @@ -880,10 +886,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s /* 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; - status_local->vtol_transition_failure_cmd = false; + status_flags.data_link_lost_cmd = false; + status_flags.gps_failure_cmd = false; + status_flags.rc_signal_lost_cmd = false; + status_flags.vtol_transition_failure_cmd = false; if ((int)cmd->param2 <= 0) { /* reset all commanded failure modes */ @@ -896,22 +902,22 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } else if ((int)cmd->param2 == 2) { /* trigger data link loss mode */ - status_local->data_link_lost_cmd = true; + status_flags.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; + status_flags.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; + status_flags.rc_signal_lost_cmd = true; warnx("rc loss mode commanded"); } else if ((int)cmd->param2 == 5) { /* trigger vtol transition failure mode */ - status_local->vtol_transition_failure_cmd = true; + status_flags.vtol_transition_failure_cmd = true; warnx("vtol transition failure mode commanded"); } @@ -1222,7 +1228,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&status, 0, sizeof(status)); // We want to accept RC inputs as default - status.rc_input_blocked = false; + status_flags.rc_input_blocked = false; status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT; status.main_state =vehicle_status_s::MAIN_STATE_MANUAL; main_state_prev = vehicle_status_s::MAIN_STATE_MAX; @@ -1238,7 +1244,7 @@ int commander_thread_main(int argc, char *argv[]) /* neither manual nor offboard control commands have been received */ status_flags.offboard_control_signal_found_once = false; - status.rc_signal_found_once = false; + status_flags.rc_signal_found_once = false; /* mark all signals lost as long as they haven't been found */ status.rc_signal_lost = true; @@ -1251,7 +1257,7 @@ int commander_thread_main(int argc, char *argv[]) status.timestamp = hrt_absolute_time(); status_flags.condition_power_input_valid = true; - status.avionics_power_rail_voltage = -1.0f; + avionics_power_rail_voltage = -1.0f; status_flags.usb_connected = false; // CIRCUIT BREAKERS @@ -1706,15 +1712,15 @@ int commander_thread_main(int argc, char *argv[]) * */ if (hrt_elapsed_time(&sensors.baro_timestamp[0]) < FAILSAFE_DEFAULT_TIMEOUT) { /* handle the case where baro was regained */ - if (status.barometer_failure) { - status.barometer_failure = false; + if (status_flags.barometer_failure) { + status_flags.barometer_failure = false; status_changed = true; mavlink_log_critical(&mavlink_log_pub, "baro healthy"); } } else { - if (!status.barometer_failure) { - status.barometer_failure = true; + if (!status_flags.barometer_failure) { + status_flags.barometer_failure = true; status_changed = true; mavlink_log_critical(&mavlink_log_pub, "baro failed"); } @@ -1744,7 +1750,7 @@ int commander_thread_main(int argc, char *argv[]) } /* copy avionics voltage */ - status.avionics_power_rail_voltage = system_power.voltage5V_v; + avionics_power_rail_voltage = system_power.voltage5V_v; /* if the USB hardware connection went away, reboot */ if (status_flags.usb_connected && !system_power.usb_connected) { @@ -1782,7 +1788,8 @@ int commander_thread_main(int argc, char *argv[]) &armed, true /* fRunPreArmChecks */, &mavlink_log_pub, - &status_flags)) { + &status_flags, + avionics_power_rail_voltage)) { mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } @@ -1814,8 +1821,8 @@ int commander_thread_main(int argc, char *argv[]) if (is_vtol(&status)) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; status.in_transition_mode = vtol_status.vtol_in_trans_mode; - status.vtol_transition_failure = vtol_status.vtol_transition_failsafe; - status.vtol_transition_failure_cmd = vtol_status.vtol_transition_failsafe; + status_flags.vtol_transition_failure = vtol_status.vtol_transition_failsafe; + status_flags.vtol_transition_failure_cmd = vtol_status.vtol_transition_failsafe; } status_changed = true; @@ -2039,7 +2046,8 @@ int commander_thread_main(int argc, char *argv[]) &armed, true /* fRunPreArmChecks */, &mavlink_log_pub, - &status_flags); + &status_flags, + avionics_power_rail_voltage); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2082,26 +2090,26 @@ int commander_thread_main(int argc, char *argv[]) //Check if GPS receiver is too noisy while we are disarmed if (!armed.armed && gpsIsNoisy) { - if (!status.gps_failure) { + if (!status_flags.gps_failure) { mavlink_log_critical(&mavlink_log_pub, "GPS signal noisy"); set_tune_override(TONE_GPS_WARNING_TUNE); //GPS suffers from signal jamming or excessive noise, disable GPS-aided flight - status.gps_failure = true; + status_flags.gps_failure = true; status_changed = true; } } if (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 && !gpsIsNoisy) { - status.gps_failure = false; + if (status_flags.gps_failure && !gpsIsNoisy) { + status_flags.gps_failure = false; status_changed = true; mavlink_log_critical(&mavlink_log_pub, "gps fix regained"); } - } else if (!status.gps_failure) { - status.gps_failure = true; + } else if (!status_flags.gps_failure) { + status_flags.gps_failure = true; status_changed = true; mavlink_log_critical(&mavlink_log_pub, "gps fix lost"); } @@ -2257,17 +2265,17 @@ int commander_thread_main(int argc, char *argv[]) } /* RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && + if (!status_flags.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; + if (!status_flags.rc_signal_found_once) { + status_flags.rc_signal_found_once = true; status_changed = true; } else { if (status.rc_signal_lost) { mavlink_log_info(&mavlink_log_pub, "MANUAL CONTROL REGAINED after %llums", - (hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000); + (hrt_absolute_time() - rc_signal_lost_timestamp) / 1000); status_changed = true; } } @@ -2294,7 +2302,8 @@ int commander_thread_main(int argc, char *argv[]) &armed, true /* fRunPreArmChecks */, &mavlink_log_pub, - &status_flags); + &status_flags, + avionics_power_rail_voltage); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2336,7 +2345,8 @@ int commander_thread_main(int argc, char *argv[]) &armed, true /* fRunPreArmChecks */, &mavlink_log_pub, - &status_flags); + &status_flags, + avionics_power_rail_voltage); if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; @@ -2405,10 +2415,10 @@ int commander_thread_main(int argc, char *argv[]) /* no else case: do not change lockdown flag in unconfigured case */ } else { - if (!status.rc_input_blocked && !status.rc_signal_lost) { + if (!status_flags.rc_input_blocked && !status.rc_signal_lost) { mavlink_log_critical(&mavlink_log_pub, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000); status.rc_signal_lost = true; - status.rc_signal_lost_timestamp = sp_man.timestamp; + rc_signal_lost_timestamp = sp_man.timestamp; status_changed = true; } } @@ -2552,8 +2562,8 @@ int commander_thread_main(int argc, char *argv[]) status.main_state !=vehicle_status_s::MAIN_STATE_STAB && status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && - ((status.data_link_lost && status.gps_failure) || - (status.data_link_lost_cmd && status.gps_failure_cmd))) { + ((status.data_link_lost && status_flags.gps_failure) || + (status_flags.data_link_lost_cmd && status_flags.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; @@ -2578,8 +2588,8 @@ int commander_thread_main(int argc, char *argv[]) status.main_state ==vehicle_status_s::MAIN_STATE_STAB || status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status.gps_failure) || - (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { + ((status.rc_signal_lost && status_flags.gps_failure) || + (status_flags.rc_signal_lost_cmd && status_flags.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; @@ -3529,7 +3539,8 @@ void *commander_low_prio_loop(void *arg) &armed, false /* fRunPreArmChecks */, &mavlink_log_pub, - &status_flags)) { + &status_flags, + avionics_power_rail_voltage)) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack); break; } else { @@ -3554,7 +3565,7 @@ void *commander_low_prio_loop(void *arg) /* RC calibration */ answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); /* disable RC control input completely */ - status.rc_input_blocked = true; + status_flags.rc_input_blocked = true; calib_ret = OK; mavlink_log_info(&mavlink_log_pub, "CAL: Disabling RC IN"); @@ -3583,10 +3594,10 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 0) { /* RC calibration ended - have we been in one worth confirming? */ - if (status.rc_input_blocked) { + if (status_flags.rc_input_blocked) { answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); /* enable RC control input */ - status.rc_input_blocked = false; + status_flags.rc_input_blocked = false; mavlink_log_info(&mavlink_log_pub, "CAL: Re-enabling RC IN"); calib_ret = OK; } @@ -3621,7 +3632,8 @@ void *commander_low_prio_loop(void *arg) &armed, false /* fRunPreArmChecks */, &mavlink_log_pub, - &status_flags); + &status_flags, + avionics_power_rail_voltage); } else { tune_negative(true); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 03e7c37589..a9e79432d4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -114,8 +114,9 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, - orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log - status_flags_s *status_flags) + orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log + status_flags_s *status_flags, + float avionics_power_rail_voltage) { // Double check that our static arrays are still valid ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0); @@ -221,17 +222,17 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, // Fail transition if power levels on the avionics rail // are measured but are insufficient - if (status_flags->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { + if (status_flags->condition_power_input_valid && (avionics_power_rail_voltage > 0.0f)) { // Check avionics rail voltages - if (status->avionics_power_rail_voltage < AVIONICS_ERROR_VOLTAGE) { - mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + if (avionics_power_rail_voltage < 4.5f) { + mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage); feedback_provided = true; valid_transition = false; - } else if (status->avionics_power_rail_voltage < AVIONICS_WARN_VOLTAGE) { - mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + } else if (avionics_power_rail_voltage < 4.9f) { + mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage); feedback_provided = true; - } else if (status->avionics_power_rail_voltage > 5.4f) { - mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); + } else if (avionics_power_rail_voltage > 5.4f) { + mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)avionics_power_rail_voltage); feedback_provided = true; } } @@ -613,7 +614,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en case vehicle_status_s::MAIN_STATE_ALTCTL: case vehicle_status_s::MAIN_STATE_POSCTL: /* require RC for all manual modes */ - if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !landed) { + if ((status->rc_signal_lost || status_flags->rc_signal_lost_cmd) && armed && !landed) { status->failsafe = true; if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { @@ -670,21 +671,21 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en /* first look at the commands */ if (status->engine_failure_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status->data_link_lost_cmd) { + } else if (status_flags->data_link_lost_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS; - } else if (status->gps_failure_cmd) { + } else if (status_flags->gps_failure_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; - } else if (status->rc_signal_lost_cmd) { + } else if (status_flags->rc_signal_lost_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER; - } else if (status->vtol_transition_failure_cmd) { + } else if (status_flags->vtol_transition_failure_cmd) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; /* finished handling commands which have priority, now handle failures */ - } else if (status->gps_failure) { + } else if (status_flags->gps_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status->vtol_transition_failure) { + } else if (status_flags->vtol_transition_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } else if (status->mission_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 546c761e64..b8805f17d4 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -79,17 +79,27 @@ struct status_flags_s { bool offboard_control_signal_lost; bool offboard_control_signal_weak; bool offboard_control_set_by_command; // true if the offboard mode was set by a mavlink command and should not be overridden by RC + bool rc_signal_found_once; + bool rc_signal_lost_cmd; // true if RC lost mode is commanded + bool rc_input_blocked; // set if RC input should be ignored temporarily + bool data_link_lost_cmd; // datalink to GCS lost mode commanded + bool vtol_transition_failure; // Set to true if vtol transition failed + bool vtol_transition_failure_cmd; // Set to true if vtol transition failure mode is commanded + bool gps_failure; // Set to true if a gps failure is detected + bool gps_failure_cmd; // Set to true if a gps failure mode is commanded + bool barometer_failure; // Set to true if a barometer failure is detected }; 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, +transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, - status_flags_s *status_flags); + status_flags_s *status_flags, + float avionics_power_rail_voltage); transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,