fw pos ctrl: encapsulate wing tip strike constraint for roll angle

- apply constraint only for takeoff and landing modes
- add two params, wing span and wing height, to calculate a reasonable height at which roll limits can be opened
This commit is contained in:
Thomas Stastny
2022-07-18 10:49:27 -04:00
committed by Daniel Agar
parent 5241f016f7
commit 7c6ce436ca
3 changed files with 66 additions and 10 deletions
@@ -646,6 +646,16 @@ private:
bool climbout_mode, float climbout_pitch_min_rad,
bool disable_underspeed_detection = false, float hgt_rate_sp = NAN);
/**
* @brief Constrains the roll angle setpoint near ground to avoid wingtip strike.
*
* @param roll_setpoint Unconstrained roll angle setpoint [rad]
* @param altitude Vehicle altitude (AMSL) [m]
* @param terrain_altitude Terrain altitude (AMSL) [m]
* @return Constrained roll angle setpoint [rad]
*/
float constrainRollNearGround(const float roll_setpoint, const float altitude, const float terrain_altitude) const;
DEFINE_PARAMETERS(
(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
@@ -736,8 +746,10 @@ private:
(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad,
(ParamFloat<px4::params::WEIGHT_BASE>) _param_weight_base,
(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross
(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross,
(ParamFloat<px4::params::FW_WING_SPAN>) _param_fw_wing_span,
(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height
)
};