diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 286460c745..7a984bc8e7 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -234,14 +234,14 @@ 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_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; } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 63767f6443..b59d714eb6 100644 --- a/ArduCopter/system.pde +++ b/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: diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 8d8ecde4f6..02b4d35c6f 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -88,7 +88,6 @@ 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), @@ -179,8 +178,8 @@ void AC_WPNav::init_loiter_target(const Vector3f& position, const Vector3f& velo 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 @@ -207,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); } @@ -616,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 diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 20b137e4e6..9942611a86 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -67,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; @@ -149,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: @@ -220,7 +214,6 @@ 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