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

FlightTaskAuto: purge separate triplet type enum

Browse Source 
This allows for full text search of a type
to see all the cases it's used for.
This commit is contained in:
Matthias Grob
2024-10-12 08:58:43 +02:00
parent dbba9adb14
commit 67fb4cfec8
2 changed files with 14 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+12 -12
View File
@@ -111,7 +111,7 @@ bool FlightTaskAuto::update()
// The only time a thrust set-point is sent out is during
// idle. Hence, reset thrust set-point to NAN in case the
// vehicle exits idle.
if (_type_previous == WaypointType::idle) {
if (_type_previous == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_acceleration_setpoint.setNaN();
}
@@ -122,18 +122,18 @@ bool FlightTaskAuto::update()
}
switch (_type) {
case WaypointType::idle:
case position_setpoint_s::SETPOINT_TYPE_IDLE:
// Send zero thrust setpoint
_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
_velocity_setpoint.setNaN();
_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
break;
case WaypointType::land:
case position_setpoint_s::SETPOINT_TYPE_LAND:
_prepareLandSetpoints();
break;
case WaypointType::velocity:
case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
// XY Velocity waypoint
// TODO : Rewiew that. What is the expected behavior?
_position_setpoint = Vector3f(NAN, NAN, _position(2));
@@ -141,14 +141,14 @@ bool FlightTaskAuto::update()
_velocity_setpoint(2) = NAN;
break;
case WaypointType::loiter:
case position_setpoint_s::SETPOINT_TYPE_LOITER:
if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) {
rcHelpModifyYaw(_yaw_setpoint);
}
// FALLTHROUGH
case WaypointType::takeoff:
case WaypointType::position:
case position_setpoint_s::SETPOINT_TYPE_TAKEOFF:
case position_setpoint_s::SETPOINT_TYPE_POSITION:
default:
// Simple waypoint navigation: go to xyz target, with standard limitations
_position_setpoint = _target;
@@ -244,7 +244,7 @@ void FlightTaskAuto::_prepareLandSetpoints()
vertical_speed = _param_mpc_land_crawl_speed.get();
}
if (_type_previous != WaypointType::land) {
if (_type_previous != position_setpoint_s::SETPOINT_TYPE_LAND) {
// initialize yaw and xy-position
_land_heading = _yaw_setpoint;
_stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1)));
@@ -361,7 +361,7 @@ bool FlightTaskAuto::_evaluateTriplets()
if (!_sub_triplet_setpoint.get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.alt)) {
// Best we can do is to just set all waypoints to current state
_prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position;
_type = WaypointType::loiter;
_type = position_setpoint_s::SETPOINT_TYPE_LOITER;
_yaw_setpoint = _yaw;
_yawspeed_setpoint = NAN;
_target_acceptance_radius = _sub_triplet_setpoint.get().current.acceptance_radius;
@@ -369,7 +369,7 @@ bool FlightTaskAuto::_evaluateTriplets()
return true;
}
_type = (WaypointType)_sub_triplet_setpoint.get().current.type;
_type = _sub_triplet_setpoint.get().current.type;
// Prioritize cruise speed from the triplet when it's valid and more recent than the previously commanded cruise speed
const float cruise_speed_from_triplet = _sub_triplet_setpoint.get().current.cruising_speed;
@@ -457,7 +457,7 @@ bool FlightTaskAuto::_evaluateTriplets()
_prev_was_valid = _sub_triplet_setpoint.get().previous.valid;
if (_type == WaypointType::loiter) {
if (_type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
_triplet_next_wp = _triplet_target;
} else if (_isFinite(_sub_triplet_setpoint.get().next) && _sub_triplet_setpoint.get().next.valid) {
@@ -830,7 +830,7 @@ void FlightTaskAuto::_updateTrajConstraints()
// until the constraints don't do things like cause controller integrators to saturate. Once the controller
// doesn't use z speed constraints, this can go in _prepareTakeoffSetpoints(). Accel limit is to
// emulate the motor ramp (also done in the controller) so that the controller can actually track the setpoint.
if (_type == WaypointType::takeoff && _dist_to_ground < _param_mpc_land_alt1.get()) {
if (_type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _dist_to_ground < _param_mpc_land_alt1.get()) {
z_vel_constraint = _param_mpc_tko_speed.get();
z_accel_constraint = math::min(z_accel_constraint, _param_mpc_tko_speed.get() / _param_mpc_tko_ramp_t.get());
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp
+2 -17
View File
@@ -60,20 +60,6 @@
#include <lib/avoidance/ObstacleAvoidance.hpp>
#endif
/**
* This enum has to agree with position_setpoint_s type definition
* The only reason for not using the struct position_setpoint is because
* of the size
*/
enum class WaypointType : int {
position = position_setpoint_s::SETPOINT_TYPE_POSITION,
velocity = position_setpoint_s::SETPOINT_TYPE_VELOCITY,
loiter = position_setpoint_s::SETPOINT_TYPE_LOITER,
takeoff = position_setpoint_s::SETPOINT_TYPE_TAKEOFF,
land = position_setpoint_s::SETPOINT_TYPE_LAND,
idle = position_setpoint_s::SETPOINT_TYPE_IDLE
};
enum class State {
offtrack, /**< Vehicle is more than cruise speed away from track */
target_behind, /**< Vehicle is in front of target. */
@@ -135,7 +121,6 @@ protected:
matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */
bool _next_was_valid{false};
float _mc_cruise_speed{NAN}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)};
@@ -156,7 +141,6 @@ protected:
StickYaw _stick_yaw{this};
matrix::Vector3f _land_position;
float _land_heading;
WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
bool _is_emergency_braking_active{false};
bool _want_takeoff{false};
@@ -196,7 +180,8 @@ private:
bool _yaw_lock{false}; /**< if within acceptance radius, lock yaw to current yaw */
uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
uint8_t _type{position_setpoint_s::SETPOINT_TYPE_IDLE}; ///< Type of current target triplet
uint8_t _type_previous{position_setpoint_s::SETPOINT_TYPE_IDLE}; ///< Previous type of current target triplet
matrix::Vector3f
_triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */
matrix::Vector3f