dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

WIP: FW attitude control limit airspeed scaling changes per iteration

This commit is contained in:
Daniel Agar 2019-04-22 09:54:07 -04:00 committed by Lorenz Meier
parent eba0bb389a
commit 36aeb9defc
2 changed files with 25 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -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;

5
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -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();
};