diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h index 99f022024a..5c51cb034d 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h +++ b/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; diff --git a/libraries/AP_HAL_Linux/ToneAlarmDriver.cpp b/libraries/AP_HAL_Linux/ToneAlarmDriver.cpp index 14447d2697..a3b715714e 100644 --- a/libraries/AP_HAL_Linux/ToneAlarmDriver.cpp +++ b/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(){ diff --git a/libraries/AP_HAL_Linux/ToneAlarmDriver.h b/libraries/AP_HAL_Linux/ToneAlarmDriver.h index 757b913761..20ab1d5c80 100644 --- a/libraries/AP_HAL_Linux/ToneAlarmDriver.h +++ b/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; }; diff --git a/libraries/AP_HAL_Linux/ToneAlarmDriver_Raspilot.cpp b/libraries/AP_HAL_Linux/ToneAlarmDriver_Raspilot.cpp new file mode 100644 index 0000000000..c0bacf55d9 --- /dev/null +++ b/libraries/AP_HAL_Linux/ToneAlarmDriver_Raspilot.cpp @@ -0,0 +1,192 @@ +#include + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT + +#include "ToneAlarmDriver_Raspilot.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 diff --git a/libraries/AP_HAL_Linux/ToneAlarmDriver_Raspilot.h b/libraries/AP_HAL_Linux/ToneAlarmDriver_Raspilot.h new file mode 100644 index 0000000000..e2050cc083 --- /dev/null +++ b/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 diff --git a/libraries/AP_HAL_Linux/Util.cpp b/libraries/AP_HAL_Linux/Util.cpp index 768e0df9e6..a29fdc9a7e 100644 --- a/libraries/AP_HAL_Linux/Util.cpp +++ b/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; diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h index 07ef4937b9..48051133ea 100644 --- a/libraries/AP_HAL_Linux/Util.h +++ b/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;