diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 37868eca2a..7da24f55f8 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -269,6 +269,8 @@ float sin_nav_y, cos_nav_x; // used in calc_waypoint_nav bool simple_bearing_is_set = false; long initial_simple_bearing; // used for Simple mode +float Y6_scaling = Y6_MOTOR_SCALER; + // Airspeed // -------- int airspeed; // m/s * 100 @@ -1198,6 +1200,6 @@ void tuning(){ g.pid_sonar_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1 #elif CHANNEL_6_TUNING == CH6_Y6_SCALING - g.Y6_scaling.set((float)g.rc_6.control_in / 1000.0) + Y6_scaling = (float)g.rc_6.control_in / 1000.0; #endif } \ No newline at end of file diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index a903df325a..742a46852b 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -164,15 +164,15 @@ set_servos_4() int pitch_out = g.rc_2.pwm_out / 2; //left - motor_out[CH_2] = ((g.rc_3.radio_out * Y6_MOTOR_SCALER) + roll_out + pitch_out); // CCW TOP + motor_out[CH_2] = ((g.rc_3.radio_out * Y6_scaling) + roll_out + pitch_out); // CCW TOP motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW //right - motor_out[CH_7] = ((g.rc_3.radio_out * Y6_MOTOR_SCALER) - roll_out + pitch_out); // CCW TOP + motor_out[CH_7] = ((g.rc_3.radio_out * Y6_scaling) - roll_out + pitch_out); // CCW TOP motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW //back - motor_out[CH_8] = ((g.rc_3.radio_out * Y6_MOTOR_SCALER) - g.rc_2.pwm_out); // CCW TOP + motor_out[CH_8] = ((g.rc_3.radio_out * Y6_scaling) - g.rc_2.pwm_out); // CCW TOP motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW //yaw