ArduCopter - Camera.pde - turn off camera pitch control (which uses channel 6) when channel 6 is being used for tuning.

This commit is contained in:
Randy Mackay 2012-02-08 23:51:53 +09:00
parent 70f91e3cc3
commit 3517964e14
1 changed files with 8 additions and 9 deletions

View File

@ -20,20 +20,19 @@ camera_stabilization()
{
// PITCH
// -----
// allow control mixing
g.rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here.
g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(dcm.pitch_sensor);
// Allow user to control camera pitch with channel 6 (mixed with pitch DCM)
if(g.radio_tuning == 0) {
g.rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6));
g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(dcm.pitch_sensor);
}else{
// unless channel 6 is already being used for tuning
g.rc_camera_pitch.servo_out = dcm.pitch_sensor * -1;
}
g.rc_camera_pitch.servo_out = (float)g.rc_camera_pitch.servo_out * g.camera_pitch_gain;
// limit
//g.rc_camera_pitch.servo_out = constrain(g.rc_camera_pitch.servo_out, -4500, 4500);
// dont allow control mixing
/*
g.rc_camera_pitch.servo_out = dcm.pitch_sensor * -1;
*/
// ROLL
// -----