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

FW Position Control: also run airspeed adaptions based on wind, accelerated stall etc. in manual modes

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-11-18 17:35:34 +01:00
parent cec16dd9b3
commit f2ca9387cf
2 changed files with 14 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+11 -9
View File
@@ -403,7 +403,7 @@ FixedwingPositionControl::get_manual_airspeed_setpoint()
} }
float float
FixedwingPositionControl::get_auto_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint, FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint,
float calibrated_min_airspeed, const Vector2f &ground_speed) float calibrated_min_airspeed, const Vector2f &ground_speed)
{ {
if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) { if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) {
@@ -1151,7 +1151,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
} }
} }
float target_airspeed = get_auto_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
_param_fw_airspd_min.get(), ground_speed); _param_fw_airspd_min.get(), ground_speed);
Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
@@ -1225,7 +1225,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy}; Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy};
_target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0))); _target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0)));
float target_airspeed = get_auto_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
_param_fw_airspd_min.get(), ground_speed); _param_fw_airspd_min.get(), ground_speed);
if (_param_fw_use_npfg.get()) { if (_param_fw_use_npfg.get()) {
@@ -1327,7 +1327,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF; _att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
} }
float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_sp, _param_fw_airspd_min.get(), float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_sp, _param_fw_airspd_min.get(),
ground_speed); ground_speed);
Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
@@ -1423,7 +1423,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
adjusted_min_airspeed = takeoff_airspeed; adjusted_min_airspeed = takeoff_airspeed;
} }
float target_airspeed = get_auto_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed, float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed,
ground_speed); ground_speed);
// yaw control is disabled once in "taking off" state // yaw control is disabled once in "taking off" state
@@ -1557,7 +1557,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) { if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) {
/* Launch has been detected, hence we have to control the plane. */ /* Launch has been detected, hence we have to control the plane. */
float target_airspeed = get_auto_airspeed_setpoint(control_interval, _param_fw_airspd_trim.get(), float target_airspeed = adapt_airspeed_setpoint(control_interval, _param_fw_airspd_trim.get(),
_param_fw_airspd_min.get(), ground_speed); _param_fw_airspd_min.get(), ground_speed);
if (_param_fw_use_npfg.get()) { if (_param_fw_use_npfg.get()) {
@@ -1670,7 +1670,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
adjusted_min_airspeed = airspeed_land; adjusted_min_airspeed = airspeed_land;
} }
float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed, float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed,
ground_speed); ground_speed);
// calculate the altitude setpoint based on the landing glide slope // calculate the altitude setpoint based on the landing glide slope
@@ -1867,7 +1867,8 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
{ {
updateManualTakeoffStatus(); updateManualTakeoffStatus();
const float calibrated_airspeed_sp = get_manual_airspeed_setpoint(); const float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(),
_param_fw_airspd_min.get(), ground_speed);
const float height_rate_sp = getManualHeightRateSetpoint(); const float height_rate_sp = getManualHeightRateSetpoint();
// TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are // TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are
@@ -1913,7 +1914,8 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
{ {
updateManualTakeoffStatus(); updateManualTakeoffStatus();
float calibrated_airspeed_sp = get_manual_airspeed_setpoint(); float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(),
_param_fw_airspd_min.get(), ground_speed);
const float height_rate_sp = getManualHeightRateSetpoint(); const float height_rate_sp = getManualHeightRateSetpoint();
// TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are // TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+3 -3
View File
@@ -633,7 +633,7 @@ private:
float get_manual_airspeed_setpoint(); float get_manual_airspeed_setpoint();
/** /**
* @brief Returns a calibrated airspeed setpoint for auto modes. * @brief Returns am adapted calibrated airspeed setpoint
* *
* Adjusts the setpoint for wind, accelerated stall, and slew rates. * Adjusts the setpoint for wind, accelerated stall, and slew rates.
* *
@@ -643,8 +643,8 @@ private:
* @param ground_speed Vehicle ground velocity vector (NE) [m/s] * @param ground_speed Vehicle ground velocity vector (NE) [m/s]
* @return Adjusted calibrated airspeed setpoint [m/s] * @return Adjusted calibrated airspeed setpoint [m/s]
*/ */
float get_auto_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint, float adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint,
float calibrated_min_airspeed, const Vector2f &ground_speed); float calibrated_min_airspeed, const Vector2f &ground_speed);
void reset_takeoff_state(); void reset_takeoff_state();
void reset_landing_state(); void reset_landing_state();