Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3        3852 : void ModeSteering::update()
       4             : {
       5             :     // get speed forward
       6             :     float speed;
       7        3852 :     if (!attitude_control.get_forward_speed(speed)) {
       8             :         // no valid speed so stop
       9          34 :         g2.motors.set_throttle(0.0f);
      10          34 :         g2.motors.set_steering(0.0f);
      11          34 :         _desired_lat_accel = 0.0f;
      12          34 :         return;
      13             :     }
      14             : 
      15             :     float desired_steering, desired_speed;
      16        3818 :     get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
      17             : 
      18        3818 :     bool reversed = is_negative(desired_speed);
      19             : 
      20             :     // determine if pilot is requesting pivot turn
      21        3818 :     if (g2.motors.have_skid_steering() && is_zero(desired_speed)) {
      22             :         // pivot turning using turn rate controller
      23             :         // convert pilot steering input to desired turn rate in radians/sec
      24         220 :         const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
      25         220 :         _desired_lat_accel = 0.0f;
      26             : 
      27             :         // run steering turn rate controller and throttle controller
      28         220 :         const float steering_out = attitude_control.get_steering_out_rate(target_turn_rate,
      29         220 :                                                                           g2.motors.limit.steer_left,
      30         220 :                                                                           g2.motors.limit.steer_right,
      31             :                                                                           rover.G_Dt);
      32         220 :         set_steering(steering_out * 4500.0f);
      33             :     } else {
      34             :         // In steering mode we control lateral acceleration directly.
      35             :         // For regular steering vehicles we use the maximum lateral acceleration
      36             :         //  at full steering lock for this speed: V^2/R where R is the radius of turn.
      37        3598 :         float max_g_force = speed * speed / MAX(g2.turn_radius, 0.1f);
      38        3598 :         max_g_force = constrain_float(max_g_force, 0.1f, attitude_control.get_turn_lat_accel_max());
      39             : 
      40             :         // convert pilot steering input to desired lateral acceleration
      41        3598 :         _desired_lat_accel = max_g_force * (desired_steering / 4500.0f);
      42             : 
      43             :         // reverse target lateral acceleration if backing up
      44        3598 :         if (reversed) {
      45          85 :             _desired_lat_accel = -_desired_lat_accel;
      46             :         }
      47             : 
      48             :         // run lateral acceleration to steering controller
      49        3598 :         calc_steering_from_lateral_acceleration(_desired_lat_accel, reversed);
      50             :     }
      51             : 
      52             :     // run speed to throttle controller
      53        3818 :     calc_throttle(desired_speed, true);
      54             : }