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 #include "platform.h"
  11
  12 #ifdef USE_PWM_SERVO_DRIVER
  13
  14 #define PWM_DRIVER_IMPLEMENTATION_COUNT 1
  15 #define PWM_DRIVER_MAX_CYCLE 4
  16
  17 static bool driverEnabled = false;
  18 static uint8_t driverImplementationIndex = 0;
  19
  20 typedef struct {
  21     bool (*initFunction)(void);
  22     void (*writeFunction)(uint8_t servoIndex, uint16_t off);
  23     void (*setFrequencyFunction)(uint16_t freq);
  24     void (*syncFunction)(uint8_t cycleIndex);
  25 } pwmDriverDriver_t;
  26
  27 pwmDriverDriver_t pwmDrivers[PWM_DRIVER_IMPLEMENTATION_COUNT] = {
  28     [0] = {
  29         .initFunction = pca9685Initialize,
  30         .writeFunction = pca9685setServoPulse,
  31         .setFrequencyFunction = pca9685setPWMFreq,
  32         .syncFunction = pca9685sync
  33     }
  34 };
  35
  36 bool isPwmDriverEnabled(void) {
  37     return driverEnabled;
  38 }
  39
  40 void pwmDriverSetPulse(uint8_t servoIndex, uint16_t length) {
  41     (pwmDrivers[driverImplementationIndex].writeFunction)(servoIndex, length);
  42 }
  43
  44 void pwmDriverInitialize(void) {
  45     driverEnabled = (pwmDrivers[driverImplementationIndex].initFunction)();
  46
  47     if (driverEnabled) {
  48         ENABLE_STATE(PWM_DRIVER_AVAILABLE);
  49     } else {
  50         DISABLE_STATE(PWM_DRIVER_AVAILABLE);
  51     }
  52
  53 }
  54
  55 void pwmDriverSync(void) {
  56     if (!STATE(PWM_DRIVER_AVAILABLE)) {
  57         return;
  58     }
  59
  60     static uint8_t cycle = 0;
  61
  62     (pwmDrivers[driverImplementationIndex].syncFunction)(cycle);
  63
  64     cycle++;
  65     if (cycle == PWM_DRIVER_MAX_CYCLE) {
  66         cycle = 0;
  67     }
  68 }
  69
  70 #endif