Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3         113 : void ModeSimple::init_heading()
       4             : {
       5         113 :     _initial_heading_cd = ahrs.yaw_sensor;
       6         113 :     _desired_heading_cd = ahrs.yaw_sensor;
       7         113 : }
       8             : 
       9           0 : void ModeSimple::update()
      10             : {
      11             :     float desired_heading_cd, desired_speed;
      12             : 
      13             :     // get pilot input
      14           0 :     get_pilot_desired_heading_and_speed(desired_heading_cd, desired_speed);
      15             : 
      16             :     // rotate heading around based on initial heading
      17           0 :     if (g2.simple_type == Simple_InitialHeading) {
      18           0 :         desired_heading_cd = wrap_360_cd(_initial_heading_cd + desired_heading_cd);
      19             :     }
      20             : 
      21             :     // if sticks in middle, use previous desired heading (important when vehicle is slowing down)
      22           0 :     if (!is_positive(desired_speed)) {
      23           0 :         desired_heading_cd = _desired_heading_cd;
      24             :     } else {
      25             :         // record desired heading for next iteration
      26           0 :         _desired_heading_cd = desired_heading_cd;
      27             :     }
      28             : 
      29             :     // run throttle and steering controllers
      30           0 :     calc_steering_to_heading(desired_heading_cd);
      31           0 :     calc_throttle(desired_speed, true);
      32           0 : }