diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index aa765f5273..602d5fffac 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.5" +#define THISFIRMWARE "ArduCopter V2.5.1" /* ArduCopter Version 2.5 Lead author: Jason Short diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 3e9fa8d90a..8c489897f9 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -723,7 +723,7 @@ // Throttle control gains // #ifndef THROTTLE_CRUISE -# define THROTTLE_CRUISE 350 // +# define THROTTLE_CRUISE 450 // #endif #ifndef ALT_HOLD_P diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 221f665151..f74ffd58e1 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -5,7 +5,7 @@ //**************************************************************** static byte navigate() { - // waypoint distance from plane in meters + // waypoint distance from plane in cm // --------------------------------------- wp_distance = get_distance(¤t_loc, &next_WP); home_distance = get_distance(¤t_loc, &home); @@ -32,7 +32,7 @@ static bool check_missed_wp() int32_t temp; temp = target_bearing - original_target_bearing; temp = wrap_180(temp); - return (abs(temp) > 10000); //we pased the waypoint by 10 ° + return (abs(temp) > 10000); // we passed the waypoint by 100 degrees } // ------------------------------ @@ -529,7 +529,7 @@ static int32_t wrap_180(int32_t error) } */ -// distance is returned in meters +// distance is returned in cm static int32_t get_distance(struct Location *loc1, struct Location *loc2) { float dlat = (float)(loc2->lat - loc1->lat); diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 3034468352..c09478c3fa 100755 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -120,6 +120,7 @@ AC_PID::load_gains() _ki.load(); _kd.load(); _imax.load(); + _imax = abs(_imax); } void diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index b0ff88cfca..536317335b 100755 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -33,7 +33,7 @@ public: _kp (initial_p), _ki (initial_i), _kd (initial_d), - _imax(initial_imax) + _imax(abs(initial_imax)) { // no need for explicit load, assuming that the main code uses AP_Param::load_all. } @@ -78,7 +78,7 @@ public: const float i, const float d, const int16_t imaxval) { - _kp = p; _ki = i; _kd = d; _imax = imaxval; + _kp = p; _ki = i; _kd = d; _imax = abs(imaxval); } float kP() const { return _kp.get(); }