fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 09:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: implement external modes and mode executors

Browse Source
This commit is contained in:
Beat Küng
2023-11-15 13:18:58 +01:00
parent 58d2badf9f
commit fbbccf6997
32 changed files with 1632 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/ModeUtil/control_mode.cpp
+2 -9
View File
@@ -42,11 +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;
vehicle_control_mode.flag_control_allocation_enabled = true; // Always enabled by default
switch (nav_state) {
@@ -163,17 +162,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
src/modules/commander/ModeUtil/control_mode.hpp
+1 -1
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);
src/modules/commander/ModeUtil/conversions.hpp
+27 -2
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
src/modules/commander/ModeUtil/mode_requirements.cpp
+2 -1
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