fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-28 13:44:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added the vehicle_status_flags publisher with the conversion in a bit field in commander

Browse Source
This commit is contained in:
Simone Guscetti 2017-01-27 19:13:18 +01:00 committed by Lorenz Meier
parent 2c2addad53
commit b182a5eeca
1 changed files with 130 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
src/modules/commander/commander.cpp
View File

@ -118,6 +118,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/vtol_vehicle_status.h>
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);
}
}