mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 01:18:29 -04:00
490841a814
By opening with O_CLOEXEC we make sure we don't leak the file descriptor when we are exec'ing or calling out subprograms. Right now we currently don't do it so there's no harm, but it's good practice in Linux to have it.
395 lines
10 KiB
C++
395 lines
10 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/>.
|
|
*/
|
|
/*
|
|
This is an implementation of all of the code for the QFLIGHT board
|
|
that runs on the DSPs. See qflight_dsp.idl for the interface
|
|
definition for the RPC calls
|
|
*/
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include "qflight_dsp.h"
|
|
extern "C" {
|
|
#include "bmp280_api.h"
|
|
#include "mpu9x50.h"
|
|
}
|
|
|
|
#include <types.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <stdarg.h>
|
|
#include <sys/timespec.h>
|
|
#include <errno.h>
|
|
#include <string.h>
|
|
#include <time.h>
|
|
#include <dspal_time.h>
|
|
#include <sys/stat.h>
|
|
#include <sys/types.h>
|
|
#include <dirent.h>
|
|
#include <stdlib.h>
|
|
#include <dev_fs_lib_serial.h>
|
|
#include "qflight_buffer.h"
|
|
#include <AP_HAL/utility/RingBuffer.h>
|
|
|
|
const float GRAVITY_MSS = 9.80665;
|
|
const float ACCEL_SCALE_1G = GRAVITY_MSS / 2048.0;
|
|
const float GYRO_SCALE = 0.0174532 / 16.4;
|
|
const float RAD_TO_DEG = 57.295779513082320876798154814105;
|
|
|
|
static ObjectBuffer<DSPBuffer::IMU::BUF> imu_buffer(30);
|
|
static ObjectBuffer<DSPBuffer::MAG::BUF> mag_buffer(10);
|
|
static ObjectBuffer<DSPBuffer::BARO::BUF> baro_buffer(10);
|
|
static bool mpu9250_started;
|
|
static uint32_t bmp280_handle;
|
|
static uint32_t baro_counter;
|
|
|
|
/*
|
|
read buffering for UARTs
|
|
*/
|
|
static const uint8_t max_uarts = 8;
|
|
static uint8_t num_open_uarts;
|
|
static struct uartbuf {
|
|
int fd;
|
|
ByteBuffer *readbuffer;
|
|
} uarts[max_uarts];
|
|
|
|
extern "C" {
|
|
void HAP_debug(const char *msg, int level, const char *filename, int line);
|
|
}
|
|
|
|
void HAP_printf(const char *file, int line, const char *format, ...)
|
|
{
|
|
va_list ap;
|
|
char buf[300];
|
|
|
|
va_start(ap, format);
|
|
vsnprintf(buf, sizeof(buf), format, ap);
|
|
va_end(ap);
|
|
HAP_debug(buf, 0, file, line);
|
|
}
|
|
|
|
void HAP_printf(const char *file, int line, const char *format, ...);
|
|
#define HAP_PRINTF(...) HAP_printf(__FILE__, __LINE__, __VA_ARGS__)
|
|
|
|
static int init_barometer(void)
|
|
{
|
|
int ret = bmp280_open("/dev/i2c-2", &bmp280_handle);
|
|
HAP_PRINTF("**** bmp280: ret=%d handle=0x%x\n", ret, (unsigned)bmp280_handle);
|
|
return ret;
|
|
}
|
|
|
|
static int init_mpu9250(void)
|
|
{
|
|
struct mpu9x50_config config;
|
|
|
|
config.gyro_lpf = MPU9X50_GYRO_LPF_184HZ;
|
|
config.acc_lpf = MPU9X50_ACC_LPF_184HZ;
|
|
config.gyro_fsr = MPU9X50_GYRO_FSR_2000DPS;
|
|
config.acc_fsr = MPU9X50_ACC_FSR_16G;
|
|
config.gyro_sample_rate = MPU9x50_SAMPLE_RATE_1000HZ;
|
|
config.compass_enabled = true;
|
|
config.compass_sample_rate = MPU9x50_COMPASS_SAMPLE_RATE_100HZ;
|
|
config.spi_dev_path = "/dev/spi-1";
|
|
|
|
int ret;
|
|
ret = mpu9x50_validate_configuration(&config);
|
|
HAP_PRINTF("***** mpu9250 validate ret=%d\n", ret);
|
|
if (ret != 0) {
|
|
return ret;
|
|
}
|
|
ret = mpu9x50_initialize(&config);
|
|
HAP_PRINTF("***** mpu9250 initialise ret=%d\n", ret);
|
|
|
|
mpu9250_started = true;
|
|
|
|
return ret;
|
|
}
|
|
|
|
/*
|
|
thread gathering sensor data from mpu9250
|
|
*/
|
|
static void *mpu_data_ready(void *ctx)
|
|
{
|
|
struct mpu9x50_data data;
|
|
memset(&data, 0, sizeof(data));
|
|
|
|
int ret = mpu9x50_get_data(&data);
|
|
if (ret != 0) {
|
|
return nullptr;
|
|
}
|
|
DSPBuffer::IMU::BUF b;
|
|
b.timestamp = data.timestamp;
|
|
b.accel[0] = data.accel_raw[0]*ACCEL_SCALE_1G;
|
|
b.accel[1] = data.accel_raw[1]*ACCEL_SCALE_1G;
|
|
b.accel[2] = data.accel_raw[2]*ACCEL_SCALE_1G;
|
|
b.gyro[0] = data.gyro_raw[0]*GYRO_SCALE;
|
|
b.gyro[1] = data.gyro_raw[1]*GYRO_SCALE;
|
|
b.gyro[2] = data.gyro_raw[2]*GYRO_SCALE;
|
|
imu_buffer.push(b);
|
|
|
|
if (data.mag_data_ready) {
|
|
DSPBuffer::MAG::BUF m;
|
|
m.mag_raw[0] = data.mag_raw[0];
|
|
m.mag_raw[1] = data.mag_raw[1];
|
|
m.mag_raw[2] = data.mag_raw[2];
|
|
m.timestamp = data.timestamp;
|
|
mag_buffer.push(m);
|
|
}
|
|
|
|
if (bmp280_handle != 0 && baro_counter++ % 10 == 0) {
|
|
struct bmp280_sensor_data data;
|
|
memset(&data, 0, sizeof(data));
|
|
int ret = bmp280_get_sensor_data(bmp280_handle, &data, false);
|
|
if (ret == 0) {
|
|
DSPBuffer::BARO::BUF b;
|
|
b.pressure_pa = data.pressure_in_pa;
|
|
b.temperature_C = data.temperature_in_c;
|
|
b.timestamp = data.last_read_time_in_usecs;
|
|
baro_buffer.push(b);
|
|
}
|
|
}
|
|
|
|
return nullptr;
|
|
}
|
|
|
|
static void mpu9250_startup(void)
|
|
{
|
|
if (!mpu9250_started) {
|
|
if (init_mpu9250() != 0) {
|
|
return;
|
|
}
|
|
mpu9x50_register_interrupt(65, mpu_data_ready, nullptr);
|
|
}
|
|
}
|
|
|
|
/*
|
|
get any available IMU data
|
|
*/
|
|
int qflight_get_imu_data(uint8_t *buf, int len)
|
|
{
|
|
DSPBuffer::IMU &imu = *(DSPBuffer::IMU *)buf;
|
|
|
|
if (len != sizeof(imu)) {
|
|
HAP_PRINTF("incorrect size for imu data %d should be %d\n",
|
|
len, sizeof(imu));
|
|
return 1;
|
|
}
|
|
|
|
mpu9250_startup();
|
|
|
|
imu.num_samples = 0;
|
|
while (imu.num_samples < imu.max_samples &&
|
|
imu_buffer.pop(imu.buf[imu.num_samples])) {
|
|
imu.num_samples++;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
get any available mag data
|
|
*/
|
|
int qflight_get_mag_data(uint8_t *buf, int len)
|
|
{
|
|
DSPBuffer::MAG &mag = *(DSPBuffer::MAG *)buf;
|
|
|
|
if (len != sizeof(mag)) {
|
|
HAP_PRINTF("incorrect size for mag data %d should be %d\n",
|
|
len, sizeof(mag));
|
|
return 1;
|
|
}
|
|
|
|
mpu9250_startup();
|
|
|
|
mag.num_samples = 0;
|
|
while (mag.num_samples < mag.max_samples &&
|
|
mag_buffer.pop(mag.buf[mag.num_samples])) {
|
|
mag.num_samples++;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
/*
|
|
get any available baro data
|
|
*/
|
|
int qflight_get_baro_data(uint8_t *buf, int len)
|
|
{
|
|
DSPBuffer::BARO &baro = *(DSPBuffer::BARO *)buf;
|
|
|
|
if (len != sizeof(baro)) {
|
|
HAP_PRINTF("incorrect size for baro data %d should be %d\n",
|
|
len, sizeof(baro));
|
|
return 1;
|
|
}
|
|
|
|
mpu9250_startup();
|
|
|
|
if (bmp280_handle == 0) {
|
|
if (init_barometer() != 0) {
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
baro.num_samples = 0;
|
|
while (baro.num_samples < baro.max_samples &&
|
|
baro_buffer.pop(baro.buf[baro.num_samples])) {
|
|
baro.num_samples++;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
extern "C" {
|
|
static void read_callback_trampoline(void *, char *, size_t );
|
|
}
|
|
|
|
static void read_callback_trampoline(void *ctx, char *buf, size_t size)
|
|
{
|
|
if (size > 0) {
|
|
((ByteBuffer *)ctx)->write((const uint8_t *)buf, size);
|
|
}
|
|
}
|
|
|
|
/*
|
|
open a UART
|
|
*/
|
|
int qflight_UART_open(const char *device, int32_t *_fd)
|
|
{
|
|
if (num_open_uarts == max_uarts) {
|
|
return -1;
|
|
}
|
|
struct uartbuf &b = uarts[num_open_uarts];
|
|
int fd = open(device, O_RDWR | O_NONBLOCK|O_CLOEXEC);
|
|
if (fd == -1) {
|
|
return -1;
|
|
}
|
|
b.fd = fd;
|
|
b.readbuffer = new ByteBuffer(16384);
|
|
|
|
struct dspal_serial_open_options options;
|
|
options.bit_rate = DSPAL_SIO_BITRATE_57600;
|
|
options.tx_flow = DSPAL_SIO_FCTL_OFF;
|
|
options.rx_flow = DSPAL_SIO_FCTL_OFF;
|
|
options.rx_data_callback = nullptr;
|
|
options.tx_data_callback = nullptr;
|
|
options.is_tx_data_synchronous = false;
|
|
int ret = ioctl(fd, SERIAL_IOCTL_OPEN_OPTIONS, (void *)&options);
|
|
if (ret != 0) {
|
|
HAP_PRINTF("Failed to setup UART flow control options");
|
|
}
|
|
|
|
struct dspal_serial_ioctl_receive_data_callback callback {};
|
|
callback.context = b.readbuffer;
|
|
callback.rx_data_callback_func_ptr = read_callback_trampoline;
|
|
ret = ioctl(fd, SERIAL_IOCTL_SET_RECEIVE_DATA_CALLBACK, (void *)&callback);
|
|
if (ret != 0) {
|
|
HAP_PRINTF("Failed to setup UART read trampoline");
|
|
delete b.readbuffer;
|
|
close(fd);
|
|
return -1;
|
|
}
|
|
|
|
HAP_PRINTF("UART open %s fd=%d num_open=%u",
|
|
device, fd, num_open_uarts);
|
|
num_open_uarts++;
|
|
*_fd = fd;
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
close a UART
|
|
*/
|
|
int qflight_UART_close(int32_t fd)
|
|
{
|
|
uint8_t i;
|
|
for (i=0; i<num_open_uarts; i++) {
|
|
if (fd == uarts[i].fd) break;
|
|
}
|
|
if (i == num_open_uarts) {
|
|
return -1;
|
|
}
|
|
close(fd);
|
|
delete uarts[i].readbuffer;
|
|
if (i < num_open_uarts-1) {
|
|
memmove(&uarts[i], &uarts[i+1], ((num_open_uarts-1)-i)*sizeof(uarts[0]));
|
|
}
|
|
num_open_uarts--;
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
read from a UART
|
|
*/
|
|
int qflight_UART_read(int32_t fd, uint8_t *buf, int size, int32_t *nread)
|
|
{
|
|
uint8_t i;
|
|
for (i=0; i<num_open_uarts; i++) {
|
|
if (fd == uarts[i].fd) break;
|
|
}
|
|
if (i == num_open_uarts) {
|
|
return -1;
|
|
}
|
|
*nread = uarts[i].readbuffer->read(buf, size);
|
|
return 0;
|
|
}
|
|
|
|
/*
|
|
write to a UART
|
|
*/
|
|
int qflight_UART_write(int32_t fd, const uint8_t *buf, int size, int32_t *nwritten)
|
|
{
|
|
*nwritten = write(fd, buf, size);
|
|
return 0;
|
|
}
|
|
|
|
static const struct {
|
|
uint32_t baudrate;
|
|
enum DSPAL_SERIAL_BITRATES arg;
|
|
} baudrate_table[] = {
|
|
{ 9600, DSPAL_SIO_BITRATE_9600 },
|
|
{ 14400, DSPAL_SIO_BITRATE_14400 },
|
|
{ 19200, DSPAL_SIO_BITRATE_19200 },
|
|
{ 38400, DSPAL_SIO_BITRATE_38400 },
|
|
{ 57600, DSPAL_SIO_BITRATE_57600 },
|
|
{ 76800, DSPAL_SIO_BITRATE_76800 },
|
|
{ 115200, DSPAL_SIO_BITRATE_115200 },
|
|
{ 230400, DSPAL_SIO_BITRATE_230400 },
|
|
{ 250000, DSPAL_SIO_BITRATE_250000 },
|
|
{ 460800, DSPAL_SIO_BITRATE_460800 },
|
|
{ 921600, DSPAL_SIO_BITRATE_921600 },
|
|
{ 2000000, DSPAL_SIO_BITRATE_2000000 },
|
|
};
|
|
|
|
/*
|
|
set UART baudrate
|
|
*/
|
|
int qflight_UART_set_baudrate(int32_t fd, uint32_t baudrate)
|
|
{
|
|
for (uint8_t i=0; i<sizeof(baudrate_table)/sizeof(baudrate_table[0]); i++) {
|
|
if (baudrate <= baudrate_table[i].baudrate) {
|
|
struct dspal_serial_ioctl_data_rate rate {};
|
|
rate.bit_rate = baudrate_table[i].arg;
|
|
int ret = ioctl(fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&rate);
|
|
HAP_PRINTF("set_rate -> %d\n", ret);
|
|
return 0;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|