Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Common/AP_Common.h>
       4             : #include <APM_Control/AR_AttitudeControl.h>
       5             : 
       6             : /*
       7             :  * Pivot Turn controller for skid-steering rovers and boats
       8             :  *
       9             :  * How-to-Use:
      10             :  *    1. call "enable(true)" once for skid-steering vehicles to enable this controller
      11             :  *    2. before vehicle starts towards a waypoint call "check_activation" and provide the earth-frame heading to the waypoint
      12             :  *       this may change the controller's internal state to "active"
      13             :  *    3. on each main loop iteration call "active()" to see if this controller thinks it is controllering the vehicle
      14             :  *    4. call "get_turn_rate_rads()" to retrieve the desired turn rate towards the next waypoint
      15             :  *    5. pass above turn rate into the rate controller, set speed controller's speed to zero
      16             :  *    6. this controller's "active" state will change to false once it has completed the pivot turn
      17             :  */
      18             : 
      19             : class AR_PivotTurn {
      20             : public:
      21             : 
      22             :     // constructor
      23             :     AR_PivotTurn(AR_AttitudeControl& atc);
      24             : 
      25             :     // enable or disable pivot controller
      26             :     void enable(bool enable_pivot);
      27             : 
      28             :     // true if this controller is controlling vehicle
      29             :     bool active() const;
      30             : 
      31             :     // checks if pivot turns should be activated or deactivated
      32             :     // force_active should be true if the caller wishes to trigger the start of a pivot turn regardless of the heading error
      33             :     void check_activation(float desired_heading_deg, bool force_active = false);
      34             : 
      35             :     // check if pivot turn would be activated given an expected change in yaw in degrees
      36             :     // note this does not actually active the pivot turn.  To activate use the check_activation method
      37             :     bool would_activate(float yaw_change_deg) const WARN_IF_UNUSED;
      38             : 
      39             :     // forcibly deactivate this controller
      40         235 :     void deactivate() { _active = false; };
      41             : 
      42             :     // get turn rate (in rad/sec)
      43             :     // desired heading should be the heading towards the next waypoint in degrees
      44             :     // dt should be the time since the last call in seconds
      45             :     float get_turn_rate_rads(float desired_heading_deg, float dt);
      46             : 
      47             :     // accessors for parameter values
      48         786 :     float get_rate_max() const { return _rate_max; }
      49             : 
      50             :     // parameter var table
      51             :     static const struct AP_Param::GroupInfo var_info[];
      52             : 
      53             : private:
      54             : 
      55             :     // return post-turn delay duration in milliseconds
      56             :     uint32_t get_delay_duration_ms() const;
      57             : 
      58             :     // parameters
      59             :     AP_Int16 _angle;                // minimum angle error (in degrees) that leads to pivot turn
      60             :     AP_Int16 _rate_max;             // maximum turn rate (in degrees) during pivot turn
      61             :     AP_Float _delay;                // waiting time (in seconds) after pivot turn completes
      62             : 
      63             :     // references
      64             :     AR_AttitudeControl& _atc;       // rover attitude control library
      65             : 
      66             :     // local variables
      67             :     bool _enabled;                  // true if vehicle can pivot
      68             :     bool _active;                   // true if vehicle is currently pivoting
      69             :     uint32_t _delay_start_ms;       // system time when post-turn delay started
      70             : };