Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include <AP_AHRS/AP_AHRS.h>
      17             : #include <AP_HAL/AP_HAL.h>
      18             : #include <AP_Math/AP_Math.h>
      19             : #include "AR_PivotTurn.h"
      20             : 
      21             : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
      22             : #include <stdio.h>
      23             : #endif
      24             : 
      25             : extern const AP_HAL::HAL& hal;
      26             : 
      27             : #define AR_PIVOT_TIMEOUT_MS     100 // pivot controller timesout and reset target if not called within this many milliseconds
      28             : #define AR_PIVOT_ANGLE_DEFAULT  0   // default PIVOT_ANGLE parameter value (disabled by default)
      29             : #define AR_PIVOT_ANGLE_ACCURACY 5   // vehicle will pivot to within this many degrees of destination
      30             : #define AR_PIVOT_RATE_DEFAULT   60  // default PIVOT_RATE parameter value
      31             : #define AR_PIVOT_DELAY_DEFAULT  0   // default PIVOT_DELAY parameter value
      32             : 
      33             : const AP_Param::GroupInfo AR_PivotTurn::var_info[] = {
      34             : 
      35             :     // @Param: ANGLE
      36             :     // @DisplayName: Pivot Angle
      37             :     // @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.
      38             :     // @Units: deg
      39             :     // @Range: 0 360
      40             :     // @Increment: 1
      41             :     // @User: Standard
      42             :     AP_GROUPINFO("ANGLE", 1, AR_PivotTurn, _angle, AR_PIVOT_ANGLE_DEFAULT),
      43             : 
      44             :     // @Param: RATE
      45             :     // @DisplayName: Pivot Turn Rate
      46             :     // @Description: Turn rate during pivot turns
      47             :     // @Units: deg/s
      48             :     // @Range: 0 360
      49             :     // @Increment: 1
      50             :     // @User: Standard
      51             :     AP_GROUPINFO("RATE", 2, AR_PivotTurn, _rate_max, AR_PIVOT_RATE_DEFAULT),
      52             : 
      53             :     // @Param: DELAY
      54             :     // @DisplayName: Pivot Delay
      55             :     // @Description: Vehicle waits this many seconds after completing a pivot turn before proceeding
      56             :     // @Units: s
      57             :     // @Range: 0 60
      58             :     // @Increment: 0.1
      59             :     // @User: Standard
      60             :     AP_GROUPINFO("DELAY", 3, AR_PivotTurn, _delay, AR_PIVOT_DELAY_DEFAULT),
      61             : 
      62             :     AP_GROUPEND
      63             : };
      64             : 
      65         285 : AR_PivotTurn::AR_PivotTurn(AR_AttitudeControl& atc) :
      66         285 :     _atc(atc)
      67             : {
      68         285 :     AP_Param::setup_object_defaults(this, var_info);
      69         285 : }
      70             : 
      71             : // enable or disable pivot controller
      72       12526 : void AR_PivotTurn::enable(bool enable_pivot)
      73             : {
      74       12526 :     _enabled = enable_pivot;
      75       12526 :     if (!_enabled) {
      76       10781 :         _active = false;
      77             :     }
      78       12526 : }
      79             : 
      80             : // true if update has been called recently
      81      340742 : bool AR_PivotTurn::active() const
      82             : {
      83      340742 :     return _enabled && _active;
      84             : }
      85             : 
      86             : // checks if pivot turns should be activated or deactivated
      87             : // force_active should be true if the caller wishes to trigger the start of a pivot turn regardless of the heading error
      88         673 : void AR_PivotTurn::check_activation(float desired_heading_deg, bool force_active)
      89             : {
      90             :     // check cases where we clearly cannot use pivot steering
      91         673 :     if (!_enabled || (_angle <= AR_PIVOT_ANGLE_ACCURACY)) {
      92         673 :         _active = false;
      93         673 :         return;
      94             :     }
      95             : 
      96             :     // calc yaw error in degrees
      97           0 :     const float yaw_error = fabsf(wrap_180(desired_heading_deg - (AP::ahrs().get_yaw_deg())));
      98             : 
      99             :     // if error is larger than _pivot_angle start pivot steering
     100           0 :     if (yaw_error > _angle || force_active) {
     101           0 :         _active = true;
     102           0 :         _delay_start_ms = 0;
     103           0 :         return;
     104             :     }
     105             : 
     106           0 :     uint32_t now_ms = AP_HAL::millis();
     107             : 
     108             :     // if within 5 degrees of the target heading, set start time of pivot steering
     109           0 :     if (_active && (yaw_error < AR_PIVOT_ANGLE_ACCURACY) && (_delay_start_ms == 0)) {
     110           0 :         _delay_start_ms = now_ms;
     111             :     }
     112             : 
     113             :     // exit pivot steering after the time set by pivot_delay has elapsed
     114           0 :     if ((_delay_start_ms > 0) &&
     115           0 :         (now_ms - _delay_start_ms) >= get_delay_duration_ms()) {
     116           0 :         _active = false;
     117           0 :         _delay_start_ms = 0;
     118             :     }
     119             : }
     120             : 
     121             : // check if pivot turn would be activated given an expected change in yaw in degrees
     122             : // note this does not actually active the pivot turn.  To activate use the check_activation method
     123         112 : bool AR_PivotTurn::would_activate(float yaw_change_deg) const
     124             : {
     125             :     // check cases where we clearly cannot use pivot steering
     126         112 :     if (!_enabled || (_angle <= AR_PIVOT_ANGLE_ACCURACY)) {
     127         112 :         return false;
     128             :     }
     129             : 
     130             :     // return true if yaw change is larger than _pivot_angle
     131           0 :     return fabsf(wrap_180(yaw_change_deg)) > _angle;
     132             : }
     133             : 
     134             : // get turn rate (in rad/sec)
     135             : // desired heading should be the heading towards the next waypoint in degrees
     136             : // dt should be the time since the last call in seconds
     137           0 : float AR_PivotTurn::get_turn_rate_rads(float desired_heading_deg, float dt)
     138             : {
     139             :     // handle pivot turns
     140           0 :     const float desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(desired_heading_deg), radians(_rate_max));
     141             : 
     142             :     // update flag so that it can be cleared
     143           0 :     check_activation(desired_heading_deg);
     144             : 
     145           0 :     return desired_turn_rate_rads;
     146             : }
     147             : 
     148             : // return post-turn delay duration in milliseconds
     149           0 : uint32_t AR_PivotTurn::get_delay_duration_ms() const
     150             : {
     151           0 :     return constrain_float(_delay.get(), 0.0f, 60.0f) * 1000;
     152             : }