From 1b0f482d37e2ea017a25372c58cd6cd0c076be8a Mon Sep 17 00:00:00 2001 From: mirkix Date: Mon, 6 Jul 2015 21:24:06 +0200 Subject: [PATCH] AP_RangeFinder: Add support for HC-SR04 Range Finder connected to BBB --- .../AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp | 102 ++++++++++++++++++ .../AP_RangeFinder/AP_RangeFinder_BBB_PRU.h | 35 ++++++ libraries/AP_RangeFinder/RangeFinder.cpp | 14 ++- libraries/AP_RangeFinder/RangeFinder.h | 3 +- 4 files changed, 151 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp new file mode 100644 index 0000000000..560da79ffe --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp @@ -0,0 +1,102 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + HC-SR04 Ultrasonic Distance Sensor connected to BeagleBone Black + by Mirko Denecke + */ + +#include +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI + +#include "AP_RangeFinder_BBB_PRU.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +volatile struct range *rangerpru; + +/* + The constructor also initialises the rangefinder. Note that this + constructor is not called until detect() returns true, so we + already know that we should setup the rangefinder +*/ +AP_RangeFinder_BBB_PRU::AP_RangeFinder_BBB_PRU(RangeFinder &_ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state) : + AP_RangeFinder_Backend(_ranger, instance, _state) +{ +} + +/* + Stop PRU, load firmware (check if firmware is present), start PRU. + If we get a result the sensor seems to be there. +*/ +bool AP_RangeFinder_BBB_PRU::detect(RangeFinder &_ranger, uint8_t instance) +{ + bool result = true; + uint32_t mem_fd; + uint32_t *ctrl; + void *ram; + + mem_fd = open("/dev/mem", O_RDWR | O_SYNC); + ctrl = (uint32_t*)mmap(0, 0x1000, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, PRU0_CTRL_BASE); + ram = mmap(0, PRU0_IRAM_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, PRU0_IRAM_BASE); + + // Reset PRU 0 + *ctrl = 0; + hal.scheduler->delay(1); + + // Load firmware + FILE *file = fopen("/lib/firmware/rangefinderprutext.bin", "rb"); + if(file == NULL) + { + result = false; + } + + if(fread(ram, PRU0_IRAM_SIZE, 1, file) != 1) + { + result = false; + } + + fclose(file); + + munmap(ram, PRU0_IRAM_SIZE); + + // Map PRU RAM + ram = mmap(0, 0x1000, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, PRU0_DRAM_BASE); + close(mem_fd); + + // Start PRU 0 + *ctrl = 2; + + rangerpru = (volatile struct range*)ram; + + return result; +} + +/* + update the state of the sensor +*/ +void AP_RangeFinder_BBB_PRU::update(void) +{ + state.status = (RangeFinder::RangeFinder_Status)rangerpru->status; + state.distance_cm = rangerpru->distance; +} +#endif // CONFIG_HAL_BOARD_SUBTYPE diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h new file mode 100644 index 0000000000..c1a381c511 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.h @@ -0,0 +1,35 @@ +#ifndef __AP_RANGEFINDER_PRU_H__ +#define __AP_RANGEFINDER_PRU_H__ + +#include "RangeFinder.h" +#include "RangeFinder_Backend.h" + + +#define PRU0_CTRL_BASE 0x4a322000 + +#define PRU0_IRAM_BASE 0x4a334000 +#define PRU0_IRAM_SIZE 0x2000 + +#define PRU0_DRAM_BASE 0x4a300000 + +struct range { + uint32_t distance; + uint32_t status; +}; + +class AP_RangeFinder_BBB_PRU : public AP_RangeFinder_Backend +{ +public: + // constructor + AP_RangeFinder_BBB_PRU(RangeFinder &ranger, uint8_t instance, RangeFinder::RangeFinder_State &_state); + + // static detection function + static bool detect(RangeFinder &ranger, uint8_t instance); + + // update state + void update(void); + +private: + +}; +#endif // __AP_RANGEFINDER_PRU_H__ diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 1b30a22e20..3fd1da2c6c 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -20,13 +20,14 @@ #include "AP_RangeFinder_MaxsonarI2CXL.h" #include "AP_RangeFinder_PX4.h" #include "AP_RangeFinder_PX4_PWM.h" +#include "AP_RangeFinder_BBB_PRU.h" // table of user settable parameters const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = { // @Param: _TYPE // @DisplayName: Rangefinder type // @Description: What type of rangefinder device that is connected - // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,4:PX4-I2C,5:PX4-PWM + // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,4:PX4-I2C,5:PX4-PWM,6:BBB-PRU AP_GROUPINFO("_TYPE", 0, RangeFinder, _type[0], 0), // @Param: _PIN @@ -110,7 +111,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] PROGMEM = { // @Param: 2_TYPE // @DisplayName: Second Rangefinder type // @Description: What type of rangefinder device that is connected - // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,4:PX4-I2C,5:PX4-PWM + // @Values: 0:None,1:Analog,2:APM2-MaxbotixI2C,3:APM2-PulsedLightI2C,4:PX4-I2C,5:PX4-PWM,6:BBB-PRU AP_GROUPINFO("2_TYPE", 12, RangeFinder, _type[1], 0), // @Param: 2_PIN @@ -295,6 +296,15 @@ void RangeFinder::detect_instance(uint8_t instance) return; } } +#endif +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI + if (type == RangeFinder_TYPE_BBB_PRU) { + if (AP_RangeFinder_BBB_PRU::detect(*this, instance)) { + state[instance].instance = instance; + drivers[instance] = new AP_RangeFinder_BBB_PRU(*this, instance, state[instance]); + return; + } + } #endif if (type == RangeFinder_TYPE_ANALOG) { // note that analog must be the last to be checked, as it will diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 4e7c78a881..19eb4d6d25 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -44,7 +44,8 @@ public: RangeFinder_TYPE_MBI2C = 2, RangeFinder_TYPE_PLI2C = 3, RangeFinder_TYPE_PX4 = 4, - RangeFinder_TYPE_PX4_PWM= 5 + RangeFinder_TYPE_PX4_PWM= 5, + RangeFinder_TYPE_BBB_PRU= 6 }; enum RangeFinder_Function {