Pritam Ghanghas
9 years ago
committed by
Lucas De Marchi
7 changed files with 237 additions and 25 deletions
@ -0,0 +1,192 @@
@@ -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 |
||||
@ -0,0 +1,28 @@
@@ -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 |
||||
Loading…
Reference in new issue