dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

refactor vehicle_status_flags: rename to failsafe_flags

This commit is contained in:
Beat Küng 2022-10-13 08:58:40 +02:00 committed by Daniel Agar
parent f9c8e760b1
commit d542ffc10c
34 changed files with 125 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.ci/Jenkinsfile-hardware
View File

@ -937,7 +937,7 @@ void printTopics() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_odometry" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_rates_setpoint" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status_flags" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener failsafe_flags" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vtol_vehicle_status" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener yaw_estimator_status" || true'
}

2
msg/CMakeLists.txt
View File

@ -80,6 +80,7 @@ set(msg_files
estimator_status_flags.msg
event.msg
failure_detector_status.msg
failsafe_flags.msg
follow_target.msg
follow_target_estimator.msg
follow_target_status.msg
@ -201,7 +202,6 @@ set(msg_files
vehicle_rates_setpoint.msg
vehicle_roi.msg
vehicle_status.msg
vehicle_status_flags.msg
vehicle_thrust_setpoint.msg
vehicle_torque_setpoint.msg
vehicle_trajectory_bezier.msg

0
msg/vehicle_status_flags.msg → msg/failsafe_flags.msg
View File

1
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp
View File

@ -39,7 +39,6 @@
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
typedef enum {
TRANSITION_DENIED = -1,

22
src/modules/commander/Commander.cpp
View File

@ -542,7 +542,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
if (run_preflight_checks && !_arm_state_machine.isArmed()) {
if (_vehicle_control_mode.flag_control_manual_enabled) {
if (_vehicle_control_mode.flag_control_climb_rate_enabled &&
!_vehicle_status_flags.manual_control_signal_lost && _is_throttle_above_center) {
!_failsafe_flags.manual_control_signal_lost && _is_throttle_above_center) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t");
events::send(events::ID("commander_arm_denied_throttle_center"),
{events::Log::Critical, events::LogInternal::Info},
@ -552,7 +552,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
}
if (!_vehicle_control_mode.flag_control_climb_rate_enabled &&
!_vehicle_status_flags.manual_control_signal_lost && !_is_throttle_low
!_failsafe_flags.manual_control_signal_lost && !_is_throttle_low
&& _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t");
events::send(events::ID("commander_arm_denied_throttle_high"),
@ -1052,7 +1052,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
// check if current mission and first item are valid
if (!_vehicle_status_flags.auto_mission_missing) {
if (!_failsafe_flags.auto_mission_missing) {
// requested first mission item valid
if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
@ -1791,7 +1791,7 @@ void Commander::run()
_actuator_armed.prearmed = getPrearmState();
// publish states (armed, control_mode, vehicle_status, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed
// publish states (armed, control_mode, vehicle_status, failure_detector_status) at 2 Hz or immediately when changed
if (now - _vehicle_status.timestamp >= 500_ms || _status_changed || nav_state_or_failsafe_changed
|| !(_actuator_armed == actuator_armed_prev)) {
@ -1869,7 +1869,7 @@ void Commander::checkForMissionUpdate()
if (mission_result_ok) {
/* Only evaluate mission state if home is set */
if (!_vehicle_status_flags.home_position_invalid &&
if (!_failsafe_flags.home_position_invalid &&
(prev_mission_instance_count != mission_result.instance_count)) {
if (!auto_mission_available) {
@ -2196,7 +2196,7 @@ bool Commander::handleModeIntentionAndFailsafe()
uint8_t updated_user_intented_mode = _failsafe.update(hrt_absolute_time(), state, mode_change_requested,
_failsafe_user_override_request,
_vehicle_status_flags);
_failsafe_flags);
_failsafe_user_override_request = false;
// Force intended mode if changed by the failsafe state machine
@ -2339,7 +2339,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
led_color = led_control_s::COLOR_RED;
} else {
if (!_vehicle_status_flags.home_position_invalid && !_vehicle_status_flags.global_position_invalid) {
if (!_failsafe_flags.home_position_invalid && !_failsafe_flags.global_position_invalid) {
led_color = led_control_s::COLOR_GREEN;
} else {
@ -2698,9 +2698,9 @@ void Commander::dataLinkCheck()
void Commander::battery_status_check()
{
// Handle shutdown request from emergency battery action
if (_battery_warning != _vehicle_status_flags.battery_warning) {
if (_battery_warning != _failsafe_flags.battery_warning) {
if (_vehicle_status_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
if (_failsafe_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
#if defined(BOARD_HAS_POWER_CONTROL)
if (shutdownIfAllowed() && (px4_shutdown_request(60_s) == 0)) {
@ -2723,7 +2723,7 @@ void Commander::battery_status_check()
}
}
_battery_warning = _vehicle_status_flags.battery_warning;
_battery_warning = _failsafe_flags.battery_warning;
}
void Commander::manualControlCheck()
@ -2786,7 +2786,7 @@ void Commander::manualControlCheck()
void Commander::offboardControlCheck()
{
if (_offboard_control_mode_sub.update()) {
if (_vehicle_status_flags.offboard_control_signal_lost) {
if (_failsafe_flags.offboard_control_signal_lost) {
// Run arming checks immediately to allow for offboard mode activation
_status_changed = true;
}

4
src/modules/commander/Commander.hpp
View File

@ -209,8 +209,8 @@ private:
UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
WorkerThread _worker_thread{};
const vehicle_status_flags_s &_vehicle_status_flags{_health_and_arming_checks.failsafeFlags()};
HomePosition _home_position{_vehicle_status_flags};
const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()};
HomePosition _home_position{_failsafe_flags};
Hysteresis _auto_disarm_landed{false};

2
src/modules/commander/HealthAndArmingChecks/Common.cpp
View File

@ -178,7 +178,7 @@ void Report::reset()
void Report::prepare(uint8_t vehicle_type)
{
// Get mode requirements before running any checks (in particular the mode checks require them)
mode_util::getModeRequirements(vehicle_type, _status_flags);
mode_util::getModeRequirements(vehicle_type, _failsafe_flags);
}
NavModes Report::getModeGroup(uint8_t nav_state) const

12
src/modules/commander/HealthAndArmingChecks/Common.hpp
View File

@ -38,7 +38,7 @@
#include <px4_platform_common/module_params.h>
#include <uORB/topics/health_report.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/failsafe_flags.h>
#include <systemlib/mavlink_log.h>
#include <stdint.h>
@ -188,11 +188,11 @@ public:
}
};
Report(vehicle_status_flags_s &status_flags, hrt_abstime min_reporting_interval = 2_s)
: _min_reporting_interval(min_reporting_interval), _status_flags(status_flags) { }
Report(failsafe_flags_s &failsafe_flags, hrt_abstime min_reporting_interval = 2_s)
: _min_reporting_interval(min_reporting_interval), _failsafe_flags(failsafe_flags) { }
~Report() = default;
vehicle_status_flags_s &failsafeFlags() { return _status_flags; }
failsafe_flags_s &failsafeFlags() { return _failsafe_flags; }
orb_advert_t *mavlink_log_pub() { return _mavlink_log_pub; }
@ -255,7 +255,7 @@ public:
const HealthResults &healthResults() const { return _results[_current_result].health; }
const ArmingCheckResults &armingCheckResults() const { return _results[_current_result].arming_checks; }
bool modePreventsArming(uint8_t nav_state) const { return _status_flags.mode_req_prevent_arming & (1u << nav_state); }
bool modePreventsArming(uint8_t nav_state) const { return _failsafe_flags.mode_req_prevent_arming & (1u << nav_state); }
private:
/**
@ -346,7 +346,7 @@ private:
Results _results[2]; ///< Previous and current results to check for changes
int _current_result{0};
vehicle_status_flags_s &_status_flags;
failsafe_flags_s &_failsafe_flags;
orb_advert_t *_mavlink_log_pub{nullptr}; ///< mavlink log publication for legacy reporting
};

8
src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp
View File

@ -38,7 +38,7 @@
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/health_report.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/failsafe_flags.h>
#include "checks/accelerometerCheck.hpp"
#include "checks/airspeedCheck.hpp"
@ -98,7 +98,7 @@ public:
*/
bool modePreventsArming(uint8_t nav_state) const { return _reporter.modePreventsArming(nav_state); }
const vehicle_status_flags_s &failsafeFlags() const { return _failsafe_flags; }
const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; }
protected:
void updateParams() override;
@ -107,10 +107,10 @@ private:
Report _reporter;
orb_advert_t _mavlink_log_pub{nullptr};
vehicle_status_flags_s _failsafe_flags{};
failsafe_flags_s _failsafe_flags{};
uORB::Publication<health_report_s> _health_report_pub{ORB_ID(health_report)};
uORB::Publication<vehicle_status_flags_s> _failsafe_flags_pub{ORB_ID(vehicle_status_flags)};
uORB::Publication<failsafe_flags_s> _failsafe_flags_pub{ORB_ID(failsafe_flags)};
// all checks
AccelerometerChecks _accelerometer_checks;

24
src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp
View File

@ -63,8 +63,8 @@ public:
TEST_F(ReporterTest, basic_no_checks)
{
vehicle_status_flags_s status_flags{};
Report reporter{status_flags, 0_s};
failsafe_flags_s failsafe_flags{};
Report reporter{failsafe_flags, 0_s};
ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION));
reporter.reset();
@ -83,8 +83,8 @@ TEST_F(ReporterTest, basic_no_checks)
TEST_F(ReporterTest, basic_fail_all_modes)
{
vehicle_status_flags_s status_flags{};
Report reporter{status_flags, 0_s};
failsafe_flags_s failsafe_flags{};
Report reporter{failsafe_flags, 0_s};
// ensure arming is always denied with a NavModes::All failure
for (uint8_t nav_state = 0; nav_state < vehicle_status_s::NAVIGATION_STATE_MAX; ++nav_state) {
@ -101,8 +101,8 @@ TEST_F(ReporterTest, basic_fail_all_modes)
TEST_F(ReporterTest, arming_checks_mode_category)
{
vehicle_status_flags_s status_flags{};
Report reporter{status_flags, 0_s};
failsafe_flags_s failsafe_flags{};
Report reporter{failsafe_flags, 0_s};
// arming must still be possible for non-relevant failures
reporter.reset();
@ -130,8 +130,8 @@ TEST_F(ReporterTest, arming_checks_mode_category)
TEST_F(ReporterTest, arming_checks_mode_category2)
{
vehicle_status_flags_s status_flags{};
Report reporter{status_flags, 0_s};
failsafe_flags_s failsafe_flags{};
Report reporter{failsafe_flags, 0_s};
// A matching mode category must deny arming
reporter.reset();
@ -153,8 +153,8 @@ TEST_F(ReporterTest, arming_checks_mode_category2)
TEST_F(ReporterTest, reporting)
{
vehicle_status_flags_s status_flags{};
Report reporter{status_flags, 0_s};
failsafe_flags_s failsafe_flags{};
Report reporter{failsafe_flags, 0_s};
uORB::Subscription event_sub{ORB_ID(event)};
event_sub.subscribe();
@ -247,8 +247,8 @@ TEST_F(ReporterTest, reporting)
TEST_F(ReporterTest, reporting_multiple)
{
vehicle_status_flags_s status_flags{};
Report reporter{status_flags, 0_s};
failsafe_flags_s failsafe_flags{};
Report reporter{failsafe_flags, 0_s};
uORB::Subscription event_sub{ORB_ID(event)};
event_sub.subscribe();

2
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
View File

@ -670,7 +670,7 @@ void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter)
void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
bool pre_flt_fail_innov_vel_horiz,
const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, vehicle_status_flags_s &failsafe_flags)
const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags)
{
// The following flags correspond to mode requirements, and are reported in the corresponding mode checks

2
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
View File

@ -69,7 +69,7 @@ private:
void gpsNoLongerValid(const Context &context, Report &reporter) const;
void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz,
const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position,
vehicle_status_flags_s &failsafe_flags);
failsafe_flags_s &failsafe_flags);
bool checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy,
const float required_accuracy,

22
src/modules/commander/HomePosition.cpp
View File

@ -37,8 +37,8 @@
#include <lib/geo/geo.h>
#include "commander_helper.h"
HomePosition::HomePosition(const vehicle_status_flags_s &vehicle_status_flags)
: _vehicle_status_flags(vehicle_status_flags)
HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags)
: _failsafe_flags(failsafe_flags)
{
}
@ -69,7 +69,7 @@ bool HomePosition::hasMovedFromCurrentHomeLocation()
eph = gpos.eph;
epv = gpos.epv;
} else if (!_vehicle_status_flags.gps_position_invalid) {
} else if (!_failsafe_flags.gps_position_invalid) {
sensor_gps_s gps;
_vehicle_gps_position_sub.copy(&gps);
const double lat = static_cast<double>(gps.lat) * 1e-7;
@ -98,7 +98,7 @@ bool HomePosition::setHomePosition(bool force)
bool updated = false;
home_position_s home{};
if (!_vehicle_status_flags.local_position_invalid) {
if (!_failsafe_flags.local_position_invalid) {
// Set home position in local coordinates
const vehicle_local_position_s &lpos = _local_position_sub.get();
_heading_reset_counter = lpos.heading_reset_counter; // TODO: should not be here
@ -107,14 +107,14 @@ bool HomePosition::setHomePosition(bool force)
updated = true;
}
if (!_vehicle_status_flags.global_position_invalid) {
if (!_failsafe_flags.global_position_invalid) {
// Set home using the global position estimate (fused INS/GNSS)
const vehicle_global_position_s &gpos = _global_position_sub.get();
fillGlobalHomePos(home, gpos);
setHomePosValid();
updated = true;
} else if (!_vehicle_status_flags.gps_position_invalid) {
} else if (!_failsafe_flags.gps_position_invalid) {
// Set home using GNSS position
sensor_gps_s gps_pos;
_vehicle_gps_position_sub.copy(&gps_pos);
@ -184,7 +184,7 @@ void HomePosition::setInAirHomePosition()
const bool local_home_valid = home.valid_lpos;
if (local_home_valid && !global_home_valid) {
if (!_vehicle_status_flags.local_position_invalid && !_vehicle_status_flags.global_position_invalid) {
if (!_failsafe_flags.local_position_invalid && !_failsafe_flags.global_position_invalid) {
// Back-compute lon, lat and alt of home position given the local home position
// and current positions in local and global (GNSS fused) frames
const vehicle_local_position_s &lpos = _local_position_sub.get();
@ -203,7 +203,7 @@ void HomePosition::setInAirHomePosition()
home.timestamp = hrt_absolute_time();
_home_position_pub.update();
} else if (!_vehicle_status_flags.local_position_invalid && !_vehicle_status_flags.gps_position_invalid) {
} else if (!_failsafe_flags.local_position_invalid && !_failsafe_flags.gps_position_invalid) {
// Back-compute lon, lat and alt of home position given the local home position
// and current positions in local and global (GNSS raw) frames
const vehicle_local_position_s &lpos = _local_position_sub.get();
@ -231,7 +231,7 @@ void HomePosition::setInAirHomePosition()
} else if (!local_home_valid && global_home_valid) {
const vehicle_local_position_s &lpos = _local_position_sub.get();
if (!_vehicle_status_flags.local_position_invalid && lpos.xy_global && lpos.z_global) {
if (!_failsafe_flags.local_position_invalid && lpos.xy_global && lpos.z_global) {
// Back-compute x, y and z of home position given the global home position
// and the global reference of the local frame
MapProjection ref_pos{lpos.ref_lat, lpos.ref_lon};
@ -326,9 +326,9 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
}
if (check_if_changed && set_automatically) {
const bool can_set_home_lpos_first_time = !home.valid_lpos && !_vehicle_status_flags.local_position_invalid;
const bool can_set_home_lpos_first_time = !home.valid_lpos && !_failsafe_flags.local_position_invalid;
const bool can_set_home_gpos_first_time = ((!home.valid_hpos || !home.valid_alt)
&& (!_vehicle_status_flags.global_position_invalid || !_vehicle_status_flags.gps_position_invalid));
&& (!_failsafe_flags.global_position_invalid || !_failsafe_flags.gps_position_invalid));
const bool can_set_home_alt_first_time = (!home.valid_alt && lpos.z_global);
if (can_set_home_lpos_first_time

6
src/modules/commander/HomePosition.hpp
View File

@ -39,12 +39,12 @@
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/failsafe_flags.h>
class HomePosition
{
public:
HomePosition(const vehicle_status_flags_s &vehicle_status_flags);
HomePosition(const failsafe_flags_s &failsafe_flags);
~HomePosition() = default;
bool setHomePosition(bool force = false);
@ -74,5 +74,5 @@ private:
uint8_t _heading_reset_counter{0};
bool _valid{false};
const vehicle_status_flags_s &_vehicle_status_flags;
const failsafe_flags_s &_failsafe_flags;
};

2
src/modules/commander/ModeUtil/mode_requirements.cpp
View File

@ -43,7 +43,7 @@ static inline void setRequirement(uint8_t nav_state, uint32_t &mode_requirement)
}
void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags)
void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
{
flags.mode_req_angular_velocity = 0;
flags.mode_req_attitude = 0;

4
src/modules/commander/ModeUtil/mode_requirements.hpp
View File

@ -33,7 +33,7 @@
#pragma once
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/failsafe_flags.h>
namespace mode_util
{
@ -43,7 +43,7 @@ namespace mode_util
* @param vehicle_type one of vehicle_status_s::VEHICLE_TYPE_*
* @param flags output flags, all mode_req_* entries are set
*/
void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags);
void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags);
} // namespace mode_util

10
src/modules/commander/failsafe/CMakeLists.txt
View File

@ -31,20 +31,20 @@
#
############################################################################
# Extract information from status msg file
set(status_msg_file ${PX4_SOURCE_DIR}/msg/vehicle_status_flags.msg)
# Extract information from failsafe msg file
set(failsafe_flags_msg_file ${PX4_SOURCE_DIR}/msg/failsafe_flags.msg)
set(generated_uorb_struct_field_mapping_header ${PX4_BINARY_DIR}/generated_uorb_struct_field_mapping.h)
set(html_template_file ${CMAKE_CURRENT_SOURCE_DIR}/emscripten_template.html)
set(html_output_file ${PX4_BINARY_DIR}/failsafe_html_template.html)
add_custom_command(OUTPUT ${generated_uorb_struct_field_mapping_header} ${html_output_file}
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/parse_flags_from_msg.py
${status_msg_file} ${generated_uorb_struct_field_mapping_header}
${failsafe_flags_msg_file} ${generated_uorb_struct_field_mapping_header}
${html_template_file} ${html_output_file}
DEPENDS
${status_msg_file}
${failsafe_flags_msg_file}
${html_template_file}
${CMAKE_CURRENT_SOURCE_DIR}/parse_flags_from_msg.py
COMMENT "Extracting info from status msg file"
COMMENT "Extracting info from failsafe flags msg file"
)
add_custom_target(failsafe_uorb_struct_header DEPENDS ${generated_uorb_struct_field_mapping_header})

4
src/modules/commander/failsafe/emscripten.cpp
View File

@ -167,7 +167,7 @@ void set_param_value_float(const std::string &name, float value)
int failsafe_update(bool armed, bool vtol_in_transition_mode, bool mission_finished,
bool user_override, uint8_t user_intended_mode, uint8_t vehicle_type,
vehicle_status_flags_s status_flags)
failsafe_flags_s status_flags)
{
uint64_t time_ms = emscripten_date_now();
FailsafeBase::State state{};
@ -203,7 +203,7 @@ std::string action_str(int action)
EMSCRIPTEN_BINDINGS(failsafe)
{
class_<vehicle_status_flags_s>("state")
class_<failsafe_flags_s>("state")
.constructor<>()
UORB_STRUCT_FIELD_MAPPING
;

6
src/modules/commander/failsafe/failsafe.cpp
View File

@ -324,7 +324,7 @@ FailsafeBase::Action Failsafe::fromOffboardLossActParam(int param_value, uint8_t
}
void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
const vehicle_status_flags_s &status_flags)
const failsafe_flags_s &status_flags)
{
updateArmingState(time_us, state.armed, status_flags);
@ -445,7 +445,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
ActionOptions(mode_fallback_action).allowUserTakeover(UserTakeoverAllowed::Always));
}
void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const vehicle_status_flags_s &status_flags)
void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags)
{
if (!_was_armed && armed) {
_armed_time = time_us;
@ -459,7 +459,7 @@ void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const v
_was_armed = armed;
}
FailsafeBase::Action Failsafe::checkModeFallback(const vehicle_status_flags_s &status_flags,
FailsafeBase::Action Failsafe::checkModeFallback(const failsafe_flags_s &status_flags,
uint8_t user_intended_mode) const
{
Action action = Action::None;

6
src/modules/commander/failsafe/failsafe.h
View File

@ -44,14 +44,14 @@ public:
protected:
void checkStateAndMode(const hrt_abstime &time_us, const State &state,
const vehicle_status_flags_s &status_flags) override;
Action checkModeFallback(const vehicle_status_flags_s &status_flags, uint8_t user_intended_mode) const override;
const failsafe_flags_s &status_flags) override;
Action checkModeFallback(const failsafe_flags_s &status_flags, uint8_t user_intended_mode) const override;
uint8_t modifyUserIntendedMode(Action previous_action, Action current_action,
uint8_t user_intended_mode) const override;
private:
void updateArmingState(const hrt_abstime &time_us, bool armed, const vehicle_status_flags_s &status_flags);
void updateArmingState(const hrt_abstime &time_us, bool armed, const failsafe_flags_s &status_flags);
enum class ManualControlLossExceptionBits : int32_t {
Mission = (1 << 0),

10
src/modules/commander/failsafe/failsafe_test.cpp
View File

@ -48,7 +48,7 @@ public:
protected:
void checkStateAndMode(const hrt_abstime &time_us, const State &state,
const vehicle_status_flags_s &status_flags) override
const failsafe_flags_s &status_flags) override
{
CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
@ -65,7 +65,7 @@ protected:
&& status_flags.fd_critical_failure, Action::Terminate);
}
Action checkModeFallback(const vehicle_status_flags_s &status_flags, uint8_t user_intended_mode) const override
Action checkModeFallback(const failsafe_flags_s &status_flags, uint8_t user_intended_mode) const override
{
return Action::None;
}
@ -96,7 +96,7 @@ TEST_F(FailsafeTest, general)
{
FailsafeTester failsafe(nullptr);
vehicle_status_flags_s failsafe_flags{};
failsafe_flags_s failsafe_flags{};
FailsafeBase::State state{};
state.armed = true;
state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
@ -153,7 +153,7 @@ TEST_F(FailsafeTest, takeover)
{
FailsafeTester failsafe(nullptr);
vehicle_status_flags_s failsafe_flags{};
failsafe_flags_s failsafe_flags{};
FailsafeBase::State state{};
state.armed = true;
state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
@ -210,7 +210,7 @@ TEST_F(FailsafeTest, takeover_denied)
{
FailsafeTester failsafe(nullptr);
vehicle_status_flags_s failsafe_flags{};
failsafe_flags_s failsafe_flags{};
FailsafeBase::State state{};
state.armed = true;
state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;

8
src/modules/commander/failsafe/framework.cpp
View File

@ -52,7 +52,7 @@ FailsafeBase::FailsafeBase(ModuleParams *parent) : ModuleParams(parent)
uint8_t FailsafeBase::update(const hrt_abstime &time_us, const State &state, bool user_intended_mode_updated,
bool rc_sticks_takeover_request,
const vehicle_status_flags_s &status_flags)
const failsafe_flags_s &status_flags)
{
if (_last_update == 0) {
_last_update = time_us;
@ -390,7 +390,7 @@ void FailsafeBase::removeNonActivatedActions()
}
void FailsafeBase::getSelectedAction(const State &state, const vehicle_status_flags_s &status_flags,
void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s &status_flags,
bool user_intended_mode_updated,
bool rc_sticks_takeover_request,
SelectedActionState &returned_state) const
@ -566,7 +566,7 @@ bool FailsafeBase::actionAllowsUserTakeover(Action action) const
}
void FailsafeBase::clearDelayIfNeeded(const State &state,
const vehicle_status_flags_s &status_flags)
const failsafe_flags_s &status_flags)
{
// Clear delay if one of the following is true:
// - Already in a failsafe
@ -611,7 +611,7 @@ uint8_t FailsafeBase::modeFromAction(const Action &action, uint8_t user_intended
return user_intended_mode;
}
bool FailsafeBase::modeCanRun(const vehicle_status_flags_s &status_flags, uint8_t mode)
bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode)
{
uint32_t mode_mask = 1u << mode;
return

22
src/modules/commander/failsafe/framework.h
View File

@ -33,14 +33,14 @@
#pragma once
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/failsafe_flags.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/module_params.h>
#include <cstddef>
#define CHECK_FAILSAFE(status_flags, flag_name, options) \
checkFailsafe((int)offsetof(vehicle_status_flags_s, flag_name), lastStatusFlags().flag_name, status_flags.flag_name, options)
checkFailsafe((int)offsetof(failsafe_flags_s, flag_name), lastStatusFlags().flag_name, status_flags.flag_name, options)
class FailsafeBase: public ModuleParams
{
@ -139,7 +139,7 @@ public:
*/
uint8_t update(const hrt_abstime &time_us, const State &state, bool user_intended_mode_updated,
bool rc_sticks_takeover_request,
const vehicle_status_flags_s &status_flags);
const failsafe_flags_s &status_flags);
bool inFailsafe() const { return _selected_action != Action::None; }
@ -185,21 +185,21 @@ protected:
};
virtual void checkStateAndMode(const hrt_abstime &time_us, const State &state,
const vehicle_status_flags_s &status_flags) = 0;
virtual Action checkModeFallback(const vehicle_status_flags_s &status_flags, uint8_t user_intended_mode) const = 0;
const failsafe_flags_s &status_flags) = 0;
virtual Action checkModeFallback(const failsafe_flags_s &status_flags, uint8_t user_intended_mode) const = 0;
const vehicle_status_flags_s &lastStatusFlags() const { return _last_status_flags; }
const failsafe_flags_s &lastStatusFlags() const { return _last_status_flags; }
bool checkFailsafe(int caller_id, bool last_state_failure, bool cur_state_failure, const ActionOptions &options);
virtual void getSelectedAction(const State &state, const vehicle_status_flags_s &status_flags,
virtual void getSelectedAction(const State &state, const failsafe_flags_s &status_flags,
bool user_intended_mode_updated,
bool rc_sticks_takeover_request,
SelectedActionState &returned_state) const;
int genCallerId() { return ++_next_caller_id; }
static bool modeCanRun(const vehicle_status_flags_s &status_flags, uint8_t mode);
static bool modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode);
/**
* Allows to modify the user intended mode. Use only in limited cases.
@ -228,7 +228,7 @@ private:
void updateDelay(const hrt_abstime &elapsed_us);
void notifyUser(uint8_t user_intended_mode, Action action, Action delayed_action, Cause cause);
void clearDelayIfNeeded(const State &state, const vehicle_status_flags_s &status_flags);
void clearDelayIfNeeded(const State &state, const failsafe_flags_s &status_flags);
bool actionAllowsUserTakeover(Action action) const;
@ -240,7 +240,7 @@ private:
hrt_abstime _last_update{};
bool _last_armed{false};
uint8_t _last_user_intended_mode{0};
vehicle_status_flags_s _last_status_flags{};
failsafe_flags_s _last_status_flags{};
Action _selected_action{Action::None};
bool _user_takeover_active{false};
bool _notification_required{false};
@ -248,7 +248,7 @@ private:
hrt_abstime _current_start_delay{0}; ///< _current_delay is set to this value when starting the delay
hrt_abstime _current_delay{0}; ///< If > 0, stay in Hold, and take action once delay reaches 0
int _next_caller_id{sizeof(vehicle_status_flags_s) + 1};
int _next_caller_id{sizeof(failsafe_flags_s) + 1};
bool _duplicate_reported_once{false};
orb_advert_t _mavlink_log_pub{nullptr};

2
src/modules/commander/failsafe/parse_flags_from_msg.py
View File

@ -12,7 +12,7 @@ import re
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='Extract fields from .msg files')
parser.add_argument('msg_file', help='vehicle_status_flags.msg file')
parser.add_argument('msg_file', help='failsafe_flags.msg file')
parser.add_argument('header_file', help='generated_uorb_struct_field_mapping.h')
parser.add_argument('html_template', help='HTML template input file')
parser.add_argument('html_output', help='HTML output file')

8
src/modules/events/rc_loss_alarm.cpp
View File

@ -58,9 +58,9 @@ void RC_Loss_Alarm::process()
return;
}
vehicle_status_flags_s status_flags{};
failsafe_flags_s failsafe_flags{};
_vehicle_status_flags_sub.copy(&status_flags);
_failsafe_flags_sub.copy(&failsafe_flags);
if (!_was_armed &&
status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
@ -68,12 +68,12 @@ void RC_Loss_Alarm::process()
_was_armed = true; // Once true, impossible to go back to false
}
if (!_had_manual_control && !status_flags.manual_control_signal_lost) {
if (!_had_manual_control && !failsafe_flags.manual_control_signal_lost) {
_had_manual_control = true;
}
if (_was_armed && _had_manual_control && status_flags.manual_control_signal_lost &&
if (_was_armed && _had_manual_control && failsafe_flags.manual_control_signal_lost &&
status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
play_tune();
_alarm_playing = true;

4
src/modules/events/rc_loss_alarm.h
View File

@ -42,7 +42,7 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/failsafe_flags.h>
#include <uORB/topics/tune_control.h>
namespace events
@ -68,7 +68,7 @@ private:
uORB::Publication<tune_control_s> _tune_control_pub{ORB_ID(tune_control)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
uORB::Subscription _failsafe_flags_sub{ORB_ID(failsafe_flags)};
bool _was_armed = false;
bool _had_manual_control = false; // Don't trigger alarm for systems without RC

4
src/modules/events/set_leds.cpp
View File

@ -54,8 +54,8 @@ namespace status
void StatusDisplay::set_leds()
{
bool gps_lock_valid = !_vehicle_status_flags_sub.get().global_position_invalid;
bool home_position_valid = !_vehicle_status_flags_sub.get().home_position_invalid;
bool gps_lock_valid = !_failsafe_flags_sub.get().global_position_invalid;
bool home_position_valid = !_failsafe_flags_sub.get().home_position_invalid;
int nav_state = _vehicle_status_sub.get().nav_state;
#if defined(BOARD_FRONT_LED_MASK)

2
src/modules/events/status_display.cpp
View File

@ -73,7 +73,7 @@ bool StatusDisplay::check_for_updates()
got_updates = true;
}
if (_vehicle_status_flags_sub.update()) {
if (_failsafe_flags_sub.update()) {
got_updates = true;
}

4
src/modules/events/status_display.h
View File

@ -48,7 +48,7 @@
#include <uORB/topics/cpuload.h>
#include <uORB/topics/led_control.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/failsafe_flags.h>
namespace events
{
@ -82,7 +82,7 @@ protected:
uORB::SubscriptionData<battery_status_s> _battery_status_sub{ORB_ID(battery_status)};
uORB::SubscriptionData<cpuload_s> _cpu_load_sub{ORB_ID(cpuload)};
uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionData<vehicle_status_flags_s> _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
uORB::SubscriptionData<failsafe_flags_s> _failsafe_flags_sub{ORB_ID(failsafe_flags)};
led_control_s _led_control{};

6
src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp
View File

@ -72,10 +72,10 @@ bool Sticks::checkAndUpdateStickInputs()
_input_available = valid_sticks;
} else {
vehicle_status_flags_s vehicle_status_flags;
failsafe_flags_s failsafe_flags;
if (_vehicle_status_flags_sub.update(&vehicle_status_flags)) {
if (vehicle_status_flags.manual_control_signal_lost) {
if (_failsafe_flags_sub.update(&failsafe_flags)) {
if (failsafe_flags.manual_control_signal_lost) {
_input_available = false;
}
}

4
src/modules/flight_mode_manager/tasks/Utility/Sticks.hpp
View File

@ -45,7 +45,7 @@
#include <matrix/matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/failsafe_flags.h>
class Sticks : public ModuleParams
{
@ -92,7 +92,7 @@ private:
matrix::Vector<float, 4> _positions_expo; ///< modified manual sticks using expo function
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
uORB::Subscription _failsafe_flags_sub{ORB_ID(failsafe_flags)};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz,

2
src/modules/logger/logged_topics.cpp
View File

@ -61,6 +61,7 @@ void LoggedTopics::add_default_topics()
add_topic("cpuload");
add_optional_topic("esc_status", 250);
add_topic("failure_detector_status", 100);
add_topic("failsafe_flags");
add_optional_topic("follow_target", 500);
add_optional_topic("follow_target_estimator", 200);
add_optional_topic("follow_target_status", 400);
@ -118,7 +119,6 @@ void LoggedTopics::add_default_topics()
add_topic("vehicle_rates_setpoint", 20);
add_topic("vehicle_roi", 1000);
add_topic("vehicle_status");
add_topic("vehicle_status_flags");
add_optional_topic("vtol_vehicle_status", 200);
add_topic("wind", 1000);

20
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
View File

@ -54,7 +54,7 @@
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/failsafe_flags.h>
#include <uORB/topics/health_report.h>
#include <px4_platform_common/events.h>
@ -135,7 +135,7 @@ private:
updated |= write_mission_result(&msg);
updated |= write_tecs_status(&msg);
updated |= write_vehicle_status(&msg);
updated |= write_vehicle_status_flags(&msg);
updated |= write_failsafe_flags(&msg);
updated |= write_wind(&msg);
if (updated) {
@ -443,24 +443,24 @@ private:
return false;
}
bool write_vehicle_status_flags(mavlink_high_latency2_t *msg)
bool write_failsafe_flags(mavlink_high_latency2_t *msg)
{
vehicle_status_flags_s status_flags;
failsafe_flags_s failsafe_flags;
if (_status_flags_sub.update(&status_flags)) {
if (status_flags.gps_position_invalid) {
if (_failsafe_flags_sub.update(&failsafe_flags)) {
if (failsafe_flags.gps_position_invalid) {
msg->failure_flags |= HL_FAILURE_FLAG_GPS;
}
if (status_flags.offboard_control_signal_lost) {
if (failsafe_flags.offboard_control_signal_lost) {
msg->failure_flags |= HL_FAILURE_FLAG_OFFBOARD_LINK;
}
if (status_flags.mission_failure) {
if (failsafe_flags.mission_failure) {
msg->failure_flags |= HL_FAILURE_FLAG_MISSION;
}
if (status_flags.manual_control_signal_lost) {
if (failsafe_flags.manual_control_signal_lost) {
msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER;
}
@ -634,7 +634,7 @@ private:
uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _status_flags_sub{ORB_ID(vehicle_status_flags)};
uORB::Subscription _failsafe_flags_sub{ORB_ID(failsafe_flags)};
uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
uORB::Subscription _wind_sub{ORB_ID(wind)};
uORB::Subscription _health_report_sub{ORB_ID(health_report)};

14
src/systemcmds/microbench/test_microbench_uorb.cpp
View File

@ -52,7 +52,7 @@
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/failsafe_flags.h>
namespace MicroBenchORB
{
@ -105,7 +105,7 @@ private:
void reset();
vehicle_status_flags_s status;
failsafe_flags_s status;
vehicle_local_position_s lpos;
sensor_gyro_s gyro;
sensor_gyro_fifo_s gyro_fifo;
@ -146,7 +146,7 @@ ut_declare_test_c(test_microbench_uorb, MicroBenchORB)
bool MicroBenchORB::time_px4_uorb()
{
int fd_status = orb_subscribe(ORB_ID(vehicle_status_flags));
int fd_status = orb_subscribe(ORB_ID(failsafe_flags));
int fd_lpos = orb_subscribe(ORB_ID(vehicle_local_position));
int fd_gyro = orb_subscribe(ORB_ID(sensor_gyro));
int fd_gyro_fifo = orb_subscribe(ORB_ID(sensor_gyro_fifo));
@ -155,7 +155,7 @@ bool MicroBenchORB::time_px4_uorb()
bool updated = false;
PERF("orb_check vehicle_status", ret = orb_check(fd_status, &updated), 100);
PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status_flags), fd_status, &status), 100);
PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(failsafe_flags), fd_status, &status), 100);
printf("\n");
@ -198,9 +198,9 @@ bool MicroBenchORB::time_px4_uorb_direct()
{
bool ret = false;
uORB::Subscription vehicle_status_flags{ORB_ID(vehicle_status_flags)};
PERF("uORB::Subscription orb_check vehicle_status", ret = vehicle_status_flags.updated(), 100);
PERF("uORB::Subscription orb_copy vehicle_status", ret = vehicle_status_flags.copy(&status), 100);
uORB::Subscription failsafe_flags{ORB_ID(failsafe_flags)};
PERF("uORB::Subscription orb_check vehicle_status", ret = failsafe_flags.updated(), 100);
PERF("uORB::Subscription orb_copy vehicle_status", ret = failsafe_flags.copy(&status), 100);
printf("\n");