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

dead-reckoning rtl: implemented wind compensation

Browse Source
This commit is contained in:
oravla5 2025-07-09 09:01:02 +02:00
parent 37e959ab01
commit d107f7ae50
2 changed files with 76 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.cpp
View File

@ -55,7 +55,7 @@ bool FlightTaskReturnDeadReckoning::activate(const trajectory_setpoint_s &last_s
PX4_ERR("Failed to initialize task");
return false;
}
_readWindEstimate();
_computeReturnParameters();
return true;
@ -75,6 +75,7 @@ bool FlightTaskReturnDeadReckoning::update()
PX4_ERR("Failed to compute bearing to home");
return false;
}
_readWindEstimate();
}
_updateState();
@ -132,10 +133,10 @@ void FlightTaskReturnDeadReckoning::_updateState()
void FlightTaskReturnDeadReckoning::_updateSetpoints()
{
_updateAccelerationSetpoints();
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);
// Hold current altitude
@ -143,8 +144,6 @@ void FlightTaskReturnDeadReckoning::_updateSetpoints()
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);
// Ascent until reaching the return altitude
@ -152,8 +151,6 @@ void FlightTaskReturnDeadReckoning::_updateSetpoints()
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);
// Stay at the return altitude
@ -161,8 +158,6 @@ void FlightTaskReturnDeadReckoning::_updateSetpoints()
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);
// Stay at the return altitude
@ -189,6 +184,43 @@ void FlightTaskReturnDeadReckoning::_updateSetpoints()
return;
}
void FlightTaskReturnDeadReckoning::_updateAccelerationSetpoints()
{
matrix::Vector3f accel_wind_compensation_local{0.f, 0.f, 0.f};
matrix::Vector3f accel_wind_compensation{0.f, 0.f, 0.f};
matrix::Vector2f accel_return{0.f, 0.f};
matrix::Vector2f accel_setpoint{0.f, 0.f};
float density = _sub_vehicle_air_data.get().rho;
matrix::Quaternionf q = matrix::Quatf(_sub_vehicle_attitude.get().q);
Vector3f wind_estimate_local = q.rotateVectorInverse(matrix::Vector3f(_wind_estimate(0), _wind_estimate(1), 0.f));
accel_wind_compensation_local(0) = -0.5f * wind_estimate_local(0)*wind_estimate_local(0) * density / _param_ekf2_bcoef_x.get();
accel_wind_compensation_local(1) = -0.5f * wind_estimate_local(1)*wind_estimate_local(1) * density / _param_ekf2_bcoef_y.get();
accel_wind_compensation = q.rotateVector(accel_wind_compensation_local);
if (_state == State::RETURN) {
if (accel_wind_compensation.norm() > 0.8f * _rtl_acc) {
// Wind compensation acceleration is too high, limit it
accel_wind_compensation = accel_wind_compensation.normalized() * 0.8f * _rtl_acc;
}
float wind_compensation_factor = accel_wind_compensation.norm() / _rtl_acc;
accel_return(0) = _rtl_acc * cosf(_bearing_to_home) * (1.f - wind_compensation_factor);
accel_return(1) = _rtl_acc * sinf(_bearing_to_home) * (1.f - wind_compensation_factor);
}
accel_setpoint(0) = accel_return(0) + accel_wind_compensation(0);
accel_setpoint(1) = accel_return(1) + accel_wind_compensation(1);
_slew_rate_acceleration_x.update(accel_setpoint(0), _deltatime);
_slew_rate_acceleration_y.update(accel_setpoint(1), _deltatime);
return;
}
void FlightTaskReturnDeadReckoning::_updateDistanceFlownEstimate()
{
if (_state == State::RETURN) {
@ -211,6 +243,17 @@ bool FlightTaskReturnDeadReckoning::_updateBearingToHome()
return !isnanf(_bearing_to_home);
}
void FlightTaskReturnDeadReckoning::_readWindEstimate()
{
_wind_estimate.setZero();
if (_param_ekf2_drag_ctrl.get() == 1) {
if (_sub_wind.get().timestamp_sample != 0) {
_wind_estimate = matrix::Vector2f(_sub_wind.get().windspeed_north, _sub_wind.get().windspeed_east);
}
}
}
bool FlightTaskReturnDeadReckoning::_readHomePosition(matrix::Vector3d &home_position)
{
home_position.setNaN();
@ -228,7 +271,6 @@ bool FlightTaskReturnDeadReckoning::_readHomePosition(matrix::Vector3d &home_pos
void FlightTaskReturnDeadReckoning::_readGlobalPosition(matrix::Vector3d &global_position)
{
global_position.setNaN();
global_position(0) = _sub_vehicle_global_position.get().lat;
@ -260,6 +302,9 @@ void FlightTaskReturnDeadReckoning::_computeReturnParameters()
void FlightTaskReturnDeadReckoning::_updateSubscriptions()
{
_sub_vehicle_global_position.update();
_sub_wind.update();
_sub_vehicle_air_data.update();
_sub_vehicle_attitude.update();
}
bool FlightTaskReturnDeadReckoning::_initializeSmoothers()
@ -307,7 +352,7 @@ bool FlightTaskReturnDeadReckoning::_isReturnComplete()
}
if (!ret) {
ret = _distance_flown_estimate > 2.5f * _initial_distance_to_home; // 2.5x the initial distance to home
ret = _distance_flown_estimate > 3.5f * _initial_distance_to_home; // 2.5x the initial distance to home
}
return ret;

21
src/modules/flight_mode_manager/tasks/ReturnDeadReckoning/FlightTaskReturnDeadReckoning.hpp
View File

@ -40,6 +40,8 @@
#include "FlightTask.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/wind.h>
#include <uORB/topics/vehicle_air_data.h>
#include <lib/motion_planning/HeadingSmoothing.hpp>
#include <lib/motion_planning/PositionSmoothing.hpp>
#include <lib/slew_rate/SlewRate.hpp>
@ -55,6 +57,9 @@ public:
private:
uORB::SubscriptionData<vehicle_global_position_s> _sub_vehicle_global_position{ORB_ID(vehicle_global_position)};
uORB::SubscriptionData<wind_s> _sub_wind{ORB_ID(wind)};
uORB::SubscriptionData<vehicle_air_data_s> _sub_vehicle_air_data{ORB_ID(vehicle_air_data)};
uORB::SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
/**
* Update the flight task state machine
@ -66,6 +71,11 @@ private:
*/
void _updateSetpoints();
/**
* Update return acceleration setpoints.
*/
void _updateAccelerationSetpoints();
/**
* Update estimated flown distance since the last time home bearing was updated
*/
@ -77,6 +87,11 @@ private:
*/
bool _updateBearingToHome();
/**
* Save the the latests available wind estimate.
*/
void _readWindEstimate();
/**
* Initialize home position
* @return true on success, false on error
@ -132,6 +147,7 @@ private:
matrix::Vector3d _home_position; /**< Stores home position */
matrix::Vector3d _start_vehicle_global_position; /**< Stores vehicle last known GNSS position */
matrix::Vector2f _wind_estimate{0.f, 0.f}; /**< Stores wind velocity estimate in North, East, Down frame */
float _bearing_to_home{0.0f}; /**< Stores bearing between home and last GNSS position */
float _initial_distance_to_home{0.0f}; /**< Initial distance to home position */
float _distance_flown_estimate{0.0f};
@ -165,6 +181,9 @@ private:
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max, //< max vertical acceleration up
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max, //< max horizontal velocity
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad, //< max vertical error
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad //< max horizontal error
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad, //< max horizontal error
(ParamInt<px4::params::EKF2_DRAG_CTRL>) _param_ekf2_drag_ctrl, //< wind esitmation control
(ParamFloat<px4::params::EKF2_BCOEF_X>) _param_ekf2_bcoef_x, //< x-axis ballistic coefficient
(ParamFloat<px4::params::EKF2_BCOEF_Y>) _param_ekf2_bcoef_y //< y-axis ballistic coefficient
);
};