Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3             : #if MODE_FOLLOW_ENABLED
       4             : 
       5             : // Return true if this mode is enabled
       6       12526 : bool ModeFollow::enabled() const
       7             : {
       8             :     // Follow mode requires follow lib
       9       12526 :     return g2.follow.enabled();
      10             : }
      11             : 
      12             : // initialize follow mode
      13           0 : bool ModeFollow::_enter()
      14             : {
      15           0 :     if (!enabled()) {
      16           0 :         return false;
      17             :     }
      18             : 
      19             :     // initialise speed to waypoint speed
      20           0 :     _desired_speed = g2.wp_nav.get_default_speed();
      21             : 
      22           0 :     return true;
      23             : }
      24             : 
      25             : // exit handling
      26           0 : void ModeFollow::_exit()
      27             : {
      28           0 :     g2.follow.clear_offsets_if_required();
      29           0 : }
      30             : 
      31           0 : void ModeFollow::update()
      32             : {
      33             :     // stop vehicle if no speed estimate
      34             :     float speed;
      35           0 :     if (!attitude_control.get_forward_speed(speed)) {
      36             :         // no valid speed so stop
      37           0 :         g2.motors.set_throttle(0.0f);
      38           0 :         g2.motors.set_steering(0.0f);
      39           0 :         return;
      40             :     }
      41             : 
      42           0 :     Vector3f dist_vec;  // vector to lead vehicle
      43           0 :     Vector3f dist_vec_offs; // vector to lead vehicle + offset
      44           0 :     Vector3f vel_of_target; // velocity of lead vehicle
      45             : 
      46             :     // if no target simply stop the vehicle
      47           0 :     if (!g2.follow.get_target_dist_and_vel_NED_m(dist_vec, dist_vec_offs, vel_of_target)) {
      48           0 :         _reached_destination = true;
      49           0 :         stop_vehicle();
      50           0 :         return;
      51             :     }
      52             : 
      53             :     // calculate desired velocity vector
      54           0 :     Vector2f desired_velocity_ne;
      55           0 :     const float kp = g2.follow.get_pos_p().kP();
      56           0 :     desired_velocity_ne.x = vel_of_target.x + (dist_vec_offs.x * kp);
      57           0 :     desired_velocity_ne.y = vel_of_target.y + (dist_vec_offs.y * kp);
      58             : 
      59             :     // if the desired velocity is less than 3cm/sec, stop vehicle
      60           0 :     if (desired_velocity_ne.length() < 0.03f) {
      61           0 :         _reached_destination = true;
      62           0 :         stop_vehicle();
      63           0 :         return;
      64             :     }
      65             : 
      66             :     //if the target vehicle velocity is less than 3cm/sec and the distance to the target vehicle is less than the turn radius, stop vehicle
      67           0 :     if (vel_of_target.length() < 0.03f && dist_vec_offs.length() < g2.turn_radius) {
      68           0 :         _reached_destination = true;
      69           0 :         stop_vehicle();
      70           0 :         return;
      71             :     }
      72             : 
      73             :     // we have not reached the target
      74           0 :     _reached_destination = false;
      75             : 
      76             :     // scale desired velocity to stay within horizontal speed limit
      77           0 :     float desired_speed = safe_sqrt(sq(desired_velocity_ne.x) + sq(desired_velocity_ne.y));
      78           0 :     if (!is_zero(desired_speed) && (desired_speed > _desired_speed)) {
      79           0 :         const float scalar_xy = _desired_speed / desired_speed;
      80           0 :         desired_velocity_ne *= scalar_xy;
      81           0 :         desired_speed = _desired_speed;
      82             :     }
      83             : 
      84             :     // calculate vehicle heading
      85           0 :     const float desired_yaw_cd = wrap_180_cd(rad_to_cd(atan2f(desired_velocity_ne.y, desired_velocity_ne.x)));
      86             : 
      87             :     // run steering and throttle controllers
      88           0 :     calc_steering_to_heading(desired_yaw_cd);
      89           0 :     calc_throttle(desired_speed, true);
      90             : }
      91             : 
      92             : // return desired heading (in degrees) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
      93           0 : float ModeFollow::wp_bearing() const
      94             : {
      95           0 :     return g2.follow.get_bearing_to_target_deg();
      96             : }
      97             : 
      98             : // return straight-line distance (in meters) to destination
      99           0 : float ModeFollow::get_distance_to_destination() const
     100             : {
     101           0 :     return g2.follow.get_distance_to_target_m();
     102             : }
     103             : 
     104             : // set desired speed in m/s
     105           0 : bool ModeFollow::set_desired_speed(float speed_ms)
     106             : {
     107           0 :     if (is_negative(speed_ms)) {
     108           0 :         return false;
     109             :     }
     110           0 :     _desired_speed = speed_ms;
     111           0 :     return true;
     112             : }
     113             : 
     114             : #endif // MODE_FOLLOW_ENABLED