zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_AttitudeControl: use deadzone for pitch 

when pitch for nose-in and tail-in is enabled we should use the
deadzone
apm_2208
Andrew Tridgell 3 years ago
parent
f4548daa5a
commit
cdfa682be0
2 changed files with 6 additions and 6 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      libraries/AC_AttitudeControl/AC_WeatherVane.cpp
  2. 2
      libraries/AC_AttitudeControl/AC_WeatherVane.h

10
libraries/AC_AttitudeControl/AC_WeatherVane.cpp
Unescape View File

@ -104,7 +104,7 @@ AC_WeatherVane::AC_WeatherVane(void)
AP_Param::setup_object_defaults(this, var_info); 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(); Direction dir = (Direction)_direction.get();
if ((dir == Direction::OFF) || !allowed || (pilot_yaw != 0) || !is_positive(_gain)) { 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; return false;
case Direction::NOSE_IN: case Direction::NOSE_IN:
if (pitch_enable && is_positive(pitch_cdeg)) { if (pitch_enable && is_positive(pitch_cdeg - deadzone_cdeg)) {
output = fabsf(roll_cdeg) + pitch_cdeg; output = fabsf(roll_cdeg) + (pitch_cdeg - deadzone_cdeg);
} else { } else {
output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0); 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; break;
case Direction::TAIL_IN: case Direction::TAIL_IN:
if (pitch_enable && is_negative(pitch_cdeg)) { if (pitch_enable && is_negative(pitch_cdeg + deadzone_cdeg)) {
output = fabsf(roll_cdeg) - pitch_cdeg; output = fabsf(roll_cdeg) - (pitch_cdeg + deadzone_cdeg);
} else { } else {
output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0); output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0);
} }

2
libraries/AC_AttitudeControl/AC_WeatherVane.h
Unescape View File

@ -10,7 +10,7 @@ class AC_WeatherVane {
CLASS_NO_COPY(AC_WeatherVane); CLASS_NO_COPY(AC_WeatherVane);
// Calculate and return the yaw output to weathervane the vehicle // 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 // Function to reset all flags and set values. Invoked whenever the weather vaning process is interrupted
void reset(void); void reset(void);

Write Preview
Loading…
Cancel
Save