Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3          48 : bool ModeLoiter::_enter()
       4             : {
       5             :     // set _destination to reasonable stopping point
       6          48 :     if (!g2.wp_nav.get_stopping_location(_destination)) {
       7          19 :         return false;
       8             :     }
       9             : 
      10             :     // initialise desired speed to current speed
      11          29 :     if (!attitude_control.get_forward_speed(_desired_speed)) {
      12           0 :         _desired_speed = 0.0f;
      13             :     }
      14             : 
      15             :     // initialise heading to current heading
      16          29 :     _desired_yaw_cd = ahrs.yaw_sensor;
      17             : 
      18          29 :     return true;
      19             : }
      20             : 
      21       12119 : void ModeLoiter::update()
      22             : {
      23             :     // get distance (in meters) to destination
      24       12119 :     _distance_to_destination = rover.current_loc.get_distance(_destination);
      25             : 
      26       12119 :     const float loiter_radius = g2.sailboat.tack_enabled() ? g2.sailboat.get_loiter_radius() : g2.loit_radius;
      27             : 
      28             :     // if within loiter radius slew desired speed towards zero and use existing desired heading
      29       12119 :     if (_distance_to_destination <= loiter_radius) {
      30             :         // sailboats should not stop unless motoring
      31       11851 :         const float desired_speed_within_radius = g2.sailboat.tack_enabled() ? 0.1f : 0.0f;
      32       11851 :         _desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt);
      33             : 
      34             :         // if we have a sail but not trying to use it then point into the wind
      35       11851 :         if (!g2.sailboat.tack_enabled() && g2.sailboat.sail_enabled()) {
      36           0 :             _desired_yaw_cd = degrees(g2.windvane.get_true_wind_direction_rad()) * 100.0f;
      37             :         }
      38             :     } else {
      39             :         // P controller with hard-coded gain to convert distance to desired speed
      40         268 :         _desired_speed = MIN((_distance_to_destination - loiter_radius) * g2.loiter_speed_gain, g2.wp_nav.get_default_speed());
      41             : 
      42             :         // calculate bearing to destination
      43         268 :         _desired_yaw_cd = rover.current_loc.get_bearing_to(_destination);
      44         268 :         float yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
      45             :         // if destination is behind vehicle, reverse towards it
      46         268 :         if ((fabsf(yaw_error_cd) > 9000 && g2.loit_type == 0) || g2.loit_type == 2) {
      47           0 :             _desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
      48           0 :             yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
      49           0 :             _desired_speed = -_desired_speed;
      50             :         }
      51             : 
      52             :         // reduce desired speed if yaw_error is large
      53             :         // 45deg of error reduces speed to 75%, 90deg of error reduces speed to 50%
      54         268 :         float yaw_error_ratio = 1.0f - constrain_float(fabsf(yaw_error_cd / 9000.0f), 0.0f, 1.0f) * 0.5f;
      55         268 :         _desired_speed *= yaw_error_ratio;
      56             :     }
      57             : 
      58             :     // 0 turn rate is no limit
      59       12119 :     float turn_rate = 0.0;
      60             : 
      61             :     // make sure sailboats don't try and sail directly into the wind
      62       12119 :     if (g2.sailboat.use_indirect_route(_desired_yaw_cd)) {
      63           2 :         _desired_yaw_cd = g2.sailboat.calc_heading(_desired_yaw_cd);
      64           2 :         if (g2.sailboat.tacking()) {
      65             :             // use pivot turn rate for tacks
      66           0 :             turn_rate = g2.wp_nav.get_pivot_rate();
      67             :         }
      68             :     }
      69             : 
      70             :     // run steering and throttle controllers
      71       12119 :     calc_steering_to_heading(_desired_yaw_cd, turn_rate);
      72       12119 :     calc_throttle(_desired_speed, true);
      73       12119 : }
      74             : 
      75             : // get desired location
      76        2292 : bool ModeLoiter::get_desired_location(Location& destination) const
      77             : {
      78        2292 :     destination = _destination;
      79        2292 :     return true;
      80             : }