add flip/unflip logic

main/ugv_main.cc

@@ -290,6 +290,10 @@ struct State {
     outputs.left_motor  = 0.0;
     outputs.right_motor = 0.0;
 
+    float pitch = ahrs_.getPitch();
+
+    bool is_upside_down = (pitch > 90.) || (pitch < -90.);
+
     switch (ugv_state) {
       default:
         ESP_LOGW(TAG, "unhandled state: %d", ugv_state);
@@ -297,6 +301,10 @@ struct State {
       case UGV_State::STATE_IDLE:
       case UGV_State::STATE_FINISHED: angle_controller_.Disable(); break;
       case UGV_State::STATE_AQUIRING: {
+        if (is_upside_down) {
+          next_state = UGV_State::STATE_FLIPPING;
+          break;
+        }
         angle_controller_.Disable();
         TickType_t current_tick    = xTaskGetTickCount();
         TickType_t ticks_since_gps = current_tick - inputs.gps.last_update;
@@ -309,9 +317,19 @@ struct State {
       }
       case UGV_State::STATE_FLIPPING: {
         angle_controller_.Disable();
+        outputs.left_motor = -1.0;
+        outputs.right_motor = -1.0;
+        if (!is_upside_down) {
+          next_state = UGV_State::STATE_AQUIRING;
+          break;
+        }
         break;
       }
       case UGV_State::STATE_TURNING: {
+        if (is_upside_down) {
+          next_state = UGV_State::STATE_FLIPPING;
+          break;
+        }
         if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) {
           next_state = UGV_State::STATE_AQUIRING;
           break;
@@ -328,6 +346,10 @@ struct State {
         break;
       }
       case UGV_State::STATE_DRIVING: {
+        if (is_upside_down) {
+          next_state = UGV_State::STATE_FLIPPING;
+          break;
+        }
         if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) {
           next_state = UGV_State::STATE_AQUIRING;
           break;