Copter: inav circle initial implementation

ArduCopter/ArduCopter.pde

@@ -571,18 +571,15 @@ AverageFilterInt32_Size5 baro_filter;
 ////////////////////////////////////////////////////////////////////////////////
 // Circle Mode / Loiter control
 ////////////////////////////////////////////////////////////////////////////////
-// used to determin the desired location in Circle mode
-// increments at circle_rate / second
-static float circle_angle;
-// used to control the speed of Circle mode
-// units are in radians, default is 5° per second
+// used to control the speed of Circle mode in radians/second, default is 5° per second
 static const float circle_rate = 0.0872664625;
-// used to track the delat in Circle Mode
-static int32_t old_wp_bearing;
-// deg : how many times to circle * 360 for Loiter/Circle Mission command
-static int16_t loiter_total;
-// deg : how far we have turned around a waypoint
-static int16_t loiter_sum;
+Vector2f circle_center;     // circle position expressed in cm from home location.  x = lat, y = lon
+// angle from the circle center to the copter's desired location.  Incremented at circle_rate / second
+static float circle_angle;
+// the total angle (in radians) travelled
+static float circle_angle_total;
+// deg : how many times to circle as specified by mission command
+static uint8_t circle_desired_rotations;
 // How long we should stay in Loiter Mode for mission scripting
 static uint16_t loiter_time_max;
 // How long have we been loitering - The start time in millis

ArduCopter/commands_logic.pde

@@ -25,7 +25,7 @@ static void process_nav_command()
         break;
 
     case MAV_CMD_NAV_LOITER_TURNS:              //18 Loiter N Times
-        do_loiter_turns();
+        do_circle();
         break;
 
     case MAV_CMD_NAV_LOITER_TIME:              // 19
@@ -159,7 +159,7 @@ static bool verify_must()
         break;
 
     case MAV_CMD_NAV_LOITER_TURNS:
-        return verify_loiter_turns();
+        return verify_circle();
         break;
 
     case MAV_CMD_NAV_LOITER_TIME:
@@ -346,8 +346,8 @@ static void do_loiter_unlimited()
 
 }
 
-// do_loiter_turns - initiate moving in a circle
-static void do_loiter_turns()
+// do_circle - initiate moving in a circle
+static void do_circle()
 {
     // set roll-pitch mode (no pilot input)
     set_roll_pitch_mode(AUTO_RP);
@@ -363,22 +363,22 @@ static void do_loiter_turns()
     // set nav mode to CIRCLE
     set_nav_mode(NAV_CIRCLE);
 
-    // set horizontal location target
+    // override default horizontal location target
     if( command_nav_queue.lat != 0 || command_nav_queue.lng != 0) {
-        set_next_WP_latlon(command_nav_queue.lat, command_nav_queue.lng);
+        circle_set_center(pv_latlon_to_vector(command_nav_queue.lat, command_nav_queue.lng), ahrs.yaw);
     }
+
+    // set yaw to point to center of circle
     circle_WP = next_WP;
     yaw_look_at_WP = circle_WP;
-    // set yaw to point to center of circle
     set_yaw_mode(CIRCLE_YAW);
 
-    loiter_total = command_nav_queue.p1 * 360;
-    loiter_sum       = 0;
-    old_wp_bearing = wp_bearing;
+    // set angle travelled so far to zero
+    circle_angle_total = 0;
+
+    // record number of desired rotations from mission command
+    circle_desired_rotations = command_nav_queue.p1;
 
-    circle_angle = wp_bearing + 18000;
-    circle_angle = wrap_360(circle_angle);
-    circle_angle *= RADX100;
 }
 
 // do_loiter_time - initiate loitering at a point for a given time period
@@ -502,20 +502,11 @@ static bool verify_loiter_time()
     return false;
 }
 
-// verify_loiter_turns - check if we have circled the point enough
-static bool verify_loiter_turns()
+// verify_circle - check if we have circled the point enough
+static bool verify_circle()
 {
-    //cliSerial->printf("loiter_sum: %d \n", loiter_sum);
     // have we rotated around the center enough times?
-    // -----------------------------------------------
-    if(abs(loiter_sum) > loiter_total) {
-        loiter_total    = 0;
-        loiter_sum              = 0;
-        //gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete"));
-        // clear the command queue;
-        return true;
-    }
-    return false;
+    return fabs(circle_desired_rotations/M_PI) > circle_desired_rotations;
 }
 
 // verify_RTL - handles any state changes required to implement RTL
@@ -588,9 +579,8 @@ static bool verify_RTL()
             if(current_loc.alt <= g.rtl_alt_final || alt_change_flag == REACHED_ALT) {
                 // switch to regular loiter mode
                 set_mode(LOITER);
-                // override location and altitude
-                // To-Do: ensure this target location is being sent to loiter controller
-                set_next_WP(&home);
+                // set loiter target to home position
+                loiter_set_target(0,0);
                 // override altitude to RTL altitude
                 set_new_altitude(g.rtl_alt_final);
                 retval = true;

ArduCopter/navigation.pde

@@ -1,7 +1,7 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 // update_navigation - invokes navigation routines
-// called at 50hz
+// called at 10hz
 static void update_navigation()
 {
     static uint32_t nav_last_update = 0;        // the system time of the last time nav was run update
@@ -150,10 +150,8 @@ static bool set_nav_mode(uint8_t new_nav_mode)
             break;
 
         case NAV_CIRCLE:
-            // start circling around current location
-            set_next_WP(&current_loc);
-            circle_WP       = next_WP;
-            circle_angle    = 0;
+            // set center of circle to current position
+            circle_set_center(Vector2f(inertial_nav.get_latitude_diff(), inertial_nav.get_longitude_diff()), ahrs.yaw);
             nav_initialised = true;
             break;
 
@@ -180,8 +178,6 @@ static bool set_nav_mode(uint8_t new_nav_mode)
 // update_nav_mode - run navigation controller based on nav_mode
 static void update_nav_mode()
 {
-    int16_t loiter_delta;
-
     switch( nav_mode ) {
 
         case NAV_NONE:
@@ -189,30 +185,8 @@ static void update_nav_mode()
             break;
 
         case NAV_CIRCLE:
-            // check if we have missed the WP
-            loiter_delta = (wp_bearing - old_wp_bearing)/100;
-
-            // reset the old value
-            old_wp_bearing = wp_bearing;
-
-            // wrap values
-            if (loiter_delta > 180) loiter_delta -= 360;
-            if (loiter_delta < -180) loiter_delta += 360;
-
-            // sum the angle around the WP
-            loiter_sum += loiter_delta;
-
-            circle_angle += (circle_rate * dTnav);
-
-            // wrap
-            if (circle_angle > 6.28318531f)
-                circle_angle -= 6.28318531f;
-
-            // update target location
-            // To-Do: ensure this target is updated for inertial navigation controller
-            set_next_WP_latlon(
-                circle_WP.lat + (g.circle_radius * 100 * sinf(1.57f - circle_angle)),
-                circle_WP.lng + (g.circle_radius * 100 * cosf(1.57f - circle_angle) * scaleLongUp));
+            // call circle controller which in turn calls loiter controller
+            circle_get_pos(dTnav);
             break;
 
         case NAV_LOITER:
@@ -222,9 +196,9 @@ static void update_nav_mode()
         case NAV_WP:
             // move forward on the waypoint
             // To-Do: slew up the speed to the max waypoint speed instead of immediately jumping to max
-            wpinav_advance_track_desired(g.waypoint_speed_max, 0.1f);
+            wpinav_advance_track_desired(g.waypoint_speed_max, dTnav);
             // run the navigation controller
-            get_wpinav_pos(0.1f);
+            get_wpinav_pos(dTnav);
             break;
     }
 
@@ -296,9 +270,6 @@ static void verify_altitude()
 // Keeps old data out of our calculation / logs
 static void reset_nav_params(void)
 {
-    // always start Circle mode at same angle
-    circle_angle                    = 0;
-
     // We must be heading to a new WP, so XTrack must be 0
     crosstrack_error                = 0;
 
@@ -610,3 +581,55 @@ wpinav_advance_track_desired(float velocity_cms, float dt)
     wpinav_target.x = wpinav_origin.x + wpinav_pos_delta.x * track_length_pct;
     wpinav_target.y = wpinav_origin.y + wpinav_pos_delta.y * track_length_pct;
 }
+
+//////////////////////////////////////////////////////////
+// circle navigation controller
+//////////////////////////////////////////////////////////
+
+// circle_set_center -- set circle controller's center position and starting angle
+static void
+circle_set_center(const Vector2f pos_vec, float heading_in_radians)
+{
+    // set circle center
+    circle_center = pos_vec;
+
+    // set starting angle to current heading - 180 degrees
+    circle_angle = heading_in_radians-ToRad(180);
+    if( circle_angle > 180 ) {
+        circle_angle -= 180;
+    }
+    if( circle_angle < -180 ) {
+        circle_angle -= 180;
+    }
+
+    // initialise other variables
+    circle_angle_total = 0;
+}
+
+// circle_get_pos - circle position controller's main call which in turn calls loiter controller with updated target position
+static void
+circle_get_pos(float dt)
+{
+    float angle_delta = circle_rate * dt;
+    float cir_radius = g.circle_radius * 100;
+    Vector2f circle_target;
+
+    // update the target angle
+    circle_angle += angle_delta;
+    if( circle_angle > 180 ) {
+        circle_angle -= 360;
+    }
+    if( circle_angle <= -180 ) {
+        circle_angle += 360;
+    }
+
+    // update the total angle travelled
+    circle_angle_total += angle_delta;
+
+    // calculate target position
+    circle_target.x = circle_center.x + cir_radius * sinf(1.57f - circle_angle);
+    circle_target.y = circle_center.y + cir_radius * cosf(1.57f - circle_angle);
+
+    // re-use loiter position controller
+    get_loiter_pos_lat_lon(circle_target.x, circle_target.y, dt);
+}

ArduCopter/position_vector.pde

@@ -0,0 +1,44 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// position_vector.pde related utility functions
+
+// position vectors are Vector2f
+//    .x = latitude from home in cm
+//    .y = longitude from home in cm
+
+// pv_latlon_to_vector - convert lat/lon coordinates to a position vector
+const Vector2f pv_latlon_to_vector(int32_t lat, int32_t lon)
+{
+    Vector2f tmp(lat-home.lat * LATLON_TO_CM, lon-home.lng * LATLON_TO_CM * scaleLongDown);
+    return tmp;
+}
+
+// pv_get_lon - extract latitude from position vector
+const int32_t pv_get_lat(const Vector2f pos_vec)
+{
+    return home.lat + (int32_t)(pos_vec.x / LATLON_TO_CM);
+}
+
+// pv_get_lon - extract longitude from position vector
+const int32_t pv_get_lon(const Vector2f pos_vec)
+{
+    return home.lng + (int32_t)(pos_vec.y / LATLON_TO_CM * scaleLongUp);
+}
+
+// pv_get_distance_cm - return distance between two positions in cm
+const float pv_get_distance_cm(const Vector2f origin, const Vector2f destination)
+{
+    Vector2f dist = destination - origin;
+    return pythagorous2(dist.x,dist.y);
+}
+
+// pv_get_bearing_cd - return bearing in centi-degrees between two locations
+const float pv_get_bearing_cd(const Vector2f origin, const Vector2f destination)
+{
+    Vector2f dist = destination - origin;
+    int32_t bearing = 9000 + atan2f(dist.x, dist.y) * 5729.57795f;
+    if (bearing < 0) {
+        bearing += 36000;
+    }
+    return bearing;
+}