ardupilot/libraries/AP_RangeFinder/AP_RangeFinder_BBB_PRU.cpp

118 lines
3.2 KiB
C++

/*
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 <http://www.gnu.org/licenses/>.
HC-SR04 Ultrasonic Distance Sensor connected to BeagleBone Black
by Mirko Denecke <mirkix@gmail.com>
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
#include "AP_RangeFinder_BBB_PRU.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>
#include <sys/types.h>
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::RangeFinder_State &_state, AP_RangeFinder_Params &_params) :
AP_RangeFinder_Backend(_state, _params)
{
}
/*
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()
{
bool result = true;
uint32_t mem_fd;
uint32_t *ctrl;
void *ram;
mem_fd = open("/dev/mem", O_RDWR | O_SYNC | O_CLOEXEC);
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 (.text)
FILE *file = fopen("/lib/firmware/rangefinderprutext.bin", "rb");
if (file == nullptr) {
result = false;
}
if (fread(ram, PRU0_IRAM_SIZE, 1, file) != 1) {
result = false;
}
fclose(file);
munmap(ram, PRU0_IRAM_SIZE);
ram = mmap(0, PRU0_DRAM_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, PRU0_DRAM_BASE);
// Load firmware (.data)
file = fopen("/lib/firmware/rangefinderprudata.bin", "rb");
if (file == nullptr) {
result = false;
}
if (fread(ram, PRU0_DRAM_SIZE, 1, file) != 1) {
result = false;
}
fclose(file);
munmap(ram, PRU0_DRAM_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;
state.last_reading_ms = AP_HAL::millis();
}
#endif // CONFIG_HAL_BOARD_SUBTYPE