Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3        3989 : void ModeAcro::update()
       4             : {
       5             :     // get speed forward
       6             :     float speed, desired_steering;
       7        3989 :     if (!attitude_control.get_forward_speed(speed)) {
       8             :         float desired_throttle;
       9             :         // convert pilot stick input into desired steering and throttle
      10         185 :         get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle);
      11             : 
      12             :         // if vehicle is balance bot, calculate actual throttle required for balancing
      13         185 :         if (rover.is_balancebot()) {
      14           0 :             rover.balancebot_pitch_control(desired_throttle);
      15             :         }
      16             : 
      17             :         // no valid speed, just use the provided throttle
      18         185 :         g2.motors.set_throttle(desired_throttle);
      19             :     } else {
      20             :         float desired_speed;
      21             :         // convert pilot stick input into desired steering and speed
      22        3804 :         get_pilot_desired_steering_and_speed(desired_steering, desired_speed);
      23        3804 :         calc_throttle(desired_speed, true);
      24             :     }
      25             : 
      26             :     float steering_out;
      27             : 
      28             :     // handle sailboats
      29        3989 :     if (!is_zero(desired_steering)) {
      30             :         // steering input return control to user
      31           0 :         g2.sailboat.clear_tack();
      32             :     }
      33        3989 :     if (g2.sailboat.tacking()) {
      34             :         // call heading controller during tacking
      35             : 
      36           0 :         steering_out = attitude_control.get_steering_out_heading(g2.sailboat.get_tack_heading_rad(),
      37           0 :                                                                  g2.wp_nav.get_pivot_rate(),
      38           0 :                                                                  g2.motors.limit.steer_left,
      39           0 :                                                                  g2.motors.limit.steer_right,
      40             :                                                                  rover.G_Dt);
      41             :     } else {
      42             :         // convert pilot steering input to desired turn rate in radians/sec
      43        3989 :         const float target_turn_rate = (desired_steering / 4500.0f) * radians(g2.acro_turn_rate);
      44             : 
      45             :         // run steering turn rate controller and throttle controller
      46        3989 :         steering_out = attitude_control.get_steering_out_rate(target_turn_rate,
      47        3989 :                                                               g2.motors.limit.steer_left,
      48        3989 :                                                               g2.motors.limit.steer_right,
      49             :                                                               rover.G_Dt);
      50             :     }
      51             : 
      52        3989 :     set_steering(steering_out * 4500.0f);
      53        3989 : }
      54             : 
      55          69 : bool ModeAcro::requires_velocity() const
      56             : {
      57          69 :     return !g2.motors.have_skid_steering();
      58             : }
      59             : 
      60             : // sailboats in acro mode support user manually initiating tacking from transmitter
      61           0 : void ModeAcro::handle_tack_request()
      62             : {
      63           0 :     g2.sailboat.handle_tack_request_acro();
      64           0 : }