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

AC_AttitudeControl: use deadzone for pitch

Browse Source 
when pitch for nose-in and tail-in is enabled we should use the
deadzone
This commit is contained in:
Andrew Tridgell 2022-04-12 08:17:12 +10:00
parent f4548daa5a
commit cdfa682be0
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_AttitudeControl/AC_WeatherVane.cpp
View File

@ -104,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, 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, 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) || !is_positive(_gain)) {
@ -176,8 +176,8 @@ bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, con
return false;
case Direction::NOSE_IN:
if (pitch_enable && is_positive(pitch_cdeg)) {
output = fabsf(roll_cdeg) + pitch_cdeg;
if (pitch_enable && is_positive(pitch_cdeg - deadzone_cdeg)) {
output = fabsf(roll_cdeg) + (pitch_cdeg - deadzone_cdeg);
} else {
output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0);
}
@ -204,8 +204,8 @@ bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, con
break;
case Direction::TAIL_IN:
if (pitch_enable && is_negative(pitch_cdeg)) {
output = fabsf(roll_cdeg) - pitch_cdeg;
if (pitch_enable && is_negative(pitch_cdeg + deadzone_cdeg)) {
output = fabsf(roll_cdeg) - (pitch_cdeg + deadzone_cdeg);
} else {
output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0);
}

2
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, 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, 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);