diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 5e50d236e4..a68d975530 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -897,7 +897,11 @@ static void slow_loop() // ---------------------------------- update_servo_switches(); +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); +#else + 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); +#endif break; case 2: @@ -1056,7 +1060,7 @@ static void update_current_flight_mode(void) // Substitute stick inputs for Navigation control output // We use g.pitch_limit_min because its magnitude is // normally greater than g.pitch_limit_max - + // Thanks to Yury MonZon for the altitude limit code! nav_roll = g.channel_roll.norm_input() * g.roll_limit; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 67f13be597..a5ca8da3a1 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -151,6 +151,9 @@ public: k_param_gcs_heartbeat_fs_enabled, k_param_throttle_slewrate, + k_param_rc_9, + k_param_rc_10, + k_param_rc_11, // // 200: Feed-forward gains // @@ -365,6 +368,11 @@ public: RC_Channel_aux rc_6; RC_Channel_aux rc_7; RC_Channel_aux rc_8; +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + RC_Channel_aux rc_9; + RC_Channel_aux rc_10; + RC_Channel_aux rc_11; +#endif // PID controllers // diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 305a900c12..ab768daa5d 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -393,7 +393,7 @@ static const AP_Param::Info var_info[] PROGMEM = { GSCALAR(min_gndspeed, "MIN_GNDSPD_CM"), // @Param: TRIM_PITCH_CD - // @DisplayName: Pitch angle offset + // @DisplayName: Pitch angle offset // @Description: offset to add to pitch - used for trimming tail draggers // @Units: centi-Degrees // @User: Advanced @@ -476,6 +476,20 @@ static const AP_Param::Info var_info[] PROGMEM = { // @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp GGROUP(rc_8, "RC8_", RC_Channel_aux), +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + // @Group: RC9_ + // @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_9, "RC9_", RC_Channel_aux), + + // @Group: RC10_ + // @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_10, "RC10_", RC_Channel_aux), + + // @Group: RC11_ + // @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp + GGROUP(rc_11, "RC11_", RC_Channel_aux), +#endif + GGROUP(pidNavRoll, "HDNG2RLL_", PID), GGROUP(pidServoRoll, "RLL2SRV_", PID), GGROUP(pidServoPitch, "PTCH2SRV_", PID), diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index cfd96b5d5d..d2f6580f8c 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -28,7 +28,11 @@ static void init_rc_in() //g.channel_throttle.dead_zone = 6; //set auxiliary ranges +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); +#else + 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); +#endif } static void init_rc_out()