Copter: use new RC_Channels API

This commit is contained in:
Andrew Tridgell 2014-02-06 10:12:31 +11:00
parent 04f2c07b2b
commit 5a3a7f1cec
2 changed files with 1 additions and 54 deletions

View File

@ -1193,8 +1193,7 @@ static void one_hz_loop()
}
// update assigned functions and enable auxiliar servos
aux_servos_update_fn();
enable_aux_servos();
RC_Channel_aux::enable_aux_servos();
#if MOUNT == ENABLED
camera_mount.update_mount_type();

View File

@ -47,9 +47,6 @@ static void init_rc_in()
g.rc_7.set_range(0,1000);
g.rc_8.set_range(0,1000);
// update assigned functions for auxiliary servos
aux_servos_update_fn();
// set default dead zones
default_dead_zones();
}
@ -186,55 +183,6 @@ static void set_throttle_and_failsafe(uint16_t throttle_pwm)
}
}
// aux_servos_update - update auxiliary servos assigned functions in case the user has changed them
void aux_servos_update_fn()
{
// Quads can use RC5 and higher as auxiliary channels
#if (FRAME_CONFIG == QUAD_FRAME)
#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);
#else // APM1, APM2, SITL
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8, &g.rc_10, &g.rc_11);
#endif
// Tri's and Singles can use RC5, RC6, RC8 and higher
#elif (FRAME_CONFIG == TRI_FRAME || FRAME_CONFIG == SINGLE_FRAME)
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
#else // APM1, APM2, SITL
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_8, &g.rc_10, &g.rc_11);
#endif
// Hexa and Y6 can use RC7 and higher
#elif (FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME)
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
update_aux_servo_function(&g.rc_7, &g.rc_8, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
#else
update_aux_servo_function(&g.rc_7, &g.rc_8, &g.rc_10, &g.rc_11);
#endif
// Octa and X8 can use RC9 and higher
#elif (FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME)
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
update_aux_servo_function(&g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
#else
update_aux_servo_function(&g.rc_10, &g.rc_11);
#endif
// Heli's can use RC5, RC6, RC7, not RC8, and higher
#elif (FRAME_CONFIG == HELI_FRAME)
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_9, &g.rc_10, &g.rc_11, &g.rc_12);
#else // APM1, APM2, SITL
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_10, &g.rc_11);
#endif
// throw compile error if frame type is unrecognise
#else
#error Unrecognised frame type
#endif
}
static void trim_radio()
{
for (uint8_t i = 0; i < 30; i++) {