From d843f32a21e94fedc5d7764d315114bff4df1b60 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Tue, 10 May 2011 02:17:25 +0000 Subject: [PATCH] Camera stabilization updates git-svn-id: https://arducopter.googlecode.com/svn/trunk@2239 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/Camera.pde | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/ArduCopterMega/Camera.pde b/ArduCopterMega/Camera.pde index c015b2660b..0452e8e5bc 100644 --- a/ArduCopterMega/Camera.pde +++ b/ArduCopterMega/Camera.pde @@ -3,9 +3,10 @@ void init_camera() { g.rc_camera_pitch.set_angle(4500); - g.rc_camera_pitch.radio_min = g.rc_6.radio_min; - g.rc_camera_pitch.radio_trim = g.rc_6.radio_trim; - g.rc_camera_pitch.radio_max = g.rc_6.radio_max; + g.rc_camera_pitch.radio_min = 1000; + g.rc_camera_pitch.radio_trim = 1500; + g.rc_camera_pitch.radio_max = 2000; + g.rc_camera_roll.set_angle(4500); g.rc_camera_roll.radio_min = 1000; @@ -19,14 +20,14 @@ camera_stabilization() g.rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here. // allow control mixing - //g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(dcm.pitch_sensor / 2); + g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(-dcm.pitch_sensor); // dont allow control mixing - g.rc_camera_pitch.servo_out = dcm.pitch_sensor / 2; + //g.rc_camera_pitch.servo_out = dcm.pitch_sensor * -1; g.rc_camera_pitch.calc_pwm(); APM_RC.OutputCh(CH_5, g.rc_camera_pitch.radio_out); - g.rc_camera_roll.servo_out = dcm.roll_sensor; + g.rc_camera_roll.servo_out = -dcm.roll_sensor; g.rc_camera_roll.calc_pwm(); APM_RC.OutputCh(CH_6, g.rc_camera_roll.radio_out);