Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

WPNav: replace LOITER_ACCEL_MAX with parameter

Browse Source 
Also removed unused _lean_angle_max variable
This commit is contained in:
Randy Mackay 2013-07-10 19:44:24 +09:00
parent 204f613467
commit 345924ddec
4 changed files with 13 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/navigation.pde
View File

@ -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;
}

1
ArduCopter/system.pde
View File

@ -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:

17
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -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

13
libraries/AC_WPNav/AC_WPNav.h
View File

@ -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