From 8839a7897cf00908f876a3538c46d7941496731f Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 10 Jul 2013 19:44:24 +0900
Subject: [PATCH] WPNav: replace LOITER_ACCEL_MAX with parameter
Also removed unused _lean_angle_max variable
---
ArduCopter/navigation.pde | 4 ++--
ArduCopter/system.pde | 1 -
libraries/AC_WPNav/AC_WPNav.cpp | 17 ++++++++---------
libraries/AC_WPNav/AC_WPNav.h | 13 +++----------
4 files changed, 13 insertions(+), 22 deletions(-)
diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde
index 286460c74..7a984bc8e 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 63767f644..b59d714eb 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 8d8ecde4f..02b4d35c6 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 20b137e4e..9942611a8 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
--
GitLab