STM32F4-Discovery Servo controller class for ChibiOS using PCA9685 class