From 36aeb9defc1a4059e9a7ff7c508649b8c4f95c7d Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 22 Apr 2019 09:54:07 -0400
Subject: [PATCH] WIP: FW attitude control limit airspeed scaling changes per
iteration
---
.../FixedwingAttitudeControl.cpp | 37 +++++++++++--------
.../FixedwingAttitudeControl.hpp | 5 ++-
2 files changed, 25 insertions(+), 17 deletions(-)
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
index fc94ad4bab..b91d146b57 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -33,6 +33,8 @@
#include "FixedwingAttitudeControl.hpp"
+using namespace time_literals;
+
/**
* Fixedwing attitude control app start / stop handling function
*
@@ -466,30 +468,31 @@ FixedwingAttitudeControl::vehicle_land_detected_poll()
}
}
-void FixedwingAttitudeControl::get_airspeed_and_scaling(float &airspeed, float &airspeed_scaling)
+float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
{
+ _airspeed_sub.update();
const bool airspeed_valid = PX4_ISFINITE(_airspeed_sub.get().indicated_airspeed_m_s)
- && (_airspeed_sub.get().indicated_airspeed_m_s > 0.0f)
- && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1e6);
+ && (_airspeed_sub.get().indicated_airspeed_m_s >= 0.0f)
+ && (hrt_elapsed_time(&_airspeed_sub.get().timestamp) < 1_s);
+
+ if (!airspeed_valid) {
+ perf_count(_nonfinite_input_perf);
+ }
+
+ // if no airspeed measurement is available out best guess is to use the trim airspeed
+ float airspeed = _parameters.airspeed_trim;
if (!_parameters.airspeed_disabled && airspeed_valid) {
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed_sub.get().indicated_airspeed_m_s);
} else {
- // if no airspeed measurement is available out best guess is to use the trim airspeed
- airspeed = _parameters.airspeed_trim;
-
// VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible
// this assumption is good as long as the vehicle is not hovering in a headwind which is much larger
// than the minimum airspeed
if (_vehicle_status.is_vtol && _vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
airspeed = _parameters.airspeed_min;
}
-
- if (!airspeed_valid) {
- perf_count(_nonfinite_input_perf);
- }
}
/*
@@ -499,7 +502,12 @@ void FixedwingAttitudeControl::get_airspeed_and_scaling(float &airspeed, float &
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
- airspeed_scaling = _parameters.airspeed_trim / math::max(airspeed, _parameters.airspeed_min);
+ const float airspeed_constrained = math::constrain(airspeed, _parameters.airspeed_min, _parameters.airspeed_max);
+ const float airspeed_scaling = _parameters.airspeed_trim / airspeed_constrained;
+
+ _airspeed_scaling = math::constrain(airspeed_scaling, _airspeed_scaling - 0.01f, _airspeed_scaling + 0.01f);
+
+ return airspeed;
}
void FixedwingAttitudeControl::run()
@@ -604,7 +612,6 @@ void FixedwingAttitudeControl::run()
matrix::Eulerf euler_angles(R);
- _airspeed_sub.update();
vehicle_attitude_setpoint_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
@@ -637,9 +644,7 @@ void FixedwingAttitudeControl::run()
/* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_rates_enabled) {
- float airspeed;
- float airspeed_scaling;
- get_airspeed_and_scaling(airspeed, airspeed_scaling);
+ const float airspeed = get_airspeed_and_update_scaling();
/* Use min airspeed to calculate ground speed scaling region.
* Don't scale below gspd_scaling_trim
@@ -689,7 +694,7 @@ void FixedwingAttitudeControl::run()
control_input.airspeed_min = _parameters.airspeed_min;
control_input.airspeed_max = _parameters.airspeed_max;
control_input.airspeed = airspeed;
- control_input.scaler = airspeed_scaling;
+ control_input.scaler = _airspeed_scaling;
control_input.lock_integrator = lock_integrator;
control_input.groundspeed = groundspeed;
control_input.groundspeed_scaler = groundspeed_scaler;
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
index 0867b63900..abf960073b 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
@@ -132,6 +132,8 @@ private:
float _flaps_applied{0.0f};
float _flaperons_applied{0.0f};
+ float _airspeed_scaling{1.0f};
+
bool _landed{true};
float _battery_scale{1.0f};
@@ -298,5 +300,6 @@ private:
void global_pos_poll();
void vehicle_status_poll();
void vehicle_land_detected_poll();
- void get_airspeed_and_scaling(float &airspeed, float &airspeed_scaling);
+
+ float get_airspeed_and_update_scaling();
};
--
GitLab