Plane: Implement issue 80: counterclockwise loiter

- libraries/AP_Common/AP_Common.h: Use bit 2 of Location.options to store
  loiter direction
- ArduPlane/defines.h: New bitmask MASK_OPTIONS_LOITER_DIRECTION for struct
  Location bit 2
- ArduPlane/ArduPlane.pde: New variable loiter_direction
- ArduPlane/GCS_Mavlink.pde: For mavlink loiter-commands use sign of param3 to
  detemine direction. Set Location.option flag accordingly
- ArduPlane/commands.pde: Make sure loiter-directions get saved into EEPROM
  correctly
- ArduPlane/commands_logic.pde: Set loiter_direction on all loiter-actions
  as well as RTL/instant loiter
- ArduPlane/navigation.pde: Yield loiter_direction in update_loiter

ArduPlane/ArduPlane.pde

@@ -472,6 +472,9 @@ static int32_t old_target_bearing_cd;
 // Total desired rotation in a loiter.  Used for Loiter Turns commands.  Degrees
 static int32_t loiter_total;
 
+// Direction for loiter. 1 for clockwise, -1 for counter-clockwise
+static int8_t loiter_direction = 1;
+
 // The amount in degrees we have turned since recording old_target_bearing
 static int16_t loiter_delta;
 

ArduPlane/GCS_Mavlink.pde

@@ -1206,13 +1206,25 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
         switch (tell_command.id) {                                              // Switch to map APM command fields inot MAVLink command fields
 
+        case MAV_CMD_NAV_LOITER_TIME:
         case MAV_CMD_NAV_LOITER_TURNS:
+            if (tell_command.options & MASK_OPTIONS_LOITER_DIRECTION) {
+                param3 = -g.loiter_radius;
+            } else {
+                param3 = g.loiter_radius;
+            }
         case MAV_CMD_NAV_TAKEOFF:
         case MAV_CMD_DO_SET_HOME:
-        case MAV_CMD_NAV_LOITER_TIME:
             param1 = tell_command.p1;
             break;
 
+        case MAV_CMD_NAV_LOITER_UNLIM:
+            if (tell_command.options & MASK_OPTIONS_LOITER_DIRECTION) {
+                param3 = -g.loiter_radius;;
+            } else {
+                param3 = g.loiter_radius;
+            }
+            break;
         case MAV_CMD_CONDITION_CHANGE_ALT:
             x=0;                                // Clear fields loaded above that we don't want sent for this command
             y=0;
@@ -1481,16 +1493,22 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
 
         // Switch to map APM command fields into MAVLink command fields
         switch (tell_command.id) {
-        case MAV_CMD_NAV_WAYPOINT:
         case MAV_CMD_NAV_LOITER_UNLIM:
+		    if (packet.param3 < 0) {
+		        tell_command.options |= MASK_OPTIONS_LOITER_DIRECTION;
+		    }
+        case MAV_CMD_NAV_WAYPOINT:
         case MAV_CMD_NAV_RETURN_TO_LAUNCH:
         case MAV_CMD_NAV_LAND:
             break;
 
         case MAV_CMD_NAV_LOITER_TURNS:
+        case MAV_CMD_NAV_LOITER_TIME:
+            if (packet.param3 < 0) {
+                tell_command.options |= MASK_OPTIONS_LOITER_DIRECTION;
+            } 
         case MAV_CMD_NAV_TAKEOFF:
         case MAV_CMD_DO_SET_HOME:
-        case MAV_CMD_NAV_LOITER_TIME:
             tell_command.p1 = packet.param1;
             break;
 

ArduPlane/commands.pde

@@ -102,13 +102,12 @@ static void set_cmd_with_index(struct Location temp, int16_t i)
     i = constrain_int16(i, 0, g.command_total.get());
     uint16_t mem = WP_START_BYTE + (i * WP_SIZE);
 
-    // Set altitude options bitmask
-    // XXX What is this trying to do?
-    if ((temp.options & MASK_OPTIONS_RELATIVE_ALT) && i != 0) {
-        temp.options = MASK_OPTIONS_RELATIVE_ALT;
-    } else {
-        temp.options = 0;
+    // force home wp to absolute height
+    if (i == 0) {
+        temp.options &= ~(MASK_OPTIONS_RELATIVE_ALT);
     }
+    // zero unused bits
+    temp.options &= (MASK_OPTIONS_RELATIVE_ALT + MASK_OPTIONS_LOITER_DIRECTION);
 
     hal.storage->write_byte(mem, temp.id);
 

ArduPlane/commands_logic.pde

@@ -229,6 +229,7 @@ static void do_RTL(void)
     control_mode    = RTL;
     crash_timer     = 0;
     next_WP                 = home;
+    loiter_direction = 1;
 
     // Altitude to hold over home
     // Set by configuration tool
@@ -261,15 +262,26 @@ static void do_land()
     set_next_WP(&next_nav_command);
 }
 
+static void loiter_set_direction_wp(struct Location *nav_command) 
+{
+    if (nav_command->options & MASK_OPTIONS_LOITER_DIRECTION) {
+        loiter_direction = -1;
+    } else {
+        loiter_direction=1;
+    }
+}
+
 static void do_loiter_unlimited()
 {
     set_next_WP(&next_nav_command);
+    loiter_set_direction_wp(&next_nav_command);
 }
 
 static void do_loiter_turns()
 {
     set_next_WP(&next_nav_command);
     loiter_total = next_nav_command.p1 * 360;
+    loiter_set_direction_wp(&next_nav_command);
 }
 
 static void do_loiter_time()
@@ -277,6 +289,7 @@ 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_set_direction_wp(&next_nav_command);
 }
 
 /********************************************************************************/
@@ -490,6 +503,7 @@ static bool verify_within_distance()
 
 static void do_loiter_at_location()
 {
+    loiter_direction = 1;
     next_WP = current_loc;
 }
 

ArduPlane/defines.h

@@ -85,6 +85,9 @@ enum FlightMode {
 //--------------------
 #define MASK_OPTIONS_RELATIVE_ALT       (1<<0)          // 1 = Relative
                                                         // altitude
+#define MASK_OPTIONS_LOITER_DIRECTION   (1<<2)          // 0 = CW
+                                                        // 1 = CCW
+
 
 //repeating events
 #define NO_REPEAT 0

ArduPlane/navigation.pde

@@ -151,11 +151,11 @@ static void update_loiter()
     if(wp_distance <= (uint32_t)max(g.loiter_radius,0)) {
         power = float(wp_distance) / float(g.loiter_radius);
         power = constrain(power, 0.5, 1);
-        nav_bearing_cd += 9000.0 * (2.0 + power);
+        nav_bearing_cd += 9000.0 * (2.0 + power) * loiter_direction;
     } else if(wp_distance < (uint32_t)max((g.loiter_radius + LOITER_RANGE),0)) {
         power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
         power = constrain(power, 0.5, 1);                               //power = constrain(power, 0, 1);
-        nav_bearing_cd -= power * 9000;
+        nav_bearing_cd -= power * 9000 * loiter_direction;
     } else{
         update_crosstrack();
         loiter_time_ms = millis();                              // keep start time for loiter updating till we get within LOITER_RANGE of orbit

libraries/AP_Common/AP_Common.h

@@ -55,7 +55,7 @@
 /// Data structures and types used throughout the libraries and applications. 0 = default
 /// bit 0: Altitude is stored               0: Absolute,	1: Relative
 /// bit 1: Chnage Alt between WP            0: Gradually,	1: ASAP
-/// bit 2:
+/// bit 2: Direction of loiter command      0: Clockwise	1: Counter-Clockwise
 /// bit 3: Req.to hit WP.alt to continue    0: No,          1: Yes
 /// bit 4: Relative to Home					0: No,          1: Yes
 /// bit 5: