Support up to 11 servos in APM2

Conflicts:

	ArduPlane/ArduPlane.pde
This commit is contained in:
Amilcar Lucas 2012-07-18 22:01:19 +02:00
parent dea5b88379
commit d911e4f61e
4 changed files with 32 additions and 2 deletions

View File

@ -897,7 +897,11 @@ static void slow_loop()
// ---------------------------------- // ----------------------------------
update_servo_switches(); 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); 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; break;
case 2: case 2:

View File

@ -151,6 +151,9 @@ public:
k_param_gcs_heartbeat_fs_enabled, k_param_gcs_heartbeat_fs_enabled,
k_param_throttle_slewrate, k_param_throttle_slewrate,
k_param_rc_9,
k_param_rc_10,
k_param_rc_11,
// //
// 200: Feed-forward gains // 200: Feed-forward gains
// //
@ -365,6 +368,11 @@ public:
RC_Channel_aux rc_6; RC_Channel_aux rc_6;
RC_Channel_aux rc_7; RC_Channel_aux rc_7;
RC_Channel_aux rc_8; 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 // PID controllers
// //

View File

@ -476,6 +476,20 @@ static const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp // @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_8, "RC8_", RC_Channel_aux), 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(pidNavRoll, "HDNG2RLL_", PID),
GGROUP(pidServoRoll, "RLL2SRV_", PID), GGROUP(pidServoRoll, "RLL2SRV_", PID),
GGROUP(pidServoPitch, "PTCH2SRV_", PID), GGROUP(pidServoPitch, "PTCH2SRV_", PID),

View File

@ -28,7 +28,11 @@ static void init_rc_in()
//g.channel_throttle.dead_zone = 6; //g.channel_throttle.dead_zone = 6;
//set auxiliary ranges //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); 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() static void init_rc_out()