AP_HAL_Linux: separate class for tone alarm driver

libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h

@@ -46,6 +46,7 @@ namespace Linux {
     class Util;
     class UtilRPI;
     class ToneAlarm;
+    class ToneAlarm_Raspilot;
     class Thread;
     class Heat;
     class HeatPwm;

libraries/AP_HAL_Linux/ToneAlarmDriver.cpp

@@ -13,11 +13,6 @@
 
 using namespace Linux;
 
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
-#include "GPIO.h"
-#define RASPILOT_TONE_PIN RPI_GPIO_18
-#endif
-
 extern const AP_HAL::HAL& hal;
 static uint16_t notes[] = { 0,
 NOTE_C4, NOTE_CS4, NOTE_D4, NOTE_DS4, NOTE_E4, NOTE_F4, NOTE_FS4, NOTE_G4, NOTE_GS4, NOTE_A4, NOTE_AS4, NOTE_B4,
@@ -55,16 +50,10 @@ ToneAlarm::ToneAlarm()
 bool ToneAlarm::init()
 {
     tune_num = 0;                    //play startup tune
-
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
-    hal.gpio->pinMode(RASPILOT_TONE_PIN, HAL_GPIO_ALT, 5);
-#else
     if((period_fd == -1) || (duty_fd == -1) || (run_fd == -1)){
         hal.console->printf("ToneAlarm: Error!! please check if PWM overlays are loaded correctly");
         return false;
     }
-#endif
-
     return true;
 }
 
@@ -80,11 +69,7 @@ bool ToneAlarm::is_tune_comp()
 
 void ToneAlarm::stop()
 {
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
-    hal.gpio->setPWMDuty(RASPILOT_TONE_PIN, 0);
-#else
     dprintf(run_fd,"0");
-#endif
 }
 
 bool ToneAlarm::play()
@@ -95,15 +80,10 @@ bool ToneAlarm::play()
         return true;
     }
     if(cur_note != 0){
-#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
-        hal.gpio->setPWMPeriod(RASPILOT_TONE_PIN, 1000000/cur_note);
-        hal.gpio->setPWMDuty(RASPILOT_TONE_PIN, 50);
-#else
         dprintf(run_fd,"0");
         dprintf(period_fd,"%u",1000000000/cur_note);
         dprintf(duty_fd,"%u",500000000/cur_note);
         dprintf(run_fd,"1");
-#endif
         cur_note =0;
         prev_time = cur_time;
     }
@@ -128,7 +108,7 @@ bool ToneAlarm::set_note(){
     uint16_t scale,note,num =0;
     duration = 0;
 
-    while(isdigit(tune[tune_num][tune_pos])){                   //this is a safe while loop as it can't go further than 
+    while(isdigit(tune[tune_num][tune_pos])){                   //this is a safe while loop as it can't go further than
                                                                 //the length of the rtttl tone string
         num = (num * 10) + (tune[tune_num][tune_pos++] - '0');
     }
@@ -209,6 +189,7 @@ bool ToneAlarm::set_note(){
         cur_note = 0;
         return true;
     }
+
 }
 
 bool ToneAlarm::init_tune(){

libraries/AP_HAL_Linux/ToneAlarmDriver.h

@@ -104,7 +104,7 @@
 #define     TONE_BATTERY_WARNING_SLOW_TUNE      6
 #define     TONE_BATTERY_WARNING_FAST_TUNE      7
 #define     TONE_GPS_WARNING_TUNE               8
-#define     TONE_ARMING_FAILURE_TUNE            9    
+#define     TONE_ARMING_FAILURE_TUNE            9
 #define     TONE_PARACHUTE_RELEASE_TUNE         10
 
 #define TONE_NUMBER_OF_TUNES 11
@@ -120,7 +120,7 @@ public:
 	bool set_note();
 	bool init_tune();
 
-private:
+protected:
 	bool tune_comp;
 	static const char *tune[TONE_NUMBER_OF_TUNES];
 	static bool tune_repeat[TONE_NUMBER_OF_TUNES];
@@ -133,9 +133,11 @@ private:
 	uint16_t duration;
 	int32_t prev_tune_num;
 	uint32_t prev_time;
+	int8_t tune_num;
+	uint8_t tune_pos;
+
+private:
 	int32_t period_fd;
 	int32_t duty_fd;
 	int32_t run_fd;
-	int8_t tune_num;
-	uint8_t tune_pos;
 };

libraries/AP_HAL_Linux/ToneAlarmDriver_Raspilot.cpp

@@ -0,0 +1,192 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
+
+#include "ToneAlarmDriver_Raspilot.h"
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <iostream>
+#include <string.h>
+#include <errno.h>
+#include <poll.h>
+#include <sys/mman.h>
+
+#include "GPIO.h"
+#include "Util_RPI.h"
+
+#define RASPILOT_TONE_PIN     RPI_GPIO_18
+#define RASPILOT_TONE_PIN_ALT 5
+
+#define RPI1_GPIO_BASE 0x20200000
+#define RPI1_PWM_BASE  0x2020C000
+#define RPI1_CLK_BASE  0x20101000
+
+#define RPI2_GPIO_BASE 0x3F200000
+#define RPI2_PWM_BASE  0x3F20C000
+#define RPI2_CLK_BASE  0x3F101000
+
+#define	RPI_PWM_CTL  0
+#define	RPI_PWM_RNG1 4
+#define	RPI_PWM_DAT1 5
+
+#define	RPI_PWMCLK_CNTL 40
+#define	RPI_PWMCLK_DIV  41
+
+using namespace Linux;
+
+extern const AP_HAL::HAL& hal;
+
+ToneAlarm_Raspilot::ToneAlarm_Raspilot()
+{
+    tune_num = -1;                    //initialy no tune to play
+    tune_pos = 0;
+}
+
+bool ToneAlarm_Raspilot::init()
+{
+    tune_num = 0;                    //play startup tune
+
+    int rpi_version = UtilRPI::from(hal.util)->get_rpi_version();
+    uint32_t gpio_address = rpi_version == 1? RPI1_GPIO_BASE: RPI2_GPIO_BASE;
+    uint32_t pwm_address  = rpi_version == 1? RPI1_PWM_BASE: RPI2_PWM_BASE;
+    uint32_t clk_address  = rpi_version == 1? RPI1_CLK_BASE: RPI2_CLK_BASE;
+    // open /dev/mem
+    if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
+        AP_HAL::panic("Can't open /dev/mem");
+    }
+
+    // mmap GPIO
+    gpio_map = mmap(
+        NULL,                 // Any adddress in our space will do
+        BLOCK_SIZE,           // Map length
+        PROT_READ|PROT_WRITE, // Enable reading & writting to mapped memory
+        MAP_SHARED,           // Shared with other processes
+        mem_fd,               // File to map
+        gpio_address          // Offset to GPIO peripheral
+    );
+
+    pwm_map = mmap(
+        NULL,                 // Any adddress in our space will do
+        BLOCK_SIZE,           // Map length
+        PROT_READ|PROT_WRITE, // Enable reading & writting to mapped memory
+        MAP_SHARED,           // Shared with other processes
+        mem_fd,               // File to map
+        pwm_address           // Offset to GPIO peripheral
+    );
+
+    clk_map = mmap(
+        NULL,                 // Any adddress in our space will do
+        BLOCK_SIZE,           // Map length
+        PROT_READ|PROT_WRITE, // Enable reading & writting to mapped memory
+        MAP_SHARED,           // Shared with other processes
+        mem_fd,               // File to map
+        clk_address           // Offset to GPIO peripheral
+    );
+
+    close(mem_fd); // No need to keep mem_fd open after mmap
+
+    if (gpio_map == MAP_FAILED || pwm_map == MAP_FAILED || clk_map == MAP_FAILED) {
+        AP_HAL::panic("ToneAlarm: Error!! Can't open /dev/mem");
+    }
+
+    gpio = (volatile uint32_t *)gpio_map; // Always use volatile pointer!
+    pwm  = (volatile uint32_t *)pwm_map;
+    clk  = (volatile uint32_t *)clk_map;
+
+    GPIO_MODE_IN(RASPILOT_TONE_PIN);
+    GPIO_MODE_ALT(RASPILOT_TONE_PIN, RASPILOT_TONE_PIN_ALT);
+
+    return true;
+}
+
+void ToneAlarm_Raspilot::stop()
+{
+    setPWM0Duty(0);
+}
+
+bool ToneAlarm_Raspilot::play()
+{
+    uint32_t cur_time = AP_HAL::millis();
+    if(tune_num != prev_tune_num){
+        tune_changed = true;
+        return true;
+    }
+    if(cur_note != 0){
+        setPWM0Period(1000000/cur_note);
+        setPWM0Duty(50);
+        cur_note =0;
+        prev_time = cur_time;
+    }
+    if((cur_time - prev_time) > duration){
+        stop();
+        if(tune[tune_num][tune_pos] == '\0'){
+            if(!tune_repeat[tune_num]){
+                tune_num = -1;
+            }
+
+            tune_pos = 0;
+            tune_comp = true;
+            return false;
+        }
+        return true;
+    }
+    return false;
+}
+
+void ToneAlarm_Raspilot::setPWM0Period(uint32_t time_us)
+{
+    // stop clock and waiting for busy flag doesn't work, so kill clock
+    *(clk + RPI_PWMCLK_CNTL) = 0x5A000000 | (1 << 5);
+    usleep(10);
+
+    // set frequency
+    // DIVI is the integer part of the divisor
+    // the fractional part (DIVF) drops clock cycles to get the output frequency, bad for servo motors
+    // 320 bits for one cycle of 20 milliseconds = 62.5 us per bit = 16 kHz
+    int idiv = (int) (19200000.0f / (320000000.0f / time_us));
+    if (idiv < 1 || idiv > 0x1000) {
+      return;
+    }
+    *(clk + RPI_PWMCLK_DIV)  = 0x5A000000 | (idiv<<12);
+
+    // source=osc and enable clock
+    *(clk + RPI_PWMCLK_CNTL) = 0x5A000011;
+
+    // disable PWM
+    *(pwm + RPI_PWM_CTL) = 0;
+
+    // needs some time until the PWM module gets disabled, without the delay the PWM module crashs
+    usleep(10);
+
+    // filled with 0 for 20 milliseconds = 320 bits
+    *(pwm + RPI_PWM_RNG1) = 320;
+
+    // init with 0%
+    setPWM0Duty(0);
+
+    // start PWM1 in serializer mode
+    *(pwm + RPI_PWM_CTL) = 3;
+}
+
+void ToneAlarm_Raspilot::setPWM0Duty(uint8_t percent)
+{
+    int bitCount;
+    unsigned int bits = 0;
+
+    bitCount = 320 * percent / 100;
+    if (bitCount > 320) bitCount = 320;
+    if (bitCount < 0) bitCount = 0;
+    bits = 0;
+    while (bitCount) {
+      bits <<= 1;
+      bits |= 1;
+      bitCount--;
+    }
+    *(pwm + RPI_PWM_DAT1) = bits;
+}
+
+#endif

libraries/AP_HAL_Linux/ToneAlarmDriver_Raspilot.h

@@ -0,0 +1,28 @@
+#ifndef __TONE_ALARM_DRIVER_RASPILOT_H__
+#define __TONE_ALARM_DRIVER_RASPILOT_H__
+
+#include "AP_HAL_Linux.h"
+
+#include "ToneAlarmDriver.h"
+
+class Linux::ToneAlarm_Raspilot : public Linux::ToneAlarm {
+public:
+  ToneAlarm_Raspilot();
+  bool init();
+	void stop();
+	bool play();
+
+private:
+  int  mem_fd;
+  void *gpio_map;
+  void *pwm_map;
+  void *clk_map;
+  volatile uint32_t *gpio;
+  volatile uint32_t *pwm;
+  volatile uint32_t *clk;
+
+  void setPWM0Period(uint32_t time_us);
+  void setPWM0Duty(uint8_t percent);
+};
+
+#endif

libraries/AP_HAL_Linux/Util.cpp

@@ -18,7 +18,11 @@ using namespace Linux;
 
 
 static int state;
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
+ToneAlarm_Raspilot Util::_toneAlarm;
+#else
 ToneAlarm Util::_toneAlarm;
+#endif
 
 void Util::init(int argc, char * const *argv) {
     saved_argc = argc;

libraries/AP_HAL_Linux/Util.h

@@ -74,7 +74,11 @@ public:
     int get_hw_arm32();
 
 private:
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
+    static Linux::ToneAlarm_Raspilot _toneAlarm;
+#else
     static Linux::ToneAlarm _toneAlarm;
+#endif
     Linux::Heat *_heat;
     int saved_argc;
     char* const *saved_argv;