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

refactor commander: rename rc_signal_lost -> manual_control_signal_lost, data_link_lost -> gcs_connection_lost

This commit is contained in:
Beat Küng 2022-09-19 13:43:38 +02:00 committed by Daniel Agar
parent e2d8ca73a5
commit 38d3739b6d
14 changed files with 75 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/vehicle_status.msg
View File

@ -87,8 +87,8 @@ bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate,
bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control
# Link loss
bool data_link_lost # datalink to GCS lost
uint8 data_link_lost_counter # counts unique data link lost events
bool gcs_connection_lost # datalink to GCS lost
uint8 gcs_connection_lost_counter # counts unique GCS connection lost events
bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost
# VTOL flags

4
msg/vehicle_status_flags.msg
View File

@ -33,8 +33,8 @@ bool offboard_control_signal_lost # Offboard signal lost
bool home_position_invalid # No home position available
# Control links
bool rc_signal_lost # RC signal lost
bool data_link_lost # Data link lost
bool manual_control_signal_lost # Manual control (RC) signal lost
bool gcs_connection_lost # GCS connection lost
# Battery
uint8 battery_warning # Battery warning level

6
src/lib/events/enums.json
View File

@ -285,12 +285,12 @@
"description": ""
},
"1": {
"name": "rc_loss",
"name": "manual_control_loss",
"description": "manual control loss"
},
"2": {
"name": "datalink_loss",
"description": "data link loss"
"name": "gcs_connection_loss",
"description": "GCS connection loss"
},
"3": {
"name": "low_battery_level",

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.rc_signal_lost && _is_throttle_above_center) {
!_vehicle_status_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.rc_signal_lost && !_is_throttle_low
!_vehicle_status_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"),
@ -654,7 +654,7 @@ Commander::Commander() :
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
/* mark all signals lost as long as they haven't been found */
_vehicle_status.data_link_lost = true;
_vehicle_status.gcs_connection_lost = true;
_vehicle_status.power_input_valid = true;
@ -2548,13 +2548,13 @@ void Commander::dataLinkCheck()
if (telemetry.heartbeat_type_gcs) {
// Initial connection or recovery from data link lost
if (_vehicle_status.data_link_lost) {
_vehicle_status.data_link_lost = false;
if (_vehicle_status.gcs_connection_lost) {
_vehicle_status.gcs_connection_lost = false;
_status_changed = true;
if (_datalink_last_heartbeat_gcs != 0) {
mavlink_log_info(&_mavlink_log_pub, "Data link regained\t");
events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained");
mavlink_log_info(&_mavlink_log_pub, "GCS connection regained\t");
events::send(events::ID("commander_dl_regained"), events::Log::Info, "GCS connection regained");
}
}
@ -2623,16 +2623,16 @@ void Commander::dataLinkCheck()
// GCS data link loss failsafe
if (!_vehicle_status.data_link_lost) {
if (!_vehicle_status.gcs_connection_lost) {
if ((_datalink_last_heartbeat_gcs != 0)
&& hrt_elapsed_time(&_datalink_last_heartbeat_gcs) > (_param_com_dl_loss_t.get() * 1_s)) {
_vehicle_status.data_link_lost = true;
_vehicle_status.data_link_lost_counter++;
_vehicle_status.gcs_connection_lost = true;
_vehicle_status.gcs_connection_lost_counter++;
mavlink_log_info(&_mavlink_log_pub, "Connection to ground station lost\t");
events::send(events::ID("commander_gcs_lost"), {events::Log::Warning, events::LogInternal::Info},
"Connection to ground station lost");
"Connection to ground control station lost");
_status_changed = true;
}

30
src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp
View File

@ -41,7 +41,7 @@ void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporte
bool rc_is_optional = true;
if (_param_com_rc_in_mode.get() == 4) { // RC disabled
reporter.failsafeFlags().rc_signal_lost = false;
reporter.failsafeFlags().manual_control_signal_lost = false;
} else {
@ -49,36 +49,36 @@ void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporte
if (!_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
manual_control_setpoint = {};
reporter.failsafeFlags().rc_signal_lost = true;
reporter.failsafeFlags().manual_control_signal_lost = true;
}
// Check if RC is valid
if (!manual_control_setpoint.valid
|| hrt_elapsed_time(&manual_control_setpoint.timestamp) > _param_com_rc_loss_t.get() * 1_s) {
if (!reporter.failsafeFlags().rc_signal_lost && _last_valid_manual_control_setpoint > 0) {
if (!reporter.failsafeFlags().manual_control_signal_lost && _last_valid_manual_control_setpoint > 0) {
events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info},
"Manual control lost");
}
reporter.failsafeFlags().rc_signal_lost = true;
reporter.failsafeFlags().manual_control_signal_lost = true;
} else {
reporter.setIsPresent(health_component_t::remote_control);
if (reporter.failsafeFlags().rc_signal_lost && _last_valid_manual_control_setpoint > 0) {
if (reporter.failsafeFlags().manual_control_signal_lost && _last_valid_manual_control_setpoint > 0) {
float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f;
events::send<float>(events::ID("commander_rc_regained"), events::Log::Info,
"Manual control regained after {1:.1} s", elapsed);
}
reporter.failsafeFlags().rc_signal_lost = false;
reporter.failsafeFlags().manual_control_signal_lost = false;
_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
}
if (reporter.failsafeFlags().rc_signal_lost) {
if (reporter.failsafeFlags().manual_control_signal_lost) {
NavModes affected_modes = rc_is_optional ? NavModes::None : NavModes::All;
events::LogLevel log_level = rc_is_optional ? events::Log::Info : events::Log::Error;
@ -97,16 +97,16 @@ void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporte
}
}
// Data Link
reporter.failsafeFlags().data_link_lost = context.status().data_link_lost;
// GCS connection
reporter.failsafeFlags().gcs_connection_lost = context.status().gcs_connection_lost;
if (reporter.failsafeFlags().data_link_lost) {
if (reporter.failsafeFlags().gcs_connection_lost) {
// Prevent arming if we neither have RC nor a Data Link. TODO: disabled for now due to MAVROS tests
bool data_link_required = _param_nav_dll_act.get() > 0
/*|| (rc_is_optional && reporter.failsafeFlags().rc_signal_lost) */;
NavModes affected_modes = data_link_required ? NavModes::All : NavModes::None;
events::LogLevel log_level = data_link_required ? events::Log::Error : events::Log::Info;
// Prevent arming if we neither have RC nor a GCS connection. TODO: disabled for now due to MAVROS tests
bool gcs_connection_required = _param_nav_dll_act.get() > 0
/*|| (rc_is_optional && reporter.failsafeFlags().manual_control_signal_lost) */;
NavModes affected_modes = gcs_connection_required ? NavModes::All : NavModes::None;
events::LogLevel log_level = gcs_connection_required ? events::Log::Error : events::Log::Info;
/* EVENT
* @description
* To arm, at least a data link or manual control (RC) must be present.

8
src/modules/commander/commander_params.c
View File

@ -90,9 +90,9 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
/**
* Datalink loss time threshold
* GCS connection loss time threshold
*
* After this amount of seconds without datalink the data link lost mode triggers
* After this amount of seconds without datalink, the GCS connection lost mode triggers
*
* @group Commander
* @unit s
@ -785,9 +785,9 @@ PARAM_DEFINE_INT32(COM_FLIGHT_UUID, 0);
PARAM_DEFINE_INT32(COM_TAKEOFF_ACT, 0);
/**
* Set data link loss failsafe mode
* Set GCS connection loss failsafe mode
*
* The data link loss failsafe will only be entered after a timeout,
* The GCS connection loss failsafe will only be entered after a timeout,
* set by COM_DL_LOSS_T in seconds. Once the timeout occurs the selected
* action will be executed.
*

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

@ -336,36 +336,36 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
const bool ignore_link_failsafe = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF
&& in_forward_flight && !state.mission_finished;
// RC loss
if (!status_flags.rc_signal_lost) {
// If RC was lost and arming was allowed, consider it optional until we regain RC
_rc_lost_at_arming = false;
// Manual control (RC) loss
if (!status_flags.manual_control_signal_lost) {
// If manual control was lost and arming was allowed, consider it optional until we regain manual control
_manual_control_lost_at_arming = false;
}
const bool rc_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
&& (_param_com_rcl_except.get() & (int)RCLossExceptionBits::Mission);
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Mission);
const bool rc_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
&& (_param_com_rcl_except.get() & (int)RCLossExceptionBits::Hold);
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold);
const bool rc_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
&& (_param_com_rcl_except.get() & (int)RCLossExceptionBits::Offboard);
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Offboard);
const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
&& (_param_com_rcl_except.get() & (int)RCLossExceptionBits::Hold);
&& (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold);
const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard ||
rc_loss_ignored_takeoff || ignore_link_failsafe || _rc_lost_at_arming;
rc_loss_ignored_takeoff || ignore_link_failsafe || _manual_control_lost_at_arming;
if (_param_com_rc_in_mode.get() != 4 && !rc_loss_ignored) {
CHECK_FAILSAFE(status_flags, rc_signal_lost,
fromNavDllOrRclActParam(_param_nav_rcl_act.get()).causedBy(Cause::RCLoss));
CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
fromNavDllOrRclActParam(_param_nav_rcl_act.get()).causedBy(Cause::ManualControlLoss));
}
// Datalink loss
const bool data_link_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND ||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || ignore_link_failsafe;
// GCS connection loss
const bool gcs_connection_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND ||
state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || ignore_link_failsafe;
if (_param_nav_dll_act.get() != 0 && !data_link_loss_ignored) {
CHECK_FAILSAFE(status_flags, data_link_lost,
fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::DatalinkLoss));
if (_param_nav_dll_act.get() != 0 && !gcs_connection_loss_ignored) {
CHECK_FAILSAFE(status_flags, gcs_connection_lost,
fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss));
}
// VTOL transition failure (quadchute)
@ -380,12 +380,12 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
CHECK_FAILSAFE(status_flags, mission_failure, Action::RTL);
// If RC loss and datalink loss are disabled and we lose both command links and the mission finished,
// If manual control loss and GCS connection loss are disabled and we lose both command links and the mission finished,
// trigger RTL to avoid losing the vehicle
if ((_param_com_rc_in_mode.get() == 4 || rc_loss_ignored_mission) && _param_nav_dll_act.get() == 0
&& state.mission_finished) {
_last_state_mission_control_lost = checkFailsafe(_caller_id_mission_control_lost, _last_state_mission_control_lost,
status_flags.data_link_lost, Action::RTL);
status_flags.gcs_connection_lost, Action::RTL);
}
}
@ -449,10 +449,10 @@ void Failsafe::updateArmingState(const hrt_abstime &time_us, bool armed, const v
{
if (!_was_armed && armed) {
_armed_time = time_us;
_rc_lost_at_arming = status_flags.rc_signal_lost;
_manual_control_lost_at_arming = status_flags.manual_control_signal_lost;
} else if (!armed) {
_rc_lost_at_arming = status_flags.rc_signal_lost; // ensure action isn't added while disarmed
_manual_control_lost_at_arming = status_flags.manual_control_signal_lost; // ensure action isn't added while disarmed
_armed_time = 0;
}

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

@ -53,7 +53,7 @@ protected:
private:
void updateArmingState(const hrt_abstime &time_us, bool armed, const vehicle_status_flags_s &status_flags);
enum class RCLossExceptionBits : int32_t {
enum class ManualControlLossExceptionBits : int32_t {
Mission = (1 << 0),
Hold = (1 << 1),
Offboard = (1 << 2)
@ -90,7 +90,7 @@ private:
hrt_abstime _armed_time{0};
bool _was_armed{false};
bool _rc_lost_at_arming{false}; ///< true if RC was lost at arming time
bool _manual_control_lost_at_arming{false}; ///< true if manual control was lost at arming time
DEFINE_PARAMETERS_CUSTOM_PARENT(FailsafeBase,
(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act,

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

@ -50,8 +50,9 @@ protected:
void checkStateAndMode(const hrt_abstime &time_us, const State &state,
const vehicle_status_flags_s &status_flags) override
{
CHECK_FAILSAFE(status_flags, rc_signal_lost, ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
CHECK_FAILSAFE(status_flags, data_link_lost, Action::Descend);
CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
ActionOptions(Action::RTL).clearOn(ClearCondition::OnModeChangeOrDisarm));
CHECK_FAILSAFE(status_flags, gcs_connection_lost, Action::Descend);
if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
CHECK_FAILSAFE(status_flags, mission_failure, Action::Descend);
@ -110,7 +111,7 @@ TEST_F(FailsafeTest, general)
// RC lost -> Hold, then RTL
time += 10_ms;
failsafe_flags.rc_signal_lost = true;
failsafe_flags.manual_control_signal_lost = true;
updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
@ -121,21 +122,21 @@ TEST_F(FailsafeTest, general)
// DL link lost -> Descend
time += 10_ms;
failsafe_flags.data_link_lost = true;
failsafe_flags.gcs_connection_lost = true;
updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Descend);
// DL link regained -> RTL (RC still lost)
time += 10_ms;
failsafe_flags.data_link_lost = false;
failsafe_flags.gcs_connection_lost = false;
updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
// RC lost cleared -> keep RTL
time += 10_ms;
failsafe_flags.rc_signal_lost = false;
failsafe_flags.manual_control_signal_lost = false;
updated_user_intented_mode = failsafe.update(time, state, false, stick_override_request, failsafe_flags);
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);

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

@ -74,8 +74,8 @@ public:
enum class Cause : uint8_t {
Generic,
RCLoss,
DatalinkLoss,
ManualControlLoss,
GCSConnectionLoss,
BatteryLow,
BatteryCritical,
BatteryEmergency,

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

@ -68,12 +68,12 @@ void RC_Loss_Alarm::process()
_was_armed = true; // Once true, impossible to go back to false
}
if (!_had_rc && !status_flags.rc_signal_lost) {
if (!_had_manual_control && !status_flags.manual_control_signal_lost) {
_had_rc = true;
_had_manual_control = true;
}
if (_was_armed && _had_rc && status_flags.rc_signal_lost &&
if (_was_armed && _had_manual_control && status_flags.manual_control_signal_lost &&
status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
play_tune();
_alarm_playing = true;

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

@ -71,7 +71,7 @@ private:
uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
bool _was_armed = false;
bool _had_rc = false; // Don't trigger alarm for systems without RC
bool _had_manual_control = false; // Don't trigger alarm for systems without RC
bool _alarm_playing = false;
};

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

@ -75,7 +75,7 @@ bool Sticks::checkAndUpdateStickInputs()
vehicle_status_flags_s vehicle_status_flags;
if (_vehicle_status_flags_sub.update(&vehicle_status_flags)) {
if (vehicle_status_flags.rc_signal_lost) {
if (vehicle_status_flags.manual_control_signal_lost) {
_input_available = false;
}
}

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

@ -460,7 +460,7 @@ private:
msg->failure_flags |= HL_FAILURE_FLAG_MISSION;
}
if (status_flags.rc_signal_lost) {
if (status_flags.manual_control_signal_lost) {
msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER;
}