Plane: support 12 output channels on PX4

This commit is contained in:
Andrew Tridgell 2013-04-25 20:10:40 +10:00
parent 4f225eff57
commit 0b8c2da4df
5 changed files with 30 additions and 7 deletions

View File

@ -932,7 +932,9 @@ static void slow_loop()
case 1:
slow_loopCounter++;
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
#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 CONFIG_HAL_BOARD == HAL_BOARD_APM2
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);
#else
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);

View File

@ -725,11 +725,14 @@ 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_HAL_BOARD == HAL_BOARD_APM2
# if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
g.rc_9.output_ch(CH_9);
g.rc_10.output_ch(CH_10);
g.rc_11.output_ch(CH_11);
# endif
# if CONFIG_HAL_BOARD == HAL_BOARD_PX4
g.rc_12.output_ch(CH_12);
# endif
}
static bool demoing_servos;

View File

@ -180,6 +180,7 @@ public:
k_param_throttle_slewrate,
k_param_throttle_suppress_manual,
k_param_throttle_passthru_stabilize,
k_param_rc_12,
//
// 200: Feed-forward gains
@ -376,11 +377,14 @@ public:
RC_Channel_aux rc_6;
RC_Channel_aux rc_7;
RC_Channel_aux rc_8;
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
RC_Channel_aux rc_9;
RC_Channel_aux rc_10;
RC_Channel_aux rc_11;
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
RC_Channel_aux rc_12;
#endif
// PID controllers
//
@ -410,11 +414,14 @@ public:
rc_6 (CH_6),
rc_7 (CH_7),
rc_8 (CH_8),
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
rc_9 (CH_9),
rc_10 (CH_10),
rc_11 (CH_11),
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
rc_12 (CH_12),
#endif
// PID controller initial P initial I initial D initial imax
//-----------------------------------------------------------------------------------

View File

@ -655,7 +655,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp, ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_8, "RC8_", RC_Channel_aux),
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
// @Group: RC9_
// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp, ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_9, "RC9_", RC_Channel_aux),
@ -669,6 +669,12 @@ const AP_Param::Info var_info[] PROGMEM = {
GGROUP(rc_11, "RC11_", RC_Channel_aux),
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
// @Group: RC12_
// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp, ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(rc_12, "RC12_", RC_Channel_aux),
#endif
GGROUP(pidNavPitchAirspeed, "ARSP2PTCH_", PID),
GGROUP(pidTeThrottle, "ENRGY2THR_", PID),
GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID),

View File

@ -36,7 +36,9 @@ static void init_rc_in()
rc_ch[CH_8] = &g.rc_8;
//set auxiliary ranges
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
#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 CONFIG_HAL_BOARD == HAL_BOARD_APM2
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);
#else
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
@ -62,11 +64,14 @@ static void init_rc_out()
servo_write(CH_7, g.rc_7.radio_trim);
servo_write(CH_8, g.rc_8.radio_trim);
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4
servo_write(CH_9, g.rc_9.radio_trim);
servo_write(CH_10, g.rc_10.radio_trim);
servo_write(CH_11, g.rc_11.radio_trim);
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
servo_write(CH_12, g.rc_12.radio_trim);
#endif
}
static void read_radio()