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

commander: implement external modes and mode executors

This commit is contained in:
Beat Küng 2022-11-07 15:57:51 +01:00
parent ab71ef4447
commit af62227a83
No known key found for this signature in database
GPG Key ID: 866DB5F0E24821BB
32 changed files with 1599 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
msg/ArmingCheckReply.msg Normal file
View File

@ -0,0 +1,33 @@
uint64 timestamp # time since system start (microseconds)
uint8 request_id
uint8 registration_id
uint8 HEALTH_COMPONENT_INDEX_NONE = 0
uint8 HEALTH_COMPONENT_INDEX_AVOIDANCE = 19
uint8 health_component_index # HEALTH_COMPONENT_INDEX_*
bool health_component_is_present
bool health_component_warning
bool health_component_error
bool can_arm_and_run # whether arming is possible, and if it's a navigation mode, if it can run
uint8 num_events
Event[5] events
# Mode requirements
bool mode_req_angular_velocity
bool mode_req_attitude
bool mode_req_local_alt
bool mode_req_local_position
bool mode_req_local_position_relaxed
bool mode_req_global_position
bool mode_req_mission
bool mode_req_home_position
bool mode_req_prevent_arming
uint8 ORB_QUEUE_LENGTH = 4

7
msg/ArmingCheckRequest.msg Normal file
View File

@ -0,0 +1,7 @@
uint64 timestamp # time since system start (microseconds)
# broadcast message to request all registered arming checks to be reported
uint8 request_id

5
msg/CMakeLists.txt
View File

@ -49,6 +49,8 @@ set(msg_files
Airspeed.msg
AirspeedValidated.msg
AirspeedWind.msg
ArmingCheckReply.msg
ArmingCheckRequest.msg
AutotuneAttitudeControlStatus.msg
BatteryStatus.msg
ButtonEvent.msg
@ -154,6 +156,8 @@ set(msg_files
RateCtrlStatus.msg
RcChannels.msg
RcParameterMap.msg
RegisterExtComponentReply.msg
RegisterExtComponentRequest.msg
Rpm.msg
RtlTimeEstimate.msg
SatelliteInfo.msg
@ -190,6 +194,7 @@ set(msg_files
UavcanParameterValue.msg
UlogStream.msg
UlogStreamAck.msg
UnregisterExtComponent.msg
UwbDistance.msg
UwbGrid.msg
VehicleAcceleration.msg

12
msg/RegisterExtComponentReply.msg Normal file
View File

@ -0,0 +1,12 @@
uint64 timestamp # time since system start (microseconds)
uint64 request_id # ID from the request
char[25] name # name from the request
bool success
int8 arming_check_id # arming check registration ID (-1 if invalid)
int8 mode_id # assigned mode ID (-1 if invalid)
int8 mode_executor_id # assigned mode executor ID (-1 if invalid)
uint8 ORB_QUEUE_LENGTH = 2

17
msg/RegisterExtComponentRequest.msg Normal file
View File

@ -0,0 +1,17 @@
# Request to register an external component
uint64 timestamp # time since system start (microseconds)
uint64 request_id # ID, set this to a random value
char[25] name # either the requested mode name, or component name
# Components to be registered
bool register_arming_check
bool register_mode # registering a mode also requires arming_check to be set
bool register_mode_executor # registering an executor also requires a mode to be registered (which is the owned mode by the executor)
bool enable_replace_internal_mode # set to true if an internal mode should be replaced
uint8 replace_internal_mode # vehicle_status::NAVIGATION_STATE_*
bool activate_mode_immediately # switch to the registered mode (can only be set in combination with an executor)
uint8 ORB_QUEUE_LENGTH = 2

10
msg/UnregisterExtComponent.msg Normal file
View File

@ -0,0 +1,10 @@
uint64 timestamp # time since system start (microseconds)
char[25] name # either the mode name, or component name
int8 arming_check_id # arming check registration ID (-1 if not registered)
int8 mode_id # assigned mode ID (-1 if not registered)
int8 mode_executor_id # assigned mode executor ID (-1 if not registered)

6
msg/VehicleCommand.msg
View File

@ -173,8 +173,10 @@ uint32 command # Command ID
uint8 target_system # System which should execute the command
uint8 target_component # Component which should execute the command, 0 for all components
uint8 source_system # System sending the command
uint8 source_component # Component sending the command
uint16 source_component # Component / mode executor sending the command
uint8 confirmation # 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
bool from_external
# TOPICS vehicle_command gimbal_v1_command
uint16 COMPONENT_MODE_EXECUTOR_START = 1000
# TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor

2
msg/VehicleCommandAck.msg
View File

@ -28,6 +28,6 @@ uint8 result # Command result
uint8 result_param1 # Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS
int32 result_param2 # Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied.
uint8 target_system
uint8 target_component
uint16 target_component # Target component / mode executor
bool from_external # Indicates if the command came from an external source

5
msg/VehicleControlMode.msg
View File

@ -14,3 +14,8 @@ bool flag_control_position_enabled # true if position is controlled
bool flag_control_altitude_enabled # true if altitude is controlled
bool flag_control_climb_rate_enabled # true if climb rate is controlled
bool flag_control_termination_enabled # true if flighttermination is enabled
# TODO: use dedicated topic for external requests
uint8 source_id # Mode ID (nav_state)
# TOPICS vehicle_control_mode config_control_setpoints

12
msg/VehicleStatus.msg
View File

@ -57,7 +57,17 @@ uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow
uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target
uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle
uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter
uint8 NAVIGATION_STATE_MAX = 23
uint8 NAVIGATION_STATE_EXTERNAL1 = 23
uint8 NAVIGATION_STATE_EXTERNAL2 = 24
uint8 NAVIGATION_STATE_EXTERNAL3 = 25
uint8 NAVIGATION_STATE_EXTERNAL4 = 26
uint8 NAVIGATION_STATE_EXTERNAL5 = 27
uint8 NAVIGATION_STATE_EXTERNAL6 = 28
uint8 NAVIGATION_STATE_EXTERNAL7 = 29
uint8 NAVIGATION_STATE_EXTERNAL8 = 30
uint8 NAVIGATION_STATE_MAX = 31
uint8 executor_in_charge # Current mode executor in charge (0=Autopilot)
# Bitmask of detected failures
uint16 failure_detector_status

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

@ -110,6 +110,38 @@
"4194304": {
"name": "vtol_takeoff",
"description": "VTOL Takeoff"
},
"8388608": {
"name": "external1",
"description": "External 1"
},
"16777216": {
"name": "external2",
"description": "External 2"
},
"33554432": {
"name": "external3",
"description": "External 3"
},
"67108864": {
"name": "external4",
"description": "External 4"
},
"134217728": {
"name": "external5",
"description": "External 5"
},
"268435456": {
"name": "external6",
"description": "External 6"
},
"536870912": {
"name": "external7",
"description": "External 7"
},
"1073741824": {
"name": "external8",
"description": "External 8"
}
}
},
@ -528,6 +560,38 @@
"name": "auto_vtol_takeoff",
"description": "Vtol Takeoff"
},
"16": {
"name": "external1",
"description": "External 1"
},
"17": {
"name": "external2",
"description": "External 2"
},
"18": {
"name": "external3",
"description": "External 3"
},
"19": {
"name": "external4",
"description": "External 4"
},
"20": {
"name": "external5",
"description": "External 5"
},
"21": {
"name": "external6",
"description": "External 6"
},
"22": {
"name": "external7",
"description": "External 7"
},
"23": {
"name": "external8",
"description": "External 8"
},
"255": {
"name": "unknown",
"description": "[Unknown]"
@ -701,7 +765,15 @@
"19": [134479872],
"20": [151257088],
"21": [16973824],
"22": [168034304]
"22": [168034304],
"23": [184811520],
"24": [201588736],
"25": [218365952],
"26": [235143168],
"27": [251920384],
"28": [268697600],
"29": [285474816],
"30": [302252032]
}
},
"supported_protocols": [ "health_and_arming_check" ]

1
src/modules/commander/CMakeLists.txt
View File

@ -52,6 +52,7 @@ px4_add_module(
factory_calibration_storage.cpp
gyro_calibration.cpp
HomePosition.cpp
ModeManagement.cpp
UserModeIntention.cpp
level_calibration.cpp
lm_fit.cpp

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

@ -407,6 +407,10 @@ int Commander::custom_command(int argc, char *argv[])
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND);
} else if (!strcmp(argv[1], "ext1")) {
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_SUB_MODE_EXTERNAL1);
} else {
PX4_ERR("argument %s unsupported.", argv[1]);
}
@ -475,8 +479,9 @@ int Commander::print_status()
{
PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state_user_intention]);
PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
_mode_management.printStatus();
perf_print_counter(_loop_perf);
perf_print_counter(_preflight_check_perf);
return 0;
@ -698,7 +703,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
// Check if a mode switch had been requested
if ((((uint32_t)cmd.param2) & 1) > 0) {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, getSourceFromCommand(cmd))) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
@ -781,6 +786,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
break;
case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8:
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1);
break;
default:
main_ret = TRANSITION_DENIED;
mavlink_log_critical(&_mavlink_log_pub, "Unsupported auto mode\t");
@ -822,7 +831,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
}
if (desired_nav_state != vehicle_status_s::NAVIGATION_STATE_MAX) {
if (_user_mode_intention.change(desired_nav_state)) {
if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) {
main_ret = TRANSITION_CHANGED;
} else {
@ -974,7 +983,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
/* switch to RTL which ends the mission */
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, getSourceFromCommand(cmd))) {
mavlink_log_info(&_mavlink_log_pub, "Returning to launch\t");
events::send(events::ID("commander_rtl"), events::Log::Info, "Returning to launch");
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
@ -988,7 +997,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
/* ok, home set, use it to take off */
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)) {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, getSourceFromCommand(cmd))) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
@ -1001,7 +1010,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF:
/* ok, home set, use it to take off */
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)) {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, getSourceFromCommand(cmd))) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
@ -1012,7 +1021,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
break;
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd))) {
mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t");
events::send(events::ID("commander_landing_current_pos"), events::Log::Info,
"Landing at current position");
@ -1026,7 +1035,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
break;
case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND)) {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, getSourceFromCommand(cmd))) {
mavlink_log_info(&_mavlink_log_pub, "Precision landing\t");
events::send(events::ID("commander_landing_prec_land"), events::Log::Info,
"Landing using precision landing");
@ -1050,7 +1059,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
// switch to AUTO_MISSION and ARM
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, getSourceFromCommand(cmd))
&& (TRANSITION_DENIED != arm(arm_disarm_reason_t::mission_start))) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
@ -1089,7 +1098,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
// for fixed wings the behavior of orbit is the same as loiter
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, getSourceFromCommand(cmd))) {
main_ret = TRANSITION_CHANGED;
} else {
@ -1098,7 +1107,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else {
// Switch to orbit state and let the orbit task handle the command further
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT)) {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT, getSourceFromCommand(cmd))) {
main_ret = TRANSITION_CHANGED;
} else {
@ -1422,6 +1431,43 @@ Commander::handle_command(const vehicle_command_s &cmd)
return true;
}
ModeChangeSource Commander::getSourceFromCommand(const vehicle_command_s &cmd)
{
return cmd.source_component >= vehicle_command_s::COMPONENT_MODE_EXECUTOR_START ? ModeChangeSource::ModeExecutor :
ModeChangeSource::User;
}
void Commander::handleCommandsFromModeExecutors()
{
if (_vehicle_command_mode_executor_sub.updated()) {
const unsigned last_generation = _vehicle_command_mode_executor_sub.get_last_generation();
vehicle_command_s cmd;
if (_vehicle_command_mode_executor_sub.copy(&cmd)) {
if (_vehicle_command_mode_executor_sub.get_last_generation() != last_generation + 1) {
PX4_ERR("vehicle_command from executor lost, generation %u -> %u", last_generation,
_vehicle_command_mode_executor_sub.get_last_generation());
}
// For commands from mode executors, we check if it is in charge and then publish it on the official
// command topic
const int mode_executor_in_charge = _mode_management.modeExecutorInCharge();
// source_system is set to the mode executor
if (cmd.source_component == vehicle_command_s::COMPONENT_MODE_EXECUTOR_START + mode_executor_in_charge) {
cmd.source_system = _vehicle_status.system_id;
cmd.timestamp = hrt_absolute_time();
_vehicle_command_pub.publish(cmd);
} else {
cmd.source_system = _vehicle_status.system_id;
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
PX4_WARN("Got cmd from executor %i not in charge (in charge: %i)", cmd.source_system, mode_executor_in_charge);
}
}
}
}
unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
{
if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
@ -1547,7 +1593,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
case action_request_s::ACTION_SWITCH_MODE:
if (!_user_mode_intention.change(action_request.mode, true)) {
if (!_user_mode_intention.change(action_request.mode, ModeChangeSource::User, true)) {
printRejectMode(action_request.mode);
}
@ -1703,6 +1749,8 @@ void Commander::run()
_status_changed = true;
}
modeManagementUpdate();
const hrt_abstime now = hrt_absolute_time();
const bool nav_state_or_failsafe_changed = handleModeIntentionAndFailsafe();
@ -1725,6 +1773,8 @@ void Commander::run()
}
// handle commands last, as the system needs to be updated to handle them
handleCommandsFromModeExecutors();
if (_vehicle_command_sub.updated()) {
// got command
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
@ -1795,6 +1845,7 @@ void Commander::run()
updateControlMode();
// vehicle_status publish (after prearm/preflight updates above)
_vehicle_status.executor_in_charge = _mode_management.modeExecutorInCharge();
_vehicle_status.arming_state = _arm_state_machine.getArmState();
_vehicle_status.timestamp = hrt_absolute_time();
_vehicle_status_pub.publish(_vehicle_status);
@ -1879,7 +1930,8 @@ void Commander::checkForMissionUpdate()
if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed
&& (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
&& mission_result.finished) {
&& mission_result.finished
&& _mode_management.modeExecutorInCharge() == ModeExecutors::AUTOPILOT_EXECUTOR_ID) {
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
@ -2195,13 +2247,15 @@ bool Commander::handleModeIntentionAndFailsafe()
// Force intended mode if changed by the failsafe state machine
if (state.user_intended_mode != updated_user_intented_mode) {
_user_mode_intention.change(updated_user_intented_mode, false, true);
_user_mode_intention.change(updated_user_intented_mode, ModeChangeSource::User, false, true);
_user_mode_intention.getHadModeChangeAndClear();
}
// Handle failsafe action
_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
_vehicle_status.nav_state = FailsafeBase::modeFromAction(_failsafe.selectedAction(), _user_mode_intention.get());
_vehicle_status.nav_state_user_intention = _mode_management.getNavStateReplacementIfValid(_user_mode_intention.get(),
false);
_vehicle_status.nav_state = _mode_management.getNavStateReplacementIfValid(FailsafeBase::modeFromAction(
_failsafe.selectedAction(), _user_mode_intention.get()));
switch (_failsafe.selectedAction()) {
case FailsafeBase::Action::Disarm:
@ -2250,6 +2304,21 @@ void Commander::checkAndInformReadyForTakeoff()
#endif // CONFIG_ARCH_BOARD_PX4_SITL
}
void Commander::modeManagementUpdate()
{
ModeManagement::UpdateRequest mode_management_update{};
_mode_management.update(_arm_state_machine.isArmed(), _vehicle_status.nav_state_user_intention,
_failsafe.selectedAction() > FailsafeBase::Action::Warn, mode_management_update);
if (!_arm_state_machine.isArmed() && mode_management_update.change_user_intended_nav_state) {
_user_mode_intention.change(mode_management_update.user_intended_nav_state);
}
if (mode_management_update.control_setpoint_update) {
_status_changed = true;
}
}
void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
{
switch (blink_msg_state()) {
@ -2404,8 +2473,19 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
void Commander::updateControlMode()
{
_vehicle_control_mode = {};
mode_util::getVehicleControlMode(_arm_state_machine.isArmed(), _vehicle_status.nav_state,
mode_util::getVehicleControlMode(_vehicle_status.nav_state,
_vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode);
_mode_management.updateControlMode(_vehicle_status.nav_state, _vehicle_control_mode);
_vehicle_control_mode.flag_armed = _arm_state_machine.isArmed();
_vehicle_control_mode.flag_multicopter_position_control_enabled =
(_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
&& (_vehicle_control_mode.flag_control_altitude_enabled
|| _vehicle_control_mode.flag_control_climb_rate_enabled
|| _vehicle_control_mode.flag_control_position_enabled
|| _vehicle_control_mode.flag_control_velocity_enabled
|| _vehicle_control_mode.flag_control_acceleration_enabled);
_vehicle_control_mode.timestamp = hrt_absolute_time();
_vehicle_control_mode_pub.publish(_vehicle_control_mode);
}
@ -2755,7 +2835,7 @@ void Commander::manualControlCheck()
if (override_enabled) {
// If no failsafe is active, directly change the mode, otherwise pass the request to the failsafe state machine
if (_failsafe.selectedAction() <= FailsafeBase::Action::Warn) {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, true)) {
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, true)) {
tune_positive(true);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over using sticks\t");
events::send(events::ID("commander_rc_override"), events::Log::Info, "Pilot took over using sticks");
@ -2772,7 +2852,7 @@ void Commander::manualControlCheck()
// if there's never been a mode change force position control as initial state
if (!_user_mode_intention.everHadModeChange() && (is_mavlink || !_mode_switch_mapped)) {
_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, false, true);
_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, false, true);
}
}
}
@ -2835,7 +2915,7 @@ The commander module contains the state machine for mode switching and failsafe
PRINT_MODULE_USAGE_COMMAND("land");
PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition");
PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode");
PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland",
PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1",
"Flight mode", false);
PRINT_MODULE_USAGE_COMMAND("pair");
PRINT_MODULE_USAGE_COMMAND("lockdown");

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

@ -41,6 +41,7 @@
#include "worker_thread.hpp"
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
#include "HomePosition.hpp"
#include "ModeManagement.hpp"
#include "UserModeIntention.hpp"
#include <lib/controllib/blocks.hpp>
@ -116,6 +117,8 @@ public:
void enable_hil();
private:
static ModeChangeSource getSourceFromCommand(const vehicle_command_s &cmd);
void answer_command(const vehicle_command_s &cmd, uint8_t result);
transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
@ -183,6 +186,10 @@ private:
void checkAndInformReadyForTakeoff();
void handleCommandsFromModeExecutors();
void modeManagementUpdate();
enum class PrearmedMode {
DISABLED = 0,
SAFETY_BUTTON = 1,
@ -206,8 +213,13 @@ private:
FailureDetector _failure_detector{this};
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
Safety _safety{};
UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
WorkerThread _worker_thread{};
ModeManagement _mode_management{
#ifndef CONSTRAINED_FLASH
_health_and_arming_checks.externalChecks()
#endif
};
UserModeIntention _user_mode_intention {this, _vehicle_status, _health_and_arming_checks, &_mode_management};
const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()};
HomePosition _home_position{_failsafe_flags};
@ -275,6 +287,7 @@ private:
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_command_mode_executor_sub{ORB_ID(vehicle_command_mode_executor)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
@ -294,6 +307,7 @@ private:
uORB::Publication<actuator_test_s> _actuator_test_pub{ORB_ID(actuator_test)};
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
uORB::Publication<vehicle_control_mode_s> _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
uORB::Publication<vehicle_status_s> _vehicle_status_pub{ORB_ID(vehicle_status)};

1
src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
View File

@ -61,6 +61,7 @@ px4_add_library(health_and_arming_checks
checks/rcAndDataLinkCheck.cpp
checks/vtolCheck.cpp
checks/offboardCheck.cpp
checks/externalChecks.cpp
)
add_dependencies(health_and_arming_checks mode_util)

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

@ -87,6 +87,30 @@ Report::EventBufferHeader *Report::addEventToBuffer(uint32_t event_id, const eve
return header;
}
bool Report::addExternalEvent(const event_s &event, NavModes modes)
{
unsigned args_size = sizeof(event.arguments);
// trim 0's
while (args_size > 0 && event.arguments[args_size - 1] == '\0') {
--args_size;
}
unsigned total_size = sizeof(EventBufferHeader) + args_size;
if (total_size > sizeof(_event_buffer) - _next_buffer_idx) {
_buffer_overflowed = true;
return false;
}
events::LogLevels log_levels{events::externalLogLevel(event.log_levels), events::internalLogLevel((event.log_levels))};
memcpy(_event_buffer + _next_buffer_idx + sizeof(EventBufferHeader), &event.arguments, args_size);
addEventToBuffer(event.id, log_levels, (uint32_t)modes, args_size);
return true;
}
NavModes Report::reportedModes(NavModes required_modes)
{
// Make sure optional checks are still shown in the UI

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

@ -249,8 +249,8 @@ public:
void clearArmingBits(NavModes modes);
/**
* Clear can_run bits for certain modes. This will prevent mode switching and trigger failsafe if the
* mode is being run.
* Clear can_run bits for certain modes. This will prevent mode switching.
* For failsafe use the mode requirements instead, which then will clear the can_run bits.
* @param modes affected modes
*/
void clearCanRunBits(NavModes modes);
@ -259,6 +259,8 @@ public:
const ArmingCheckResults &armingCheckResults() const { return _results[_current_result].arming_checks; }
bool modePreventsArming(uint8_t nav_state) const { return _failsafe_flags.mode_req_prevent_arming & (1u << nav_state); }
bool addExternalEvent(const event_s &event, NavModes modes);
private:
/**
@ -307,6 +309,7 @@ private:
NavModes getModeGroup(uint8_t nav_state) const;
friend class HealthAndArmingChecks;
friend class ExternalChecks;
FRIEND_TEST(ReporterTest, basic_no_checks);
FRIEND_TEST(ReporterTest, basic_fail_all_modes);
FRIEND_TEST(ReporterTest, arming_checks_mode_category);

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

@ -67,6 +67,7 @@
#include "checks/rcAndDataLinkCheck.hpp"
#include "checks/vtolCheck.hpp"
#include "checks/offboardCheck.hpp"
#include "checks/externalChecks.hpp"
class HealthAndArmingChecks : public ModuleParams
{
@ -99,6 +100,10 @@ public:
const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; }
#ifndef CONSTRAINED_FLASH
ExternalChecks &externalChecks() { return _external_checks; }
#endif
protected:
void updateParams() override;
private:
@ -139,8 +144,14 @@ private:
RcAndDataLinkChecks _rc_and_data_link_checks;
VtolChecks _vtol_checks;
OffboardChecks _offboard_checks;
#ifndef CONSTRAINED_FLASH
ExternalChecks _external_checks;
#endif
HealthAndArmingCheckBase *_checks[30] = {
HealthAndArmingCheckBase *_checks[40] = {
#ifndef CONSTRAINED_FLASH
&_external_checks,
#endif
&_accelerometer_checks,
&_airspeed_checks,
&_baro_checks,
@ -156,7 +167,7 @@ private:
&_home_position_checks,
&_mission_checks,
&_offboard_checks, // must be after _estimator_checks
&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks
&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks, _external_checks
&_parachute_checks,
&_power_checks,
&_rc_calibration_checks,

333
src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp Normal file
View File

@ -0,0 +1,333 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "externalChecks.hpp"
static void setOrClearRequirementBits(bool requirement_set, int8_t nav_state, int8_t replaces_nav_state, uint32_t &bits)
{
if (requirement_set) {
bits |= 1u << nav_state;
}
if (replaces_nav_state != -1) {
if (requirement_set) {
bits |= 1u << replaces_nav_state;
} else {
bits &= ~(1u << replaces_nav_state);
}
}
}
int ExternalChecks::addRegistration(int8_t nav_mode_id, int8_t replaces_nav_state)
{
int free_registration_index = -1;
for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) {
if (!registrationValid(i)) {
free_registration_index = i;
break;
}
}
if (free_registration_index != -1) {
_active_registrations_mask |= 1 << free_registration_index;
_registrations[free_registration_index].nav_mode_id = nav_mode_id;
_registrations[free_registration_index].replaces_nav_state = replaces_nav_state;
_registrations[free_registration_index].num_no_response = 0;
_registrations[free_registration_index].unresponsive = false;
_registrations[free_registration_index].total_num_unresponsive = 0;
if (!_registrations[free_registration_index].reply) {
_registrations[free_registration_index].reply = new arming_check_reply_s();
}
}
return free_registration_index;
}
bool ExternalChecks::removeRegistration(int registration_id, int8_t nav_mode_id)
{
if (registration_id < 0 || registration_id >= MAX_NUM_REGISTRATIONS) {
return false;
}
if (registrationValid(registration_id)) {
if (_registrations[registration_id].nav_mode_id == nav_mode_id) {
_active_registrations_mask &= ~(1u << registration_id);
return true;
}
}
PX4_ERR("trying to remove inactive external check");
return false;
}
bool ExternalChecks::isUnresponsive(int registration_id)
{
if (registration_id < 0 || registration_id >= MAX_NUM_REGISTRATIONS) {
return false;
}
if (registrationValid(registration_id)) {
return _registrations[registration_id].unresponsive;
}
return false;
}
void ExternalChecks::checkAndReport(const Context &context, Report &reporter)
{
checkNonRegisteredModes(context, reporter);
if (_active_registrations_mask == 0) {
return;
}
NavModes unresponsive_modes{NavModes::None};
for (int reg_idx = 0; reg_idx < MAX_NUM_REGISTRATIONS; ++reg_idx) {
if (!registrationValid(reg_idx) || !_registrations[reg_idx].reply) {
continue;
}
arming_check_reply_s &reply = *_registrations[reg_idx].reply;
int8_t nav_mode_id = _registrations[reply.registration_id].nav_mode_id;
if (_registrations[reply.registration_id].unresponsive) {
if (nav_mode_id != -1) {
unresponsive_modes = unresponsive_modes | reporter.getModeGroup(nav_mode_id);
setOrClearRequirementBits(true, nav_mode_id, -1, reporter.failsafeFlags().mode_req_other);
}
} else {
NavModes modes;
// We distinguish between two cases:
// - external navigation mode: in that case we set the single arming can_run bit for the mode
// - generic external arming check: set all arming bits
if (nav_mode_id == -1) {
modes = NavModes::All;
} else {
modes = reporter.getModeGroup(nav_mode_id);
int8_t replaces_nav_state = _registrations[reply.registration_id].replaces_nav_state;
if (replaces_nav_state != -1) {
modes = modes | reporter.getModeGroup(replaces_nav_state);
// Also clear the arming bits for the replaced mode, as the user intention is always set to the
// replaced mode.
// We only have to clear the bits, as for the internal/replaced mode, the bits are not cleared yet.
}
if (!reply.can_arm_and_run) {
setOrClearRequirementBits(true, nav_mode_id, replaces_nav_state, reporter.failsafeFlags().mode_req_other);
}
// Mode requirements
// A replacement mode will also replace the mode requirements of the internal/replaced mode
setOrClearRequirementBits(reply.mode_req_angular_velocity, nav_mode_id, replaces_nav_state,
reporter.failsafeFlags().mode_req_angular_velocity);
setOrClearRequirementBits(reply.mode_req_attitude, nav_mode_id, replaces_nav_state,
reporter.failsafeFlags().mode_req_attitude);
setOrClearRequirementBits(reply.mode_req_local_alt, nav_mode_id, replaces_nav_state,
reporter.failsafeFlags().mode_req_local_alt);
setOrClearRequirementBits(reply.mode_req_local_position, nav_mode_id, replaces_nav_state,
reporter.failsafeFlags().mode_req_local_position);
setOrClearRequirementBits(reply.mode_req_local_position_relaxed, nav_mode_id, replaces_nav_state,
reporter.failsafeFlags().mode_req_local_position_relaxed);
setOrClearRequirementBits(reply.mode_req_global_position, nav_mode_id, replaces_nav_state,
reporter.failsafeFlags().mode_req_global_position);
setOrClearRequirementBits(reply.mode_req_mission, nav_mode_id, replaces_nav_state,
reporter.failsafeFlags().mode_req_mission);
setOrClearRequirementBits(reply.mode_req_home_position, nav_mode_id, replaces_nav_state,
reporter.failsafeFlags().mode_req_home_position);
setOrClearRequirementBits(reply.mode_req_prevent_arming, nav_mode_id, replaces_nav_state,
reporter.failsafeFlags().mode_req_prevent_arming);
}
if (!reply.can_arm_and_run) {
reporter.clearArmingBits(modes);
}
if (reply.health_component_index > 0) {
reporter.setHealth((health_component_t)(1ull << reply.health_component_index),
reply.health_component_is_present, reply.health_component_warning,
reply.health_component_error);
}
for (int i = 0; i < reply.num_events; ++i) {
// set the modes, which is the first argument
memcpy(reply.events[i].arguments, &modes, sizeof(modes));
reporter.addExternalEvent(reply.events[i], modes);
}
}
}
if (unresponsive_modes != NavModes::None) {
/* EVENT
* @description
* The application running the mode might have crashed or the CPU load is too high.
*/
reporter.armingCheckFailure(unresponsive_modes, health_component_t::system,
events::ID("check_external_modes_unresponsive"),
events::Log::Critical, "Mode is unresponsive");
}
}
void ExternalChecks::update()
{
if (_active_registrations_mask == 0) {
return;
}
const hrt_abstime now = hrt_absolute_time();
// Check for incoming replies
arming_check_reply_s reply;
int max_num_updates = arming_check_reply_s::ORB_QUEUE_LENGTH;
while (_arming_check_reply_sub.update(&reply) && --max_num_updates >= 0) {
if (reply.registration_id < MAX_NUM_REGISTRATIONS && registrationValid(reply.registration_id)
&& _current_request_id == reply.request_id) {
_reply_received_mask |= 1u << reply.registration_id;
_registrations[reply.registration_id].num_no_response = 0;
// Prevent toggling between unresponsive & responsive state
if (_registrations[reply.registration_id].total_num_unresponsive <= 3) {
_registrations[reply.registration_id].unresponsive = false;
}
if (_registrations[reply.registration_id].reply) {
*_registrations[reply.registration_id].reply = reply;
}
// PX4_DEBUG("Registration id=%i: %i events", reply.registration_id, reply.num_events);
}
}
if (_last_update > 0) {
if (_reply_received_mask == _active_registrations_mask) { // Got all responses
// Nothing to do
} else if (now > _last_update + REQUEST_TIMEOUT && !_had_timeout) { // Timeout
_had_timeout = true;
unsigned no_reply = _active_registrations_mask & ~_reply_received_mask;
for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) {
if ((1u << i) & no_reply) {
if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response >= NUM_NO_REPLY_UNTIL_UNRESPONSIVE) {
// Clear immediately if not a mode
if (_registrations[i].nav_mode_id == -1) {
removeRegistration(i, -1);
PX4_WARN("No response from %i, removing", i);
} else {
_registrations[i].unresponsive = true;
if (_registrations[i].total_num_unresponsive < 100) {
++_registrations[i].total_num_unresponsive;
}
PX4_WARN("No response from %i, flagging unresponsive", i);
}
}
}
}
}
}
// Start a new request?
if (now > _last_update + UPDATE_INTERVAL) {
_reply_received_mask = 0;
_last_update = now;
_had_timeout = false;
// Request the state from all registered components
arming_check_request_s request{};
request.request_id = ++_current_request_id;
request.timestamp = hrt_absolute_time();
_arming_check_request_pub.publish(request);
}
}
void ExternalChecks::setExternalNavStates(uint8_t first_external_nav_state, uint8_t last_external_nav_state)
{
_first_external_nav_state = first_external_nav_state;
_last_external_nav_state = last_external_nav_state;
}
void ExternalChecks::checkNonRegisteredModes(const Context &context, Report &reporter) const
{
// Clear the arming bits for all non-registered external modes.
// But only report if one of them is selected, so we don't need to generate the extra event in most cases.
bool report_mode_not_available = false;
for (uint8_t external_nav_state = _first_external_nav_state; external_nav_state <= _last_external_nav_state;
++external_nav_state) {
bool found = false;
for (int reg_idx = 0; reg_idx < MAX_NUM_REGISTRATIONS; ++reg_idx) {
if (registrationValid(reg_idx) && _registrations[reg_idx].nav_mode_id == external_nav_state) {
found = true;
break;
}
}
if (!found) {
if (external_nav_state == context.status().nav_state_user_intention) {
report_mode_not_available = true;
}
reporter.clearArmingBits(reporter.getModeGroup(external_nav_state));
setOrClearRequirementBits(true, external_nav_state, -1, reporter.failsafeFlags().mode_req_other);
}
}
if (report_mode_not_available) {
/* EVENT
* @description
* The application running the mode is not started.
*/
reporter.armingCheckFailure(reporter.getModeGroup(context.status().nav_state_user_intention),
health_component_t::system,
events::ID("check_external_modes_unavailable"),
events::Log::Error, "Mode is not registered");
}
}

108
src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp Normal file
View File

@ -0,0 +1,108 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "../Common.hpp"
#include <uORB/topics/arming_check_request.h>
#include <uORB/topics/arming_check_reply.h>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
static_assert((1ull << arming_check_reply_s::HEALTH_COMPONENT_INDEX_AVOIDANCE) == (uint64_t)
health_component_t::avoidance, "enum definition missmatch");
class ExternalChecks : public HealthAndArmingCheckBase
{
public:
static constexpr int MAX_NUM_REGISTRATIONS = 8;
ExternalChecks() = default;
~ExternalChecks() = default;
void setExternalNavStates(uint8_t first_external_nav_state, uint8_t last_external_nav_state);
void checkAndReport(const Context &context, Report &reporter) override;
bool hasFreeRegistrations() const { return _active_registrations_mask != (1u << MAX_NUM_REGISTRATIONS) - 1; }
/**
* Add registration
* @param nav_mode_id associated mode, -1 if none
* @param replaces_nav_state replaced mode, -1 if none
* @return registration id, or -1
*/
int addRegistration(int8_t nav_mode_id, int8_t replaces_nav_state);
bool removeRegistration(int registration_id, int8_t nav_mode_id);
void update();
bool isUnresponsive(int registration_id);
private:
static constexpr hrt_abstime REQUEST_TIMEOUT = 50_ms;
static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms;
static_assert(REQUEST_TIMEOUT < UPDATE_INTERVAL, "keep timeout < update interval");
static constexpr int NUM_NO_REPLY_UNTIL_UNRESPONSIVE = 3; ///< Mode timeout = this value * UPDATE_INTERVAL
void checkNonRegisteredModes(const Context &context, Report &reporter) const;
bool registrationValid(int reg_idx) const { return ((1u << reg_idx) & _active_registrations_mask) != 0; }
struct Registration {
~Registration() { delete reply; }
int8_t nav_mode_id{-1}; ///< associated mode, -1 if none
int8_t replaces_nav_state{-1};
uint8_t num_no_response{0};
bool unresponsive{false};
uint8_t total_num_unresponsive{0};
arming_check_reply_s *reply{nullptr};
};
unsigned _active_registrations_mask{0};
Registration _registrations[MAX_NUM_REGISTRATIONS] {};
uint8_t _first_external_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX;
uint8_t _last_external_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX;
// Current requests (async updates)
hrt_abstime _last_update{0};
unsigned _reply_received_mask{0};
bool _had_timeout{false};
uint8_t _current_request_id{0};
uORB::Subscription _arming_check_reply_sub{ORB_ID(arming_check_reply)};
uORB::Publication<arming_check_request_s> _arming_check_request_pub{ORB_ID(arming_check_request)};
};

453
src/modules/commander/ModeManagement.cpp Normal file
View File

@ -0,0 +1,453 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef CONSTRAINED_FLASH
#include "ModeManagement.hpp"
#include <px4_platform_common/events.h>
bool ModeExecutors::hasFreeExecutors() const
{
for (int i = 0; i < MAX_NUM; ++i) {
if (!_mode_executors[i].valid) {
return true;
}
}
return false;
}
int ModeExecutors::addExecutor(const ModeExecutors::ModeExecutor &executor)
{
for (int i = 0; i < MAX_NUM; ++i) {
if (!_mode_executors[i].valid) {
_mode_executors[i] = executor;
_mode_executors[i].valid = true;
return i + FIRST_EXECUTOR_ID;
}
}
PX4_ERR("logic error");
return -1;
}
void ModeExecutors::removeExecutor(int id)
{
if (valid(id)) {
_mode_executors[id - FIRST_EXECUTOR_ID].valid = false;
}
}
void ModeExecutors::printStatus(int executor_in_charge) const
{
for (int i = 0; i < MAX_NUM; ++i) {
if (_mode_executors[i].valid) {
int executor_id = i + FIRST_EXECUTOR_ID;
PX4_INFO("Mode Executor %i: owned nav_state: %i, in charge: %s", executor_id, _mode_executors[i].owned_nav_state,
executor_id == executor_in_charge ? "yes" : "no");
}
}
}
bool Modes::hasFreeExternalModes() const
{
for (int i = 0; i < MAX_NUM; ++i) {
if (!_modes[i].valid) {
return true;
}
}
return false;
}
uint8_t Modes::addExternalMode(const Modes::Mode &mode)
{
for (int i = 0; i < MAX_NUM; ++i) {
if (!_modes[i].valid) {
_modes[i] = mode;
_modes[i].valid = true;
return i + FIRST_EXTERNAL_NAV_STATE;
}
}
PX4_ERR("logic error");
return -1;
}
bool Modes::removeExternalMode(uint8_t nav_state, const char *name)
{
if (valid(nav_state) && strncmp(name, _modes[nav_state - FIRST_EXTERNAL_NAV_STATE].name, sizeof(Mode::name)) == 0) {
_modes[nav_state - FIRST_EXTERNAL_NAV_STATE].valid = false;
return true;
}
PX4_ERR("trying to remove invalid mode %s", name);
return false;
}
void Modes::printStatus() const
{
for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
if (valid(i)) {
const Modes::Mode &cur_mode = mode(i);
PX4_INFO("External Mode %i: nav_state: %i, name: %s", i - vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + 1, i,
cur_mode.name);
if (cur_mode.replaces_nav_state != Mode::REPLACES_NAV_STATE_NONE
&& cur_mode.replaces_nav_state < vehicle_status_s::NAVIGATION_STATE_MAX) {
PX4_INFO(" Replaces mode: %s", mode_util::nav_state_names[cur_mode.replaces_nav_state]);
}
}
}
}
ModeManagement::ModeManagement(ExternalChecks &external_checks)
: _external_checks(external_checks)
{
_external_checks.setExternalNavStates(Modes::FIRST_EXTERNAL_NAV_STATE, Modes::LAST_EXTERNAL_NAV_STATE);
}
void ModeManagement::checkNewRegistrations(UpdateRequest &update_request)
{
register_ext_component_request_s request;
int max_updates = 5;
while (!update_request.change_user_intended_nav_state && _register_ext_component_request_sub.update(&request)
&& --max_updates >= 0) {
request.name[sizeof(request.name) - 1] = '\0';
PX4_DEBUG("got registration request: %s %llu, arming: %i mode: %i executor: %i", request.name, request.request_id,
request.register_arming_check, request.register_mode, request.register_mode_executor);
register_ext_component_reply_s reply{};
reply.mode_executor_id = -1;
reply.mode_id = -1;
reply.arming_check_id = -1;
static_assert(sizeof(request.name) == sizeof(reply.name), "size mismatch");
memcpy(reply.name, request.name, sizeof(request.name));
reply.request_id = request.request_id;
// validate
bool request_valid = true;
if (request.register_mode_executor && !request.register_mode) {
request_valid = false;
}
if (request.register_mode && !request.register_arming_check) {
request_valid = false;
}
reply.success = false;
if (request_valid) {
// check free space
reply.success = true;
if (request.register_arming_check && !_external_checks.hasFreeRegistrations()) {
PX4_WARN("No free slots for arming checks");
reply.success = false;
}
if (request.register_mode) {
if (!_modes.hasFreeExternalModes()) {
PX4_WARN("No free slots for modes");
reply.success = false;
} else if (request.enable_replace_internal_mode) {
// Check if another one already replaces the same mode
for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
if (_modes.valid(i)) {
const Modes::Mode &cur_mode = _modes.mode(i);
if (cur_mode.replaces_nav_state == request.replace_internal_mode) {
// TODO: we could add priorities and allow the highest priority to do the replacement
PX4_ERR("Trying to replace an already replaced mode (%i)", request.replace_internal_mode);
reply.success = false;
}
}
}
}
}
if (request.register_mode_executor && !_mode_executors.hasFreeExecutors()) {
PX4_WARN("No free slots for executors");
reply.success = false;
}
// register component(s)
if (reply.success) {
int nav_mode_id = -1;
if (request.register_mode) {
Modes::Mode mode{};
strncpy(mode.name, request.name, sizeof(mode.name));
if (request.enable_replace_internal_mode) {
mode.replaces_nav_state = request.replace_internal_mode;
}
nav_mode_id = _modes.addExternalMode(mode);
reply.mode_id = nav_mode_id;
}
if (request.register_mode_executor) {
ModeExecutors::ModeExecutor executor{};
executor.owned_nav_state = nav_mode_id;
int registration_id = _mode_executors.addExecutor(executor);
if (nav_mode_id != -1) {
_modes.mode(nav_mode_id).mode_executor_registration_id = registration_id;
}
reply.mode_executor_id = registration_id;
}
if (request.register_arming_check) {
int8_t replace_nav_state = request.enable_replace_internal_mode ? request.replace_internal_mode : -1;
int registration_id = _external_checks.addRegistration(nav_mode_id, replace_nav_state);
if (nav_mode_id != -1) {
_modes.mode(nav_mode_id).arming_check_registration_id = registration_id;
}
reply.arming_check_id = registration_id;
}
// Activate the mode?
if (request.register_mode_executor && request.activate_mode_immediately && nav_mode_id != -1) {
update_request.change_user_intended_nav_state = true;
update_request.user_intended_nav_state = nav_mode_id;
}
}
}
reply.timestamp = hrt_absolute_time();
_register_ext_component_reply_pub.publish(reply);
}
}
void ModeManagement::checkUnregistrations(uint8_t user_intended_nav_state, UpdateRequest &update_request)
{
unregister_ext_component_s request;
int max_updates = 5;
while (!update_request.change_user_intended_nav_state && _unregister_ext_component_sub.update(&request)
&& --max_updates >= 0) {
request.name[sizeof(request.name) - 1] = '\0';
PX4_DEBUG("got unregistration request: %s arming: %i mode: %i executor: %i", request.name,
(int)request.arming_check_id, (int)request.mode_id, (int)request.mode_executor_id);
if (request.arming_check_id != -1) {
_external_checks.removeRegistration(request.arming_check_id, request.mode_id);
}
if (request.mode_id != -1) {
if (_modes.removeExternalMode(request.mode_id, request.name)) {
removeModeExecutor(request.mode_executor_id);
// else: if the mode was already removed (due to a timeout), the executor was also removed already
}
// If the removed mode is currently active, switch to Hold
if (user_intended_nav_state == request.mode_id) {
update_request.change_user_intended_nav_state = true;
update_request.user_intended_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
}
}
}
}
void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active,
UpdateRequest &update_request)
{
_failsafe_action_active = failsafe_action_active;
_external_checks.update();
if (armed) {
// Drop registration requests (we could send a failure response)
register_ext_component_request_s request;
_register_ext_component_request_sub.update(&request);
} else {
// Check for unresponsive modes
for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
if (_modes.valid(i)) {
const Modes::Mode &mode = _modes.mode(i);
// Remove only if not currently selected
if (user_intended_nav_state != i && _external_checks.isUnresponsive(mode.arming_check_registration_id)) {
PX4_DEBUG("Removing unresponsive mode %i", i);
_external_checks.removeRegistration(mode.arming_check_registration_id, i);
removeModeExecutor(mode.mode_executor_registration_id);
_modes.removeExternalMode(i, mode.name);
}
}
}
// As we're disarmed we can use the user intended mode, as no failsafe will be active
checkNewRegistrations(update_request);
checkUnregistrations(user_intended_nav_state, update_request);
}
update_request.control_setpoint_update = checkConfigControlSetpointUpdates();
}
void ModeManagement::onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state)
{
// Update mode executor in charge
int mode_executor_for_intended_nav_state = -1;
if (_modes.valid(user_intended_nav_state)) {
mode_executor_for_intended_nav_state = _modes.mode(user_intended_nav_state).mode_executor_registration_id;
}
if (mode_executor_for_intended_nav_state == -1) {
// Not an owned mode: check source
if (source == ModeChangeSource::User) {
// Give control to the pilot
_mode_executor_in_charge = ModeExecutors::AUTOPILOT_EXECUTOR_ID;
}
} else {
// Switched into an owned mode: put executor in charge
_mode_executor_in_charge = mode_executor_for_intended_nav_state;
}
}
uint8_t ModeManagement::getNavStateReplacementIfValid(uint8_t nav_state, bool report_error)
{
for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
if (_modes.valid(i)) {
Modes::Mode &mode = _modes.mode(i);
if (mode.replaces_nav_state == nav_state) {
if (_external_checks.isUnresponsive(mode.arming_check_registration_id)) {
if (!mode.unresponsive_reported && report_error) {
mode.unresponsive_reported = true;
events::send(events::ID("commander_mode_fallback_internal"), events::Log::Critical,
"External mode is unresponsive, falling back to internal");
}
return nav_state;
} else {
return i;
}
}
}
}
return nav_state;
}
uint8_t ModeManagement::getReplacedModeIfAny(uint8_t nav_state)
{
if (_modes.valid(nav_state)) {
const Modes::Mode &mode = _modes.mode(nav_state);
if (mode.replaces_nav_state != Modes::Mode::REPLACES_NAV_STATE_NONE) {
return mode.replaces_nav_state;
}
}
return nav_state;
}
void ModeManagement::removeModeExecutor(int mode_executor_id)
{
if (mode_executor_id == -1) {
return;
}
if (_mode_executor_in_charge == mode_executor_id) {
_mode_executor_in_charge = ModeExecutors::AUTOPILOT_EXECUTOR_ID;
}
_mode_executors.removeExecutor(mode_executor_id);
}
int ModeManagement::modeExecutorInCharge() const
{
if (_failsafe_action_active) {
return ModeExecutors::AUTOPILOT_EXECUTOR_ID;
}
return _mode_executor_in_charge;
}
bool ModeManagement::updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const
{
bool ret = false;
if (nav_state >= Modes::FIRST_EXTERNAL_NAV_STATE && nav_state <= Modes::LAST_EXTERNAL_NAV_STATE) {
if (_modes.valid(nav_state)) {
control_mode = _modes.mode(nav_state).config_control_setpoint;
ret = true;
} else {
Modes::Mode::setControlModeDefaults(control_mode);
}
}
return ret;
}
void ModeManagement::printStatus() const
{
_modes.printStatus();
_mode_executors.printStatus(modeExecutorInCharge());
}
bool ModeManagement::checkConfigControlSetpointUpdates()
{
bool had_update = false;
vehicle_control_mode_s config_control_setpoint;
int max_updates = 5;
while (_config_control_setpoints_sub.update(&config_control_setpoint) && --max_updates >= 0) {
if (_modes.valid(config_control_setpoint.source_id)) {
_modes.mode(config_control_setpoint.source_id).config_control_setpoint = config_control_setpoint;
had_update = true;
} else {
if (!_invalid_mode_printed) {
PX4_ERR("Control sp config request for invalid mode: %i", config_control_setpoint.source_id);
_invalid_mode_printed = true;
}
}
}
return had_update;
}
#endif /* CONSTRAINED_FLASH */

215
src/modules/commander/ModeManagement.hpp Normal file
View File

@ -0,0 +1,215 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/register_ext_component_request.h>
#include <uORB/topics/register_ext_component_reply.h>
#include <uORB/topics/unregister_ext_component.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include "ModeUtil/ui.hpp"
#include "UserModeIntention.hpp"
#include "HealthAndArmingChecks/checks/externalChecks.hpp"
class ModeExecutors
{
public:
static constexpr int AUTOPILOT_EXECUTOR_ID = 0;
static constexpr int FIRST_EXECUTOR_ID = 1;
static constexpr int MAX_NUM = 5;
struct ModeExecutor {
uint8_t owned_nav_state{};
bool valid{false};
};
void printStatus(int executor_in_charge) const;
bool valid(int id) const { return id >= FIRST_EXECUTOR_ID && id < FIRST_EXECUTOR_ID + MAX_NUM && _mode_executors[id - FIRST_EXECUTOR_ID].valid; }
const ModeExecutor &executor(int id) const { return _mode_executors[id - FIRST_EXECUTOR_ID]; }
ModeExecutor &executor(int id) { return _mode_executors[id - FIRST_EXECUTOR_ID]; }
bool hasFreeExecutors() const;
int addExecutor(const ModeExecutor &executor);
void removeExecutor(int id);
private:
ModeExecutor _mode_executors[MAX_NUM] {};
};
class Modes
{
public:
static constexpr uint8_t FIRST_EXTERNAL_NAV_STATE = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1;
static constexpr uint8_t LAST_EXTERNAL_NAV_STATE = vehicle_status_s::NAVIGATION_STATE_EXTERNAL8;
static constexpr int MAX_NUM = LAST_EXTERNAL_NAV_STATE - FIRST_EXTERNAL_NAV_STATE + 1;
struct Mode {
Mode()
{
// Set defaults for control mode
setControlModeDefaults(config_control_setpoint);
}
static void setControlModeDefaults(vehicle_control_mode_s &config_control_setpoint_)
{
config_control_setpoint_.flag_control_position_enabled = true;
config_control_setpoint_.flag_control_velocity_enabled = true;
config_control_setpoint_.flag_control_altitude_enabled = true;
config_control_setpoint_.flag_control_climb_rate_enabled = true;
config_control_setpoint_.flag_control_acceleration_enabled = true;
config_control_setpoint_.flag_control_rates_enabled = true;
config_control_setpoint_.flag_control_attitude_enabled = true;
}
static constexpr uint8_t REPLACES_NAV_STATE_NONE = 0xff;
char name[sizeof(register_ext_component_request_s::name)] {};
bool valid{false};
uint8_t replaces_nav_state{REPLACES_NAV_STATE_NONE};
bool unresponsive_reported{false};
int arming_check_registration_id{-1};
int mode_executor_registration_id{-1};
vehicle_control_mode_s config_control_setpoint{};
};
void printStatus() const;
bool valid(uint8_t nav_state) const { return nav_state >= FIRST_EXTERNAL_NAV_STATE && nav_state <= LAST_EXTERNAL_NAV_STATE && _modes[nav_state - FIRST_EXTERNAL_NAV_STATE].valid; }
Mode &mode(uint8_t nav_state) { return _modes[nav_state - FIRST_EXTERNAL_NAV_STATE]; }
const Mode &mode(uint8_t nav_state) const { return _modes[nav_state - FIRST_EXTERNAL_NAV_STATE]; }
bool hasFreeExternalModes() const;
uint8_t addExternalMode(const Mode &mode);
bool removeExternalMode(uint8_t nav_state, const char *name);
private:
Mode _modes[MAX_NUM] {};
};
#ifndef CONSTRAINED_FLASH
class ModeManagement : public ModeChangeHandler
{
public:
ModeManagement(ExternalChecks &external_checks);
~ModeManagement() = default;
struct UpdateRequest {
bool change_user_intended_nav_state{false};
uint8_t user_intended_nav_state{};
bool control_setpoint_update{false};
};
void update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active, UpdateRequest &update_request);
/**
* Mode executor ID for who is currently in charge (and can send commands etc).
* This is ModeExecutors::AUTOPILOT_EXECUTOR_ID if no executor is in charge currently.
*/
int modeExecutorInCharge() const;
void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override;
uint8_t getReplacedModeIfAny(uint8_t nav_state) override;
uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true);
bool updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const;
void printStatus() const;
private:
bool checkConfigControlSetpointUpdates();
void checkNewRegistrations(UpdateRequest &update_request);
void checkUnregistrations(uint8_t user_intended_nav_state, UpdateRequest &update_request);
void removeModeExecutor(int mode_executor_id);
uORB::Subscription _config_control_setpoints_sub{ORB_ID(config_control_setpoints)};
uORB::Subscription _register_ext_component_request_sub{ORB_ID(register_ext_component_request)};
uORB::Subscription _unregister_ext_component_sub{ORB_ID(unregister_ext_component)};
uORB::Publication<register_ext_component_reply_s> _register_ext_component_reply_pub{ORB_ID(register_ext_component_reply)};
ExternalChecks &_external_checks;
ModeExecutors _mode_executors;
Modes _modes;
bool _failsafe_action_active{false};
int _mode_executor_in_charge{ModeExecutors::AUTOPILOT_EXECUTOR_ID};
bool _invalid_mode_printed{false};
};
#else /* CONSTRAINED_FLASH */
class ModeManagement : public ModeChangeHandler
{
public:
ModeManagement() = default;
~ModeManagement() = default;
struct UpdateRequest {
bool change_user_intended_nav_state{false};
uint8_t user_intended_nav_state{};
bool control_setpoint_update{false};
};
void update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active, UpdateRequest &update_request) {}
int modeExecutorInCharge() const { return ModeExecutors::AUTOPILOT_EXECUTOR_ID; }
void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override {}
uint8_t getReplacedModeIfAny(uint8_t nav_state) override { return nav_state; }
uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true) { return nav_state; }
bool updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const { return false; }
void printStatus() const {}
void getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const
{
valid_nav_state_mask = mode_util::getValidNavStates();
can_set_nav_state_mask = valid_nav_state_mask & ~(1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION);
}
void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out) { }
private:
};
#endif /* CONSTRAINED_FLASH */

12
src/modules/commander/ModeUtil/control_mode.cpp
View File

@ -42,12 +42,10 @@ static bool stabilization_required(uint8_t vehicle_type)
return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
}
void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
const offboard_control_mode_s &offboard_control_mode,
vehicle_control_mode_s &vehicle_control_mode)
{
vehicle_control_mode.flag_armed = armed;
switch (nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
vehicle_control_mode.flag_control_manual_enabled = true;
@ -162,17 +160,11 @@ void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
vehicle_control_mode.flag_control_velocity_enabled = true;
break;
// vehicle_status_s::NAVIGATION_STATE_EXTERNALx: handled in ModeManagement
default:
break;
}
vehicle_control_mode.flag_multicopter_position_control_enabled =
(vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
&& (vehicle_control_mode.flag_control_altitude_enabled
|| vehicle_control_mode.flag_control_climb_rate_enabled
|| vehicle_control_mode.flag_control_position_enabled
|| vehicle_control_mode.flag_control_velocity_enabled
|| vehicle_control_mode.flag_control_acceleration_enabled);
}
} // namespace mode_util

2
src/modules/commander/ModeUtil/control_mode.hpp
View File

@ -41,7 +41,7 @@
namespace mode_util
{
void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
const offboard_control_mode_s &offboard_control_mode,
vehicle_control_mode_s &vehicle_control_mode);

29
src/modules/commander/ModeUtil/conversions.hpp
View File

@ -75,9 +75,25 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state)
case vehicle_status_s::NAVIGATION_STATE_ORBIT: return navigation_mode_t::orbit;
case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1: return navigation_mode_t::external1;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2: return navigation_mode_t::external2;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3: return navigation_mode_t::external3;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4: return navigation_mode_t::external4;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5: return navigation_mode_t::external5;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6: return navigation_mode_t::external6;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7: return navigation_mode_t::external7;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: return navigation_mode_t::external8;
}
static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 23, "code requires update");
static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "code requires update");
return navigation_mode_t::unknown;
}
@ -104,7 +120,16 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
"AUTO_LAND",
"AUTO_FOLLOW_TARGET",
"AUTO_PRECLAND",
"ORBIT"
"ORBIT",
"AUTO_VTOL_TAKEOFF",
"EXTERNAL1",
"EXTERNAL2",
"EXTERNAL3",
"EXTERNAL4",
"EXTERNAL5",
"EXTERNAL6",
"EXTERNAL7",
"EXTERNAL8",
};
} // namespace mode_util

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

@ -170,8 +170,9 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_position);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_alt);
// NAVIGATION_STATE_EXTERNALx: handled outside
static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 23, "update mode requirements");
static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "update mode requirements");
}
} // namespace mode_util

18
src/modules/commander/UserModeIntention.cpp
View File

@ -35,14 +35,22 @@
#include "UserModeIntention.hpp"
UserModeIntention::UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
const HealthAndArmingChecks &health_and_arming_checks)
: ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks)
const HealthAndArmingChecks &health_and_arming_checks, ModeChangeHandler *handler)
: ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks),
_handler(handler)
{
}
bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallback, bool force)
bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource source, bool allow_fallback,
bool force)
{
_ever_had_mode_change = true;
_had_mode_change = true;
if (_handler) {
// If a replacement mode is selected, select the internal one instead. The replacement will be selected after.
user_intended_nav_state = _handler->getReplacedModeIfAny(user_intended_nav_state);
}
// Always allow mode change while disarmed
bool always_allow = force || !isArmed();
@ -68,6 +76,10 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallb
if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state)) {
_nav_state_after_disarming = user_intended_nav_state;
}
if (_handler) {
_handler->onUserIntendedNavStateChange(source, user_intended_nav_state);
}
}
return allow_change;

26
src/modules/commander/UserModeIntention.hpp
View File

@ -37,21 +37,42 @@
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
#include <px4_platform_common/module_params.h>
enum class ModeChangeSource {
User, ///< RC or MAVLink
ModeExecutor,
};
class ModeChangeHandler
{
public:
virtual void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) = 0;
/**
* Get the replaced (internal) mode for a given (external) mode
* @param nav_state
* @return nav_state or the mode that nav_state replaces
*/
virtual uint8_t getReplacedModeIfAny(uint8_t nav_state) = 0;
};
class UserModeIntention : ModuleParams
{
public:
UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
const HealthAndArmingChecks &health_and_arming_checks);
const HealthAndArmingChecks &health_and_arming_checks, ModeChangeHandler *handler);
~UserModeIntention() = default;
/**
* Change the user intended mode
* @param user_intended_nav_state new mode
* @param source calling reason
* @param allow_fallback allow to fallback to a lower mode if current mode cannot run
* @param force always set if true
* @return true if successfully set (also if unchanged)
*/
bool change(uint8_t user_intended_nav_state, bool allow_fallback = false, bool force = false);
bool change(uint8_t user_intended_nav_state, ModeChangeSource source = ModeChangeSource::User,
bool allow_fallback = false, bool force = false);
uint8_t get() const { return _user_intented_nav_state; }
@ -72,6 +93,7 @@ private:
const vehicle_status_s &_vehicle_status;
const HealthAndArmingChecks &_health_and_arming_checks;
ModeChangeHandler *const _handler{nullptr};
uint8_t _user_intented_nav_state{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Current user intended mode
uint8_t _nav_state_after_disarming{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Mode that is switched into after landing/disarming

50
src/modules/commander/px4_custom_mode.h
View File

@ -63,7 +63,15 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05
PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,
PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF
PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF,
PX4_CUSTOM_SUB_MODE_EXTERNAL1,
PX4_CUSTOM_SUB_MODE_EXTERNAL2,
PX4_CUSTOM_SUB_MODE_EXTERNAL3,
PX4_CUSTOM_SUB_MODE_EXTERNAL4,
PX4_CUSTOM_SUB_MODE_EXTERNAL5,
PX4_CUSTOM_SUB_MODE_EXTERNAL6,
PX4_CUSTOM_SUB_MODE_EXTERNAL7,
PX4_CUSTOM_SUB_MODE_EXTERNAL8,
};
enum PX4_CUSTOM_SUB_MODE_POSCTL {
@ -171,6 +179,46 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF;
break;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1:
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL1;
break;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2:
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL2;
break;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3:
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL3;
break;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4:
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL4;
break;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5:
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL5;
break;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6:
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL6;
break;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7:
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL7;
break;
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8:
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL8;
break;
}
return custom_mode;

3
src/modules/mavlink/mavlink_command_sender.cpp
View File

@ -68,7 +68,8 @@ MavlinkCommandSender::~MavlinkCommandSender()
int MavlinkCommandSender::handle_vehicle_command(const vehicle_command_s &command, mavlink_channel_t channel)
{
// commands > uint16 are PX4 internal only
if (command.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) {
if (command.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START
|| command.source_component >= vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) {
return 0;
}

3
src/modules/mavlink/mavlink_main.cpp
View File

@ -2434,7 +2434,8 @@ Mavlink::task_main(int argc, char *argv[])
if (!command_ack.from_external
&& command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START
&& is_target_known) {
&& is_target_known
&& command_ack.target_component < vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) {
mavlink_command_ack_t msg{};
msg.result = command_ack.result;

29
src/modules/uxrce_dds_client/dds_topics.yaml
View File

@ -5,6 +5,15 @@
#####
publications:
- topic: /fmu/out/register_ext_component_reply
type: px4_msgs::msg::RegisterExtComponentReply
- topic: /fmu/out/arming_check_request
type: px4_msgs::msg::ArmingCheckRequest
- topic: /fmu/out/mode_completed
type: px4_msgs::msg::ModeCompleted
- topic: /fmu/out/collision_constraints
type: px4_msgs::msg::CollisionConstraints
@ -29,6 +38,9 @@ publications:
- topic: /fmu/out/vehicle_control_mode
type: px4_msgs::msg::VehicleControlMode
- topic: /fmu/out/vehicle_command_ack
type: px4_msgs::msg::VehicleCommandAck
- topic: /fmu/out/vehicle_global_position
type: px4_msgs::msg::VehicleGlobalPosition
@ -48,6 +60,20 @@ publications:
type: px4_msgs::msg::VehicleTrajectoryWaypoint
subscriptions:
- topic: /fmu/in/register_ext_component_request
type: px4_msgs::msg::RegisterExtComponentRequest
- topic: /fmu/in/unregister_ext_component
type: px4_msgs::msg::UnregisterExtComponent
- topic: /fmu/in/arming_check_reply
type: px4_msgs::msg::ArmingCheckReply
- topic: /fmu/in/mode_completed
type: px4_msgs::msg::ModeCompleted
- topic: /fmu/in/config_control_setpoints
type: px4_msgs::msg::VehicleControlMode
- topic: /fmu/in/offboard_control_mode
type: px4_msgs::msg::OffboardControlMode
@ -82,6 +108,9 @@ subscriptions:
- topic: /fmu/in/vehicle_command
type: px4_msgs::msg::VehicleCommand
- topic: /fmu/in/vehicle_command_mode_executor
type: px4_msgs::msg::VehicleCommand
- topic: /fmu/in/vehicle_trajectory_bezier
type: px4_msgs::msg::VehicleTrajectoryBezier