AR_PivotTurn: pivot turn controller

also reduces default pivot speed to 60deg/sec (was 90deg/sec)
add would_active and allow activation to be forced

libraries/AR_WPNav/AR_PivotTurn.cpp

@@ -0,0 +1,151 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+#include "AR_PivotTurn.h"
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include <stdio.h>
+#endif
+
+extern const AP_HAL::HAL& hal;
+
+#define AR_PIVOT_TIMEOUT_MS     100 // pivot controller timesout and reset target if not called within this many milliseconds
+#define AR_PIVOT_ANGLE_DEFAULT  60  // default PIVOT_ANGLE parameter value
+#define AR_PIVOT_ANGLE_ACCURACY 5   // vehicle will pivot to within this many degrees of destination
+#define AR_PIVOT_RATE_DEFAULT   60  // default PIVOT_RATE parameter value
+#define AR_PIVOT_DELAY_DEFAULT  0   // default PIVOT_DELAY parameter value
+
+const AP_Param::GroupInfo AR_PivotTurn::var_info[] = {
+
+    // @Param: ANGLE
+    // @DisplayName: Pivot Angle
+    // @Description: Pivot when the difference between the vehicle's heading and its target heading is more than this many degrees. Set to zero to disable pivot turns.  This parameter should be greater than 5 degrees for pivot turns to work.
+    // @Units: deg
+    // @Range: 0 360
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("ANGLE", 1, AR_PivotTurn, _angle, AR_PIVOT_ANGLE_DEFAULT),
+
+    // @Param: RATE
+    // @DisplayName: Pivot Turn Rate
+    // @Description: Turn rate during pivot turns
+    // @Units: deg/s
+    // @Range: 0 360
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("RATE", 2, AR_PivotTurn, _rate_max, AR_PIVOT_RATE_DEFAULT),
+
+    // @Param: DELAY
+    // @DisplayName: Pivot Delay
+    // @Description: Vehicle waits this many seconds after completing a pivot turn before proceeding
+    // @Units: s
+    // @Range: 0 60
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("DELAY", 3, AR_PivotTurn, _delay, AR_PIVOT_DELAY_DEFAULT),
+
+    AP_GROUPEND
+};
+
+AR_PivotTurn::AR_PivotTurn(AR_AttitudeControl& atc) :
+    _atc(atc)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+// enable or disable pivot controller
+void AR_PivotTurn::enable(bool enable_pivot)
+{
+    _enabled = enable_pivot;
+    if (!_enabled) {
+        _active = false;
+    }
+}
+
+// true if update has been called recently
+bool AR_PivotTurn::active() const
+{
+    return _enabled && _active;
+}
+
+// checks if pivot turns should be activated or deactivated
+// force_active should be true if the caller wishes to trigger the start of a pivot turn regardless of the heading error
+void AR_PivotTurn::check_activation(float desired_heading_deg, bool force_active)
+{
+    // check cases where we clearly cannot use pivot steering
+    if (!_enabled || (_angle <= AR_PIVOT_ANGLE_ACCURACY)) {
+        _active = false;
+        return;
+    }
+
+    // calc yaw error in degrees
+    const float yaw_error = fabsf(wrap_180(desired_heading_deg - (AP::ahrs().yaw_sensor * 0.01f)));
+
+    // if error is larger than _pivot_angle start pivot steering
+    if (yaw_error > _angle || force_active) {
+        _active = true;
+        _delay_start_ms = 0;
+        return;
+    }
+
+    uint32_t now_ms = AP_HAL::millis();
+
+    // if within 5 degrees of the target heading, set start time of pivot steering
+    if (_active && (yaw_error < AR_PIVOT_ANGLE_ACCURACY) && (_delay_start_ms == 0)) {
+        _delay_start_ms = now_ms;
+    }
+
+    // exit pivot steering after the time set by pivot_delay has elapsed
+    if ((_delay_start_ms > 0) &&
+        (now_ms - _delay_start_ms) >= get_delay_duration_ms()) {
+        _active = false;
+        _delay_start_ms = 0;
+    }
+}
+
+// check if pivot turn would be activated given an expected change in yaw in degrees
+// note this does not actually active the pivot turn.  To activate use the check_activation method
+bool AR_PivotTurn::would_activate(float yaw_change_deg) const
+{
+    // check cases where we clearly cannot use pivot steering
+    if (!_enabled || (_angle <= AR_PIVOT_ANGLE_ACCURACY)) {
+        return false;
+    }
+
+    // return true if yaw change is larger than _pivot_angle
+    return fabsf(wrap_180(yaw_change_deg)) > _angle;
+}
+
+// get turn rate (in rad/sec)
+// desired heading should be the heading towards the next waypoint in degrees
+// dt should be the time since the last call in seconds
+float AR_PivotTurn::get_turn_rate_rads(float desired_heading_deg, float dt)
+{
+    // handle pivot turns
+    const float desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(desired_heading_deg), radians(_rate_max));
+
+    // update flag so that it can be cleared
+    check_activation(desired_heading_deg);
+
+    return desired_turn_rate_rads;
+}
+
+// return post-turn delay duration in milliseconds
+uint32_t AR_PivotTurn::get_delay_duration_ms() const
+{
+    return constrain_float(_delay.get(), 0.0f, 60.0f) * 1000;
+}

libraries/AR_WPNav/AR_PivotTurn.h

@@ -0,0 +1,70 @@
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <APM_Control/AR_AttitudeControl.h>
+
+/*
+ * Pivot Turn controller for skid-steering rovers and boats
+ *
+ * How-to-Use:
+ *    1. call "enable(true)" once for skid-steering vehicles to enable this controller
+ *    2. before vehicle starts towards a waypoint call "check_activation" and provide the earth-frame heading to the waypoint
+ *       this may change the controller's internal state to "active"
+ *    3. on each main loop iteration call "active()" to see if this controller thinks it is controllering the vehicle
+ *    4. call "get_turn_rate_rads()" to retrieve the desired turn rate towards the next waypoint
+ *    5. pass above turn rate into the rate controller, set speed controller's speed to zero
+ *    6. this controller's "active" state will change to false once it has completed the pivot turn
+ */
+
+class AR_PivotTurn {
+public:
+
+    // constructor
+    AR_PivotTurn(AR_AttitudeControl& atc);
+
+    // enable or disable pivot controller
+    void enable(bool enable_pivot);
+
+    // true if this controller is controlling vehicle
+    bool active() const;
+
+    // checks if pivot turns should be activated or deactivated
+    // force_active should be true if the caller wishes to trigger the start of a pivot turn regardless of the heading error
+    void check_activation(float desired_heading_deg, bool force_active = false);
+
+    // check if pivot turn would be activated given an expected change in yaw in degrees
+    // note this does not actually active the pivot turn.  To activate use the check_activation method
+    bool would_activate(float yaw_change_deg) const WARN_IF_UNUSED;
+
+    // forcibly deactivate this controller
+    void deactivate() { _active = false; };
+
+    // get turn rate (in rad/sec)
+    // desired heading should be the heading towards the next waypoint in degrees
+    // dt should be the time since the last call in seconds
+    float get_turn_rate_rads(float desired_heading_deg, float dt);
+
+    // accessors for parameter values
+    float get_rate_max() const { return _rate_max; }
+
+    // parameter var table
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+    // return post-turn delay duration in milliseconds
+    uint32_t get_delay_duration_ms() const;
+
+    // parameters
+    AP_Int16 _angle;                // minimum angle error (in degrees) that leads to pivot turn
+    AP_Int16 _rate_max;             // maximum turn rate (in degrees) during pivot turn
+    AP_Float _delay;                // waiting time (in seconds) after pivot turn completes
+
+    // references
+    AR_AttitudeControl& _atc;       // rover attitude control library
+
+    // local variables
+    bool _enabled;                  // true if vehicle can pivot
+    bool _active;                   // true if vehicle is currently pivoting
+    uint32_t _delay_start_ms;       // system time when post-turn delay started
+};