diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 8036534340..7abfeece12 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -1,61 +1,35 @@ -# define mask for conditions flags -uint16 CONDITION_CALIBRATION_ENABLE_MASK = 1 -uint16 CONDITION_SYSTEM_SENSORS_INITIALIZED_MASK = 2 -uint16 CONDITION_SYSTEM_PREARM_ERROR_REPORTED_MASK = 4 -uint16 CONDITION_SYSTEM_HOTPLUG_TIMEOUT_MASK = 8 -uint16 CONDITION_SYSTEM_RETURNED_TO_HOME_MASK = 16 -uint16 CONDITION_AUTO_MISSION_AVAILABLE_MASK = 32 -uint16 CONDITION_GLOBAL_POSITION_VALID_MASK = 64 -uint16 CONDITION_HOME_POSITION_VALID_MASK = 128 -uint16 CONDITION_LOCAL_POSITION_VALID_MASK = 256 -uint16 CONDITION_LOCAL_ALTITUDE_VALID_MASK = 512 -uint16 CONDITION_AIRSPEED_VALID_MASK = 1024 -uint16 CONDITION_POWER_INPUT_VALID_MASK = 2048 -# uint16 reserved for future implementation -# uint16 reserved for future implementation -# uint16 reserved for future implementation -# uint16 reserved for future implementation +# This is a struct used by the commander internally. -uint16 conditions +bool condition_calibration_enabled +bool condition_system_sensors_initialized +bool condition_system_prearm_error_reported # true if errors have already been reported +bool condition_system_hotplug_timeout # true if the hotplug sensor search is over +bool condition_system_returned_to_home +bool condition_auto_mission_available +bool condition_global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation +bool condition_global_velocity_valid # set to true by the commander app if the quality of the global horizontal velocity data is good enough to use for navigation +bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) +bool condition_local_position_valid # set to true by the commander app if the quality of the local position estimate is good enough to use for navigation +bool condition_local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation +bool condition_local_altitude_valid +bool condition_airspeed_valid # set to true by the commander app if there is a valid airspeed measurement available +bool condition_power_input_valid # set if input power is valid -# define mask for circuit breaker flags -uint8 CIRCUIT_BREAKER_ENGAGED_POWER_CHECK_MASK = 1 -uint8 CIRCUIT_BREAKER_ENGAGED_AIRSPD_CHECK_MASK = 2 -uint8 CIRCUIT_BREAKER_ENGAGED_ENGINEFAILURE_CHECK_MASK = 4 -uint8 CIRCUIT_BREAKER_ENGAGED_GPSFAILURE_CHECK_MASK = 8 -uint8 CIRCUIT_BREAKER_FLIGHT_TERMINATION_DISABLED_MASK = 16 -uint8 CIRCUIT_BREAKER_ENGAGED_USB_CHECK_MASK = 32 +bool circuit_breaker_engaged_power_check +bool circuit_breaker_engaged_airspd_check +bool circuit_breaker_engaged_enginefailure_check +bool circuit_breaker_engaged_gpsfailure_check +bool circuit_breaker_flight_termination_disabled +bool circuit_breaker_engaged_usb_check +bool circuit_breaker_engaged_posfailure_check # set to true when the position valid checks have been disabled -# 0x0001 circuit_breaker_engaged_power_check -# 0x0002 circuit_breaker_engaged_airspd_check -# 0x0004 circuit_breaker_engaged_enginefailure_check -# 0x0008 circuit_breaker_engaged_gpsfailure_check -# 0x0010 circuit_breaker_flight_termination_disabled -# 0x0020 circuit_breaker_engaged_usb_check +bool offboard_control_signal_found_once +bool offboard_control_signal_lost +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 offboard_control_loss_timeout # true if offboard is lost for a certain amount of time -uint8 circuit_breakers - -# define mask for other flags -uint16 USB_CONNECTED_MASK = 1 -uint16 OFFBOARD_CONTROL_SIGNAL_FOUND_ONCE_MASK = 2 -uint16 OFFBOARD_CONTROL_SIGNAL_LOST_MASK = 4 -uint16 OFFBOARD_CONTROL_SIGNAL_WEAK_MASK = 8 -uint16 OFFBOARD_CONTROL_SET_BY_COMMAND_MASK = 16 -uint16 OFFBOARD_CONTROL_LOSS_TIMEOUT_MASK = 32 -uint16 RC_SIGNAL_FOUND_ONCE_MASK = 64 -uint16 RC_INPUT_BLOCKED_MASK = 256 -uint16 VTOL_TRANSITION_FAILURE_MASK = 1024 -uint16 GPS_FAILURE_MASK = 4096 - -# 0x0001 usb_connected: status of the USB power supply -# 0x0002 offboard_control_signal_found_once -# 0x0004 offboard_control_signal_lost -# 0x0008 offboard_control_signal_weak -# 0x0010 offboard_control_set_by_command: true if the offboard mode was set by a mavlink command and should not be overridden by RC -# 0x0020 offboard_control_loss_timeout: true if offboard is lost for a certain amount of time -# 0x0040 rc_signal_found_once -# 0x0100 rc_input_blocked: set if RC input should be ignored temporarily -# 0x0400 vtol_transition_failure: Set to true if vtol transition failed -# 0x1000 gps_failure: Set to true if a gps failure is detected - -uint16 other_flags +bool rc_signal_found_once +bool rc_input_blocked # set if RC input should be ignored temporarily +bool vtol_transition_failure # Set to true if vtol transition failed +bool gps_failure # Set to true if a gps failure is detected +bool usb_connected # status of the USB power supply diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7088a2cbfe..dcde4a202e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -240,7 +240,7 @@ static uint8_t main_state_prev = 0; static bool warning_action_on = false; static bool last_overload = false; -static struct status_flags_s status_flags = {}; +static struct vehicle_status_flags_s status_flags = {}; static uint64_t rc_signal_lost_timestamp; // Time at which the RC reception was lost @@ -312,12 +312,7 @@ transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const ch */ void *commander_low_prio_loop(void *arg); -static void answer_command(struct vehicle_command_s &cmd, unsigned result, - orb_advert_t &command_ack_pub); - -/* publish vehicle status flags from the global variable status_flags*/ -static void publish_status_flags(orb_advert_t &vehicle_status_flags_pub, vehicle_status_flags_s& vehicle_status_flags); - +static void answer_command(struct vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub); static int power_button_state_notification_cb(board_power_button_state_notification_e request) { @@ -1382,7 +1377,6 @@ Commander::run() orb_advert_t command_ack_pub = nullptr; orb_advert_t commander_state_pub = nullptr; orb_advert_t vehicle_status_flags_pub = nullptr; - vehicle_status_flags_s vehicle_status_flags = {}; /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ mission_init(); @@ -3062,7 +3056,13 @@ Commander::run() } /* publish vehicle_status_flags */ - publish_status_flags(vehicle_status_flags_pub, vehicle_status_flags); + status_flags.timestamp = hrt_absolute_time(); + + if (vehicle_status_flags_pub != nullptr) { + orb_publish(ORB_ID(vehicle_status_flags), vehicle_status_flags_pub, &status_flags); + } else { + vehicle_status_flags_pub = orb_advertise(ORB_ID(vehicle_status_flags), &status_flags); + } } /* play arming and battery warning tunes */ @@ -4368,116 +4368,6 @@ void *commander_low_prio_loop(void *arg) return nullptr; } -void publish_status_flags(orb_advert_t &vehicle_status_flags_pub, vehicle_status_flags_s& vehicle_status_flags) { - - vehicle_status_flags_s v_flags = {}; - - /* set condition status flags */ - if (status_flags.condition_calibration_enabled) { - v_flags.conditions |= vehicle_status_flags_s::CONDITION_CALIBRATION_ENABLE_MASK; - } - if (status_flags.condition_system_sensors_initialized) { - v_flags.conditions |= vehicle_status_flags_s::CONDITION_SYSTEM_SENSORS_INITIALIZED_MASK; - } - if (status_flags.condition_system_prearm_error_reported) { - v_flags.conditions |= vehicle_status_flags_s::CONDITION_SYSTEM_PREARM_ERROR_REPORTED_MASK; - } - if (status_flags.condition_system_hotplug_timeout) { - v_flags.conditions |= vehicle_status_flags_s::CONDITION_SYSTEM_HOTPLUG_TIMEOUT_MASK; - } - if (status_flags.condition_system_returned_to_home) { - v_flags.conditions |= vehicle_status_flags_s::CONDITION_SYSTEM_RETURNED_TO_HOME_MASK; - } - if (status_flags.condition_auto_mission_available) { - v_flags.conditions |= vehicle_status_flags_s::CONDITION_AUTO_MISSION_AVAILABLE_MASK; - } - if (status_flags.condition_global_position_valid) { - v_flags.conditions |= vehicle_status_flags_s::CONDITION_GLOBAL_POSITION_VALID_MASK; - } - if (status_flags.condition_home_position_valid) { - v_flags.conditions |= vehicle_status_flags_s::CONDITION_HOME_POSITION_VALID_MASK; - } - if (status_flags.condition_local_position_valid) { - v_flags.conditions |= vehicle_status_flags_s::CONDITION_LOCAL_POSITION_VALID_MASK; - } - if (status_flags.condition_local_altitude_valid) { - v_flags.conditions |= vehicle_status_flags_s::CONDITION_LOCAL_ALTITUDE_VALID_MASK; - } - if (status_flags.condition_local_altitude_valid) { - v_flags.conditions |= vehicle_status_flags_s::CONDITION_LOCAL_ALTITUDE_VALID_MASK; - } - if (status_flags.condition_airspeed_valid) { - v_flags.conditions |= vehicle_status_flags_s::CONDITION_AIRSPEED_VALID_MASK; - } - if (status_flags.condition_power_input_valid) { - v_flags.conditions |= vehicle_status_flags_s::CONDITION_POWER_INPUT_VALID_MASK; - } - - /* set circuit breaker status flags */ - if (status_flags.circuit_breaker_engaged_power_check) { - v_flags.circuit_breakers |= vehicle_status_flags_s::CIRCUIT_BREAKER_ENGAGED_POWER_CHECK_MASK; - } - if (status_flags.circuit_breaker_engaged_airspd_check) { - v_flags.circuit_breakers |= vehicle_status_flags_s::CIRCUIT_BREAKER_ENGAGED_AIRSPD_CHECK_MASK; - } - if (status_flags.circuit_breaker_engaged_enginefailure_check) { - v_flags.circuit_breakers |= vehicle_status_flags_s::CIRCUIT_BREAKER_ENGAGED_ENGINEFAILURE_CHECK_MASK; - } - if (status_flags.circuit_breaker_engaged_gpsfailure_check) { - v_flags.circuit_breakers |= vehicle_status_flags_s::CIRCUIT_BREAKER_ENGAGED_GPSFAILURE_CHECK_MASK; - } - if (status_flags.circuit_breaker_flight_termination_disabled) { - v_flags.circuit_breakers |= vehicle_status_flags_s::CIRCUIT_BREAKER_FLIGHT_TERMINATION_DISABLED_MASK; - } - if (status_flags.circuit_breaker_engaged_usb_check) { - v_flags.circuit_breakers |= vehicle_status_flags_s::CIRCUIT_BREAKER_ENGAGED_USB_CHECK_MASK; - } - - /* set other status flags */ - if (status_flags.usb_connected) { - v_flags.other_flags |= vehicle_status_flags_s::USB_CONNECTED_MASK; - } - if (status_flags.offboard_control_signal_found_once) { - v_flags.other_flags |= vehicle_status_flags_s::OFFBOARD_CONTROL_SIGNAL_FOUND_ONCE_MASK; - } - if (status_flags.offboard_control_signal_lost) { - v_flags.other_flags |= vehicle_status_flags_s::OFFBOARD_CONTROL_SIGNAL_LOST_MASK; - } - if (status_flags.offboard_control_set_by_command) { - v_flags.other_flags |= vehicle_status_flags_s::OFFBOARD_CONTROL_SET_BY_COMMAND_MASK; - } - if (status_flags.offboard_control_loss_timeout) { - v_flags.other_flags |= vehicle_status_flags_s::OFFBOARD_CONTROL_LOSS_TIMEOUT_MASK; - } - if (status_flags.rc_signal_found_once) { - v_flags.other_flags |= vehicle_status_flags_s::RC_SIGNAL_FOUND_ONCE_MASK; - } - if (status_flags.rc_input_blocked) { - v_flags.other_flags |= vehicle_status_flags_s::RC_INPUT_BLOCKED_MASK; - } - if (status_flags.vtol_transition_failure) { - v_flags.other_flags |= vehicle_status_flags_s::VTOL_TRANSITION_FAILURE_MASK; - } - if (status_flags.gps_failure) { - v_flags.other_flags |= vehicle_status_flags_s::GPS_FAILURE_MASK; - } - - if ((v_flags.conditions != vehicle_status_flags.conditions) || - (v_flags.other_flags != vehicle_status_flags.other_flags) || - (v_flags.circuit_breakers != vehicle_status_flags.circuit_breakers)) { - - /* publish vehicle_status_flags */ - vehicle_status_flags = v_flags; - vehicle_status_flags.timestamp = hrt_absolute_time(); - - if (vehicle_status_flags_pub != nullptr) { - orb_publish(ORB_ID(vehicle_status_flags), vehicle_status_flags_pub, &vehicle_status_flags); - } else { - vehicle_status_flags_pub = orb_advertise(ORB_ID(vehicle_status_flags), &vehicle_status_flags); - } - } -} - int Commander::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); 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 46d5b4f45e..87ccbbd6bd 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -269,7 +269,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() }; struct vehicle_status_s status = {}; - struct status_flags_s status_flags = {}; + struct vehicle_status_flags_s status_flags = {}; struct safety_s safety = {}; struct actuator_armed_s armed = {}; struct battery_status_s battery = {}; @@ -446,7 +446,7 @@ bool StateMachineHelperTest::mainStateTransitionTest() // Setup initial machine state struct vehicle_status_s current_vehicle_status = {}; struct commander_state_s current_commander_state = {}; - struct status_flags_s current_status_flags = {}; + struct vehicle_status_flags_s current_status_flags = {}; uint8_t main_state_prev = 0; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index cb36aa2b81..3274893c1c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -108,7 +108,7 @@ static int last_prearm_ret = 1; ///< initialize to fail void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, - status_flags_s *status_flags, + vehicle_status_flags_s *status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act, uint8_t auto_recovery_nav_state); @@ -124,7 +124,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, actuator_armed_s *armed, bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log - status_flags_s *status_flags, + vehicle_status_flags_s *status_flags, float avionics_power_rail_voltage, uint8_t arm_requirements, hrt_abstime time_since_boot) @@ -395,7 +395,7 @@ bool is_safe(const struct safety_s *safety, const struct actuator_armed_s *armed transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev, - status_flags_s *status_flags, struct commander_state_s *internal_state) + vehicle_status_flags_s *status_flags, struct commander_state_s *internal_state) { // IMPORTANT: The assumption of callers of this function is that the execution of // this check if essentially "free". Therefore any runtime checking in here has to be @@ -601,7 +601,7 @@ bool set_nav_state(struct vehicle_status_s *status, const link_loss_actions_t data_link_loss_act, const bool mission_finished, const bool stay_in_failsafe, - status_flags_s *status_flags, + vehicle_status_flags_s *status_flags, bool landed, const link_loss_actions_t rc_loss_act, const int offb_loss_act, @@ -983,7 +983,7 @@ bool set_nav_state(struct vehicle_status_s *status, void set_rc_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, - status_flags_s *status_flags, + vehicle_status_flags_s *status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act) { @@ -993,7 +993,7 @@ void set_rc_loss_nav_state(vehicle_status_s *status, bool check_invalid_pos_nav_state(struct vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, - status_flags_s *status_flags, + vehicle_status_flags_s *status_flags, const bool use_rc, // true if we can fallback to a mode that uses RC inputs const bool using_global_pos) // true if the current flight mode requires a global position { @@ -1051,7 +1051,7 @@ bool check_invalid_pos_nav_state(struct vehicle_status_s *status, void set_data_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, - status_flags_s *status_flags, + vehicle_status_flags_s *status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act) { @@ -1060,7 +1060,7 @@ void set_data_link_loss_nav_state(vehicle_status_s *status, void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, - status_flags_s *status_flags, + vehicle_status_flags_s *status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act, uint8_t auto_recovery_nav_state) @@ -1126,7 +1126,7 @@ void reset_link_loss_globals(struct actuator_armed_s *armed, const bool old_fail } int prearm_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, - status_flags_s *status_flags, battery_status_s *battery, uint8_t arm_requirements, + vehicle_status_flags_s *status_flags, battery_status_s *battery, uint8_t arm_requirements, hrt_abstime time_since_boot) { bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported && diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 3112d98924..d0be39530f 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -50,6 +50,7 @@ #include #include #include +#include typedef enum { TRANSITION_DENIED = -1, @@ -74,40 +75,6 @@ typedef enum { ARM_REQ_GPS_BIT = (1 << 2), } arm_requirements_t; -// This is a struct used by the commander internally. -struct status_flags_s { - bool condition_calibration_enabled; - bool condition_system_sensors_initialized; - bool condition_system_prearm_error_reported; // true if errors have already been reported - bool condition_system_hotplug_timeout; // true if the hotplug sensor search is over - bool condition_system_returned_to_home; - bool condition_auto_mission_available; - bool condition_global_position_valid; // set to true by the commander app if the quality of the global position estimate is good enough to use for navigation - bool condition_global_velocity_valid; // set to true by the commander app if the quality of the global horizontal velocity data is good enough to use for navigation - bool condition_home_position_valid; // indicates a valid home position (a valid home position is not always a valid launch) - bool condition_local_position_valid; // set to true by the commander app if the quality of the local position estimate is good enough to use for navigation - bool condition_local_velocity_valid; // set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation - bool condition_local_altitude_valid; - bool condition_airspeed_valid; // set to true by the commander app if there is a valid airspeed measurement available - bool condition_power_input_valid; // set if input power is valid - bool usb_connected; // status of the USB power supply - bool circuit_breaker_engaged_power_check; - bool circuit_breaker_engaged_airspd_check; - bool circuit_breaker_engaged_enginefailure_check; - bool circuit_breaker_engaged_gpsfailure_check; - bool circuit_breaker_flight_termination_disabled; - bool circuit_breaker_engaged_usb_check; - bool circuit_breaker_engaged_posfailure_check; // set to true when the position valid checks have been disabled - bool offboard_control_signal_found_once; - bool offboard_control_signal_lost; - 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 offboard_control_loss_timeout; // true if offboard is lost for a certain amount of time - bool rc_signal_found_once; - bool rc_input_blocked; // set if RC input should be ignored temporarily - bool vtol_transition_failure; // Set to true if vtol transition failed - bool gps_failure; // Set to true if a gps failure is detected -}; - extern const char *const arming_state_names[]; bool is_safe(const struct safety_s *safety, const struct actuator_armed_s *armed); @@ -119,14 +86,14 @@ transition_result_t arming_state_transition(struct vehicle_status_s *status, struct actuator_armed_s *armed, bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, ///< uORB handle for mavlink log - status_flags_s *status_flags, + vehicle_status_flags_s *status_flags, float avionics_power_rail_voltage, uint8_t arm_requirements, hrt_abstime time_since_boot); transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev, - status_flags_s *status_flags, struct commander_state_s *internal_state); + vehicle_status_flags_s *status_flags, struct commander_state_s *internal_state); transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, orb_advert_t *mavlink_log_pub); @@ -140,7 +107,7 @@ bool set_nav_state(struct vehicle_status_s *status, const link_loss_actions_t data_link_loss_act, const bool mission_finished, const bool stay_in_failsafe, - status_flags_s *status_flags, + vehicle_status_flags_s *status_flags, bool landed, const link_loss_actions_t rc_loss_act, const int offb_loss_act, @@ -154,18 +121,18 @@ bool set_nav_state(struct vehicle_status_s *status, bool check_invalid_pos_nav_state(struct vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, - status_flags_s *status_flags, + vehicle_status_flags_s *status_flags, const bool use_rc, // true if a mode using RC control can be used as a fallback const bool using_global_pos); // true when the current mode requires a global position estimate -void set_rc_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, status_flags_s *status_flags, +void set_rc_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, vehicle_status_flags_s *status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act); -void set_data_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, status_flags_s *status_flags, +void set_data_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, vehicle_status_flags_s *status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act); int prearm_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, - bool force_report, status_flags_s *status_flags, battery_status_s *battery, + bool force_report, vehicle_status_flags_s *status_flags, battery_status_s *battery, uint8_t arm_requirements, hrt_abstime time_since_boot); #endif /* STATE_MACHINE_HELPER_H_ */