AP_HAL_Linux: ToneAlarmDriver_Raspilot: cleanup driver

- Fix coding style
  - Remove unused variables and members
  - Add virtual as needed and override where needed

libraries/AP_HAL_Linux/ToneAlarmDriver.h

@@ -113,10 +113,10 @@ class Linux::ToneAlarm{
 public:
 	ToneAlarm();
 	void set_tune(uint8_t tone);
-	bool init();
+	virtual bool init();
+	virtual void stop();
+	virtual bool play();
 	bool is_tune_comp();
-	void stop();
-	bool play();
 	bool set_note();
 	bool init_tune();
 

libraries/AP_HAL_Linux/ToneAlarmDriver_Raspilot.cpp

@@ -3,17 +3,20 @@
 #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 <fcntl.h>
+#include <iostream>
 #include <poll.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
 #include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include <AP_Math/AP_Math.h>
 
 #include "GPIO.h"
 #include "Util_RPI.h"
@@ -21,11 +24,9 @@
 #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
 
@@ -42,34 +43,36 @@ extern const AP_HAL::HAL& hal;
 
 ToneAlarm_Raspilot::ToneAlarm_Raspilot()
 {
-    tune_num = -1;                    //initialy no tune to play
+    // initialy no tune to play
+    tune_num = -1;
     tune_pos = 0;
 }
 
 bool ToneAlarm_Raspilot::init()
 {
-    tune_num = 0;                    //play startup tune
+    uint32_t pwm_address, clk_address;
+    int mem_fd;
+
+    // play startup tune
+    tune_num = 0;
 
     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;
+
+    if (rpi_version == 1) {
+        pwm_address = RPI1_PWM_BASE;
+        clk_address = RPI1_CLK_BASE;
+    } else {
+        pwm_address = RPI2_PWM_BASE;
+        clk_address = RPI2_CLK_BASE;
+    }
+
     // open /dev/mem
-    if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
+    if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC|O_CLOEXEC) ) < 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(
+    void *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
@@ -78,7 +81,7 @@ bool ToneAlarm_Raspilot::init()
         pwm_address           // Offset to GPIO peripheral
     );
 
-    clk_map = mmap(
+    void *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
@@ -87,40 +90,42 @@ bool ToneAlarm_Raspilot::init()
         clk_address           // Offset to GPIO peripheral
     );
 
-    close(mem_fd); // No need to keep mem_fd open after mmap
+    // No need to keep mem_fd open after mmap
+    close(mem_fd);
 
-    if (gpio_map == MAP_FAILED || pwm_map == MAP_FAILED || clk_map == MAP_FAILED) {
+    if (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;
+    _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);
+    hal.gpio->pinMode(RASPILOT_TONE_PIN, HAL_GPIO_ALT, 5);
 
     return true;
 }
 
 void ToneAlarm_Raspilot::stop()
 {
-    setPWM0Duty(0);
+    _set_pwm0_duty(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);
+        _set_pwm0_period(1000000/cur_note);
+        _set_pwm0_duty(50);
         cur_note =0;
         prev_time = cur_time;
     }
+
     if ((cur_time - prev_time) > duration){
         stop();
         if (tune[tune_num][tune_pos] == '\0'){
@@ -134,13 +139,14 @@ bool ToneAlarm_Raspilot::play()
         }
         return true;
     }
+
     return false;
 }
 
-void ToneAlarm_Raspilot::setPWM0Period(uint32_t time_us)
+void ToneAlarm_Raspilot::_set_pwm0_period(uint32_t time_us)
 {
     // stop clock and waiting for busy flag doesn't work, so kill clock
-    *(clk + RPI_PWMCLK_CNTL) = 0x5A000000 | (1 << 5);
+    *(_clk + RPI_PWMCLK_CNTL) = 0x5A000000 | (1 << 5);
     usleep(10);
 
     // set frequency
@@ -151,42 +157,39 @@ void ToneAlarm_Raspilot::setPWM0Period(uint32_t time_us)
     if (idiv < 1 || idiv > 0x1000) {
       return;
     }
-    *(clk + RPI_PWMCLK_DIV)  = 0x5A000000 | (idiv<<12);
+    *(_clk + RPI_PWMCLK_DIV)  = 0x5A000000 | (idiv<<12);
 
     // source=osc and enable clock
-    *(clk + RPI_PWMCLK_CNTL) = 0x5A000011;
+    *(_clk + RPI_PWMCLK_CNTL) = 0x5A000011;
 
     // disable PWM
-    *(pwm + RPI_PWM_CTL) = 0;
+    *(_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;
+    *(_pwm + RPI_PWM_RNG1) = 320;
 
     // init with 0%
-    setPWM0Duty(0);
+    _set_pwm0_duty(0);
 
     // start PWM1 in serializer mode
-    *(pwm + RPI_PWM_CTL) = 3;
+    *(_pwm + RPI_PWM_CTL) = 3;
 }
 
-void ToneAlarm_Raspilot::setPWM0Duty(uint8_t percent)
+void ToneAlarm_Raspilot::_set_pwm0_duty(uint8_t percent)
 {
-    int bitCount;
+    int bit_count = constrain_int32(320 * percent / 100, 320, 0);
     unsigned int bits = 0;
 
-    bitCount = 320 * percent / 100;
-    if (bitCount > 320) bitCount = 320;
-    if (bitCount < 0) bitCount = 0;
-    bits = 0;
-    while (bitCount) {
+    while (bit_count) {
       bits <<= 1;
       bits |= 1;
-      bitCount--;
+      bit_count--;
     }
-    *(pwm + RPI_PWM_DAT1) = bits;
+
+    *(_pwm + RPI_PWM_DAT1) = bits;
 }
 
 #endif

libraries/AP_HAL_Linux/ToneAlarmDriver_Raspilot.h

@@ -1,5 +1,4 @@
-#ifndef __TONE_ALARM_DRIVER_RASPILOT_H__
-#define __TONE_ALARM_DRIVER_RASPILOT_H__
+#pragma once
 
 #include "AP_HAL_Linux.h"
 
@@ -8,21 +7,14 @@
 class Linux::ToneAlarm_Raspilot : public Linux::ToneAlarm {
 public:
     ToneAlarm_Raspilot();
-  bool init();
-	void stop();
-	bool play();
+    bool init() override;
+    void stop() override;
+    bool play() override;
 
 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 _set_pwm0_period(uint32_t time_us);
+    void _set_pwm0_duty(uint8_t percent);
 
-  void setPWM0Period(uint32_t time_us);
-  void setPWM0Duty(uint8_t percent);
+    volatile uint32_t *_pwm;
+    volatile uint32_t *_clk;
 };
-
-#endif