AP_Mission: Fractional Loiter Turn Support

ArduCopter/mode_auto.cpp

@@ -2184,8 +2184,10 @@ bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
         return false;
     }
 
+    const float turns = cmd.get_loiter_turns();
+
     // check if we have completed circling
-    return fabsf(copter.circle_nav->get_angle_total()/float(M_2PI)) >= LOWBYTE(cmd.p1);
+    return fabsf(copter.circle_nav->get_angle_total()/float(M_2PI)) >= turns;
 }
 
 // verify_spline_wp - check if we have reached the next way point using spline

ArduPlane/commands_logic.cpp

@@ -472,8 +472,9 @@ void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd)
     cmdloc.sanitize(current_loc);
     set_next_WP(cmdloc);
     loiter_set_direction_wp(cmd);
+    const float turns = cmd.get_loiter_turns();
 
-    loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL;
+    loiter.total_cd = (uint32_t)(turns * 36000UL);
     condition_value = 1; // used to signify primary turns goal not yet met
 }
 

ArduSub/commands_logic.cpp

@@ -537,9 +537,10 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
         }
         return false;
     }
+    const float turns = cmd.get_loiter_turns();
 
     // check if we have completed circling
-    return fabsf(sub.circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
+    return fabsf(sub.circle_nav.get_angle_total()/M_2PI) >= turns;
 }
 
 #if NAV_GUIDED == ENABLED

Rover/mode_auto.cpp

@@ -943,8 +943,9 @@ bool ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
 
 bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
 {
+    const float turns = cmd.get_loiter_turns();
     // check if we have completed circling
-    return ((g2.mode_circle.get_angle_total_rad() / M_2PI) >= LOWBYTE(cmd.p1));
+    return ((g2.mode_circle.get_angle_total_rad() / M_2PI) >= turns);
 }
 
 /********************************************************************************/

Tools/autotest/arduplane.py

@@ -4381,8 +4381,8 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1):
         seq = 0
         items = []
         tests = [
-            (self.home_relative_loc_ne(50, -50), 100),
-            (self.home_relative_loc_ne(100, 50), 1005),
+            (self.home_relative_loc_ne(50, -50), 100, 0.3),
+            (self.home_relative_loc_ne(100, 50), 1005, 3),
         ]
         # add a home position:
         items.append(self.mav.mav.mission_item_int_encode(
@@ -4423,7 +4423,7 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1):
         seq += 1
 
         # add circles
-        for (loc, radius) in tests:
+        for (loc, radius, turn) in tests:
             items.append(self.mav.mav.mission_item_int_encode(
                 target_system,
                 target_component,
@@ -4432,7 +4432,7 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1):
                 mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS,
                 0, # current
                 0, # autocontinue
-                3, # p1
+                turn, # p1
                 0, # p2
                 radius, # p3
                 0, # p4
@@ -4465,7 +4465,7 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1):
         downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_MISSION)
         ofs = 2
         self.progress("Checking downloaded mission is as expected")
-        for (loc, radius) in tests:
+        for (loc, radius, turn) in tests:
             downloaded = downloaded_items[ofs]
             if radius > 255:
                 # ArduPilot only stores % 10
@@ -4474,6 +4474,13 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1):
                 raise NotAchievedException(
                     "Did not get expected radius for item %u; want=%f got=%f" %
                     (ofs, radius, downloaded.param3))
+            if turn > 0 and turn < 1:
+                # ArduPilot stores fractions in 8 bits (*256) and unpacks it (/256)
+                turn = int(turn*256) / 256.0
+            if downloaded.param1 != turn:
+                raise NotAchievedException(
+                    "Did not get expected turn for item %u; want=%f got=%f" %
+                    (ofs, turn, downloaded.param1))
             ofs += 1
 
         self.change_mode('AUTO')
@@ -4483,7 +4490,7 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1):
 
         self.wait_current_waypoint(2)
 
-        for (loc, expected_radius) in tests:
+        for (loc, expected_radius, _) in tests:
             self.wait_circling_point_with_radius(
                 loc,
                 expected_radius,

libraries/AP_Mission/AP_Mission.cpp

@@ -728,6 +728,7 @@ struct PACKED Packed_Location_Option_Flags {
     uint8_t origin_alt   : 1;           // this altitude is above ekf origin
     uint8_t loiter_xtrack : 1;          // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location
     uint8_t type_specific_bit_0 : 1;    // each mission item type can use this for storing 1 bit of extra data
+    uint8_t type_specific_bit_1 : 1;    // each mission item type can use this for storing 1 bit of extra data
 };
 
 struct PACKED PackedLocation {
@@ -815,7 +816,10 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con
         cmd.content.location.lng = packed_content.location.lng;
 
         if (packed_content.location.flags.type_specific_bit_0) {
-            cmd.type_specific_bits = 1U << 0;
+            cmd.type_specific_bits |= 1U << 0;
+        }
+        if (packed_content.location.flags.type_specific_bit_1) {
+            cmd.type_specific_bits |= 1U << 1;
         }
     } else {
         // all other options in Content are assumed to be packed:
@@ -882,7 +886,8 @@ bool AP_Mission::write_cmd_to_storage(uint16_t index, const Mission_Command& cmd
         packed.location.flags.terrain_alt = cmd.content.location.terrain_alt;
         packed.location.flags.origin_alt = cmd.content.location.origin_alt;
         packed.location.flags.loiter_xtrack = cmd.content.location.loiter_xtrack;
-        packed.location.flags.type_specific_bit_0 = cmd.type_specific_bits & (1U<<0);
+        packed.location.flags.type_specific_bit_0 = (cmd.type_specific_bits & (1U<<0)) >> 0;
+        packed.location.flags.type_specific_bit_1 = (cmd.type_specific_bits & (1U<<1)) >> 1;
         packed.location.alt = cmd.content.location.alt;
         packed.location.lat = cmd.content.location.lat;
         packed.location.lng = cmd.content.location.lng;
@@ -1029,18 +1034,25 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
         break;
 
     case MAV_CMD_NAV_LOITER_TURNS: {                    // MAV ID: 18
-        // number of turns is stored in the lowest bits.  radii below
-        // 255m are stored in the top 8 bits as an 8-bit integer.
+        // number of turns is stored in the lowest bits. Number of
+        // turns 0 < N < 1 are stored multiplied by 256 and a bit set
+        // in storage so that on retrieval they are divided by 256.
+        // Radii below 255m are stored in the top 8 bits as an 8-bit integer.
         // Radii above 255m are stored divided by 10 and a bit set in
         // storage so that on retrieval they are multiplied by 10
-        cmd.p1 = MIN(255, packet.param1); // store number of times to circle in low p1
+        float param1_stored = packet.param1;
+        if (param1_stored > 0 && param1_stored < 1) {
+            param1_stored *= 256.0;
+            cmd.type_specific_bits |= (1U << 1);
+        }
+        cmd.p1 = MIN(255, param1_stored); // store number of times to circle in low p1
         uint8_t radius_m;
         const float abs_radius = fabsf(packet.param3);
         if (abs_radius <= 255) {
             radius_m = abs_radius;
         } else {
             radius_m = MIN(255, abs_radius * 0.1);
-            cmd.type_specific_bits = 1U << 0;
+            cmd.type_specific_bits |= (1U << 0);
         }
         cmd.p1 |= (radius_m<<8);   // store radius in high byte of p1
         cmd.content.location.loiter_ccw = (packet.param3 < 0);
@@ -1550,6 +1562,9 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
         if (cmd.type_specific_bits & (1U<<0)) {
             packet.param3 *= 10;
         }
+        if (cmd.type_specific_bits & (1U<<1)) {
+            packet.param1 /= 256.0;
+        }
         packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
         break;
 

libraries/AP_Mission/AP_Mission.h

@@ -415,6 +415,19 @@ class AP_Mission
         // comparison operator (relies on all bytes in the structure even if they may not be used)
         bool operator ==(const Mission_Command &b) const { return (memcmp(this, &b, sizeof(Mission_Command)) == 0); }
         bool operator !=(const Mission_Command &b) const { return !operator==(b); }
+
+        /*
+          return the number of turns for a LOITER_TURNS command
+          this has special handling for loiter turns from cmd.p1 and type_specific_bits
+         */
+        float get_loiter_turns(void) const {
+            float turns = LOWBYTE(p1);
+            if (type_specific_bits & (1U<<1)) {
+                // special storage handling allows for fractional turns
+                turns *= (1.0/256.0);
+            }
+            return turns;
+        }
     };