mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
Added float for CH6_Y6_SCALING value.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1903 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
0063c8e60d
commit
f652c5ae91
@ -269,6 +269,8 @@ float sin_nav_y, cos_nav_x; // used in calc_waypoint_nav
|
|||||||
bool simple_bearing_is_set = false;
|
bool simple_bearing_is_set = false;
|
||||||
long initial_simple_bearing; // used for Simple mode
|
long initial_simple_bearing; // used for Simple mode
|
||||||
|
|
||||||
|
float Y6_scaling = Y6_MOTOR_SCALER;
|
||||||
|
|
||||||
// Airspeed
|
// Airspeed
|
||||||
// --------
|
// --------
|
||||||
int airspeed; // m/s * 100
|
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
|
g.pid_sonar_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1
|
||||||
|
|
||||||
#elif CHANNEL_6_TUNING == CH6_Y6_SCALING
|
#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
|
#endif
|
||||||
}
|
}
|
@ -164,15 +164,15 @@ set_servos_4()
|
|||||||
int pitch_out = g.rc_2.pwm_out / 2;
|
int pitch_out = g.rc_2.pwm_out / 2;
|
||||||
|
|
||||||
//left
|
//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
|
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
|
||||||
|
|
||||||
//right
|
//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
|
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW
|
||||||
|
|
||||||
//back
|
//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
|
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW
|
||||||
|
|
||||||
//yaw
|
//yaw
|
||||||
|
Loading…
Reference in New Issue
Block a user