mirror of https://github.com/ArduPilot/ardupilot
394 lines
10 KiB
C++
394 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 <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;
|
|
}
|