AP_HAL_Linux: GPIO_RPI: remove unused functions

These functions to use pins as pwm are never used in the codebase, so
remove them.
This commit is contained in:
Lucas De Marchi 2016-06-20 10:46:47 -03:00
parent 2108127712
commit 0bb86ae498
2 changed files with 1 additions and 93 deletions

View File

@ -30,8 +30,6 @@ void GPIO_RPI::init()
{
int rpi_version = UtilRPI::from(hal.util)->get_rpi_version();
uint32_t gpio_address = rpi_version == 1 ? GPIO_BASE(BCM2708_PERI_BASE) : GPIO_BASE(BCM2709_PERI_BASE);
uint32_t pwm_address = rpi_version == 1 ? PWM_BASE(BCM2708_PERI_BASE) : PWM_BASE(BCM2709_PERI_BASE);
uint32_t clk_address = rpi_version == 1 ? CLOCK_BASE(BCM2708_PERI_BASE) : CLOCK_BASE(BCM2709_PERI_BASE);
// open /dev/mem
if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
AP_HAL::panic("Can't open /dev/mem");
@ -47,33 +45,13 @@ void GPIO_RPI::init()
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) {
if (gpio_map == MAP_FAILED) {
AP_HAL::panic("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;
}
void GPIO_RPI::pinMode(uint8_t pin, uint8_t output)
@ -124,68 +102,6 @@ void GPIO_RPI::toggle(uint8_t pin)
write(pin, !read(pin));
}
void GPIO_RPI::setPWMPeriod(uint8_t pin, uint32_t time_us)
{
setPWM0Period(time_us);
}
void GPIO_RPI::setPWMDuty(uint8_t pin, uint8_t percent)
{
setPWM0Duty(percent);
}
void GPIO_RPI::setPWM0Period(uint32_t time_us)
{
// stop clock and waiting for busy flag doesn't work, so kill clock
*(clk + 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 + PWMCLK_DIV) = 0x5A000000 | (idiv<<12);
// source=osc and enable clock
*(clk + PWMCLK_CNTL) = 0x5A000011;
// disable PWM
*(pwm + 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 + PWM_RNG1) = 320;
// init with 0%
setPWM0Duty(0);
// start PWM1 in serializer mode
*(pwm + PWM_CTL) = 3;
}
void GPIO_RPI::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 + PWM_DAT1) = bits;
}
/* Alternative interface: */
AP_HAL::DigitalSource* GPIO_RPI::channel(uint16_t n) {
return new DigitalSource(n);

View File

@ -67,13 +67,7 @@ class Linux::GPIO_RPI : public AP_HAL::GPIO {
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);
public:
GPIO_RPI();
@ -84,8 +78,6 @@ public:
uint8_t read(uint8_t pin);
void write(uint8_t pin, uint8_t value);
void toggle(uint8_t pin);
void setPWMPeriod(uint8_t pin, uint32_t time_us);
void setPWMDuty(uint8_t pin, uint8_t percent);
/* Alternative interface: */
AP_HAL::DigitalSource* channel(uint16_t n);