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

FW adjust throttle to airspeed mapping for ALTCTL

Browse Source
This commit is contained in:
Daniel Agar 2016-02-18 21:49:00 -05:00 committed by Lorenz Meier
parent 4c8e9265c4
commit 80f3fefcc3
1 changed files with 22 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -452,6 +452,7 @@ private:
float get_tecs_pitch();
float get_tecs_thrust();
float get_demanded_airspeed();
float calculate_target_airspeed(float airspeed_demand);
void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d,
const struct position_setpoint_triplet_s &pos_sp_triplet);
@ -900,6 +901,25 @@ FixedwingPositionControl::task_main_trampoline(int argc, char *argv[])
l1_control::g_control = nullptr;
}
float
FixedwingPositionControl::get_demanded_airspeed()
{
float altctrl_airspeed = 0;
// neutral throttle corresponds to trim airspeed
if (_manual.z < 0.5f) {
// lower half of throttle is min to trim airspeed
altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_trim - _parameters.airspeed_min) *
_manual.z * 2;
} else {
// upper half of throttle is trim to max airspeed
altctrl_airspeed = _parameters.airspeed_trim +
(_parameters.airspeed_max - _parameters.airspeed_trim) *
(_manual.z * 2 - 1);
}
return altctrl_airspeed;
}
float
FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
{
@ -1769,10 +1789,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_control_mode_current = FW_POSCTRL_MODE_POSITION;
/* Get demanded airspeed */
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
float altctrl_airspeed = get_demanded_airspeed();
/* update desired altitude based on user pitch stick input */
bool climbout_requested = update_desired_altitude(dt);
@ -1888,9 +1905,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
/* Get demanded airspeed */
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
float altctrl_airspeed = get_demanded_airspeed();
/* update desired altitude based on user pitch stick input */
bool climbout_requested = update_desired_altitude(dt);