diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index ed8eabdaea..f1d14e547a 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -699,7 +699,7 @@ static void medium_loop() altitude_error = get_altitude_error(); - camera_stabilization(); + //camera_stabilization(); // invalidate the throttle hold value // ---------------------------------- @@ -805,6 +805,9 @@ static void fifty_hz_loop() //else if (throttle_slew > 0) // throttle_slew--; + camera_stabilization(); + + //throttle_slew = min((800 - g.rc_3.control_in), throttle_slew); # if HIL_MODE == HIL_MODE_DISABLED diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index d122d2a8aa..effcf76bc2 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -213,7 +213,7 @@ static int alt_hold_velocity() static float angle_boost() { float temp = cos_pitch_x * cos_roll_x; - temp = 2.0 - constrain(temp, .6, 1.0); + temp = 2.0 - constrain(temp, .5, 1.0); return temp; } diff --git a/ArduCopterMega/Camera.pde b/ArduCopterMega/Camera.pde index fa49768cba..819e11c6f6 100644 --- a/ArduCopterMega/Camera.pde +++ b/ArduCopterMega/Camera.pde @@ -5,10 +5,13 @@ static void init_camera() { g.rc_camera_pitch.set_angle(4500); - g.rc_camera_pitch.radio_min = 1000; + g.rc_camera_pitch.radio_min = 1200; g.rc_camera_pitch.radio_trim = 1500; - g.rc_camera_pitch.radio_max = 2000; - //g.rc_camera_pitch.set_reverse(1); + g.rc_camera_pitch.radio_max = 1900; + g.rc_camera_pitch.set_reverse(1); + + // ch 6 high right is down. + g.rc_camera_roll.set_angle(4500); g.rc_camera_roll.radio_min = 1000; @@ -27,6 +30,8 @@ camera_stabilization() // 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); + // limit + g.rc_camera_pitch.servo_out = constrain(g.rc_camera_pitch.servo_out, -4500, 4500); // dont allow control mixing /* @@ -43,7 +48,8 @@ camera_stabilization() */ // dont allow control mixing - g.rc_camera_roll.servo_out = -dcm.roll_sensor; + // limit + g.rc_camera_roll.servo_out = constrain(-dcm.roll_sensor, -4500, 4500);