src/main/io/pwmdriver_i2c.c

   1 #include <stdbool.h>
   2 #include <stdint.h>
   3
   4 #include "drivers/io_pca9685.h"
   5
   6 #include "fc/config.h"
   7 #include "fc/runtime_config.h"
   8
   9 #include "config/feature.h"
  10
  11 #define PWM_DRIVER_IMPLEMENTATION_COUNT 1
  12 #define PWM_DRIVER_MAX_CYCLE 4
  13
  14 static bool driverEnabled = false;
  15 static uint8_t driverImplementationIndex = 0;
  16
  17 typedef struct {
  18     bool (*initFunction)(void);
  19     void (*writeFunction)(uint8_t servoIndex, uint16_t off);
  20     void (*setFrequencyFunction)(uint16_t freq);
  21     void (*syncFunction)(uint8_t cycleIndex);
  22 } pwmDriverDriver_t;
  23
  24 pwmDriverDriver_t pwmDrivers[PWM_DRIVER_IMPLEMENTATION_COUNT] = {
  25     [0] = {
  26         .initFunction = pca9685Initialize,
  27         .writeFunction = pca9685setServoPulse,
  28         .setFrequencyFunction = pca9685setPWMFreq,
  29         .syncFunction = pca9685sync
  30     }
  31 };
  32
  33 bool isPwmDriverEnabled() {
  34     return driverEnabled;
  35 }
  36
  37 void pwmDriverSetPulse(uint8_t servoIndex, uint16_t length) {
  38     (pwmDrivers[driverImplementationIndex].writeFunction)(servoIndex, length);
  39 }
  40
  41 void pwmDriverInitialize(void) {
  42     driverEnabled = (pwmDrivers[driverImplementationIndex].initFunction)();
  43
  44     if (driverEnabled) {
  45         ENABLE_STATE(PWM_DRIVER_AVAILABLE);
  46     } else {
  47         DISABLE_STATE(PWM_DRIVER_AVAILABLE);
  48     }
  49
  50 }
  51
  52 void pwmDriverSync(void) {
  53     static uint8_t cycle = 0;
  54
  55     (pwmDrivers[driverImplementationIndex].syncFunction)(cycle);
  56
  57     cycle++;
  58     if (cycle == PWM_DRIVER_MAX_CYCLE) {
  59         cycle = 0;
  60     }
  61 }