diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index ab850f0fde..ac92a29eb0 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -50,8 +50,6 @@ uint16 VTOL_TRANSITION_FAILURE_MASK = 1024 uint16 VTOL_TRANSITION_FAILURE_CMD_MASK = 2048 uint16 GPS_FAILURE_MASK = 4096 uint16 GPS_FAILURE_CMD_MASK = 8192 -uint16 BAROMETER_FAILURE_MASK = 16384 -uint16 EVER_HAD_BAROMETER_DATA_MASK = 32768 # 0x0001 usb_connected: status of the USB power supply # 0x0002 offboard_control_signal_found_once @@ -67,7 +65,5 @@ uint16 EVER_HAD_BAROMETER_DATA_MASK = 32768 # 0x0800 vtol_transition_failure_cmd: Set to true if vtol transition failure mode is commanded # 0x1000 gps_failure: Set to true if a gps failure is detected # 0x2000 gps_failure_cmd: Set to true if a gps failure mode is commanded -# 0x4000 barometer_failure: Set to true if a barometer failure is detected -# 0x8000 ever_had_barometer_data: Set to true if ever had valid barometer data before uint16 other_flags diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c181975aed..acb82ce03d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1388,10 +1388,6 @@ Commander::run() status_flags.offboard_control_signal_found_once = false; status_flags.rc_signal_found_once = false; - /* assume we don't have a valid baro on startup */ - status_flags.barometer_failure = true; - status_flags.ever_had_barometer_data = false; - /* mark all signals lost as long as they haven't been found */ status.rc_signal_lost = true; status_flags.offboard_control_signal_lost = true; @@ -1566,11 +1562,6 @@ Commander::run() gps_position.eph = FLT_MAX; gps_position.epv = FLT_MAX; - /* Subscribe to sensor topic */ - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s sensors; - memset(&sensors, 0, sizeof(sensors)); - /* Subscribe to differential pressure topic */ int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; @@ -1971,37 +1962,6 @@ Commander::run() } } - orb_check(sensor_sub, &updated); - - 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. - * */ - hrt_abstime baro_timestamp = sensors.timestamp + sensors.baro_timestamp_relative; - if (hrt_elapsed_time(&baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { - /* handle the case where baro was regained */ - if (status_flags.barometer_failure) { - status_flags.barometer_failure = false; - status_changed = true; - if (status_flags.ever_had_barometer_data) { - mavlink_log_critical(&mavlink_log_pub, "baro healthy"); - } - status_flags.ever_had_barometer_data = true; - } - - } else { - if (!status_flags.barometer_failure) { - status_flags.barometer_failure = true; - status_changed = true; - mavlink_log_critical(&mavlink_log_pub, "baro failed"); - } - } - } - orb_check(diff_pres_sub, &updated); if (updated) { @@ -3257,7 +3217,6 @@ Commander::run() px4_close(local_position_sub); px4_close(global_position_sub); px4_close(gps_sub); - px4_close(sensor_sub); px4_close(safety_sub); px4_close(cmd_sub); px4_close(subsys_sub); @@ -4575,12 +4534,6 @@ void publish_status_flags(orb_advert_t &vehicle_status_flags_pub, vehicle_status 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; - } if ((v_flags.conditions != vehicle_status_flags.conditions) || (v_flags.other_flags != vehicle_status_flags.other_flags) || diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 1938bcddf7..54eb740d3a 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -110,8 +110,6 @@ struct status_flags_s { 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 ever_had_barometer_data; // Set to true if ever had valid barometer data before }; bool is_safe(const struct safety_s *safety, const struct actuator_armed_s *armed);