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

mode_requirements: add manual control for manual modes

This commit is contained in:
Matthias Grob 2022-11-30 16:17:32 +01:00 committed by Beat Küng
parent 636dfdec6a
commit f498b90c41
4 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/FailsafeFlags.msg
View File

@ -17,6 +17,7 @@ uint32 mode_req_offboard_signal
uint32 mode_req_home_position
uint32 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded
uint32 mode_req_prevent_arming # if set, cannot arm while in this mode
uint32 mode_req_manual_control
uint32 mode_req_other # other requirements, not covered above (for external modes)

15
src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp
View File

@ -143,6 +143,21 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_home_position);
}
if (reporter.failsafeFlags().manual_control_signal_lost && reporter.failsafeFlags().mode_req_manual_control != 0) {
/* EVENT
* @description
* Connect and enable stick input or use autonomous mode.
* <profile name="dev">
* Sticks can be enabled via <param>COM_RC_IN_MODE</param> parameter.
* </profile>
*/
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_manual_control,
health_component_t::remote_control,
events::ID("check_modes_manual_control"),
events::Log::Critical, "No manual control input");
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_manual_control);
}
if (reporter.failsafeFlags().mode_req_other != 0) {
// Here we expect there is already an event reported for the failing check (this is for external modes)
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_other);

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

@ -56,19 +56,23 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
flags.mode_req_home_position = 0;
flags.mode_req_wind_and_flight_time_compliance = 0;
flags.mode_req_prevent_arming = 0;
flags.mode_req_manual_control = 0;
flags.mode_req_other = 0;
// NAVIGATION_STATE_MANUAL
setRequirement(vehicle_status_s::NAVIGATION_STATE_MANUAL, flags.mode_req_manual_control);
// NAVIGATION_STATE_ALTCTL
setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_angular_velocity);
setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_attitude);
setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_local_alt);
setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_manual_control);
// NAVIGATION_STATE_POSCTL
setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_angular_velocity);
setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_attitude);
setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_position_relaxed);
setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_manual_control);
if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_global_position);
@ -104,6 +108,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
// NAVIGATION_STATE_ACRO
setRequirement(vehicle_status_s::NAVIGATION_STATE_ACRO, flags.mode_req_angular_velocity);
setRequirement(vehicle_status_s::NAVIGATION_STATE_ACRO, flags.mode_req_manual_control);
// NAVIGATION_STATE_DESCEND
setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_angular_velocity);
@ -122,6 +127,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
// NAVIGATION_STATE_STAB
setRequirement(vehicle_status_s::NAVIGATION_STATE_STAB, flags.mode_req_angular_velocity);
setRequirement(vehicle_status_s::NAVIGATION_STATE_STAB, flags.mode_req_attitude);
setRequirement(vehicle_status_s::NAVIGATION_STATE_STAB, flags.mode_req_manual_control);
// NAVIGATION_STATE_AUTO_TAKEOFF
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_angular_velocity);

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

@ -656,6 +656,7 @@ bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode
(!status_flags.auto_mission_missing || ((status_flags.mode_req_mission & mode_mask) == 0)) &&
(!status_flags.offboard_control_signal_lost || ((status_flags.mode_req_offboard_signal & mode_mask) == 0)) &&
(!status_flags.home_position_invalid || ((status_flags.mode_req_home_position & mode_mask) == 0)) &&
(!status_flags.manual_control_signal_lost || ((status_flags.mode_req_manual_control & mode_mask) == 0)) &&
((status_flags.mode_req_other & mode_mask) == 0);
}