Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: added an option for pitch weathervaning 

when nose-in or tail-in, if the aircraft has significant pitch
asymmetry in hover then we would spin around in no wind if we use
pitch as an input

this makes pitch input for nose-in and tail-in optional and off by
default to preserve existing behaviour
This commit is contained in:
Andrew Tridgell 2022-04-12 08:13:06 +10:00
parent da2cb552fc
commit dbab0562d7
2 changed files with 20 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AC_AttitudeControl/AC_WeatherVane.cpp
View File

@ -87,6 +87,13 @@ const AP_Param::GroupInfo AC_WeatherVane::var_info[] = {
// @User: Standard
AP_GROUPINFO("LAND", 8, AC_WeatherVane, _landing_direction, -1),
// @Param: OPTIONS
// @DisplayName: Weathervaning options
// @Description: Options impacting weathervaning behaviour
// @Bitmask: 0:Use pitch when nose or tail-in for faster weathervaning
// @User: Standard
AP_GROUPINFO("OPTIONS", 9, AC_WeatherVane, _options, 0),
AP_GROUPEND
};
@ -97,7 +104,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, const bool is_takeoff, const bool is_landing)
bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, float pitch_cdeg, const bool is_takeoff, const bool is_landing)
{
Direction dir = (Direction)_direction.get();
if ((dir == Direction::OFF) || !allowed || (pilot_yaw != 0) || !is_positive(_gain)) {
@ -159,13 +166,17 @@ bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, con
const float deadzone_cdeg = _min_dz_ang_deg*100.0;
float output = 0.0;
const char* dir_string = "";
// should we enable pitch input for nose-in and tail-in?
const bool pitch_enable = (uint8_t(_options.get()) & uint8_t(Options::PITCH_ENABLE)) != 0;
switch (dir) {
case Direction::OFF:
reset();
return false;
case Direction::NOSE_IN:
if (is_positive(pitch_cdeg)) {
if (pitch_enable && is_positive(pitch_cdeg)) {
output = fabsf(roll_cdeg) + pitch_cdeg;
} else {
output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0);
@ -193,7 +204,7 @@ bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, con
break;
case Direction::TAIL_IN:
if (is_negative(pitch_cdeg)) {
if (pitch_enable && is_negative(pitch_cdeg)) {
output = fabsf(roll_cdeg) - pitch_cdeg;
} else {
output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0);

7
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, const bool is_takeoff, const bool is_landing);
bool get_yaw_out(float &yaw_output, const int16_t pilot_yaw, const float hgt, const float roll_cdeg, 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);
@ -31,6 +31,10 @@ class AC_WeatherVane {
TAIL_IN = 4, // backwards, for tailsitters, makes it easier to descend
};
enum class Options {
PITCH_ENABLE = (1<<0),
};
// Paramaters
AP_Int8 _direction;
AP_Float _gain;
@ -40,6 +44,7 @@ class AC_WeatherVane {
AP_Float _max_vel_z;
AP_Int8 _landing_direction;
AP_Int8 _takeoff_direction;
AP_Int16 _options;
float last_output;
bool active_msg_sent;