Line data    Source code

       1             : // Code by Rustom Jehangir: [email protected]
       2             : 
       3             : #include "Sub.h"
       4             : 
       5             : // Count total vehicle turns to avoid tangling tether
       6       54502 : void Sub::update_turn_counter()
       7             : {
       8             :     // Determine state
       9             :     // 0: 0-90 deg, 1: 90-180 deg, 2: -180--90 deg, 3: -90--0 deg
      10             :     uint8_t turn_state;
      11       54502 :     if (ahrs.get_yaw_rad() >= 0.0f && ahrs.get_yaw_rad() < radians(90)) {
      12        8178 :         turn_state = 0;
      13       46324 :     } else if (ahrs.get_yaw_rad() >= radians(90)) {
      14        8808 :         turn_state = 1;
      15       37516 :     } else if (ahrs.get_yaw_rad() < -radians(90)) {
      16       33719 :         turn_state = 2;
      17             :     } else {
      18        3797 :         turn_state = 3;
      19             :     }
      20             : 
      21             :     // If yaw went from negative to positive (right turn)
      22       54502 :     if (turn_state == (last_turn_state + 1) % 4) {
      23         248 :         quarter_turn_count++;
      24       54254 :     } else if (turn_state == (uint8_t)(last_turn_state - 1) % 4) {
      25          88 :         quarter_turn_count--;
      26             :     }
      27       54502 :     last_turn_state = turn_state;
      28       54502 : }