AC_AttitudeControl: add weathervane library

libraries/AC_AttitudeControl/AC_WeatherVane.cpp

@@ -0,0 +1,187 @@
+/*
+ * Aircraft Weathervane options common to vtol plane and copters
+ */
+#include <AP_Vehicle/AP_Vehicle_Type.h>
+#include "AC_WeatherVane.h"
+#include <GCS_MAVLink/GCS.h>
+#include <AP_AHRS/AP_AHRS.h>
+
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+    #define WVANE_PARAM_ENABLED 1
+    #define WVANE_PARAM_SPD_MAX_DEFAULT 0
+    #define WVANE_PARAM_VELZ_MAX_DEFAULT 0
+#else
+    #define WVANE_PARAM_ENABLED 0
+    #define WVANE_PARAM_SPD_MAX_DEFAULT 2
+    #define WVANE_PARAM_VELZ_MAX_DEFAULT 1
+#endif
+
+
+const AP_Param::GroupInfo AC_WeatherVane::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: Enable
+    // @Description: Enable weather vaning.  When active, the aircraft will automatically yaw into wind when in a VTOL position controlled mode. Pilot yaw commands overide the weathervaning action.
+    // @Values: 0:Disabled,1:Nose into wind,2:Nose or tail into wind,3:Side into wind,4:tail into wind
+    // @User: Standard
+    AP_GROUPINFO_FLAGS("ENABLE", 1, AC_WeatherVane, _direction, WVANE_PARAM_ENABLED, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: GAIN
+    // @DisplayName: Weathervaning gain
+    // @Description: This converts the target roll/pitch angle of the aircraft into the correcting (into wind) yaw rate. e.g. Gain = 2, roll = 30 deg, pitch = 0 deg, yaw rate = 60 deg/s.
+    // @Range: 0.5 4
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("GAIN", 2, AC_WeatherVane, _gain, 1.0),
+
+    // @Param: ANG_MIN
+    // @DisplayName: Weathervaning min angle
+    // @Description: The minimum target roll/pitch angle before active weathervaning will start.  This provides a dead zone that is particularly useful for poorly trimmed quadplanes.
+    // @Units: deg
+    // @Range: 0 10
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("ANG_MIN", 3, AC_WeatherVane, _min_dz_ang_deg, 1.0),
+
+    // @Param: HGT_MIN
+    // @DisplayName: Weathervaning min height
+    // @Description{Copter}: Above this height weathervaning is permitted.  If a range finder is fitted or if terrain is enabled, this parameter sets height AGL.  Otherwise, this parameter sets height above home.  Set zero to ignore minimum height requirement to activate weathervaning.
+    // @Description{Plane}: Above this height weathervaning is permitted.  If RNGFND_LANDING is enabled or terrain is enabled then this parameter sets height AGL. Otherwise this parameter sets height above home.  Set zero to ignore minimum height requirement to activate weathervaning
+    // @Units: m
+    // @Range: 0 50
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("HGT_MIN", 4, AC_WeatherVane, _min_height, 0.0),
+
+    // @Param: SPD_MAX
+    // @DisplayName: Weathervaning max ground speed
+    // @Description: Below this ground speed weathervaning is permitted. Set to 0 to ignore this condition when checking if vehicle should weathervane.
+    // @Units: m/s
+    // @Range: 0 50
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("SPD_MAX", 5, AC_WeatherVane, _max_vel_xy, WVANE_PARAM_SPD_MAX_DEFAULT),
+
+    // @Param: VELZ_MAX
+    // @DisplayName: Weathervaning max vertical speed
+    // @Description: The maximum climb or descent speed that the vehicle will still attempt to weathervane. Set to 0 to ignore this condition to get the aircraft to weathervane at any climb/descent rate.  This is particularly useful for aircraft with low disc loading that struggle with yaw control in decent.
+    // @Units: m/s
+    // @Range: 0 5
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("VELZ_MAX", 6, AC_WeatherVane, _max_vel_z, WVANE_PARAM_VELZ_MAX_DEFAULT),
+
+    AP_GROUPEND
+};
+
+
+// Constructor
+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)
+{
+    Direction dir = (Direction)_direction.get();
+    if ((dir == Direction::OFF) || !allowed || (pilot_yaw != 0)) {
+        // parameter disabled
+        // disabled temporarily
+        // dont't override pilot
+        reset();
+        return false;
+    }
+
+    // Check if we are above the minimum height to weather vane
+    if (is_positive(_min_height) && (hgt <= _min_height)) {
+        reset();
+        return false;
+    }
+
+    // Check if we meet the velocity thresholds to allow weathervaning
+    if (is_positive(_max_vel_xy) || is_positive(_max_vel_z)) {
+        Vector3f vel_ned;
+        if (!AP::ahrs().get_velocity_NED(vel_ned) || // need speed estimate
+                (is_positive(_max_vel_xy) && (vel_ned.xy().length_squared() > (_max_vel_xy*_max_vel_xy))) || // check xy speeed
+                (is_positive(_max_vel_z) && (fabsf(vel_ned.z) > _max_vel_z))) { // check z speed
+            reset();
+            return false;
+        }
+    }
+
+    const uint32_t now = AP_HAL::millis();
+    if (now - last_check_ms > 250) {
+        // not run this function or reset recently
+        reset();
+    }
+    last_check_ms = now;
+
+    /*
+      Use a 2 second buffer to ensure weathervaning occurs once the vehicle has
+      clearly achieved an acceptable condition.
+    */
+    if (first_activate_ms == 0) {
+        first_activate_ms = now;
+    }
+    if (now - first_activate_ms < 2000) {
+        return false;
+    }
+
+    const float deadzone_cdeg = _min_dz_ang_deg*100.0;
+    float output = 0.0;
+    const char* dir_string = "";
+    switch (dir) {
+        case Direction::OFF:
+            reset();
+            return false;
+
+        case Direction::NOSE_IN:
+            if (is_positive(pitch_cdeg)) {
+                output = fabsf(roll_cdeg) + pitch_cdeg;
+            } else {
+                output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0);
+            }
+            if (is_negative(roll_cdeg)) {
+                output *= -1.0;
+            }
+            dir_string = "nose in";
+            break;
+
+        case Direction::NOSE_OR_TAIL_IN:
+            output = MAX(fabsf(roll_cdeg) - deadzone_cdeg, 0.0);
+            if (is_negative(roll_cdeg) != is_positive(pitch_cdeg)) {
+                output *= -1.0;
+            }
+            dir_string = "nose or tail in";
+            break;
+
+        case Direction::SIDE_IN:
+            output = MAX(fabsf(pitch_cdeg) - deadzone_cdeg, 0.0);
+            if (is_positive(pitch_cdeg) != is_positive(roll_cdeg)) {
+                output *= -1.0;
+            }
+            dir_string = "side in";
+            break;
+
+    }
+
+    if (!active_msg_sent) {
+        gcs().send_text(MAV_SEVERITY_INFO, "Weathervane Active: %s", dir_string);
+        active_msg_sent = true;
+    }
+
+    // Slew output and apply gain
+    last_output = 0.98 * last_output + 0.02 * output * _gain;
+    yaw_output = last_output;
+    return true;
+}
+
+// Reset the weathervane controller
+void AC_WeatherVane::reset(void)
+{
+    last_output = 0;
+    active_msg_sent = false;
+    first_activate_ms = 0;
+    last_check_ms = AP_HAL::millis();
+}
+

libraries/AC_AttitudeControl/AC_WeatherVane.h

@@ -0,0 +1,48 @@
+#include <AP_Param/AP_Param.h>
+
+// weather vane class
+class AC_WeatherVane {
+    public:
+
+        // Constructor
+        AC_WeatherVane(void);
+
+        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);
+
+        // Function to reset all flags and set values. Invoked whenever the weather vaning process is interrupted
+        void reset(void);
+
+        // allow/disallow weather vaning from other means than by the parameter
+        void allow_weathervaning(bool allow) { allowed = allow; }
+
+        static const struct AP_Param::GroupInfo var_info[];
+
+    private:
+
+        // Different options for the direction that vehicle will turn into wind
+        enum class Direction {
+            OFF = 0,
+            NOSE_IN = 1, // Only nose into wind
+            NOSE_OR_TAIL_IN = 2, // Nose in or tail into wind, which ever is closest
+            SIDE_IN = 3, // Side into wind for copter tailsitters
+        };
+
+        // Paramaters
+        AP_Int8 _direction;
+        AP_Float _gain;
+        AP_Float _min_dz_ang_deg;
+        AP_Float _min_height;
+        AP_Float _max_vel_xy;
+        AP_Float _max_vel_z;
+
+        float last_output;
+        bool active_msg_sent;
+        uint32_t first_activate_ms;
+        uint32_t last_check_ms;
+
+        // Init to true here to avoid a race between init of RC_channel and weathervane
+        bool allowed = true;
+};