diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.cpp b/libraries/AP_HAL_Linux/GPIO_RPI.cpp index 9acc68f60f..68b5f16d4f 100644 --- a/libraries/AP_HAL_Linux/GPIO_RPI.cpp +++ b/libraries/AP_HAL_Linux/GPIO_RPI.cpp @@ -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); diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.h b/libraries/AP_HAL_Linux/GPIO_RPI.h index 739ee6e86f..9545f7f979 100644 --- a/libraries/AP_HAL_Linux/GPIO_RPI.h +++ b/libraries/AP_HAL_Linux/GPIO_RPI.h @@ -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);