Plane: fixed LOITER_TURNS command

and cleanup more loiter variables

ArduPlane/ArduPlane.pde

@@ -459,15 +459,6 @@ static bool throttle_suppressed;
 // Loiter management
 ////////////////////////////////////////////////////////////////////////////////
 
-// Direction for loiter. 1 for clockwise, -1 for counter-clockwise
-static int8_t loiter_direction = 1;
-
-// The amount of time we have been in a Loiter.  Used for the Loiter Time command.  Milliseconds.
-static uint32_t loiter_time_ms;
-
-// The amount of time we should stay in a loiter for the Loiter Time command.  Milliseconds.
-static uint32_t loiter_time_max_ms;
-
 ////////////////////////////////////////////////////////////////////////////////
 // Navigation control variables
 ////////////////////////////////////////////////////////////////////////////////
@@ -491,11 +482,23 @@ static uint32_t wp_totalDistance;
   meta data to support counting the number of circles in a loiter
  */
 static struct {
+    // previous target bearing, used to update sum_cd
     int32_t old_target_bearing_cd;
-    int32_t loiter_sum_cd;
 
     // Total desired rotation in a loiter.  Used for Loiter Turns commands. 
-    int32_t loiter_total_cd;
+    int32_t total_cd;
+
+    // total angle completed in the loiter so far
+    int32_t sum_cd;
+
+	// Direction for loiter. 1 for clockwise, -1 for counter-clockwise
+    int8_t direction;
+
+	// start time of the loiter.  Milliseconds.
+    uint32_t start_time_ms;
+
+	// The amount of time we should stay in a loiter for the Loiter Time command.  Milliseconds.
+    uint32_t time_max_ms;
 } loiter;
 
 

ArduPlane/commands_logic.pde

@@ -236,9 +236,9 @@ static void do_RTL(void)
     next_WP                 = home;
 
     if (g.loiter_radius < 0) {
-        loiter_direction = -1;
+        loiter.direction = -1;
     } else {
-        loiter_direction = 1;
+        loiter.direction = 1;
     }
 
     // Altitude to hold over home
@@ -275,9 +275,9 @@ static void do_land()
 static void loiter_set_direction_wp(const struct Location *nav_command)
 {
     if (nav_command->options & MASK_OPTIONS_LOITER_DIRECTION) {
-        loiter_direction = -1;
+        loiter.direction = -1;
     } else {
-        loiter_direction = 1;
+        loiter.direction = 1;
     }
 }
 
@@ -290,15 +290,15 @@ static void do_loiter_unlimited()
 static void do_loiter_turns()
 {
     set_next_WP(&next_nav_command);
-    loiter.loiter_total_cd = next_nav_command.p1 * 36000UL;
+    loiter.total_cd = next_nav_command.p1 * 36000UL;
     loiter_set_direction_wp(&next_nav_command);
 }
 
 static void do_loiter_time()
 {
     set_next_WP(&next_nav_command);
-    loiter_time_ms = millis();
-    loiter_time_max_ms = next_nav_command.p1 * (uint32_t)1000;     // units are seconds
+    loiter.start_time_ms = millis();
+    loiter.time_max_ms = next_nav_command.p1 * (uint32_t)1000;     // units are seconds
     loiter_set_direction_wp(&next_nav_command);
 }
 
@@ -413,7 +413,7 @@ static bool verify_loiter_unlim()
 static bool verify_loiter_time()
 {
     update_loiter();
-    if ((millis() - loiter_time_ms) > loiter_time_max_ms) {
+    if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) {
         gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
         return true;
     }
@@ -423,8 +423,8 @@ static bool verify_loiter_time()
 static bool verify_loiter_turns()
 {
     update_loiter();
-    if (loiter.loiter_sum_cd > loiter.loiter_total_cd) {
-        loiter.loiter_total_cd = 0;
+    if (loiter.sum_cd > loiter.total_cd) {
+        loiter.total_cd = 0;
         gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete"));
         // clear the command queue;
         return true;
@@ -511,9 +511,9 @@ static bool verify_within_distance()
 static void do_loiter_at_location()
 {
     if (g.loiter_radius < 0) {
-        loiter_direction = -1;
+        loiter.direction = -1;
     } else {
-        loiter_direction = 1;
+        loiter.direction = 1;
     }
     next_WP = current_loc;
 }

ArduPlane/navigation.pde

@@ -16,8 +16,8 @@ static void set_nav_controller(void)
  */
 static void loiter_angle_reset(void)
 {
-    loiter.loiter_sum_cd = 0;
-    loiter.loiter_total_cd = 0;
+    loiter.sum_cd = 0;
+    loiter.total_cd = 0;
 }
 
 /*
@@ -28,15 +28,16 @@ static void loiter_angle_update(void)
 {
     int32_t target_bearing_cd = nav_controller->target_bearing_cd();
     int32_t loiter_delta_cd;
-    if (loiter.loiter_sum_cd == 0) {
-        loiter_delta_cd = 0;
+    if (loiter.sum_cd == 0) {
+        // use 1 cd for initial delta
+        loiter_delta_cd = 1;
     } else {
         loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd;
     }
     loiter.old_target_bearing_cd = target_bearing_cd;
     loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
 
-    loiter.loiter_sum_cd += loiter_delta_cd;
+    loiter.sum_cd += loiter_delta_cd;
 }
 
 //****************************************************************
@@ -142,6 +143,6 @@ static void calc_altitude_error()
 
 static void update_loiter()
 {
-    nav_controller->update_loiter(next_WP, abs(g.loiter_radius), loiter_direction);
+    nav_controller->update_loiter(next_WP, abs(g.loiter_radius), loiter.direction);
 }