diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index c8184f1efb..ca6aed4f0e 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1256,14 +1256,30 @@ static void fifty_hz_loop() #if MOUNT == ENABLED static bool mount_initialised = false; - camera_mount.update_mount_position(); - // enable camera servos. TO-DO: move to AP_Mount library + + // do camera mount initialisation. TO-DO: move to AP_Mount library or init_ardupilot if( mount_initialised == false ) { mount_initialised = true; + + // set-up channel 6 to control pitch + camera_mount.set_manual_rc_channel( &g.rc_6 ); + + // enable output channels g.rc_camera_roll.enable_out_ch(CH_CAM_ROLL); g.rc_camera_pitch.enable_out_ch(CH_CAM_PITCH); g.rc_camera_yaw.enable_out_ch(CH_CAM_YAW); } + + // update pilot's commands to mount + if( g.radio_tuning == 0 ) { + camera_mount.set_manual_rc_channel_function( AP_MOUNT_MANUAL_RC_FUNCTION_PITCH ); + }else{ + camera_mount.set_manual_rc_channel_function( AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED ); + } + + // update camera mount's position + camera_mount.update_mount_position(); + // move camera servos. TO-DO: move this to AP_Mount library g.rc_camera_roll.output_ch(CH_CAM_ROLL); g.rc_camera_pitch.output_ch(CH_CAM_PITCH); @@ -1328,7 +1344,10 @@ static void slow_loop() // ------------------------------- read_control_switch(); + #if MOUNT == ENABLED update_aux_servo_function(&g.rc_camera_roll, &g.rc_camera_pitch, &g.rc_camera_yaw); + #endif + // agmatthews - USERHOOKS #ifdef USERHOOK_SLOWLOOP USERHOOK_SLOWLOOP