diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 165f4d5968..cd2ceb2026 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -5,7 +5,7 @@ static int8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling -extern RC_Channel* rc_ch[8]; +extern RC_Channel* rc_ch[NUM_CHANNELS]; static void default_dead_zones() { @@ -65,9 +65,12 @@ static void init_rc_out() APM_RC.Init( &isr_registry ); // APM Radio initialization #if CONFIG_APM_HARDWARE != APM_HARDWARE_APM1 - APM_RC.enable_out(CH_9); - APM_RC.enable_out(CH_10); - APM_RC.enable_out(CH_11); + APM_RC.enable_out(CH_9); + APM_RC.enable_out(CH_10); + APM_RC.enable_out(CH_11); + APM_RC.OutputCh(CH_9, g.rc_9.radio_trim); + APM_RC.OutputCh(CH_10, g.rc_10.radio_trim); + APM_RC.OutputCh(CH_11, g.rc_11.radio_trim); #endif #if INSTANT_PWM == 1 motors.set_update_rate(AP_MOTORS_SPEED_INSTANT_PWM); diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index df7b64b4bd..c26fa3eb3e 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -421,6 +421,11 @@ static void set_servos(void) g.rc_6.output_ch(CH_6); g.rc_7.output_ch(CH_7); g.rc_8.output_ch(CH_8); +# if CONFIG_APM_HARDWARE != APM_HARDWARE_APM1 + g.rc_9.output_ch(CH_9); + g.rc_10.output_ch(CH_10); + g.rc_11.output_ch(CH_11); +# endif #endif } diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 3066de5682..8127519230 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -5,7 +5,7 @@ static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling -extern RC_Channel* rc_ch[8]; +extern RC_Channel* rc_ch[NUM_CHANNELS]; static void init_rc_in() { diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 83b4372297..aed36883d4 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -24,7 +24,7 @@ /// global array with pointers to all APM RC channels, will be used by AP_Mount and AP_Camera classes /// It points to RC input channels, both APM1 and APM2 only have 8 input channels. -RC_Channel* rc_ch[8]; +RC_Channel* rc_ch[NUM_CHANNELS]; APM_RC_Class *RC_Channel::_apm_rc;