mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-01 13:38:38 -04:00
Plane: support 12 output channels on PX4
This commit is contained in:
parent
4f225eff57
commit
0b8c2da4df
@ -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);
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
//-----------------------------------------------------------------------------------
|
||||
|
@ -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),
|
||||
|
@ -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()
|
||||
|
Loading…
Reference in New Issue
Block a user