From b182a5eeca3c19c430910ce1ce0337d75a80442c Mon Sep 17 00:00:00 2001 From: Simone Guscetti Date: Fri, 27 Jan 2017 19:13:18 +0100 Subject: [PATCH] Added the vehicle_status_flags publisher with the conversion in a bit field in commander --- src/modules/commander/commander.cpp | 130 ++++++++++++++++++++++++++++ 1 file changed, 130 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2ff7768d10..3d99b4d501 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -118,6 +118,7 @@ #include #include #include +#include #include typedef enum VEHICLE_MODE_FLAG @@ -298,6 +299,9 @@ void *commander_low_prio_loop(void *arg); void answer_command(struct vehicle_command_s &cmd, unsigned result, orb_advert_t &command_ack_pub, vehicle_command_ack_s &command_ack); +/* publish vehicle status flags from the global variable status_flags*/ +void publish_status_flags(orb_advert_t vehicle_status_flags_pub); + /** * check whether autostart ID is in the reserved range for HIL setups */ @@ -1461,6 +1465,8 @@ int commander_thread_main(int argc, char *argv[]) orb_advert_t commander_state_pub = nullptr; + orb_advert_t vehicle_status_flags_pub = nullptr; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { if (mission.count > 0) { @@ -3067,6 +3073,8 @@ int commander_thread_main(int argc, char *argv[]) have_taken_off_since_arming = false; } + /* publish vehicle_status_flags */ + publish_status_flags(vehicle_status_flags_pub); /* publish internal state for logging purposes */ if (commander_state_pub != nullptr) { @@ -4176,3 +4184,125 @@ void *commander_low_prio_loop(void *arg) return nullptr; } + +void publish_status_flags(orb_advert_t vehicle_status_flags_pub){ + struct vehicle_status_flags_s v_flags; + memset(&v_flags, 0, sizeof(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_signal_weak) { + v_flags.other_flags |= vehicle_status_flags_s::OFFBOARD_CONTROL_SIGNAL_WEAK_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_signal_lost_cmd) { + v_flags.other_flags |= vehicle_status_flags_s::RC_SIGNAL_LOST_CMD_MASK; + } + if (status_flags.rc_input_blocked) { + v_flags.other_flags |= vehicle_status_flags_s::RC_INPUT_BLOCKED_MASK; + } + if (status_flags.data_link_lost_cmd) { + v_flags.other_flags |= vehicle_status_flags_s::DATA_LINK_LOST_CMD_MASK; + } + if (status_flags.vtol_transition_failure) { + v_flags.other_flags |= vehicle_status_flags_s::VTOL_TRANSITION_FAILURE_MASK; + } + if (status_flags.vtol_transition_failure_cmd) { + v_flags.other_flags |= vehicle_status_flags_s::VTOL_TRANSITION_FAILURE_CMD_MASK; + } + if (status_flags.gps_failure) { + v_flags.other_flags |= vehicle_status_flags_s::GPS_FAILURE_MASK; + } + if (status_flags.gps_failure_cmd) { + v_flags.other_flags |= vehicle_status_flags_s::GPS_FAILURE_CMD_MASK; + } + if (status_flags.barometer_failure) { + v_flags.other_flags |= vehicle_status_flags_s::BAROMETER_FAILURE_MASK; + } + if (status_flags.ever_had_barometer_data) { + v_flags.other_flags |= vehicle_status_flags_s::EVER_HAD_BAROMETER_DATA_MASK; + } + + /* publish vehicle_status_flags */ + if (vehicle_status_flags_pub != nullptr) { + orb_publish(ORB_ID(vehicle_status_flags), vehicle_status_flags_pub, &v_flags); + }else{ + vehicle_status_flags_pub = orb_advertise(ORB_ID(vehicle_status_flags), &v_flags); + } +}