ArduCopter/ArduCopter.pde

 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-#define THISFIRMWARE "ArduCopter V3.0.0-rc4"
+#define THISFIRMWARE "ArduCopter V3.0.1-rc2"
 /*
  *  ArduCopter Version 3.0
  *  Creator:        Jason Short
@@ -387,7 +387,7 @@ static union {
 
 static struct AP_System{
     uint8_t GPS_light               : 1; // 0   // Solid indicates we have full 3D lock and can navigate, flash = read
-    uint8_t motor_light             : 1; // 1   // Solid indicates Armed state
+    uint8_t arming_light            : 1; // 1   // Solid indicates armed state, flashing is disarmed, double flashing is disarmed and failing pre-arm checks
     uint8_t new_radio_frame         : 1; // 2   // Set true if we have new PWM data to act on from the Radio
     uint8_t CH7_flag                : 1; // 3   // true if ch7 aux switch is high
     uint8_t CH8_flag                : 1; // 4   // true if ch8 aux switch is high
@@ -1175,6 +1175,8 @@ static void medium_loop()
         USERHOOK_MEDIUMLOOP
 #endif
 
+        // update board leds
+        update_board_leds();
 #if COPTER_LEDS == ENABLED
         update_copter_leds();
 #endif
@@ -1254,9 +1256,6 @@ static void slow_loop()
         slow_loopCounter = 0;
         update_events();
 
-        // blink if we are armed
-        update_lights();
-
         if(g.radio_tuning > 0)
             tuning();
 

ArduCopter/Attitude.pde

@@ -122,15 +122,14 @@ get_roll_rate_stabilized_ef(int32_t stick_angle)
         angle_error	= constrain_int32(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
     }
 
-    if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe_radio)) {
+    // reset target angle to current angle if motors not spinning
+    if (!motors.armed() || g.rc_3.servo_out == 0) {
         angle_error = 0;
     }
 
     // update roll_axis to be within max_angle_overshoot of our current heading
     roll_axis = wrap_180_cd(angle_error + ahrs.roll_sensor);
 
-    // set earth frame targets for rate controller
-
     // set earth frame targets for rate controller
 	set_roll_rate_target(g.pi_stabilize_roll.get_p(angle_error) + target_rate, EARTH_FRAME);
 }
@@ -158,7 +157,8 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle)
         angle_error	= constrain_int32(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT);
     }
 
-    if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe_radio)) {
+    // reset target angle to current angle if motors not spinning
+    if (!motors.armed() || g.rc_3.servo_out == 0) {
         angle_error = 0;
     }
 
@@ -189,7 +189,8 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
     // limit the maximum overshoot
     angle_error	= constrain_int32(angle_error, -MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
 
-    if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe_radio)) {
+    // reset target angle to current heading if motors not spinning
+    if (!motors.armed() || g.rc_3.servo_out == 0) {
     	angle_error = 0;
     }
 
@@ -442,11 +443,12 @@ get_rate_roll(int32_t target_rate)
     rate_error  = target_rate - current_rate;
     p           = g.pid_rate_roll.get_p(rate_error);
 
-    // freeze I term if we've breached roll-pitch limits
-    if( motors.reached_limit(AP_MOTOR_ROLLPITCH_LIMIT) ) {
-        i	= g.pid_rate_roll.get_integrator();
-    }else{
-        i   = g.pid_rate_roll.get_i(rate_error, G_Dt);
+    // get i term
+    i = g.pid_rate_roll.get_integrator();
+
+    // update i term as long as we haven't breached the limits or the I term will certainly reduce
+    if (!motors.reached_limit(AP_MOTOR_ROLLPITCH_LIMIT) || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
+        i = g.pid_rate_roll.get_i(rate_error, G_Dt);
     }
 
     d = g.pid_rate_roll.get_d(rate_error, G_Dt);
@@ -484,12 +486,15 @@ get_rate_pitch(int32_t target_rate)
     // call pid controller
     rate_error      = target_rate - current_rate;
     p               = g.pid_rate_pitch.get_p(rate_error);
-    // freeze I term if we've breached roll-pitch limits
-    if( motors.reached_limit(AP_MOTOR_ROLLPITCH_LIMIT) ) {
-        i = g.pid_rate_pitch.get_integrator();
-    }else{
+
+    // get i term
+    i = g.pid_rate_pitch.get_integrator();
+
+    // update i term as long as we haven't breached the limits or the I term will certainly reduce
+    if (!motors.reached_limit(AP_MOTOR_ROLLPITCH_LIMIT) || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
         i = g.pid_rate_pitch.get_i(rate_error, G_Dt);
     }
+
     d = g.pid_rate_pitch.get_d(rate_error, G_Dt);
     output = p + i + d;
 
@@ -521,12 +526,16 @@ get_rate_yaw(int32_t target_rate)
 
     // separately calculate p, i, d values for logging
     p = g.pid_rate_yaw.get_p(rate_error);
-    // freeze I term if we've breached yaw limits
-    if( motors.reached_limit(AP_MOTOR_YAW_LIMIT) ) {
-        i = g.pid_rate_yaw.get_integrator();
-    }else{
+
+    // get i term
+    i = g.pid_rate_yaw.get_integrator();
+
+    // update i term as long as we haven't breached the limits or the I term will certainly reduce
+    if (!motors.reached_limit(AP_MOTOR_YAW_LIMIT) || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
         i = g.pid_rate_yaw.get_i(rate_error, G_Dt);
     }
+
+    // get d value
     d = g.pid_rate_yaw.get_d(rate_error, G_Dt);
 
     output  = p+i+d;
@@ -1019,10 +1028,6 @@ get_throttle_rate(float z_target_speed)
         set_throttle_out(g.throttle_cruise+output, true);
     }
 
-    // limit loiter & waypoint navigation from causing too much lean
-    // To-Do: ensure that this limit is cleared when this throttle controller is not running so that loiter is not left constrained for Position mode
-    wp_nav.set_angle_limit(4500 - constrain_float((z_rate_error - 100) * 10, 0, 3500));
-
     // update throttle cruise
     // TO-DO: this may not be correct because g.rc_3.servo_out has not been updated for this iteration
     if( z_target_speed == 0 ) {

ArduCopter/ReleaseNotes.txt

 ArduCopter Release Notes:
 ------------------------------------------------------------------
+ArduCopter 3.0.1-rc2 10-Jun-2013
+Improvements over 3.0.1-rc1
+1) Rate Roll, Pitch and Yaw I fix when we hit motor limits
+2) pre-arm check changes:
+    a) double flash arming light when pre-arm checks fail
+    b) relax mag field checks to 35% min, 165% max of expected field
+3) reduce loiter, auto gains:
+      a) Loiter Pos P to 0.2 (was 1.0)
+      b) Loiter speed to 5 m/s
+      c) WP_ACCEL to 1 m/s/s (was 250  m/s/s)
+4) Stability Patch fix which was freezing Rate Taw I term and allowing uncommanded Yaw
+------------------------------------------------------------------
+ArduCopter 3.0.1-rc1 26-Jun-2013
+Improvements over 3.0.0
+1) bug fix to Fence checking position after GPS lock was lost
+2) bug fix to LAND so that it does not attempt to maintain horizontal position without GPS lock
+------------------------------------------------------------------
+ArduCopter 3.0.0 / 3.0.0-rc6 16-Jun-2013
+Improvements over 3.0.0-rc5
+1) bug fix to Circle mode's start position (was moving to last loiter target)
+2) WP_ACCEL parameter added to allow user to adjust acceleration during missions
+3) loiter acceleration set to half of LOIT_SPEED parameter value (was hard-coded)
+4) reduce AltHold P to 1.0 (was 2.0)
+------------------------------------------------------------------
+ArduCopter 3.0.0-rc5 04-Jun-2013
+Improvements over 3.0.0-rc4
+1) bug fix to LAND flight mode in which it could try to fly to mission's next waypoint location
+2) bug fix to Circle mode to allow counter-clockwise rotation
+3) bug fix to heading change in Loiter, RTL, Missions when pilot's throttle is zero
+4) bug fix for mission sticking at take-off command when pilot's throttle is zero
+5) bug fix for parameters not saving when new value is same as default value
+6) reduce pre-arm board min voltage check to 4.3V (was 4.5V)
+7) remove throttle controller's ability to limit lean angle in loiter, rtl, auto
+------------------------------------------------------------------
 ArduCopter 3.0.0-rc4 02-Jun-2013
 Improvements over 3.0.0-rc3
 1) loiter improvements:

ArduCopter/commands_logic.pde

@@ -323,9 +323,20 @@ static void do_land(const struct Location *cmd)
         // set landing state
         land_state = LAND_STATE_DESCENDING;
 
-        // switch to loiter which restores horizontal control to pilot
-        // To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands
-        set_roll_pitch_mode(ROLL_PITCH_LOITER);
+        // if we have gps lock, attempt to hold horizontal position
+        if( ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D ) {
+            // switch to loiter which restores horizontal control to pilot
+            // To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands
+            set_roll_pitch_mode(ROLL_PITCH_LOITER);
+            // switch into loiter nav mode
+            set_nav_mode(NAV_LOITER);
+        }else{
+            // no gps lock so give horizontal control to pilot
+            // To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands
+            set_roll_pitch_mode(ROLL_PITCH_STABLE);
+            // switch into loiter nav mode
+            set_nav_mode(NAV_NONE);
+        }
 
         // hold yaw while landing
         set_yaw_mode(YAW_HOLD);
@@ -333,8 +344,6 @@ static void do_land(const struct Location *cmd)
         // set throttle mode to land
         set_throttle_mode(THROTTLE_LAND);
 
-        // switch into loiter nav mode
-        set_nav_mode(NAV_LOITER);
     }
 }
 

ArduCopter/config.h

@@ -873,7 +873,7 @@
 // Loiter position control gains
 //
 #ifndef LOITER_P
- # define LOITER_P             		1.0f
+ # define LOITER_P             		0.2f
 #endif
 #ifndef LOITER_I
  # define LOITER_I             		0.0f
@@ -918,7 +918,7 @@
 #endif
 
 #ifndef ALT_HOLD_P
- # define ALT_HOLD_P            2.0f
+ # define ALT_HOLD_P            1.0f
 #endif
 #ifndef ALT_HOLD_I
  # define ALT_HOLD_I            0.0f

ArduCopter/events.pde

@@ -140,6 +140,11 @@ static void failsafe_gps_check()
     // take action based on flight mode
     if(mode_requires_GPS(control_mode))
         set_mode(LAND);
+
+    // land if circular fence is enabled
+    if((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) {
+        set_mode(LAND);
+    }
 }
 
 // failsafe_gps_off_event - actions to take when GPS contact is restored

ArduCopter/fence.pde

@@ -36,7 +36,7 @@ void fence_check()
                 init_disarm_motors();
             }else{
                 // if we have a GPS
-                if( ap.home_is_set ) {
+                if( ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D ) {
                     // if we are within 100m of the fence, RTL
                     if( fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
                         if(control_mode != RTL) {
@@ -44,7 +44,7 @@ void fence_check()
                         }
                     }else{
                         // if more than 100m outside the fence just force a land
-                        if(control_mode != RTL) {
+                        if(control_mode != LAND) {
                             set_mode(LAND);
                         }
                     }

ArduCopter/leds.pde

 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-static void update_lights()
+// update_board_leds - updates leds on board
+// should be called at 10hz
+static void update_board_leds()
 {
+    // we need to slow down the calls to the GPS and dancing lights to 3.33hz
+    static uint8_t counter = 0;
+    if(++counter >= 3){
+        counter = 0;
+    }
+
     switch(led_mode) {
     case NORMAL_LEDS:
-        update_motor_light();
-        update_GPS_light();
+        update_arming_light();
+        if (counter == 0) {
+            update_GPS_light();
+        }
         break;
 
     case SAVE_TRIM_LEDS:
-        dancing_light();
+        if (counter == 0) {
+            dancing_light();
+        }
         break;
     }
 }
@@ -47,20 +59,64 @@ static void update_GPS_light(void)
     }
 }
 
-static void update_motor_light(void)
+static void update_arming_light(void)
 {
-    if(motors.armed() == false) {
-        ap_system.motor_light = !ap_system.motor_light;
-
-        // blink
-        if(ap_system.motor_light) {
+    // counter to control state
+    static int8_t counter = 0;
+    counter++;
+
+    // disarmed
+    if(!motors.armed()) {
+        if(!ap.pre_arm_check) {
+            // failed pre-arm checks so double flash
+            switch(counter) {
+                case 0:
+                    ap_system.arming_light = true;
+                    break;
+                case 1:
+                    ap_system.arming_light = false;
+                    break;
+                case 2:
+                    ap_system.arming_light = true;
+                    break;
+                case 3:
+                case 4:
+                    ap_system.arming_light = false;
+                    break;
+                default:
+                    // reset counter to restart the sequence
+                    counter = -1;
+                    break;
+            }
+        }else{
+            // passed pre-arm checks so slower single flash
+            switch(counter) {
+                case 0:
+                case 1:
+                case 2:
+                    ap_system.arming_light = true;
+                    break;
+                case 3:
+                case 4:
+                case 5:
+                    ap_system.arming_light = false;
+                    break;
+                default:
+                    // reset counter to restart the sequence
+                    counter = -1;
+                    break;
+            }
+        }
+        // set arming led from arming_light flag
+        if(ap_system.arming_light) {
             digitalWrite(A_LED_PIN, LED_ON);
         }else{
             digitalWrite(A_LED_PIN, LED_OFF);
         }
     }else{
-        if(!ap_system.motor_light) {
-            ap_system.motor_light = true;
+        // armed
+        if(!ap_system.arming_light) {
+            ap_system.arming_light = true;
             digitalWrite(A_LED_PIN, LED_ON);
         }
     }
@@ -91,12 +147,13 @@ static void dancing_light()
         break;
     }
 }
+
 static void clear_leds()
 {
     digitalWrite(A_LED_PIN, LED_OFF);
     digitalWrite(B_LED_PIN, LED_OFF);
     digitalWrite(C_LED_PIN, LED_OFF);
-    ap_system.motor_light = false;
+    ap_system.arming_light = false;
     led_mode = NORMAL_LEDS;
 }
 

ArduCopter/motors.pde

@@ -159,10 +159,6 @@ static void init_arm_motors()
     init_barometer();
 #endif
 
-    // temp hack
-    ap_system.motor_light = true;
-    digitalWrite(A_LED_PIN, LED_ON);
-
     // go back to normal AHRS gains
     ahrs.set_fast_gains(false);
 #if SECONDARY_DMP_ENABLED == ENABLED
@@ -175,6 +171,9 @@ static void init_arm_motors()
     // set hover throttle
     motors.set_mid_throttle(g.throttle_mid);
 
+    // update leds on board
+    update_arming_light();
+
 #if COPTER_LEDS == ENABLED
     piezo_beep_twice();
 #endif
@@ -250,7 +249,7 @@ static void pre_arm_checks(bool display_failure)
 
     // check for unreasonable mag field length
     float mag_field = pythagorous3(compass.mag_x, compass.mag_y, compass.mag_z);
-    if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.5 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.5) {
+    if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35) {
         if (display_failure) {
             gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field"));
         }

ArduCopter/navigation.pde

@@ -68,7 +68,7 @@ static void calc_distance_and_bearing()
     }
 
     // calculate home distance and bearing
-    if( ap.home_is_set ) {
+    if( ap.home_is_set && (g_gps->status() == GPS::GPS_OK_FIX_3D || g_gps->status() == GPS::GPS_OK_FIX_2D)) {
         home_distance = pythagorous2(curr.x, curr.y);
         home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0));
 
@@ -103,8 +103,8 @@ static void run_autopilot()
 // set_nav_mode - update nav mode and initialise any variables as required
 static bool set_nav_mode(uint8_t new_nav_mode)
 {
-    // boolean to ensure proper initialisation of nav modes
-    bool nav_initialised = false;
+    bool nav_initialised = false;       // boolean to ensure proper initialisation of nav modes
+    Vector3f stopping_point;            // stopping point for circle mode
 
     // return immediately if no change
     if( new_nav_mode == nav_mode ) {
@@ -119,13 +119,14 @@ static bool set_nav_mode(uint8_t new_nav_mode)
 
         case NAV_CIRCLE:
             // set center of circle to current position
-            circle_set_center(inertial_nav.get_position(), ahrs.yaw);
+            wp_nav.get_stopping_point(inertial_nav.get_position(),inertial_nav.get_velocity(),stopping_point);
+            circle_set_center(stopping_point,ahrs.yaw);
             nav_initialised = true;
             break;
 
         case NAV_LOITER:
             // set target to current position
-            wp_nav.set_loiter_target(inertial_nav.get_position(), inertial_nav.get_velocity());
+            wp_nav.init_loiter_target(inertial_nav.get_position(), inertial_nav.get_velocity());
             nav_initialised = true;
             break;
 
@@ -217,13 +218,14 @@ static void
 circle_set_center(const Vector3f current_position, float heading_in_radians)
 {
     float max_velocity;
+    float cir_radius = g.circle_radius * 100;
 
     // set circle center to circle_radius ahead of current position
-    circle_center.x = current_position.x + (float)g.circle_radius * 100 * cos_yaw;
-    circle_center.y = current_position.y + (float)g.circle_radius * 100 * sin_yaw;
+    circle_center.x = current_position.x + cir_radius * cos_yaw;
+    circle_center.y = current_position.y + cir_radius * sin_yaw;
 
     // if we are doing a panorama set the circle_angle to the current heading
-    if( g.circle_radius == 0 ) {
+    if( g.circle_radius <= 0 ) {
         circle_angle = heading_in_radians;
         circle_angular_velocity_max = ToRad(g.circle_rate);
         circle_angular_acceleration = circle_angular_velocity_max;  // reach maximum yaw velocity in 1 second
@@ -232,16 +234,25 @@ circle_set_center(const Vector3f current_position, float heading_in_radians)
         circle_angle = wrap_PI(heading_in_radians-PI);
 
         // calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle
-        max_velocity = min(wp_nav.get_horizontal_velocity(), safe_sqrt(0.5f*WPNAV_ACCELERATION*g.circle_radius*100.0f)); 
+        max_velocity = min(wp_nav.get_horizontal_velocity(), safe_sqrt(0.5f*wp_nav.get_waypoint_acceleration()*g.circle_radius*100.0f)); 
+
+        // angular_velocity in radians per second
+        circle_angular_velocity_max = max_velocity/((float)g.circle_radius * 100.0f);
+        circle_angular_velocity_max = constrain_float(ToRad(g.circle_rate),-circle_angular_velocity_max,circle_angular_velocity_max);
 
         // angular_velocity in radians per second
-        circle_angular_velocity_max = min(ToRad(g.circle_rate), max_velocity/((float)g.circle_radius * 100.0f));
-        circle_angular_acceleration = WPNAV_ACCELERATION/((float)g.circle_radius * 100);
+        circle_angular_acceleration = wp_nav.get_waypoint_acceleration()/((float)g.circle_radius * 100);
+        if (g.circle_rate < 0.0f) {
+            circle_angular_acceleration = -circle_angular_acceleration;
+        }
     }
 
     // initialise other variables
     circle_angle_total = 0;
     circle_angular_velocity = 0;
+
+    // initialise loiter target.  Note: feed forward velocity set to zero
+    wp_nav.init_loiter_target(current_position, Vector3f(0,0,0));
 }
 
 // update_circle - circle position controller's main call which in turn calls loiter controller with updated target position
@@ -252,9 +263,16 @@ update_circle(float dt)
     Vector3f circle_target;
 
     // ramp up angular velocity to maximum
-    if(circle_angular_velocity < circle_angular_velocity_max){
-        circle_angular_velocity += circle_angular_acceleration * dt;
-        circle_angular_velocity = constrain_float(circle_angular_velocity, 0, circle_angular_velocity_max);
+    if (g.circle_rate >= 0) {
+        if (circle_angular_velocity < circle_angular_velocity_max) {
+            circle_angular_velocity += circle_angular_acceleration * dt;
+            circle_angular_velocity = constrain_float(circle_angular_velocity, 0, circle_angular_velocity_max);
+        }
+    }else{
+        if (circle_angular_velocity > circle_angular_velocity_max) {
+            circle_angular_velocity += circle_angular_acceleration * dt;
+            circle_angular_velocity = constrain_float(circle_angular_velocity, circle_angular_velocity_max, 0);
+        }
     }
 
     // update the target angle

ArduCopter/system.pde

@@ -420,7 +420,6 @@ static void set_mode(uint8_t mode)
         set_roll_pitch_mode(POSITION_RP);
         set_throttle_mode(POSITION_THR);
         set_nav_mode(POSITION_NAV);
-        wp_nav.clear_angle_limit();     // ensure there are no left over angle limits from throttle controller.  To-Do: move this to the exit routine of throttle controller
         break;
 
     case GUIDED:
@@ -434,7 +433,7 @@ static void set_mode(uint8_t mode)
 
     case LAND:
         // To-Do: it is messy to set manual_attitude here because the do_land function is reponsible for setting the roll_pitch_mode
-        if( g_gps->status() == GPS::GPS_OK_FIX_3D ) {
+        if( ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D ) {
             // switch to loiter if we have gps
             ap.manual_attitude = false;
         }else{

libraries/AC_WPNav/AC_WPNav.cpp

@@ -10,7 +10,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
     // @Param: SPEED
     // @DisplayName: Waypoint Horizontal Speed Target
     // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally during a WP mission
-    // @Units: Centimeters/Second
+    // @Units: cm/s
     // @Range: 0 2000
     // @Increment: 50
     // @User: Standard
@@ -19,7 +19,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
     // @Param: RADIUS
     // @DisplayName: Waypoint Radius
     // @Description: Defines the distance from a waypoint, that when crossed indicates the wp has been hit.
-    // @Units: Centimeters
+    // @Units: cm
     // @Range: 100 1000
     // @Increment: 1
     // @User: Standard
@@ -28,7 +28,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
     // @Param: SPEED_UP
     // @DisplayName: Waypoint Climb Speed Target
     // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain while climbing during a WP mission
-    // @Units: Centimeters/Second
+    // @Units: cm/s
     // @Range: 0 1000
     // @Increment: 50
     // @User: Standard
@@ -37,7 +37,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
     // @Param: SPEED_DN
     // @DisplayName: Waypoint Descent Speed Target
     // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain while descending during a WP mission
-    // @Units: Centimeters/Second
+    // @Units: cm/s
     // @Range: 0 1000
     // @Increment: 50
     // @User: Standard
@@ -46,12 +46,21 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
     // @Param: LOIT_SPEED
     // @DisplayName: Loiter Horizontal Maximum Speed
     // @Description: Defines the maximum speed in cm/s which the aircraft will travel horizontally while in loiter mode
-    // @Units: Centimeters/Second
+    // @Units: cm/s
     // @Range: 0 2000
     // @Increment: 50
     // @User: Standard
     AP_GROUPINFO("LOIT_SPEED",  4, AC_WPNav, _loiter_speed_cms, WPNAV_LOITER_SPEED),
 
+    // @Param: ACCEL
+    // @DisplayName: Waypoint Acceleration 
+    // @Description: Defines the horizontal acceleration in cm/s/s used during missions
+    // @Units: cm/s/s
+    // @Range: 0 980
+    // @Increment: 10
+    // @User: Standard
+    AP_GROUPINFO("ACCEL",       5, AC_WPNav, _wp_accel_cms, WPNAV_ACCELERATION),
+
     AP_GROUPEND
 };
 
@@ -79,8 +88,8 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM
     _pilot_vel_right_cms(0),
     _target_vel(0,0,0),
     _vel_last(0,0,0),
-    _lean_angle_max(MAX_LEAN_ANGLE),
     _loiter_leash(WPNAV_MIN_LEASH_LENGTH),
+    _loiter_accel_cms(WPNAV_LOITER_ACCEL_MAX),
     _wp_leash_xy(WPNAV_MIN_LEASH_LENGTH),
     _wp_leash_z(WPNAV_MIN_LEASH_LENGTH),
     _track_accel(0),
@@ -112,23 +121,23 @@ void AC_WPNav::get_stopping_point(const Vector3f& position, const Vector3f& velo
     // calculate current velocity
     vel_total = safe_sqrt(velocity.x*velocity.x + velocity.y*velocity.y);
 
-    // avoid divide by zero by using current position if the velocity is below 10cm/s or kP is very low
-    if (vel_total < 10.0f || kP <= 0.0f) {
+    // avoid divide by zero by using current position if the velocity is below 10cm/s, kP is very low or acceleration is zero
+    if (vel_total < 10.0f || kP <= 0.0f || _wp_accel_cms <= 0.0f) {
         target = position;
         return;
     }
 
     // calculate point at which velocity switches from linear to sqrt
-    linear_velocity = WPNAV_ACCELERATION/kP;
+    linear_velocity = _wp_accel_cms/kP;
 
     // calculate distance within which we can stop
     if (vel_total < linear_velocity) {
         target_dist = vel_total/kP;
     } else {
-        linear_distance = WPNAV_ACCELERATION/(2*kP*kP);
-        target_dist = linear_distance + (vel_total*vel_total)/(2*WPNAV_ACCELERATION);
+        linear_distance = _wp_accel_cms/(2.0f*kP*kP);
+        target_dist = linear_distance + (vel_total*vel_total)/(2.0f*_wp_accel_cms);
     }
-    target_dist = constrain_float(target_dist, 0, _loiter_leash*2.0);
+    target_dist = constrain_float(target_dist, 0, _loiter_leash*2.0f);
 
     target.x = position.x + (target_dist * velocity.x / vel_total);
     target.y = position.y + (target_dist * velocity.y / vel_total);
@@ -143,8 +152,8 @@ void AC_WPNav::set_loiter_target(const Vector3f& position)
     _target_vel.y = 0;
 }
 
-/// set_loiter_target - set initial loiter target based on current position and velocity
-void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& velocity)
+/// init_loiter_target - set initial loiter target based on current position and velocity
+void AC_WPNav::init_loiter_target(const Vector3f& position, const Vector3f& velocity)
 {
     // set target position and velocity based on current pos and velocity
     _target.x = position.x;
@@ -155,14 +164,22 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& veloc
     // initialise desired roll and pitch to current roll and pitch.  This avoids a random twitch between now and when the loiter controller is first run
     _desired_roll = constrain_int32(_ahrs->roll_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
     _desired_pitch = constrain_int32(_ahrs->pitch_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
+
+    // initialise pilot input
+    _pilot_vel_forward_cms = 0;
+    _pilot_vel_right_cms = 0;
+
+    // set last velocity to current velocity
+    // To-Do: remove the line below by instead forcing reset_I to be called on the first loiter_update call
+    _vel_last = _inav->get_velocity();
 }
 
 /// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s
 void AC_WPNav::move_loiter_target(float control_roll, float control_pitch, float dt)
 {
     // convert pilot input to desired velocity in cm/s
-    _pilot_vel_forward_cms = -control_pitch * WPNAV_LOITER_ACCEL_MAX / 4500.0f;
-    _pilot_vel_right_cms = control_roll * WPNAV_LOITER_ACCEL_MAX / 4500.0f;
+    _pilot_vel_forward_cms = -control_pitch * _loiter_accel_cms / 4500.0f;
+    _pilot_vel_right_cms = control_roll * _loiter_accel_cms / 4500.0f;
 }
 
 /// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
@@ -189,17 +206,17 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
     _target_vel.x += target_vel_adj.x*nav_dt;
     _target_vel.y += target_vel_adj.y*nav_dt;
     if(_target_vel.x > 0 ) {
-        _target_vel.x -= (WPNAV_LOITER_ACCEL_MAX-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.x/_loiter_speed_cms;
+        _target_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.x/_loiter_speed_cms;
         _target_vel.x = max(_target_vel.x - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
     }else if(_target_vel.x < 0) {
-        _target_vel.x -= (WPNAV_LOITER_ACCEL_MAX-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.x/_loiter_speed_cms;
+        _target_vel.x -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.x/_loiter_speed_cms;
         _target_vel.x = min(_target_vel.x + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
     }
     if(_target_vel.y > 0 ) {
-        _target_vel.y -= (WPNAV_LOITER_ACCEL_MAX-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.y/_loiter_speed_cms;
+        _target_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.y/_loiter_speed_cms;
         _target_vel.y = max(_target_vel.y - WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
     }else if(_target_vel.y < 0) {
-        _target_vel.y -= (WPNAV_LOITER_ACCEL_MAX-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.y/_loiter_speed_cms;
+        _target_vel.y -= (_loiter_accel_cms-WPNAV_LOITER_ACCEL_MIN)*nav_dt*_target_vel.y/_loiter_speed_cms;
         _target_vel.y = min(_target_vel.y + WPNAV_LOITER_ACCEL_MIN*nav_dt, 0);
     }
 
@@ -253,7 +270,7 @@ void AC_WPNav::update_loiter()
     translate_loiter_target_movements(dt);
 
     // run loiter position controller
-    get_loiter_position_to_velocity(dt, _loiter_speed_cms);
+    get_loiter_position_to_velocity(dt, WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR);
 }
 
 /// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
@@ -262,19 +279,27 @@ void AC_WPNav::calculate_loiter_leash_length()
     // get loiter position P
     float kP = _pid_pos_lat->kP();
 
+    // check loiter speed
+    if( _loiter_speed_cms < 100.0f) {
+        _loiter_speed_cms = 100.0f;
+    }
+
+    // set loiter acceleration to 1/2 loiter speed
+    _loiter_accel_cms = _loiter_speed_cms / 2.0f;
+
     // avoid divide by zero
-    if (kP <= 0.0f) {
+    if (kP <= 0.0f || _wp_accel_cms <= 0.0f) {
         _loiter_leash = WPNAV_MIN_LEASH_LENGTH;
         return;
     }
 
-    // calculate horiztonal leash length
-    if(_loiter_speed_cms <= WPNAV_ACCELERATION / kP) {
+    // calculate horizontal leash length
+    if(WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR <= _wp_accel_cms / kP) {
         // linear leash length based on speed close in
-        _loiter_leash = _loiter_speed_cms / kP;
+        _loiter_leash = WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR / kP;
     }else{
         // leash length grows at sqrt of speed further out
-        _loiter_leash = (WPNAV_ACCELERATION / (2.0*kP*kP)) + (_loiter_speed_cms*_loiter_speed_cms / (2*WPNAV_ACCELERATION));
+        _loiter_leash = (_wp_accel_cms / (2.0f*kP*kP)) + (WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR*WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR / (2.0f*_wp_accel_cms));
     }
 
     // ensure leash is at least 1m long
@@ -504,17 +529,18 @@ void AC_WPNav::get_loiter_position_to_velocity(float dt, float max_speed_cms)
         dist_error.x = _target.x - curr.x;
         dist_error.y = _target.y - curr.y;
 
-        linear_distance = WPNAV_ACCELERATION/(2*kP*kP);
-        _distance_to_target = linear_distance;      // for reporting purposes
+        linear_distance = _wp_accel_cms/(2.0f*kP*kP);
 
         dist_error_total = safe_sqrt(dist_error.x*dist_error.x + dist_error.y*dist_error.y);
-        if( dist_error_total > 2*linear_distance ) {
-            vel_sqrt = safe_sqrt(2*WPNAV_ACCELERATION*(dist_error_total-linear_distance));
+        _distance_to_target = dist_error_total;      // for reporting purposes
+
+        if( dist_error_total > 2.0f*linear_distance ) {
+            vel_sqrt = safe_sqrt(2.0f*_wp_accel_cms*(dist_error_total-linear_distance));
             desired_vel.x = vel_sqrt * dist_error.x/dist_error_total;
             desired_vel.y = vel_sqrt * dist_error.y/dist_error_total;
         }else{
-            desired_vel.x = _pid_pos_lat->get_p(dist_error.x);
-            desired_vel.y = _pid_pos_lon->get_p(dist_error.y);
+            desired_vel.x = _pid_pos_lat->kP() * dist_error.x;
+            desired_vel.y = _pid_pos_lon->kP() * dist_error.y;
         }
 
         // ensure velocity stays within limits
@@ -589,8 +615,8 @@ void AC_WPNav::get_loiter_acceleration_to_lean_angles(float accel_lat, float acc
     accel_right = -accel_lat*_sin_yaw + accel_lon*_cos_yaw;
 
     // update angle targets that will be passed to stabilize controller
-    _desired_roll = constrain_float(fast_atan(accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max);
-    _desired_pitch = constrain_float(fast_atan(-accel_forward/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max);
+    _desired_roll = constrain_float(fast_atan(accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI), -MAX_LEAN_ANGLE, MAX_LEAN_ANGLE);
+    _desired_pitch = constrain_float(fast_atan(-accel_forward/(-z_accel_meas))*(18000/M_PI), -MAX_LEAN_ANGLE, MAX_LEAN_ANGLE);
 }
 
 // get_bearing_cd - return bearing in centi-degrees between two positions
@@ -623,18 +649,23 @@ void AC_WPNav::calculate_wp_leash_length(bool climb)
     // get loiter position P
     float kP = _pid_pos_lat->kP();
 
+    // sanity check acceleration and avoid divide by zero
+    if (_wp_accel_cms <= 0.0f) {
+        _wp_accel_cms = WPNAV_ACCELERATION_MIN;
+    }
+    
     // avoid divide by zero
     if (kP <= 0.0f) {
         _wp_leash_xy = WPNAV_MIN_LEASH_LENGTH;
         return;
     }
     // calculate horiztonal leash length
-    if(_wp_speed_cms <= WPNAV_ACCELERATION / kP) {
+    if(_wp_speed_cms <= _wp_accel_cms / kP) {
         // linear leash length based on speed close in
         _wp_leash_xy = _wp_speed_cms / kP;
     }else{
         // leash length grows at sqrt of speed further out
-        _wp_leash_xy = (WPNAV_ACCELERATION / (2.0*kP*kP)) + (_wp_speed_cms*_wp_speed_cms / (2*WPNAV_ACCELERATION));
+        _wp_leash_xy = (_wp_accel_cms / (2.0f*kP*kP)) + (_wp_speed_cms*_wp_speed_cms / (2.0f*_wp_accel_cms));
     }
 
     // ensure leash is at least 1m long
@@ -672,7 +703,7 @@ void AC_WPNav::calculate_wp_leash_length(bool climb)
         _track_speed = 0;
         _track_leash_length = WPNAV_MIN_LEASH_LENGTH;
     }else if(_pos_delta_unit.z == 0){
-        _track_accel = WPNAV_ACCELERATION/pos_delta_unit_xy;
+        _track_accel = _wp_accel_cms/pos_delta_unit_xy;
         _track_speed = _wp_speed_cms/pos_delta_unit_xy;
         _track_leash_length = _wp_leash_xy/pos_delta_unit_xy;
     }else if(pos_delta_unit_xy == 0){
@@ -680,7 +711,7 @@ void AC_WPNav::calculate_wp_leash_length(bool climb)
         _track_speed = speed_vert/pos_delta_unit_z;
         _track_leash_length = _wp_leash_z/pos_delta_unit_z;
     }else{	
-        _track_accel = min(WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z, WPNAV_ACCELERATION/pos_delta_unit_xy);
+        _track_accel = min(WPNAV_ALT_HOLD_ACCEL_MAX/pos_delta_unit_z, _wp_accel_cms/pos_delta_unit_xy);
         _track_speed = min(speed_vert/pos_delta_unit_z, _wp_speed_cms/pos_delta_unit_xy);
         _track_leash_length = min(_wp_leash_z/pos_delta_unit_z, _wp_leash_xy/pos_delta_unit_xy);
     }

libraries/AC_WPNav/AC_WPNav.h

@@ -11,14 +11,16 @@
 #include <AP_InertialNav.h>     // Inertial Navigation library
 
 // loiter maximum velocities and accelerations
-#define WPNAV_ACCELERATION              250.0f      // defines the velocity vs distant curve.  maximum acceleration in cm/s/s that position controller asks for from acceleration controller
+#define WPNAV_ACCELERATION              100.0f      // defines the default velocity vs distant curve.  maximum acceleration in cm/s/s that position controller asks for from acceleration controller
+#define WPNAV_ACCELERATION_MIN           50.0f      // minimum acceleration in cm/s/s - used for sanity checking _wp_accel parameter
 #define WPNAV_ACCEL_MAX                 980.0f      // max acceleration in cm/s/s that the loiter velocity controller will ask from the lower accel controller.
                                                     // should be 1.5 times larger than WPNAV_ACCELERATION.
                                                     // max acceleration = max lean angle * 980 * pi / 180.  i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s
 
-#define WPNAV_LOITER_SPEED              750.0f      // maximum default loiter speed in cm/s
-#define WPNAV_LOITER_ACCEL_MAX          375.0f      // maximum acceleration in loiter mode
+#define WPNAV_LOITER_SPEED              500.0f      // maximum default loiter speed in cm/s
+#define WPNAV_LOITER_ACCEL_MAX          250.0f      // maximum acceleration in loiter mode
 #define WPNAV_LOITER_ACCEL_MIN           25.0f      // minimum acceleration in loiter mode
+#define WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR 200.0f      // maximum speed used to correct position error (i.e. not including feed forward)
 
 #define MAX_LEAN_ANGLE                  4500        // default maximum lean angle
 
@@ -28,7 +30,7 @@
 #define WPNAV_WP_SPEED_UP               250.0f      // default maximum climb velocity
 #define WPNAV_WP_SPEED_DOWN             150.0f      // default maximum descent velocity
 
-#define WPNAV_ALT_HOLD_P                2.0f        // default throttle controller's altitude hold's P gain.
+#define WPNAV_ALT_HOLD_P                1.0f        // default throttle controller's altitude hold's P gain.
 #define WPNAV_ALT_HOLD_ACCEL_MAX        250.0f      // hard coded copy of throttle controller's maximum acceleration in cm/s.  To-Do: remove duplication with throttle controller definition
 
 #define WPNAV_MIN_LEASH_LENGTH          100.0f      // minimum leash lengths in cm
@@ -50,8 +52,8 @@ public:
     /// set_loiter_target in cm from home
     void set_loiter_target(const Vector3f& position);
 
-    /// set_loiter_target - set initial loiter target based on current position and velocity
-    void set_loiter_target(const Vector3f& position, const Vector3f& velocity);
+    /// init_loiter_target - set initial loiter target based on current position and velocity
+    void init_loiter_target(const Vector3f& position, const Vector3f& velocity);
 
     /// move_loiter_target - move destination using pilot input
     void move_loiter_target(float control_roll, float control_pitch, float dt);
@@ -65,15 +67,6 @@ public:
     /// update_loiter - run the loiter controller - should be called at 10hz
     void update_loiter();
 
-    /// set_angle_limit - limits maximum angle in centi-degrees the copter will lean
-    void set_angle_limit(int32_t lean_angle) { _lean_angle_max = lean_angle; }
-
-    /// clear_angle_limit - reset angle limits back to defaults
-    void clear_angle_limit() { _lean_angle_max = MAX_LEAN_ANGLE; }
-
-    /// get_angle_limit - retrieve maximum angle in centi-degrees the copter will lean
-    int32_t get_angle_limit() const { return _lean_angle_max; }
-
     /// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
     void get_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target) const;
 
@@ -147,6 +140,9 @@ public:
     /// get_waypoint_radius - access for waypoint radius in cm
     float get_waypoint_radius() const { return _wp_radius_cm; }
 
+    /// get_waypoint_acceleration - returns acceleration in cm/s/s during missions
+    float get_waypoint_acceleration() const { return _wp_accel_cms.get(); }
+
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:
@@ -200,6 +196,7 @@ protected:
     AP_Float    _wp_speed_up_cms;       // climb speed target in cm/s
     AP_Float    _wp_speed_down_cms;     // descent speed target in cm/s
     AP_Float    _wp_radius_cm;          // distance from a waypoint in cm that, when crossed, indicates the wp has been reached
+    AP_Float    _wp_accel_cms;          // acceleration in cm/s/s during missions
     uint32_t	_loiter_last_update;    // time of last update_loiter call
     uint32_t	_wpnav_last_update;     // time of last update_wpnav call
     float       _cos_yaw;               // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
@@ -217,8 +214,8 @@ protected:
     int16_t     _pilot_vel_right_cms;   // pilot's desired velocity right (body-frame)
     Vector3f    _target_vel;            // pilot's latest desired velocity in earth-frame
     Vector3f    _vel_last;              // previous iterations velocity in cm/s
-    int32_t     _lean_angle_max;        // maximum lean angle.  can be set from main code so that throttle controller can stop leans that cause copter to lose altitude
     float       _loiter_leash;          // loiter's horizontal leash length in cm.  used to stop the pilot from pushing the target location too far from the current location
+    float       _loiter_accel_cms;      // loiter's acceleration in cm/s/s
 
     // waypoint controller internal variables
     Vector3f    _origin;                // starting point of trip to next waypoint in cm from home (equivalent to next_WP)

libraries/AP_InertialSensor/AP_InertialSensor.cpp

@@ -477,7 +477,7 @@ bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6],
         success = false;
     }
     // sanity check offsets (2.0 is roughly 2/10th of a G, 5.0 is roughly half a G)
-    if( accel_offsets.is_nan() || fabsf(accel_offsets.x) > 2.0f || fabsf(accel_offsets.y) > 2.0f || fabsf(accel_offsets.z) > 3.0f ) {
+    if( accel_offsets.is_nan() || fabsf(accel_offsets.x) > 3.0f || fabsf(accel_offsets.y) > 3.0f || fabsf(accel_offsets.z) > 3.0f ) {
         success = false;
     }
 

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -166,20 +166,23 @@ void AP_MotorsMatrix::output_armed()
         // calculate amount of yaw we can fit into the throttle range
         // this is always equal to or less than the requested yaw from the pilot or rate controller
         yaw_allowed = min(out_max - out_max_range, out_max_range - out_min) - (rpy_high-rpy_low)/2;
-        yaw_allowed = max(yaw_allowed, AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM);     // allow at least 200 of yaw
+        yaw_allowed = max(yaw_allowed, AP_MOTORS_MATRIX_YAW_LOWER_LIMIT_PWM);
 
-        if (_rc_yaw->pwm_out > 0) {
+        if (_rc_yaw->pwm_out >= 0) {
             // if yawing right
-            yaw_allowed = min(yaw_allowed, _rc_yaw->pwm_out);     // minimum that we can fit vs what we have asked for
-            // we haven't even been able to apply full yaw command
-            _reached_limit |= AP_MOTOR_YAW_LIMIT;
-        }else if(_rc_yaw->pwm_out < 0) {
-            // if yawing left
-            yaw_allowed = max(-yaw_allowed, _rc_yaw->pwm_out);
-            // we haven't even been able to apply full yaw command
-            _reached_limit |= AP_MOTOR_YAW_LIMIT;
+            if (yaw_allowed > _rc_yaw->pwm_out) {
+                yaw_allowed = _rc_yaw->pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
+            }else{
+                _reached_limit |= AP_MOTOR_YAW_LIMIT;
+            }
         }else{
-            yaw_allowed = 0;
+            // if yawing left
+            yaw_allowed = -yaw_allowed;
+            if( yaw_allowed < _rc_yaw->pwm_out ) {
+                yaw_allowed = _rc_yaw->pwm_out; // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
+            }else{
+                _reached_limit |= AP_MOTOR_YAW_LIMIT;
+            }
         }
 
         // add yaw to intermediate numbers for each motor