diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 1093394909..ea4ba1047d 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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)); diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 9343fbb63d..3a3109f9f3 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -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); diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 975862995d..35113e4303 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -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 // diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 8fe53b43cb..8a5974ca83 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -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 diff --git a/ArduCopter/config.h b/ArduCopter/config.h index a58b3ad58e..e6853a177c 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -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 diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 8260dffa4d..f44c600b2b 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -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; } diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 5c1296aafa..f587dc7730 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -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); diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 819c46c5e0..af01b01ea3 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -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 diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 9942611a86..80de279472 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -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)