Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: configurable max lean angle

Browse Source 
ANGLE_MAX parameter allows limiting the roll and pitch angles during
manual and auto flight modes to anywhere from 10 to 80 degrees
This commit is contained in:
Randy Mackay 2013-08-11 12:51:08 +09:00
parent 5049595a2c
commit 6969ab573d
9 changed files with 77 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduCopter/ArduCopter.pde
View File

@ -1300,6 +1300,9 @@ static void super_slow_loop()
// pass latest alt hold kP value to navigation controller
wp_nav.set_althold_kP(g.pi_alt_hold.kP());
// update latest lean angle to navigation controller
wp_nav.set_lean_angle_max(g.angle_max);
// log battery info to the dataflash
if ((g.log_bitmask & MASK_LOG_CURRENT) && motors.armed())
Log_Write_Current();
@ -1705,9 +1708,8 @@ void update_roll_pitch_mode(void)
update_simple_mode();
}
// copy control_roll and pitch for reporting purposes
control_roll = g.rc_1.control_in;
control_pitch = g.rc_2.control_in;
// convert pilot input to lean angles
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
// pass desired roll, pitch to stabilize attitude controllers
get_stabilize_roll(control_roll);
@ -1733,9 +1735,8 @@ void update_roll_pitch_mode(void)
update_simple_mode();
}
// copy pilot input to control_roll and pitch for reporting purposes
control_roll = g.rc_1.control_in;
control_pitch = g.rc_2.control_in;
// convert pilot input to lean angles
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
// mix in user control with optical flow
get_stabilize_roll(get_of_roll(control_roll));

45
ArduCopter/Attitude.pde
View File

@ -1,5 +1,30 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// get_pilot_desired_angle - transform pilot's roll or pitch input into a desired lean angle
// returns desired angle in centi-degrees
static void get_pilot_desired_lean_angles(int16_t roll_in, int16_t pitch_in, int16_t &roll_out, int16_t &pitch_out)
{
static float _scaler = 1.0;
static int16_t _angle_max = 0;
// return immediately if no scaling required
if (g.angle_max == ROLL_PITCH_INPUT_MAX) {
roll_out = roll_in;
pitch_out = pitch_in;
return;
}
// check if angle_max has been updated and redo scaler
if (g.angle_max != _angle_max) {
_angle_max = g.angle_max;
_scaler = (float)g.angle_max/(float)ROLL_PITCH_INPUT_MAX;
}
// convert pilot input to lean angle
roll_out = roll_in * _scaler;
pitch_out = pitch_in * _scaler;
}
static void
get_stabilize_roll(int32_t target_angle)
{
@ -7,7 +32,7 @@ get_stabilize_roll(int32_t target_angle)
target_angle = wrap_180_cd(target_angle - ahrs.roll_sensor);
// limit the error we're feeding to the PID
target_angle = constrain_int32(target_angle, -4500, 4500);
target_angle = constrain_int32(target_angle, -g.angle_max, g.angle_max);
// convert to desired rate
int32_t target_rate = g.pi_stabilize_roll.kP() * target_angle;
@ -23,7 +48,7 @@ get_stabilize_pitch(int32_t target_angle)
target_angle = wrap_180_cd(target_angle - ahrs.pitch_sensor);
// limit the error we're feeding to the PID
target_angle = constrain_int32(target_angle, -4500, 4500);
target_angle = constrain_int32(target_angle, -g.angle_max, g.angle_max);
// convert to desired rate
int32_t target_rate = g.pi_stabilize_pitch.kP() * target_angle;
@ -114,10 +139,10 @@ get_acro_level_rates()
int32_t target_rate = 0;
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (roll_angle > 4500){
target_rate = g.pi_stabilize_roll.get_p(4500-roll_angle);
}else if (roll_angle < -4500) {
target_rate = g.pi_stabilize_roll.get_p(-4500-roll_angle);
if (roll_angle > g.angle_max){
target_rate = g.pi_stabilize_roll.get_p(g.angle_max-roll_angle);
}else if (roll_angle < -g.angle_max) {
target_rate = g.pi_stabilize_roll.get_p(-g.angle_max-roll_angle);
}
}
roll_angle = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
@ -131,10 +156,10 @@ get_acro_level_rates()
target_rate = 0;
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (pitch_angle > 4500){
target_rate = g.pi_stabilize_pitch.get_p(4500-pitch_angle);
}else if (pitch_angle < -4500) {
target_rate = g.pi_stabilize_pitch.get_p(-4500-pitch_angle);
if (pitch_angle > g.angle_max){
target_rate = g.pi_stabilize_pitch.get_p(g.angle_max-pitch_angle);
}else if (pitch_angle < -g.angle_max) {
target_rate = g.pi_stabilize_pitch.get_p(-g.angle_max-pitch_angle);
}
}
pitch_angle = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);

4
ArduCopter/Parameters.h
View File

@ -88,7 +88,8 @@ public:
k_param_sonar_gain,
k_param_ch8_option,
k_param_arming_check_enabled,
k_param_sprayer, // 33
k_param_sprayer,
k_param_angle_max, // 34
// 65: AP_Limits Library
k_param_limits = 65, // deprecated - remove
@ -299,6 +300,7 @@ public:
AP_Int8 battery_curr_pin;
AP_Int8 rssi_pin;
AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions
AP_Int16 angle_max; // maximum lean angle of the copter in centi-degrees
// Waypoints
//

7
ArduCopter/Parameters.pde
View File

@ -423,6 +423,13 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(arming_check_enabled, "ARMING_CHECK", 1),
// @Param: ANGLE_MAX
// @DisplayName: Angle Max
// @Description: Maximum lean angle in all flight modes
// @Range 1000 8000
// @User: Advanced
GSCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX),
#if FRAME_CONFIG == HELI_FRAME
// @Group: HS1_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp

9
ArduCopter/config.h
View File

@ -814,12 +814,11 @@
//////////////////////////////////////////////////////////////////////////////
// Stabilize Rate Control
//
#ifndef MAX_INPUT_ROLL_ANGLE
# define MAX_INPUT_ROLL_ANGLE 4500
#ifndef ROLL_PITCH_INPUT_MAX
# define ROLL_PITCH_INPUT_MAX 4500 // roll, pitch input range
#endif
#ifndef MAX_INPUT_PITCH_ANGLE
# define MAX_INPUT_PITCH_ANGLE 4500
#ifndef DEFAULT_ANGLE_MAX
# define DEFAULT_ANGLE_MAX 4500 // ANGLE_MAX parameters default value
#endif
#ifndef RATE_ROLL_P
# define RATE_ROLL_P 0.150f

8
ArduCopter/motors.pde
View File

@ -322,6 +322,14 @@ static void pre_arm_checks(bool display_failure)
}
}
// lean angle parameter check
if (g.angle_max < 1000 || g.angle_max > 8000) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check ANGLE_MAX"));
}
return;
}
// if we've gotten this far then pre arm checks have completed
ap.pre_arm_check = true;
}

4
ArduCopter/radio.pde
View File

@ -20,8 +20,8 @@ static void default_dead_zones()
static void init_rc_in()
{
// set rc channel ranges
g.rc_1.set_angle(MAX_INPUT_ROLL_ANGLE);
g.rc_2.set_angle(MAX_INPUT_PITCH_ANGLE);
g.rc_1.set_angle(ROLL_PITCH_INPUT_MAX);
g.rc_2.set_angle(ROLL_PITCH_INPUT_MAX);
#if FRAME_CONFIG == HELI_FRAME
// we do not want to limit the movment of the heli's swash plate
g.rc_3.set_range(0, 1000);

13
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -90,6 +90,7 @@ AC_WPNav::AC_WPNav(AP_InertialNav* inav, AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM
_vel_last(0,0,0),
_loiter_leash(WPNAV_MIN_LEASH_LENGTH),
_loiter_accel_cms(WPNAV_LOITER_ACCEL_MAX),
_lean_angle_max_cd(MAX_LEAN_ANGLE),
_wp_leash_xy(WPNAV_MIN_LEASH_LENGTH),
_wp_leash_z(WPNAV_MIN_LEASH_LENGTH),
_track_accel(0),
@ -162,8 +163,8 @@ void AC_WPNav::init_loiter_target(const Vector3f& position, const Vector3f& velo
_target_vel.y = velocity.y;
// initialise desired roll and pitch to current roll and pitch. This avoids a random twitch between now and when the loiter controller is first run
_desired_roll = constrain_int32(_ahrs->roll_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
_desired_pitch = constrain_int32(_ahrs->pitch_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
_desired_roll = constrain_int32(_ahrs->roll_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
_desired_pitch = constrain_int32(_ahrs->pitch_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
// initialise pilot input
_pilot_vel_forward_cms = 0;
@ -366,8 +367,8 @@ void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f
_flags.fast_waypoint = false;
// initialise desired roll and pitch to current roll and pitch. This avoids a random twitch between now and when the wpnav controller is first run
_desired_roll = constrain_int32(_ahrs->roll_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
_desired_pitch = constrain_int32(_ahrs->pitch_sensor,-MAX_LEAN_ANGLE,MAX_LEAN_ANGLE);
_desired_roll = constrain_int32(_ahrs->roll_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
_desired_pitch = constrain_int32(_ahrs->pitch_sensor,-_lean_angle_max_cd,_lean_angle_max_cd);
// reset target velocity - only used by loiter controller's interpretation of pilot input
_target_vel.x = 0;
@ -615,8 +616,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), -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);
_desired_roll = constrain_float(fast_atan(accel_right*_cos_pitch/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max_cd, _lean_angle_max_cd);
_desired_pitch = constrain_float(fast_atan(-accel_forward/(-z_accel_meas))*(18000/M_PI), -_lean_angle_max_cd, _lean_angle_max_cd);
}
// get_bearing_cd - return bearing in centi-degrees between two positions

4
libraries/AC_WPNav/AC_WPNav.h
View File

@ -143,6 +143,9 @@ public:
/// get_waypoint_acceleration - returns acceleration in cm/s/s during missions
float get_waypoint_acceleration() const { return _wp_accel_cms.get(); }
/// set_lean_angle_max - limits maximum lean angle
void set_lean_angle_max(int16_t angle_cd) { if (angle_cd >= 1000 && angle_cd <= 8000) {_lean_angle_max_cd = angle_cd;} }
static const struct AP_Param::GroupInfo var_info[];
protected:
@ -216,6 +219,7 @@ protected:
Vector3f _vel_last; // previous iterations velocity in cm/s
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
int16_t _lean_angle_max_cd; // maximum lean angle in centi-degrees
// waypoint controller internal variables
Vector3f _origin; // starting point of trip to next waypoint in cm from home (equivalent to next_WP)