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

dr rtl flight task: clean up and commenting

Browse Source
This commit is contained in:
oravla5
2025-08-29 10:32:18 +02:00
parent dddfb8df6d
commit defaf72630
2 changed files with 129 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp
+93 -82
View File
@@ -40,6 +40,7 @@
bool FlightTaskReturnDeadReckoning::activate(const trajectory_setpoint_s &last_setpoint)
{
PX4_INFO("FlightTaskReturnDeadReckoning::activate");
if (!FlightTask::activate(last_setpoint)) {
PX4_ERR("Failed to activate task");
return false;
@@ -75,7 +76,7 @@ bool FlightTaskReturnDeadReckoning::update()
_updateSubscriptions();
if (_isGlobalPositionValid()) {
// Update the bearing to home
// Update the bearing to home if global position is valid
if (!_updateBearingToHome()) {
PX4_ERR("Failed to compute bearing to home");
return false;
@@ -91,84 +92,88 @@ bool FlightTaskReturnDeadReckoning::update()
void FlightTaskReturnDeadReckoning::_updateState()
{
switch (_state) {
case State::INIT:
if (_isAboveReturnAltitude()) {
_state = State::RETURN;
PX4_INFO("Returning to home position at %.2fm (MSL) with bearing %.2f deg",
(double) _rtl_alt, (double) math::degrees(_bearing_to_home));
} else {
_state = State::ASCENT;
PX4_INFO("Ascending to return altitude %.2fm (MSL) with bearing %.2f deg",
(double) _rtl_alt, (double) math::degrees(_bearing_to_home));
}
break;
case State::INIT:
if (_isAboveReturnAltitude()) {
_state = State::RETURN;
PX4_INFO("Returning to home position at %.2fm (MSL) with bearing %.2f deg",
(double) _rtl_alt, (double) math::degrees(_bearing_to_home));
case State::ASCENT:
if (_isAboveReturnAltitude()) {
_state = State::RETURN;
PX4_INFO("Returning to home position at %.2fm (MSL) with bearing %.2f deg",
(double) _rtl_alt, (double) math::degrees(_bearing_to_home));
}
break;
} else {
_state = State::ASCENT;
PX4_INFO("Ascending to return altitude %.2fm (MSL) with bearing %.2f deg",
(double) _rtl_alt, (double) math::degrees(_bearing_to_home));
}
case State::RETURN:
if (_isWithinHomePositionRadius()) {
_state = State::HOLD;
PX4_INFO("Holding altitude at %.2fm (MSL) over home position", (double) _rtl_alt);
}
break;
break;
case State::HOLD:
break;
case State::ASCENT:
if (_isAboveReturnAltitude()) {
_state = State::RETURN;
PX4_INFO("Returning to home position at %.2fm (MSL) with bearing %.2f deg",
(double) _rtl_alt, (double) math::degrees(_bearing_to_home));
}
default:
PX4_ERR("Unknown state");
return;
break;
case State::RETURN:
if (_isWithinHomePositionRadius()) {
_state = State::HOLD;
PX4_INFO("Holding altitude at %.2fm (MSL) over home position", (double) _rtl_alt);
}
break;
case State::HOLD:
break;
default:
PX4_ERR("Unknown state");
return;
}
}
void FlightTaskReturnDeadReckoning::_updateSetpoints()
{
switch (_state) {
case State::INIT:
_slew_rate_acceleration_x.update(0.0f, _deltatime);
_slew_rate_acceleration_y.update(0.0f, _deltatime);
_slew_rate_velocity_z.update(0.0f, _deltatime);
case State::INIT:
_slew_rate_acceleration_x.update(0.0f, _deltatime);
_slew_rate_acceleration_y.update(0.0f, _deltatime);
_slew_rate_velocity_z.update(0.0f, _deltatime);
// Hold current altitude
_velocity_setpoint(2) = _slew_rate_velocity_z.getState();
break;
// Hold current altitude
_velocity_setpoint(2) = _slew_rate_velocity_z.getState();
break;
case State::ASCENT:
_slew_rate_acceleration_x.update(0.0f, _deltatime);
_slew_rate_acceleration_y.update(0.0f, _deltatime);
_slew_rate_velocity_z.update(-_param_mpc_z_v_auto_up.get(), _deltatime);
case State::ASCENT:
_slew_rate_acceleration_x.update(0.0f, _deltatime);
_slew_rate_acceleration_y.update(0.0f, _deltatime);
_slew_rate_velocity_z.update(-_param_mpc_z_v_auto_up.get(), _deltatime);
// Ascent until reaching the return altitude
_velocity_setpoint(2) = _slew_rate_velocity_z.getState();
break;
// Ascent until reaching the return altitude
_velocity_setpoint(2) = _slew_rate_velocity_z.getState();
break;
case State::RETURN:
_slew_rate_acceleration_x.update(_rtl_acc*cosf(_bearing_to_home), _deltatime);
_slew_rate_acceleration_y.update(_rtl_acc*sinf(_bearing_to_home), _deltatime);
_slew_rate_velocity_z.update(0.0f, _deltatime);
case State::RETURN:
_slew_rate_acceleration_x.update(_rtl_acc * cosf(_bearing_to_home), _deltatime);
_slew_rate_acceleration_y.update(_rtl_acc * sinf(_bearing_to_home), _deltatime);
_slew_rate_velocity_z.update(0.0f, _deltatime);
// Stay at the return altitude
_position_setpoint(2) = -(_rtl_alt - (float) _home_position(2));
break;
// Stay at the return altitude
_position_setpoint(2) = -(_rtl_alt - (float) _home_position(2));
break;
case State::HOLD:
_slew_rate_acceleration_x.update(0.0f, _deltatime);
_slew_rate_acceleration_y.update(0.0f, _deltatime);
_slew_rate_velocity_z.update(0.0f, _deltatime);
case State::HOLD:
_slew_rate_acceleration_x.update(0.0f, _deltatime);
_slew_rate_acceleration_y.update(0.0f, _deltatime);
_slew_rate_velocity_z.update(0.0f, _deltatime);
// Stay at the return altitude
_position_setpoint(2) = -(_rtl_alt - (float) _home_position(2));
break;
// Stay at the return altitude
_position_setpoint(2) = -(_rtl_alt - (float) _home_position(2));
break;
default:
PX4_ERR("Unknown state");
return;
default:
PX4_ERR("Unknown state");
return;
};
@@ -179,16 +184,20 @@ void FlightTaskReturnDeadReckoning::_updateSetpoints()
// Acceleration setpoint
_acceleration_setpoint.xy() = matrix::Vector2f(
_slew_rate_acceleration_x.getState(),
_slew_rate_acceleration_y.getState()
);
_slew_rate_acceleration_x.getState(),
_slew_rate_acceleration_y.getState()
);
return;
}
void FlightTaskReturnDeadReckoning::_updateSubscriptions()
bool FlightTaskReturnDeadReckoning::_updateBearingToHome()
{
_sub_vehicle_global_position.update();
if (_readGlobalPosition(_start_vehicle_global_position) && _readHomePosition(_home_position)) {
_bearing_to_home = _computeBearing(_start_vehicle_global_position, _home_position);
}
return !isnanf(_bearing_to_home);
}
bool FlightTaskReturnDeadReckoning::_readHomePosition(matrix::Vector3d &home_position)
@@ -218,21 +227,14 @@ bool FlightTaskReturnDeadReckoning::_readGlobalPosition(matrix::Vector3d &global
return true;
}
bool FlightTaskReturnDeadReckoning::_updateBearingToHome()
{
if (_readGlobalPosition(_start_vehicle_global_position) && _readHomePosition(_home_position)) {
_bearing_to_home = _computeBearing(_start_vehicle_global_position, _home_position);
}
return !isnanf(_bearing_to_home);
}
float FlightTaskReturnDeadReckoning::_computeBearing(const matrix::Vector3d &_global_position_start, const matrix::Vector3d &_global_position_end)
float FlightTaskReturnDeadReckoning::_computeBearing(const matrix::Vector3d &_global_position_start,
const matrix::Vector3d &_global_position_end)
{
float bearing = NAN;
if (_global_position_start.isAllFinite() && _global_position_end.isAllFinite()) {
bearing = get_bearing_to_next_waypoint(_global_position_start(0), _global_position_start(1),
_global_position_end(0), _global_position_end(1));
_global_position_end(0), _global_position_end(1));
}
return bearing;
@@ -248,6 +250,11 @@ bool FlightTaskReturnDeadReckoning::_computeReturnParameters()
return true;
}
void FlightTaskReturnDeadReckoning::_updateSubscriptions()
{
_sub_vehicle_global_position.update();
}
bool FlightTaskReturnDeadReckoning::_initializeSmoothers()
{
// Initialize the heading smoother
@@ -268,9 +275,14 @@ bool FlightTaskReturnDeadReckoning::_initializeSmoothers()
return true;
}
bool FlightTaskReturnDeadReckoning::_isGlobalPositionValid() const
{
return _sub_vehicle_global_position.get().lat_lon_valid && _sub_vehicle_global_position.get().alt_valid;
}
bool FlightTaskReturnDeadReckoning::_isAboveReturnAltitude() const
{
float current_alt =_sub_vehicle_global_position.get().alt;
float current_alt = _sub_vehicle_global_position.get().alt;
float target_alt = _rtl_alt - _param_nav_mc_alt_rad.get();
return current_alt > target_alt;
}
@@ -280,10 +292,9 @@ bool FlightTaskReturnDeadReckoning::_isWithinHomePositionRadius()
if (_isGlobalPositionValid()) {
_readGlobalPosition(_start_vehicle_global_position);
return get_distance_to_next_waypoint(
_start_vehicle_global_position(0), _start_vehicle_global_position(1),
_home_position(0), _home_position(1)) < _param_nav_acc_rad.get();
_start_vehicle_global_position(0), _start_vehicle_global_position(1),
_home_position(0), _home_position(1)) < _param_nav_acc_rad.get();
}
return false;
}
src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp
+36 -16
View File
@@ -50,16 +50,27 @@ public:
FlightTaskReturnDeadReckoning() = default;
virtual ~FlightTaskReturnDeadReckoning() = default;
bool update() override;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
bool update() override;
private:
uORB::SubscriptionData<vehicle_global_position_s> _sub_vehicle_global_position{ORB_ID(vehicle_global_position)};
/**
* Update the flight task state machine
*/
void _updateState();
/**
* Update the trajectory setpoints according to the current state
*/
void _updateSetpoints();
/**
* Update bearing to home position with the last available GNSS position
* @return true on success, false on error
*/
bool _updateBearingToHome();
/**
* Initialize home position
@@ -73,15 +84,12 @@ private:
*/
bool _readGlobalPosition(matrix::Vector3d &global_position);
bool _updateBearingToHome();
/**
* Compute bearing to home position
* @return true on success, false on error
*/
float _computeBearing(const matrix::Vector3d &_global_position_start,
const matrix::Vector3d &_global_position_end);
const matrix::Vector3d &_global_position_end);
/**
* Computes return altitude velocity
@@ -89,25 +97,36 @@ private:
*/
bool _computeReturnParameters();
/**
* Update uORB subscriptions
*/
void _updateSubscriptions();
/**
* Initialize the trajectory setpoint smoothers
* @return true on success, false on error
*/
bool _initializeSmoothers();
void _updateSubscriptions();
bool _isGlobalPositionValid() const
{
return _sub_vehicle_global_position.get().lat_lon_valid && _sub_vehicle_global_position.get().alt_valid;
}
/**
* Check if the global position is valid
* @return true if valid, false otherwise
*/
bool _isGlobalPositionValid() const;
/**
* Check if the vehicle is above the return altitude
* @return true if above, false otherwise
*/
bool _isAboveReturnAltitude() const;
/**
* Check if the vehicle is within the home position waypoint acceptance radius
* @return true if within, false otherwise
*/
bool _isWithinHomePositionRadius();
// Variable storing the home position
matrix::Vector3d _home_position; /**< Stores home position */
matrix::Vector3d _start_vehicle_global_position; /**< Stores vehicle last known GNSS position */
float _bearing_to_home{0.0f}; /**< Stores bearing between home and last GNSS position */
@@ -115,12 +134,13 @@ private:
HeadingSmoothing _heading_smoothing; /**< Smoother for heading */
SlewRate<float> _slew_rate_acceleration_x{0.0f}; /**< Slew rate for x-acceleration setpoint */
SlewRate<float> _slew_rate_acceleration_y{0.0f}; /**< Slew rate for y-acceleration setpoint */
SlewRate<float> _slew_rate_velocity_z{0.0f}; /**< Smoother for vertical velocity */
SlewRate<float> _slew_rate_velocity_z{0.0f}; /**< Slew rate for vertical velocity */
float _rtl_alt{0.0f}; /**< Return altitude */
float _rtl_acc{0.0f}; /**< Return acceleration */
float _rtl_alt{0.0f}; /**< Return altitude */
float _rtl_acc{0.0f}; /**< Return acceleration */
/**< State machine for flight task */
enum class State {
UNKNOWN = 0,
INIT,
@@ -128,7 +148,7 @@ private:
RETURN,
HOLD
};
State _state{State::UNKNOWN}; /**< State machine for flight task */
State _state{State::UNKNOWN};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,