From 300a8d2bbcf76da139c0ad09767510b7230bb8c1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 25 Apr 2013 20:01:34 +1000 Subject: [PATCH] Copter: support all 12 channels on PX4 last 4 channels are on the FMU pins --- ArduCopter/ArduCopter.pde | 4 +++- ArduCopter/Parameters.h | 15 ++++++++++++++- ArduCopter/Parameters.pde | 9 +++++++++ ArduCopter/radio.pde | 4 +++- 4 files changed, 29 insertions(+), 3 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index a5d13d4101..b941259de1 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1235,7 +1235,9 @@ static void slow_loop() case 1: slow_loopCounter++; -#if MOUNT == ENABLED +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12); +#elif MOUNT == ENABLED update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_10, &g.rc_11); #endif enable_aux_servos(); diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index f026dd066b..ed0da105c5 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -194,6 +194,8 @@ public: k_param_failsafe_battery_enabled, k_param_throttle_mid, k_param_failsafe_gps_enabled, // 195 + k_param_rc_9, + k_param_rc_12, // // 200: flight modes @@ -356,6 +358,12 @@ public: RC_Channel_aux rc_10; RC_Channel_aux rc_11; #endif + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + RC_Channel_aux rc_9; + RC_Channel_aux rc_12; +#endif + AP_Int16 rc_speed; // speed of fast RC Channels in Hz // Acro parameters @@ -402,7 +410,12 @@ public: rc_6 (CH_6), rc_7 (CH_7), rc_8 (CH_8), -#if MOUNT == ENABLED +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + rc_9 (CH_9), + rc_10 (CH_10), + rc_11 (CH_11), + rc_12 (CH_12), +#elif MOUNT == ENABLED rc_10 (CH_10), rc_11 (CH_11), #endif diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 956ce2f785..7d198b7ad7 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -455,6 +455,15 @@ const AP_Param::Info var_info[] PROGMEM = { GGROUP(rc_11, "RC11_", RC_Channel_aux), #endif +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + // @Group: RC9_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_9, "RC9_", RC_Channel_aux), + // @Group: RC12_ + // @Path: ../libraries/RC_Channel/RC_Channel.cpp,../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_12, "RC12_", RC_Channel_aux), +#endif + // @Param: RC_SPEED // @DisplayName: ESC Update Speed // @Description: This is the speed in Hertz that your ESCs will receive updates diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 9e5a1b9b3a..f6940ccc23 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -54,7 +54,9 @@ static void init_rc_in() g.rc_7.set_range(0,1000); g.rc_8.set_range(0,1000); -#if MOUNT == ENABLED +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12); +#elif MOUNT == ENABLED update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_10, &g.rc_11); #endif }