HAL_Linux: add PCA9685 based RCOutput for Navio

libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h

@@ -20,6 +20,7 @@ namespace Linux {
     class LinuxRCInput;
     class LinuxRCInput_PRU;
     class LinuxRCOutput_PRU;
+    class LinuxRCOutput_Navio;
     class LinuxSemaphore;
     class LinuxScheduler;
     class LinuxUtil;

libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h

@@ -14,6 +14,7 @@
 #include "GPIO.h"
 #include "RCInput.h"
 #include "RCOutput_PRU.h"
+#include "RCOutput_Navio.h"
 #include "Semaphores.h"
 #include "Scheduler.h"
 #include "Util.h"

libraries/AP_HAL_Linux/HAL_Linux_Class.cpp

@@ -40,9 +40,15 @@ static LinuxRCInput rcinDriver;
  */
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE
 static LinuxRCOutput_PRU rcoutDriver;
+/*
+  use the PCA9685 based RCOutput driver on Navio
+ */
+#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
+static LinuxRCOutput_Navio rcoutDriver;
 #else
 static Empty::EmptyRCOutput rcoutDriver;
 #endif
+
 static LinuxScheduler schedulerInstance;
 static LinuxUtil utilInstance;
 

libraries/AP_HAL_Linux/RCOutput_Navio.cpp

@@ -0,0 +1,140 @@
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+
+#include "RCOutput_Navio.h"
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <math.h>
+
+using namespace Linux;
+
+#define PWM_CHAN_COUNT 13
+
+static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+void LinuxRCOutput_Navio::init(void* machtnicht)
+{
+    _i2c_sem = hal.i2c->get_semaphore();
+    if (_i2c_sem == NULL) {
+        hal.scheduler->panic(PSTR("PANIC: RCOutput_Navio did not get "
+                                  "valid I2C semaphore!"));
+        return; // never reached
+    }
+    
+    // Set the initial frequency
+    set_freq(0, 50);
+    
+    if (!_i2c_sem->take(10)){
+        hal.scheduler->panic(PSTR("PANIC: RCOutput_Navio: failed to take "
+                    "I2C semaphore for init"));
+        return; /* never reached */
+    }
+    
+    // Turn the LED green
+    uint8_t ledoff[4] = {0x00, 0x00, 4095 & 0xFF, 4095 >> 8};
+    hal.i2c->writeRegisters(PCA9685_ADDRESS, PCA9685_RA_LED0_ON_L, 4, ledoff);
+    hal.i2c->writeRegisters(PCA9685_ADDRESS, PCA9685_RA_LED0_ON_L + 8, 4, ledoff);
+    uint8_t ledslightlyon[4] = {0x00, 0x00, 3500 & 0xFF, 3500 >> 8};
+    hal.i2c->writeRegisters(PCA9685_ADDRESS, PCA9685_RA_LED0_ON_L + 4, 4, ledslightlyon);
+                                                                
+    _i2c_sem->give();
+}
+
+void LinuxRCOutput_Navio::set_freq(uint32_t chmask, uint16_t freq_hz)
+{    
+    if (!_i2c_sem->take(10)) {
+        return;
+    }
+    
+    uint8_t prescale = round(25000000.f / 4096.f / freq_hz)  - 1;
+    hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_PRE_SCALE, prescale);
+    hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, 0x00);
+    hal.i2c->writeRegister(PCA9685_ADDRESS, PCA9685_RA_MODE1, 
+                           PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT); 
+    _frequency = freq_hz;
+    
+    _i2c_sem->give();
+}
+
+uint16_t LinuxRCOutput_Navio::get_freq(uint8_t ch)
+{
+    return _frequency;
+}
+
+void LinuxRCOutput_Navio::enable_ch(uint8_t ch)
+{
+    
+}
+
+void LinuxRCOutput_Navio::disable_ch(uint8_t ch)
+{
+    write(ch, 0);
+}
+
+void LinuxRCOutput_Navio::write(uint8_t ch, uint16_t period_us)
+{   
+    if(ch >= PWM_CHAN_COUNT){
+        return;
+    }
+    
+    if (!_i2c_sem->take_nonblocking()) {
+        return;
+    }
+    
+    uint16_t length;
+    
+    if (period_us == 0)
+        length = 0;
+    else
+        length = round((period_us * 4096) / (1000000.f / _frequency)) - 1;
+        
+    uint8_t data[4] = {0x00, 0x00, length & 0xFF, length >> 8};
+    uint8_t status = hal.i2c->writeRegisters(PCA9685_ADDRESS, 
+                                             PCA9685_RA_LED0_ON_L + 4 * (ch + 3), 
+                                             4, 
+                                             data);
+                                             
+    _i2c_sem->give();                                         
+}
+
+void LinuxRCOutput_Navio::write(uint8_t ch, uint16_t* period_us, uint8_t len)
+{
+    for (int i = 0; i < len; i++)
+        write(ch + i, period_us[i]);
+}
+
+uint16_t LinuxRCOutput_Navio::read(uint8_t ch)
+{
+    if (!_i2c_sem->take_nonblocking()) {
+        return 0;
+    }
+    
+    uint8_t data[4] = {0x00, 0x00, 0x00, 0x00};
+    hal.i2c->readRegisters(PCA9685_ADDRESS, 
+                           PCA9685_RA_LED0_ON_L + 4 * (ch + 3), 
+                           4, 
+                           data);
+                           
+    uint16_t length = data[2] + ((data[3] & 0x0F) << 8);    
+    uint16_t period_us = (length + 1) * (1000000.f / _frequency) / 4096.f;
+    
+    _i2c_sem->give(); 
+    
+    return length == 0 ? 0 : period_us;   
+}
+
+void LinuxRCOutput_Navio::read(uint16_t* period_us, uint8_t len)
+{
+    for (int i = 0; i < len; i++) 
+        period_us[i] = read(0 + i);
+}
+
+#endif // CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO

libraries/AP_HAL_Linux/RCOutput_Navio.h

@@ -0,0 +1,53 @@
+
+#ifndef __AP_HAL_LINUX_RCOUTPUT_NAVIO_H__
+#define __AP_HAL_LINUX_RCOUTPUT_NAVIO_H__
+
+#include <AP_HAL_Linux.h>
+
+#define PCA9685_ADDRESS             0x40 // All address pins low, Navio default
+
+#define PCA9685_RA_MODE1            0x00
+#define PCA9685_RA_MODE2            0x01
+#define PCA9685_RA_LED0_ON_L        0x06
+#define PCA9685_RA_LED0_ON_H        0x07
+#define PCA9685_RA_LED0_OFF_L       0x08
+#define PCA9685_RA_LED0_OFF_H       0x09
+#define PCA9685_RA_ALL_LED_ON_L     0xFA
+#define PCA9685_RA_ALL_LED_ON_H     0xFB
+#define PCA9685_RA_ALL_LED_OFF_L    0xFC
+#define PCA9685_RA_ALL_LED_OFF_H    0xFD
+#define PCA9685_RA_PRE_SCALE        0xFE
+
+#define PCA9685_MODE1_RESTART_BIT   1<<7
+#define PCA9685_MODE1_EXTCLK_BIT    1<<6
+#define PCA9685_MODE1_AI_BIT        1<<5
+#define PCA9685_MODE1_SLEEP_BIT     1<<4
+#define PCA9685_MODE1_SUB1_BIT      1<<3
+#define PCA9685_MODE1_SUB2_BIT      1<<2
+#define PCA9685_MODE1_SUB3_BIT      1<<1
+#define PCA9685_MODE1_ALLCALL_BIT   1<<0
+
+#define PCA9685_MODE2_INVRT_BIT     1<<4
+#define PCA9685_MODE2_OCH_BIT       1<<3
+#define PCA9685_MODE2_OUTDRV_BIT    1<<2
+#define PCA9685_MODE2_OUTNE1_BIT    1<<1
+#define PCA9685_MODE2_OUTNE0_BIT    1<<0
+
+class Linux::LinuxRCOutput_Navio : public AP_HAL::RCOutput {
+    void     init(void* machtnichts);
+    void     set_freq(uint32_t chmask, uint16_t freq_hz);
+    uint16_t get_freq(uint8_t ch);
+    void     enable_ch(uint8_t ch);
+    void     disable_ch(uint8_t ch);
+    void     write(uint8_t ch, uint16_t period_us);
+    void     write(uint8_t ch, uint16_t* period_us, uint8_t len);
+    uint16_t read(uint8_t ch);
+    void     read(uint16_t* period_us, uint8_t len);
+
+private:
+    void reset();
+    AP_HAL::Semaphore *_i2c_sem;
+    uint16_t _frequency;    
+};
+
+#endif // __AP_HAL_LINUX_RCOUTPUT_NAVIO_H__