PCA9685 PWM driver support (#261)

* 1st stage of PCA9685 PWM driver support
* Servo output to PCA9685 in NAZE target.
* FEATURE_PWM_SERVO_DRIVER introduced
* task for PWM driver
* abstraction layer for pwm drivers
* feature enabled only on target with >128 flash

Makefile

@@ -398,6 +398,7 @@ COMMON_SRC = \
             drivers/sound_beeper.c \
             drivers/system.c \
             drivers/timer.c \
+			drivers/io_pca9685.c \
             flight/failsafe.c \
             flight/imu.c \
             flight/hil.c \
@@ -414,6 +415,7 @@ COMMON_SRC = \
             io/serial_msp.c \
             io/statusindicator.c \
             fc/msp_server_fc.c \
+			io/pwmdriver_i2c.c \
             rx/ibus.c \
             rx/jetiexbus.c \
             rx/msp.c \
@@ -567,7 +569,6 @@ TARGET_SRC += $(VCP_SRC)
 endif
 # end target specific make file checks
 
-
 # Search path and source files for the ST stdperiph library
 VPATH        := $(VPATH):$(STDPERIPH_DIR)/src
 

src/main/config/config.c

@@ -883,6 +883,10 @@ void validateAndFixConfig(void)
 #endif // CC3D_PPM1
 #endif // CC3D
 
+#ifndef USE_PMW_SERVO_DRIVER
+    featureClear(FEATURE_PWM_SERVO_DRIVER);
+#endif
+
 #if defined(COLIBRI_RACE)
     masterConfig.serialConfig.portConfigs[0].functionMask = FUNCTION_MSP;
     if(featureConfigured(FEATURE_RX_SERIAL)) {

src/main/config/config.h

@@ -54,6 +54,7 @@ typedef enum {
     FEATURE_VTX = 1 << 24,
     FEATURE_RX_SPI = 1 << 25,
     FEATURE_SOFTSPI = 1 << 26,
+    FEATURE_PWM_SERVO_DRIVER = 1 << 27,
 } features_e;
 
 typedef enum {

src/main/drivers/io_pca9685.c

@@ -0,0 +1,129 @@
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <platform.h>
+
+#include "gpio.h"
+#include "system.h"
+#include "bus_i2c.h"
+
+#include "common/maths.h"
+
+#include "config/config.h"
+#include "fc/runtime_config.h"
+
+#define PCA9685_ADDR 0x40
+#define PCA9685_MODE1 0x00
+#define PCA9685_PRESCALE 0xFE
+
+#define LED0_ON_L 0x06
+#define LED0_ON_H 0x07
+#define LED0_OFF_L 0x08
+#define LED0_OFF_H 0x09
+
+#define PCA9685_SERVO_FREQUENCY 60
+#define PCA9685_SERVO_COUNT 16
+#define PCA9685_SYNC_THRESHOLD 5
+
+uint16_t currentOutputState[PCA9685_SERVO_FREQUENCY] = {0};
+uint16_t temporaryOutputState[PCA9685_SERVO_FREQUENCY] = {0};
+
+void pca9685setPWMOn(uint8_t servoIndex, uint16_t on) {
+    if (servoIndex < PCA9685_SERVO_COUNT) {
+        i2cWrite(I2C_DEVICE, PCA9685_ADDR, LED0_ON_L + (servoIndex * 4), on);
+        i2cWrite(I2C_DEVICE, PCA9685_ADDR, LED0_ON_H + (servoIndex * 4), on>>8);
+    }
+}
+
+void pca9685setPWMOff(uint8_t servoIndex, uint16_t off) {
+    if (servoIndex < PCA9685_SERVO_COUNT) {
+        i2cWrite(I2C_DEVICE, PCA9685_ADDR, LED0_OFF_L + (servoIndex * 4), off);
+        i2cWrite(I2C_DEVICE, PCA9685_ADDR, LED0_OFF_H + (servoIndex * 4), off>>8);
+    }
+}
+
+/*
+Writing new state every cycle for each servo is extremely time consuming
+and does not makes sense.
+On Flip32/Naze32 trying to sync 5 servos every 2000us extends looptime
+to 3500us. Very, very bad...
+Instead of that, write desired values to temporary
+table and write it to PCA9685 only when there a need.
+Also, because of resultion of PCA9685 internal counter of about 5us, do not write
+small changes, since thwy will only clog the bandwidch and will not
+be represented on output
+PWM Driver runs at 200Hz, every cycle every 4th servo output is updated:
+cycle 0: servo 0, 4, 8, 12
+cycle 1: servo 1, 5, 9, 13
+cycle 2: servo 2, 6, 10, 14
+cycle 3: servo3, 7, 11, 15
+*/
+void pca9685sync(uint8_t cycleIndex) {
+    uint8_t i;
+
+    for (i = 0; i < PCA9685_SERVO_COUNT; i++) {
+        if (cycleIndex == i % 4 && ABS(temporaryOutputState[i] - currentOutputState[i]) > PCA9685_SYNC_THRESHOLD) {
+            pca9685setPWMOff(i, temporaryOutputState[i]);
+            currentOutputState[i] = temporaryOutputState[i];
+        }
+    }
+}
+
+void pca9685setServoPulse(uint8_t servoIndex, uint16_t pulse) {
+
+    static double pulselength = 0;
+
+    if (pulselength == 0) {
+        pulselength = 1000000;   // 1,000,000 us per second
+        pulselength /= PCA9685_SERVO_FREQUENCY;
+        pulselength /= 4096;  // 12 bits of resolution
+    }
+    pulse /= pulselength;
+
+    temporaryOutputState[servoIndex] = pulse;
+}
+
+void pca9685setPWMFreq(uint16_t freq) {
+
+  uint32_t prescaleval = 25000000;
+  prescaleval /= 4096;
+  prescaleval /= freq;
+  prescaleval -= 1;
+
+  i2cWrite(I2C_DEVICE, PCA9685_ADDR, PCA9685_MODE1, 16);
+  delay(1);
+  i2cWrite(I2C_DEVICE, PCA9685_ADDR, PCA9685_PRESCALE, (uint8_t) prescaleval);
+  delay(1);
+  i2cWrite(I2C_DEVICE, PCA9685_ADDR, PCA9685_MODE1, 128);
+}
+
+bool pca9685Initialize(void) {
+
+    bool ack = false;
+    uint8_t sig;
+
+    ack = i2cRead(I2C_DEVICE, PCA9685_ADDR, PCA9685_MODE1, 1, &sig);
+
+    if (!ack) {
+        return false;
+    } else {
+        /*
+        Reset device
+        */
+        i2cWrite(I2C_DEVICE, PCA9685_ADDR, PCA9685_MODE1, 0x0);
+
+        /*
+        Set refresh rate
+        */
+        pca9685setPWMFreq(PCA9685_SERVO_FREQUENCY);
+
+        delay(1);
+
+        for (uint8_t i = 0; i < PCA9685_SERVO_COUNT; i++) {
+            pca9685setPWMOn(i, 0);
+            pca9685setPWMOff(i, 1500);
+        }
+
+        return true;
+    }
+}

src/main/drivers/io_pca9685.h

@@ -0,0 +1,6 @@
+bool pca9685Initialize(void);
+void pca9685setPWMOn(uint8_t servoIndex, uint16_t on);
+void pca9685setPWMOff(uint8_t servoIndex, uint16_t off);
+void pca9685setPWMFreq(float freq);
+void pca9685setServoPulse(uint8_t servoIndex, uint16_t pulse);
+void pca9685sync(uint8_t cycleIndex);

src/main/drivers/pwm_output.c

@@ -28,6 +28,10 @@
 #include "timer.h"
 #include "pwm_mapping.h"
 #include "pwm_output.h"
+#include "drivers/io_pca9685.h"
+#include "io/pwmdriver_i2c.h"
+#include "config/config.h"
+#include "fc/runtime_config.h"
 
 typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value);  // function pointer used to write motors
 
@@ -129,7 +133,7 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8
     p->tim = timerHardware->tim;
 
     *p->ccr = 0;
-   
+
     return p;
 }
 
@@ -219,8 +223,18 @@ void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, ui
 
 void pwmWriteServo(uint8_t index, uint16_t value)
 {
+#ifdef USE_PMW_SERVO_DRIVER
+
+    if (feature(FEATURE_PWM_SERVO_DRIVER)) {
+        pwmDriverSetPulse(index, value);
+    } else if (servos[index] && index < MAX_SERVOS) {
+        *servos[index]->ccr = value;
+    }
+
+#else
     if (servos[index] && index < MAX_SERVOS) {
         *servos[index]->ccr = value;
     }
+#endif
 }
 #endif

src/main/fc/mw.c

@@ -88,6 +88,9 @@
 #include "config/config_master.h"
 #include "config/feature.h"
 
+#include "io/pwmdriver_i2c.h"
+#include "drivers/io_pca9685.h"
+
 // June 2013     V2.2-dev
 
 enum {
@@ -632,6 +635,7 @@ void taskMainPidLoop(void)
         handleBlackbox();
     }
 #endif
+
 }
 
 // Function for loop trigger
@@ -774,3 +778,12 @@ void taskLedStrip(void)
     }
 }
 #endif
+
+#ifdef USE_PMW_SERVO_DRIVER
+void taskSyncPwmDriver(void) {
+
+    if (feature(FEATURE_PWM_SERVO_DRIVER)) {
+        pwmDriverSync();
+    }
+}
+#endif

src/main/io/pwmdriver_i2c.c

@@ -0,0 +1,57 @@
+#include <stdbool.h>
+#include <stdint.h>
+#include "drivers/io_pca9685.h"
+
+#include "config/config.h"
+#include "fc/runtime_config.h"
+#include "config/feature.h"
+
+#define PWM_DRIVER_IMPLEMENTATION_COUNT 1
+#define PWM_DRIVER_MAX_CYCLE 4
+
+static bool driverEnabled = false;
+static uint8_t driverImplementationIndex = 0;
+
+typedef struct {
+    bool (*initFunction)(void);
+    void (*writeFunction)(uint8_t servoIndex, uint16_t off);
+    void (*setFrequencyFunction)(float freq);
+    void (*syncFunction)(uint8_t cycleIndex);
+} pwmDriverDriver_t;
+
+pwmDriverDriver_t pwmDrivers[PWM_DRIVER_IMPLEMENTATION_COUNT] = {
+    [0] = {
+        .initFunction = pca9685Initialize,
+        .writeFunction = pca9685setServoPulse,
+        .setFrequencyFunction = pca9685setPWMFreq,
+        .syncFunction = pca9685sync
+    }
+};
+
+bool isPwmDriverEnabled() {
+    return driverEnabled;
+}
+
+void pwmDriverSetPulse(uint8_t servoIndex, uint16_t length) {
+    (pwmDrivers[driverImplementationIndex].writeFunction)(servoIndex, length);
+}
+
+void pwmDriverInitialize(void) {
+    driverEnabled = (pwmDrivers[driverImplementationIndex].initFunction)();
+
+    if (!driverEnabled) {
+        featureClear(FEATURE_PWM_SERVO_DRIVER);
+    }
+
+}
+
+void pwmDriverSync(void) {
+    static uint8_t cycle = 0;
+
+    (pwmDrivers[driverImplementationIndex].syncFunction)(cycle);
+
+    cycle++;
+    if (cycle == PWM_DRIVER_MAX_CYCLE) {
+        cycle = 0;
+    }
+}

src/main/io/pwmdriver_i2c.h

@@ -0,0 +1,4 @@
+
+void pwmDriverInitialize(void);
+void pwmDriverSync(void);
+void pwmDriverSetPulse(uint8_t servoIndex, uint16_t length);

src/main/io/serial_cli.c

@@ -212,7 +212,7 @@ static const char * const featureNames[] = {
     "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
     "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125",
     "BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
-    "SUPEREXPO", "VTX", "RX_SPI", "SOFTSPI", NULL
+    "SUPEREXPO", "VTX", "RX_SPI", "SOFTSPI", "PWM_SERVO_DRIVER", NULL
 };
 
 // sync this with rxFailsafeChannelMode_e

src/main/main.c

@@ -62,6 +62,8 @@
 #include "drivers/gyro_sync.h"
 #include "drivers/io.h"
 #include "drivers/exti.h"
+#include "io/pwmdriver_i2c.h"
+#include "drivers/io_pca9685.h"
 
 #include "rx/rx.h"
 #include "rx/spektrum.h"
@@ -282,6 +284,16 @@ void init(void)
     pwmRxInit(masterConfig.inputFilteringMode);
 #endif
 
+#ifdef USE_PMW_SERVO_DRIVER
+    /*
+    If external PWM driver is enabled, for example PCA9685, disable internal
+    servo handling mechanism, since external device will do that
+    */
+    if (feature(FEATURE_PWM_SERVO_DRIVER)) {
+        pwm_params.useServos = false;
+    }
+#endif
+
     // pwmInit() needs to be called as soon as possible for ESC compatibility reasons
     pwmInit(&pwm_params);
 
@@ -573,6 +585,10 @@ void init(void)
     LED2_ON;
 #endif
 
+#ifdef USE_PMW_SERVO_DRIVER
+    pwmDriverInitialize();
+#endif
+
     // Latch active features AGAIN since some may be modified by init().
     latchActiveFeatures();
     motorControlEnable = true;
@@ -637,6 +653,10 @@ int main(void)
     setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
 #endif
 
+#ifdef USE_PMW_SERVO_DRIVER
+    setTaskEnabled(TASK_PWMDRIVER, feature(FEATURE_PWM_SERVO_DRIVER));
+#endif
+
     while (true) {
         scheduler();
         processLoopback();

src/main/scheduler/scheduler.h

@@ -68,6 +68,9 @@ typedef enum {
 #ifdef LED_STRIP
     TASK_LEDSTRIP,
 #endif
+#ifdef USE_PMW_SERVO_DRIVER
+    TASK_PWMDRIVER,
+#endif
 
     /* Count of real tasks */
     TASK_COUNT,

src/main/scheduler/scheduler_tasks.c

@@ -129,4 +129,14 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_IDLE,
     },
 #endif
+
+#ifdef USE_PMW_SERVO_DRIVER
+    [TASK_PWMDRIVER] = {
+        .taskName = "PWMDRIVER",
+        .taskFunc = taskSyncPwmDriver,
+        .desiredPeriod = 1000000 / 200,         // 200 Hz
+        .staticPriority = TASK_PRIORITY_HIGH,
+    },
+#endif
+
 };

src/main/scheduler/scheduler_tasks.h

@@ -33,4 +33,6 @@ void taskUpdateDisplay(void);
 void taskTelemetry(void);
 void taskLedStrip(void);
 void taskSystem(void);
-
+#ifdef USE_PMW_SERVO_DRIVER
+void taskSyncPwmDriver(void);
+#endif

src/main/sensors/initialisation.c

@@ -787,4 +787,3 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig,
 
     return true;
 }
-

src/main/target/common.h

@@ -49,6 +49,8 @@
 #define TELEMETRY_MAVLINK
 #define BOOTLOG_DESCRIPTIONS
 #define TELEMETRY_IBUS
+#define USE_PMW_SERVO_DRIVER
+#define PWM_DRIVER_PCA9685
 #else
 #define SKIP_CLI_COMMAND_HELP
 #define SKIP_RX_MSP