diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index f35f70572b..e3d7a16bb4 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -18,7 +18,7 @@ void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float angle_max = constrain_float(angle_max, 1000, aparm.angle_max); // scale roll_in, pitch_in to ANGLE_MAX parameter range - float scaler = aparm.angle_max/(float)ROLL_PITCH_INPUT_MAX; + float scaler = aparm.angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX; roll_in *= scaler; pitch_in *= scaler; @@ -58,10 +58,10 @@ float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle) } // yaw expo - y_in = float(stick_angle)/ROLL_PITCH_INPUT_MAX; + y_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX; y_in3 = y_in*y_in*y_in; y_out = (g2.acro_y_expo * y_in3) + ((1.0f - g2.acro_y_expo) * y_in); - yaw_request = ROLL_PITCH_INPUT_MAX * y_out * g.acro_yaw_p; + yaw_request = ROLL_PITCH_YAW_INPUT_MAX * y_out * g.acro_yaw_p; } // convert pilot input to the desired yaw rate return yaw_request; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 4fa2e74711..196136aba8 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -508,8 +508,8 @@ ////////////////////////////////////////////////////////////////////////////// // Stabilize Rate Control // -#ifndef ROLL_PITCH_INPUT_MAX - # define ROLL_PITCH_INPUT_MAX 4500 // roll, pitch input range +#ifndef ROLL_PITCH_YAW_INPUT_MAX + # define ROLL_PITCH_YAW_INPUT_MAX 4500 // roll, pitch and yaw input range #endif #ifndef DEFAULT_ANGLE_MAX # define DEFAULT_ANGLE_MAX 4500 // ANGLE_MAX parameters default value diff --git a/ArduCopter/control_acro.cpp b/ArduCopter/control_acro.cpp index 6e783c1624..65199372c2 100644 --- a/ArduCopter/control_acro.cpp +++ b/ArduCopter/control_acro.cpp @@ -62,8 +62,8 @@ void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, in // apply circular limit to pitch and roll inputs float total_in = norm(pitch_in, roll_in); - if (total_in > ROLL_PITCH_INPUT_MAX) { - float ratio = (float)ROLL_PITCH_INPUT_MAX / total_in; + if (total_in > ROLL_PITCH_YAW_INPUT_MAX) { + float ratio = (float)ROLL_PITCH_YAW_INPUT_MAX / total_in; roll_in *= ratio; pitch_in *= ratio; } @@ -82,16 +82,16 @@ void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, in } // roll expo - rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX; + rp_in = float(roll_in)/ROLL_PITCH_YAW_INPUT_MAX; rp_in3 = rp_in*rp_in*rp_in; rp_out = (g.acro_rp_expo * rp_in3) + ((1.0f - g.acro_rp_expo) * rp_in); - rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; + rate_bf_request.x = ROLL_PITCH_YAW_INPUT_MAX * rp_out * g.acro_rp_p; // pitch expo - rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX; + rp_in = float(pitch_in)/ROLL_PITCH_YAW_INPUT_MAX; rp_in3 = rp_in*rp_in*rp_in; rp_out = (g.acro_rp_expo * rp_in3) + ((1.0f - g.acro_rp_expo) * rp_in); - rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p; + rate_bf_request.y = ROLL_PITCH_YAW_INPUT_MAX * rp_out * g.acro_rp_p; } // calculate yaw rate request diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 3370551c2b..c3b661965f 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -27,9 +27,9 @@ void Copter::init_rc_in() channel_yaw = RC_Channel::rc_channel(rcmap.yaw()-1); // set rc channel ranges - channel_roll->set_angle(ROLL_PITCH_INPUT_MAX); - channel_pitch->set_angle(ROLL_PITCH_INPUT_MAX); - channel_yaw->set_angle(4500); + channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX); + channel_pitch->set_angle(ROLL_PITCH_YAW_INPUT_MAX); + channel_yaw->set_angle(ROLL_PITCH_YAW_INPUT_MAX); channel_throttle->set_range(0, 1000); channel_roll->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);