Skip to content

Commit

Permalink
cleanup handling of trim throttle
Browse files Browse the repository at this point in the history 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
  • Loading branch information
@RomanBapst
RomanBapst committed Jun 10, 2022
1 parent 5dd5199 commit fc2a907
Show file tree
Hide file tree
Showing 2 changed files with 16 additions and 40 deletions.
47 changes: 13 additions & 34 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -958,7 +958,6 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_param_fw_thr_trim.get(),
false,
_param_fw_p_lim_min.get());

Expand Down Expand Up @@ -992,7 +991,6 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_param_fw_thr_trim.get(),
false,
_param_fw_p_lim_min.get(),
false,
Expand Down Expand Up @@ -1091,7 +1089,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co

float tecs_fw_thr_min;
float tecs_fw_thr_max;
float tecs_fw_mission_throttle;

float mission_throttle = _param_fw_thr_trim.get();

Expand All @@ -1105,12 +1102,10 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
_tecs.set_speed_weight(2.0f);
tecs_fw_thr_min = 0.0;
tecs_fw_thr_max = 0.0;
tecs_fw_mission_throttle = 0.0;

} else {
tecs_fw_thr_min = _param_fw_thr_min.get();
tecs_fw_thr_max = _param_fw_thr_max.get();
tecs_fw_mission_throttle = mission_throttle;
}

// waypoint is a plain navigation waypoint
Expand Down Expand Up @@ -1190,7 +1185,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
tecs_fw_thr_max,
tecs_fw_mission_throttle,
false,
radians(_param_fw_p_lim_min.get()));
}
Expand All @@ -1201,7 +1195,6 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
{
float tecs_fw_thr_min;
float tecs_fw_thr_max;
float tecs_fw_mission_throttle;

float mission_throttle = _param_fw_thr_trim.get();

Expand All @@ -1215,12 +1208,10 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
_tecs.set_speed_weight(2.0f);
tecs_fw_thr_min = 0.0;
tecs_fw_thr_max = 0.0;
tecs_fw_mission_throttle = 0.0;

} else {
tecs_fw_thr_min = _param_fw_thr_min.get();
tecs_fw_thr_max = _param_fw_thr_max.get();
tecs_fw_mission_throttle = mission_throttle;
}

// waypoint is a plain navigation waypoint
Expand Down Expand Up @@ -1257,7 +1248,6 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
tecs_fw_thr_max,
tecs_fw_mission_throttle,
false,
radians(_param_fw_p_lim_min.get()),
tecs_status_s::TECS_MODE_NORMAL,
Expand Down Expand Up @@ -1298,7 +1288,6 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons

float tecs_fw_thr_min;
float tecs_fw_thr_max;
float tecs_fw_mission_throttle;

float mission_throttle = _param_fw_thr_trim.get();

Expand All @@ -1312,12 +1301,10 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
_tecs.set_speed_weight(2.0f);
tecs_fw_thr_min = 0.0;
tecs_fw_thr_max = 0.0;
tecs_fw_mission_throttle = 0.0;

} else {
tecs_fw_thr_min = _param_fw_thr_min.get();
tecs_fw_thr_max = _param_fw_thr_max.get();
tecs_fw_mission_throttle = mission_throttle;
}

/* waypoint is a loiter waypoint */
Expand Down Expand Up @@ -1394,7 +1381,6 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
radians(_param_fw_p_lim_max.get()),
tecs_fw_thr_min,
tecs_fw_thr_max,
tecs_fw_mission_throttle,
false,
radians(_param_fw_p_lim_min.get()));
}
Expand Down Expand Up @@ -1485,7 +1471,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
radians(takeoff_pitch_max_deg),
_param_fw_thr_min.get(),
_param_fw_thr_max.get(), // XXX should we also set runway_takeoff_throttle here?
_param_fw_thr_trim.get(),
_runway_takeoff.climbout(),
radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())));

Expand Down Expand Up @@ -1573,7 +1558,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
radians(takeoff_pitch_max_deg),
_param_fw_thr_min.get(),
takeoff_throttle,
_param_fw_thr_trim.get(),
true,
radians(_takeoff_pitch_min.get()));

Expand All @@ -1588,7 +1572,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
takeoff_throttle,
_param_fw_thr_trim.get(),
false,
radians(_param_fw_p_lim_min.get()));
}
Expand Down Expand Up @@ -1808,8 +1791,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_land, ground_speed);

const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;

/* lateral guidance */
if (_param_fw_use_npfg.get()) {
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
Expand Down Expand Up @@ -1861,7 +1842,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
radians(_param_fw_lnd_fl_pmax.get()),
0.0f,
throttle_max,
throttle_land,
false,
_land_motor_lim ? radians(_param_fw_lnd_fl_pmin.get()) : radians(_param_fw_p_lim_min.get()),
true);
Expand Down Expand Up @@ -1965,7 +1945,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_param_fw_thr_trim.get(),
false,
radians(_param_fw_p_lim_min.get()));
_att_sp.pitch_body = get_tecs_pitch();
Expand Down Expand Up @@ -2025,7 +2004,6 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
throttle_max,
_param_fw_thr_trim.get(),
false,
pitch_limit_min,
false,
Expand Down Expand Up @@ -2155,7 +2133,6 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
radians(_param_fw_p_lim_max.get()),
_param_fw_thr_min.get(),
throttle_max,
_param_fw_thr_trim.get(),
false,
pitch_limit_min,
false,
Expand Down Expand Up @@ -2575,25 +2552,27 @@ FixedwingPositionControl::get_nav_speed_2d(const Vector2f &ground_speed)
return nav_speed_2d;
}

float FixedwingPositionControl::mapAirspeedSetpointToTrimThrottle(float airspeed_sp, float throttle_trim,
float FixedwingPositionControl::mapAirspeedSetpointToTrimThrottle(float airspeed_sp,
float throttle_min, float throttle_max)
{
float throttle_trim_min = _param_fw_thr_trim_min.get();
float throttle_trim_max = _param_fw_thr_trim_max.get();

float throttle_mapped = throttle_trim;
float throttle_mapped = _param_fw_thr_trim.get();

if (throttle_trim_min > 0.f && throttle_trim_min <= throttle_trim && airspeed_sp < _param_fw_airspd_trim.get()) {
if (throttle_trim_min > 0.f && throttle_trim_min <= _param_fw_thr_trim.get()
&& airspeed_sp < _param_fw_airspd_trim.get()) {
const float airspeed_delta = _param_fw_airspd_trim.get() - airspeed_sp;
const float throttle_delta = throttle_trim - throttle_trim_min;
const float throttle_delta = _param_fw_thr_trim.get() - throttle_trim_min;
throttle_mapped = throttle_trim_min + throttle_delta * (airspeed_sp -
_param_fw_airspd_min.get()) / math::max(
airspeed_delta, FLT_EPSILON);

} else if (throttle_trim_max > 0.f && throttle_trim_max > throttle_trim && airspeed_sp > _param_fw_airspd_trim.get()) {
} else if (throttle_trim_max > 0.f && throttle_trim_max > _param_fw_thr_trim.get()
&& airspeed_sp > _param_fw_airspd_trim.get()) {
const float airspeed_delta = _param_fw_airspd_max.get() - airspeed_sp;
const float throttle_delta = throttle_trim_max - throttle_trim;
throttle_mapped = throttle_trim + throttle_delta * (airspeed_sp -
const float throttle_delta = throttle_trim_max - _param_fw_thr_trim.get();
throttle_mapped = _param_fw_thr_trim.get() + throttle_delta * (airspeed_sp -
_param_fw_airspd_trim.get()) / math::max(
airspeed_delta, FLT_EPSILON);
}
Expand Down Expand Up @@ -2623,10 +2602,10 @@ float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float
return math::constrain(throttle_trim * sqrtf(weight_ratio) * air_density_throttle_scale, throttle_min, throttle_max);
}

float FixedwingPositionControl::calculateThrottleTrimCompensated(float airspeed_sp, float throttle_trim,
float FixedwingPositionControl::calculateThrottleTrimCompensated(float airspeed_sp,
float throttle_min, float throttle_max)
{
float throttle_trim_for_desired_airspeed = mapAirspeedSetpointToTrimThrottle(airspeed_sp, throttle_trim,
float throttle_trim_for_desired_airspeed = mapAirspeedSetpointToTrimThrottle(airspeed_sp,
throttle_min, throttle_max);

return compensateTrimThrottleForDensityAndWeight(throttle_trim_for_desired_airspeed, throttle_min, throttle_max);
Expand All @@ -2635,7 +2614,7 @@ float FixedwingPositionControl::calculateThrottleTrimCompensated(float airspeed_
void
FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_trim,
float throttle_min, float throttle_max,
bool climbout_mode, float climbout_pitch_min_rad,
bool disable_underspeed_detection, float hgt_rate_sp)
{
Expand Down Expand Up @@ -2706,7 +2685,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
_tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_alt_control,
_current_altitude, _local_pos.vz);

float throttle_trim_comp = calculateThrottleTrimCompensated(throttle_trim, airspeed_sp, throttle_min, throttle_max);
float throttle_trim_comp = calculateThrottleTrimCompensated(airspeed_sp, throttle_min, throttle_max);

_tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()),
_current_altitude,
Expand Down
9 changes: 3 additions & 6 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -605,12 +605,11 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
* @brief Maps an equivalent airspeed setpoint to a trim throttle value.
*
* @param airspeed_sp Equivalent airspeed setpoint [m/s]
* @param throttle_trim Trim throttle required to fly at FW_AIRSPD_TRIM.
* @param throttle_min Minimum allowed trim throttle.
* @param throttle_max Maximum allowed trim throttle.
* @return Trim throttle required to fly level at airspeed_sp
*/
float mapAirspeedSetpointToTrimThrottle(float airspeed_sp, float throttle_trim, float throttle_min,
float mapAirspeedSetpointToTrimThrottle(float airspeed_sp, float throttle_min,
float throttle_max);

/**
Expand All @@ -628,12 +627,11 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
*
*
* @param airspeed_sp Equivalent airspeed setpoint [m/s]
* @param throttle_trim Trim throttle required to fly at equivalent trim airspeed.
* @param throttle_min Minimum allowed trim throttle.
* @param throttle_max Maximum allowed trim throttle.
* @return trim throttle value for flying at a desired equivalent airspeed setpoint compensated for air density and vehicle weight.
*/
float calculateThrottleTrimCompensated(float airspeed_sp, float throttle_trim, float throttle_min,
float calculateThrottleTrimCompensated(float airspeed_sp, float throttle_min,
float throttle_max);

void publishOrbitStatus(const position_setpoint_s pos_sp);
Expand All @@ -650,15 +648,14 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
* @param pitch_max_rad Nominal pitch angle command maximum [rad]
* @param throttle_min Minimum throttle command [0,1]
* @param throttle_max Maximum throttle command [0,1]
* @param throttle_cruise Throttle required for level flight at cruising airspeed [0,1]
* @param climbout_mode True if TECS should engage climbout mode
* @param climbout_pitch_min_rad Minimum pitch angle command in climbout mode [rad]
* @param disable_underspeed_detection True if underspeed detection should be disabled
* @param hgt_rate_sp Height rate setpoint [m/s]
*/
void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_trim,
float throttle_min, float throttle_max,
bool climbout_mode, float climbout_pitch_min_rad,
bool disable_underspeed_detection = false, float hgt_rate_sp = NAN);

Expand Down

0 comments on commit fc2a907

Please sign in to comment.