aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttitudeControl: WeatherVane: add takeoff and landing override params

Browse Source
This commit is contained in:
Peter Hall 2022-01-23 22:23:37 +00:00 committed by Andrew Tridgell
parent 8fdc85ccbe
commit a56eac0274
2 changed files with 31 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AC_AttitudeControl/AC_WeatherVane.cpp
View File

@ -71,6 +71,20 @@ const AP_Param::GroupInfo AC_WeatherVane::var_info[] = {
// @User: Standard
AP_GROUPINFO("VELZ_MAX", 6, AC_WeatherVane, _max_vel_z, WVANE_PARAM_VELZ_MAX_DEFAULT),
// @Param: TAKEOFF
// @DisplayName: Takeoff override
// @Description: Override the weather vaning behaviour when in takeoffs
// @Values: -1:No override,0:Disabled,1:Nose into wind,2:Nose or tail into wind,3:Side into wind,4:tail into wind
// @User: Standard
AP_GROUPINFO("TAKEOFF", 7, AC_WeatherVane, _takeoff_direction, -1),
// @Param: LAND
// @DisplayName: Landing override
// @Description: Override the weather vaning behaviour when in landing
// @Values: -1:No override,0:Disabled,1:Nose into wind,2:Nose or tail into wind,3:Side into wind,4:tail into wind
// @User: Standard
AP_GROUPINFO("LAND", 8, AC_WeatherVane, _landing_direction, -1),
AP_GROUPEND
};
@ -81,7 +95,7 @@ AC_WeatherVane::AC_WeatherVane(void)
AP_Param::setup_object_defaults(this, var_info);
}
bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg)
bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg, const bool is_takeoff, const bool is_landing)
{
Direction dir = (Direction)_direction.get();
if ((dir == Direction::OFF) || !allowed || (pilot_yaw != 0)) {
@ -92,6 +106,19 @@ bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, con
return false;
}
// override direction when in takeoff for landing
if (is_takeoff && (_takeoff_direction >= 0)) {
dir = (Direction)_takeoff_direction.get();
}
if (is_landing && (_landing_direction >= 0)) {
dir = (Direction)_landing_direction.get();
}
if (dir == Direction::OFF) {
// Disabled for takeoff or landing
reset();
return false;
}
// Check if we are above the minimum height to weather vane
if (is_positive(_min_height) && (hgt <= _min_height)) {
reset();

4
libraries/AC_AttitudeControl/AC_WeatherVane.h
View File

@ -10,7 +10,7 @@ class AC_WeatherVane {
CLASS_NO_COPY(AC_WeatherVane);
// Calculate and return the yaw output to weathervane the vehicle
bool get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg);
bool get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, const float pitch_cdeg, const bool is_takeoff, const bool is_landing);
// Function to reset all flags and set values. Invoked whenever the weather vaning process is interrupted
void reset(void);
@ -38,6 +38,8 @@ class AC_WeatherVane {
AP_Float _min_height;
AP_Float _max_vel_xy;
AP_Float _max_vel_z;
AP_Int8 _landing_direction;
AP_Int8 _takeoff_direction;
float last_output;
bool active_msg_sent;