Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: add angle limits and set from AC's throttle controller

This commit is contained in:
Randy Mackay 2013-04-01 15:45:23 +09:00
parent afd2f82768
commit f82ce449d7
4 changed files with 21 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Attitude.pde
View File

@ -1075,6 +1075,10 @@ get_throttle_rate(int16_t 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((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 ) {

1
ArduCopter/system.pde
View File

@ -448,6 +448,7 @@ 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:

7
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -26,7 +26,8 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lo
_pid_pos_lat(pid_pos_lat),
_pid_pos_lon(pid_pos_lon),
_pid_rate_lat(pid_rate_lat),
_pid_rate_lon(pid_rate_lon)
_pid_rate_lon(pid_rate_lon),
_lean_angle_max(MAX_LEAN_ANGLE)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -340,8 +341,8 @@ void AC_WPNav::get_loiter_accel_lat_lon(int16_t accel_lat, int16_t accel_lon)
accel_right = -accel_lat*_sin_yaw + accel_lon*_cos_yaw;
// update angle targets that will be passed to stabilize controller
_desired_roll = constrain((accel_right/(-z_accel_meas))*(18000/M_PI), -4500, 4500);
_desired_pitch = constrain((-accel_forward/(-z_accel_meas*_cos_roll))*(18000/M_PI), -4500, 4500);
_desired_roll = constrain((accel_right/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max, _lean_angle_max);
_desired_pitch = constrain((-accel_forward/(-z_accel_meas*_cos_roll))*(18000/M_PI), -_lean_angle_max, _lean_angle_max);
}
// get_bearing_cd - return bearing in centi-degrees between two positions

12
libraries/AC_WPNav/AC_WPNav.h
View File

@ -20,6 +20,7 @@
#define MAX_LOITER_OVERSHOOT 1000 // maximum distance (in cm) that we will allow the target loiter point to be from the current location when switching into loiter
#define WPINAV_MAX_POS_ERROR 2000.0f // maximum distance (in cm) that the desired track can stray from our current location.
#define WP_SPEED 500 // default horizontal speed betwen waypoints in cm/s
#define MAX_LEAN_ANGLE 4500 // default maximum lean angle
class AC_WPNav
{
@ -53,6 +54,15 @@ 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() { return _lean_angle_max; }
///
/// waypoint navigation
///
@ -145,6 +155,8 @@ protected:
int32_t _desired_pitch; // fed to stabilize controllers at 50hz
int32_t _desired_altitude; // fed to alt hold controller at 50hz
int32_t _lean_angle_max; // maximum lean angle. can we set from main code so that throttle controller can stop leans that cause copter to lose altitude
// internal variables
Vector3f _target; // loiter's target location in cm from home
Vector3f _origin; // starting point of trip to next waypoint in cm from home (equivalent to next_WP)