diff --git a/libraries/AC_AttitudeControl/AC_WeatherVane.cpp b/libraries/AC_AttitudeControl/AC_WeatherVane.cpp index e1978c8011..cd6a9c8ac2 100644 --- a/libraries/AC_AttitudeControl/AC_WeatherVane.cpp +++ b/libraries/AC_AttitudeControl/AC_WeatherVane.cpp @@ -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); diff --git a/libraries/AC_AttitudeControl/AC_WeatherVane.h b/libraries/AC_AttitudeControl/AC_WeatherVane.h index b0cb57f8a2..d8e66498da 100644 --- a/libraries/AC_AttitudeControl/AC_WeatherVane.h +++ b/libraries/AC_AttitudeControl/AC_WeatherVane.h @@ -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;