fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 14:33:34 +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-20 14:39:59 +01:00
committed by Lorenz Meier
parent 4c8e9265c4
commit 80f3fefcc3
1 changed files with 22 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+22 -7
View File
@@ -452,6 +452,7 @@ private:
float get_tecs_pitch(); float get_tecs_pitch();
float get_tecs_thrust(); float get_tecs_thrust();
float get_demanded_airspeed();
float calculate_target_airspeed(float airspeed_demand); float calculate_target_airspeed(float airspeed_demand);
void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, 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); 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; 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 float
FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) 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; _control_mode_current = FW_POSCTRL_MODE_POSITION;
/* Get demanded airspeed */ float altctrl_airspeed = get_demanded_airspeed();
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
/* update desired altitude based on user pitch stick input */ /* update desired altitude based on user pitch stick input */
bool climbout_requested = update_desired_altitude(dt); 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; _control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
/* Get demanded airspeed */ /* Get demanded airspeed */
float altctrl_airspeed = _parameters.airspeed_min + float altctrl_airspeed = get_demanded_airspeed();
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
/* update desired altitude based on user pitch stick input */ /* update desired altitude based on user pitch stick input */
bool climbout_requested = update_desired_altitude(dt); bool climbout_requested = update_desired_altitude(dt);