zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
22243 Commits
10 Branches
0 Tags
227 MiB
 
 
 
 
 
 
Tree: 45c6b750f2
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '45c6b750f2'
${ noResults }
zr-v4/libraries/AP_HAL_Linux/qflight/dsp_functions.cpp

395 lines
10 KiB
Raw Blame History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
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 NULL;
}
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 NULL;
}
static void mpu9250_startup(void)
{
if (!mpu9250_started) {
if (init_mpu9250() != 0) {
return;
}
mpu9x50_register_interrupt(65, mpu_data_ready, NULL);
}
}
/*
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);
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;
}