AP_HAL_Linux: Add RcInput and RcOutput with only one PRU when using a BBB. BBBMINI use the RC AIO PRU firmware already

libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h

@@ -23,15 +23,17 @@ namespace Linux {
     class LinuxDigitalSource;
     class LinuxRCInput;
     class LinuxRCInput_PRU;
+    class LinuxRCInput_AioPRU;
     class LinuxRCInput_Navio;
     class LinuxRCInput_ZYNQ;
     class LinuxRCOutput_PRU;
+    class LinuxRCOutput_AioPRU;
     class LinuxRCOutput_Navio;
     class LinuxRCOutput_ZYNQ;
     class LinuxSemaphore;
     class LinuxScheduler;
     class LinuxUtil;
-	class ToneAlarm;					//limit the scope of ToneAlarm driver to Linux_HAL only
+    class ToneAlarm;					//limit the scope of ToneAlarm driver to Linux_HAL only
 }
 
 #endif // __AP_HAL_LINUX_NAMESPACE_H__

libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h

@@ -15,8 +15,10 @@
 #include "Storage.h"
 #include "GPIO.h"
 #include "RCInput.h"
+#include "RCInput_AioPRU.h"
 #include "RCInput_Navio.h"
 #include "RCOutput_PRU.h"
+#include "RCOutput_AioPRU.h"
 #include "RCOutput_Navio.h"
 #include "RCOutput_ZYNQ.h"
 #include "Semaphores.h"

libraries/AP_HAL_Linux/HAL_Linux_Class.cpp

@@ -56,10 +56,12 @@ static Empty::EmptyGPIO gpioDriver;
 #endif
 
 /*
-  use the PRU based RCInput driver on ERLE, PXF and BBBMINI
+  use the PRU based RCInput driver on ERLE and PXF
  */
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE
 static LinuxRCInput_PRU rcinDriver;
+#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
+static LinuxRCInput_AioPRU rcinDriver;
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
 static LinuxRCInput_Navio rcinDriver;
 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ
@@ -69,10 +71,12 @@ static LinuxRCInput rcinDriver;
 #endif
 
 /*
-  use the PRU based RCOutput driver on ERLE, PXF and BBBMINI
+  use the PRU based RCOutput driver on ERLE and PXF
  */
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE
 static LinuxRCOutput_PRU rcoutDriver;
+#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
+static LinuxRCOutput_AioPRU rcoutDriver;
 /*
   use the PCA9685 based RCOutput driver on Navio
  */

libraries/AP_HAL_Linux/RCInput_AioPRU.cpp

@@ -0,0 +1,65 @@
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
+
+#include <stdio.h>
+#include <sys/time.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <stdint.h>
+
+#include "RCInput.h"
+#include "RCInput_AioPRU.h"
+
+extern const AP_HAL::HAL& hal;
+
+using namespace Linux;
+
+void LinuxRCInput_AioPRU::init(void*)
+{
+    int mem_fd = open("/dev/mem", O_RDWR|O_SYNC);
+    if (mem_fd == -1) {
+        hal.scheduler->panic("Unable to open /dev/mem");
+    }
+    ring_buffer = (volatile struct ring_buffer*) mmap(0, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, RCIN_PRUSS_RAM_BASE);
+    close(mem_fd);
+    ring_buffer->ring_head = 0;
+}
+
+/*
+  called at 1kHz to check for new pulse capture data from the PRU
+ */
+void LinuxRCInput_AioPRU::_timer_tick()
+{
+    while (ring_buffer->ring_head != ring_buffer->ring_tail) {
+        if (ring_buffer->ring_tail >= NUM_RING_ENTRIES) {
+            // invalid ring_tail from PRU - ignore RC input
+            return;
+        }
+        _process_rc_pulse((ring_buffer->buffer[ring_buffer->ring_head].s1_t) / TICK_PER_US,
+                          (ring_buffer->buffer[ring_buffer->ring_head].s0_t) / TICK_PER_US);
+        // move to the next ring buffer entry
+        ring_buffer->ring_head = (ring_buffer->ring_head + 1) % NUM_RING_ENTRIES;
+    }
+}
+
+#endif // CONFIG_HAL_BOARD_SUBTYPE

libraries/AP_HAL_Linux/RCInput_AioPRU.h

@@ -0,0 +1,48 @@
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+
+#ifndef __AP_HAL_LINUX_RCINPUT_AIOPRU_H__
+#define __AP_HAL_LINUX_RCINPUT_AIOPRU_H__
+
+/*
+  This class implements RCInput on the BeagleBoneBlack with a PRU
+  doing the edge detection of the PPM sum input
+ */
+
+#include <AP_HAL_Linux.h>
+
+#define RCIN_PRUSS_RAM_BASE   0x4a303000
+// we use 300 ring buffer entries to guarantee that a full 25 byte
+// frame of 12 bits per byte
+
+class Linux::LinuxRCInput_AioPRU : public Linux::LinuxRCInput 
+{
+public:
+    void init(void*);
+    void _timer_tick(void);
+
+ private:
+    static const uint32_t TICK_PER_US = 200;
+    static const uint32_t NUM_RING_ENTRIES = 300;
+    // shared ring buffer with the PRU which records pin transitions
+    struct ring_buffer {
+        volatile uint16_t ring_head; // owned by ARM CPU
+        volatile uint16_t ring_tail; // owned by the PRU
+        struct {
+               volatile uint32_t s1_t; // 5ns per tick
+               volatile uint32_t s0_t; // 5ns per tick
+        } buffer[NUM_RING_ENTRIES];
+    };
+    volatile struct ring_buffer *ring_buffer;
+};
+
+#endif // __AP_HAL_LINUX_RCINPUT_AIOPRU_H__

libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp

@@ -0,0 +1,151 @@
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+
+#include "RCOutput_AioPRU.h"
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <sys/mman.h>
+#include <signal.h>
+
+#include "../../Tools/Linux_HAL_Essentials/pru/aiopru/RcAioPRU_bin.h"
+
+using namespace Linux;
+
+static const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+static void catch_sigbus(int sig)
+{
+    hal.scheduler->panic("RCOutputAioPRU.cpp:SIGBUS error gernerated\n");
+}
+void LinuxRCOutput_AioPRU::init(void* machtnicht)
+{
+   uint32_t mem_fd, i;
+   uint32_t *iram;
+   uint32_t *ctrl;
+
+   signal(SIGBUS,catch_sigbus);
+
+   mem_fd = open("/dev/mem", O_RDWR|O_SYNC);
+
+   pwm = (struct pwm*) mmap(0, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, RCOUT_PRUSS_RAM_BASE);
+   iram = (uint32_t*)mmap(0, 0x2000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, RCOUT_PRUSS_IRAM_BASE);
+   ctrl = (uint32_t*)mmap(0, 0x1000, PROT_READ|PROT_WRITE, MAP_SHARED, mem_fd, RCOUT_PRUSS_CTRL_BASE);
+
+   close(mem_fd);
+
+   // Reset PRU 1
+   *ctrl = 0;
+   hal.scheduler->delay(1);
+
+   // Load firmware
+   for(i = 0; i < sizeof(PRUcode); i++) {
+      *(iram + i) = PRUcode[i];
+   }
+
+   // Start PRU 1
+   *ctrl = 3;
+
+   // all outputs default to 50Hz, the top level vehicle code
+   // overrides this when necessary
+   set_freq(0xFFFFFFFF, 50);
+}
+
+void LinuxRCOutput_AioPRU::set_freq(uint32_t chmask, uint16_t freq_hz)
+{
+   uint8_t i;
+   uint32_t tick = TICK_PER_S / freq_hz;
+
+   for(i = 0; i < PWM_CHAN_COUNT; i++) {
+      if(chmask & (1U << i)) {
+         pwm->channel[i].time_t = tick;
+      }
+   }
+}
+
+uint16_t LinuxRCOutput_AioPRU::get_freq(uint8_t ch)
+{
+   uint16_t ret = 0;
+
+   if(ch < PWM_CHAN_COUNT) {
+      ret = TICK_PER_S / pwm->channel[ch].time_t;
+   }
+
+   return ret;
+}
+
+void LinuxRCOutput_AioPRU::enable_ch(uint8_t ch)
+{
+   if(ch < PWM_CHAN_COUNT) {
+      pwm->channelenable |= 1U << ch;
+   }
+}
+
+void LinuxRCOutput_AioPRU::disable_ch(uint8_t ch)
+{
+   if(ch < PWM_CHAN_COUNT) {
+      pwm->channelenable &= !(1U << ch);
+   }
+}
+
+void LinuxRCOutput_AioPRU::write(uint8_t ch, uint16_t period_us)
+{
+   if(ch < PWM_CHAN_COUNT) {
+      pwm->channel[ch].time_high = TICK_PER_US * period_us;
+   }
+}
+
+void LinuxRCOutput_AioPRU::write(uint8_t ch, uint16_t* period_us, uint8_t len)
+{
+   uint8_t i;
+
+   if(len > PWM_CHAN_COUNT) {
+      len = PWM_CHAN_COUNT;
+   }
+
+   for(i = 0; i < len; i++) {
+      write(ch + i, period_us[i]);
+   }
+}
+
+uint16_t LinuxRCOutput_AioPRU::read(uint8_t ch)
+{
+   uint16_t ret = 0;
+
+   if(ch < PWM_CHAN_COUNT) {
+      ret = (pwm->channel[ch].time_high / TICK_PER_US);
+   }
+
+   return ret;
+}
+
+void LinuxRCOutput_AioPRU::read(uint16_t* period_us, uint8_t len)
+{
+   uint8_t i;
+
+   if(len > PWM_CHAN_COUNT) {
+      len = PWM_CHAN_COUNT;
+   }
+
+   for(i = 0; i < len; i++) {
+      period_us[i] = pwm->channel[i].time_high / TICK_PER_US;
+   }
+}
+
+#endif
+

libraries/AP_HAL_Linux/RCOutput_AioPRU.h

@@ -0,0 +1,48 @@
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+
+#ifndef __AP_HAL_LINUX_RCOUTPUT_AIOPRU_H__
+#define __AP_HAL_LINUX_RCOUTPUT_AIOPRU_H__
+
+#include <AP_HAL_Linux.h>
+#define RCOUT_PRUSS_RAM_BASE 0x4a302000
+#define RCOUT_PRUSS_CTRL_BASE 0x4a324000
+#define RCOUT_PRUSS_IRAM_BASE 0x4a338000
+#define PWM_CHAN_COUNT 12
+
+class Linux::LinuxRCOutput_AioPRU : 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:
+   static const uint32_t TICK_PER_US = 200;
+   static const uint32_t TICK_PER_S = 200000000;
+
+   struct pwm {
+      volatile uint32_t channelenable;
+      struct {
+        volatile uint32_t time_high;
+        volatile uint32_t time_t;
+        } channel[PWM_CHAN_COUNT];
+    };
+
+    volatile struct pwm *pwm;
+};
+
+#endif // __AP_HAL_LINUX_RCOUTPUT_AIOPRU_H__