src/main/drivers/io_pca9685.c

   1 #include <stdbool.h>
   2 #include <stdint.h>
   3
   4 #include "platform.h"
   5
   6 #include "build/build_config.h"
   7
   8 #ifdef USE_PWM_DRIVER_PCA9685
   9
  10 #include "common/time.h"
  11 #include "common/maths.h"
  12
  13 #include "drivers/io.h"
  14 #include "drivers/time.h"
  15 #include "drivers/bus.h"
  16 #include "drivers/bus_i2c.h"
  17
  18 #define PCA9685_MODE1 0x00
  19 #define PCA9685_PRESCALE 0xFE
  20
  21 #define LED0_ON_L 0x06
  22 #define LED0_ON_H 0x07
  23 #define LED0_OFF_L 0x08
  24 #define LED0_OFF_H 0x09
  25
  26 #define PCA9685_SERVO_FREQUENCY 60
  27 #define PCA9685_SERVO_COUNT 16
  28 #define PCA9685_SYNC_THRESHOLD 5
  29
  30 static busDevice_t * busDev;
  31 static uint16_t currentOutputState[PCA9685_SERVO_COUNT] = {0};
  32 static uint16_t temporaryOutputState[PCA9685_SERVO_COUNT] = {0};
  33
  34 void pca9685setPWMOn(uint8_t servoIndex, uint16_t on) {
  35     if (servoIndex < PCA9685_SERVO_COUNT) {
  36         busWrite(busDev, LED0_ON_L + (servoIndex * 4), on);
  37         busWrite(busDev, LED0_ON_H + (servoIndex * 4), on>>8);
  38     }
  39 }
  40
  41 void pca9685setPWMOff(uint8_t servoIndex, uint16_t off) {
  42     if (servoIndex < PCA9685_SERVO_COUNT) {
  43         busWrite(busDev, LED0_OFF_L + (servoIndex * 4), off);
  44         busWrite(busDev, LED0_OFF_H + (servoIndex * 4), off>>8);
  45     }
  46 }
  47
  48 /*
  49 Writing new state every cycle for each servo is extremely time consuming
  50 and does not makes sense.
  51 Trying to sync 5 servos every 2000us extends looptime
  52 to 3500us. Very, very bad...
  53 Instead of that, write desired values to temporary
  54 table and write it to PCA9685 only when there a need.
  55 Also, because of resultion of PCA9685 internal counter of about 5us, do not write
  56 small changes, since thwy will only clog the bandwidch and will not
  57 be represented on output
  58 PWM Driver runs at 200Hz, every cycle every 4th servo output is updated:
  59 cycle 0: servo 0, 4, 8, 12
  60 cycle 1: servo 1, 5, 9, 13
  61 cycle 2: servo 2, 6, 10, 14
  62 cycle 3: servo3, 7, 11, 15
  63 */
  64 void pca9685sync(uint8_t cycleIndex) {
  65     uint8_t i;
  66
  67     for (i = 0; i < PCA9685_SERVO_COUNT; i++) {
  68         if (cycleIndex == i % 4 && ABS(temporaryOutputState[i] - currentOutputState[i]) > PCA9685_SYNC_THRESHOLD) {
  69             pca9685setPWMOff(i, temporaryOutputState[i]);
  70             currentOutputState[i] = temporaryOutputState[i];
  71         }
  72     }
  73 }
  74
  75 void pca9685setServoPulse(uint8_t servoIndex, uint16_t pulse) {
  76
  77     static double pulselength = 0;
  78
  79     if (pulselength == 0) {
  80         pulselength = 1000000;   // 1,000,000 us per second
  81         pulselength /= PCA9685_SERVO_FREQUENCY;
  82         pulselength /= 4096;  // 12 bits of resolution
  83     }
  84     pulse /= pulselength;
  85
  86     temporaryOutputState[servoIndex] = pulse;
  87 }
  88
  89 void pca9685setPWMFreq(uint16_t freq) {
  90
  91   uint32_t prescaleval = 25000000;
  92   prescaleval /= 4096;
  93   prescaleval /= freq;
  94   prescaleval -= 1;
  95
  96   busWrite(busDev, PCA9685_MODE1, 16);
  97   delay(1);
  98   busWrite(busDev, PCA9685_PRESCALE, (uint8_t) prescaleval);
  99   delay(1);
 100   busWrite(busDev, PCA9685_MODE1, 128);
 101 }
 102
 103 static bool deviceDetect(busDevice_t * busDev)
 104 {
 105     for (int retry = 0; retry < 5; retry++) {
 106         uint8_t sig;
 107
 108         delay(150);
 109
 110         bool ack = busRead(busDev, PCA9685_MODE1, &sig);
 111         if (ack) {
 112             return true;
 113         }
 114     };
 115
 116     return false;
 117 }
 118
 119 bool pca9685Initialize(void)
 120 {
 121     busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_PCA9685, 0, 0);
 122     if (busDev == NULL) {
 123         return false;
 124     }
 125
 126     if (!deviceDetect(busDev)) {
 127         busDeviceDeInit(busDev);
 128         return false;
 129     }
 130
 131     /* Reset device */
 132     busWrite(busDev, PCA9685_MODE1, 0x00);
 133
 134     /* Set refresh rate */
 135     pca9685setPWMFreq(PCA9685_SERVO_FREQUENCY);
 136
 137     delay(1);
 138
 139     for (uint8_t i = 0; i < PCA9685_SERVO_COUNT; i++) {
 140         pca9685setPWMOn(i, 0);
 141         pca9685setPWMOff(i, 1500);
 142     }
 143
 144     return true;
 145 }
 146
 147 #endif