576 lines
16 KiB
C
576 lines
16 KiB
C
/****************************************************************************
|
|
* drivers/sensors/wtgahrs2_uorb.c
|
|
* Driver for the Wit-Motion WTGAHRS2 accelerometer, gyroscope, magnetic,
|
|
* angle, barometer, temperature, GNSS sensors by serial interface with host
|
|
*
|
|
* Licensed to the Apache Software Foundation (ASF) under one or more
|
|
* contributor license agreements. See the NOTICE file distributed with
|
|
* this work for additional information regarding copyright ownership. The
|
|
* ASF licenses this file to you under the Apache License, Version 2.0 (the
|
|
* "License"); you may not use this file except in compliance with the
|
|
* License. You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
|
|
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
|
|
* License for the specific language governing permissions and limitations
|
|
* under the License.
|
|
*
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Included Files
|
|
****************************************************************************/
|
|
|
|
#include <nuttx/config.h>
|
|
|
|
#include <nuttx/sensors/wtgahrs2.h>
|
|
#include <nuttx/kthread.h>
|
|
#include <nuttx/kmalloc.h>
|
|
#include <nuttx/signal.h>
|
|
|
|
#include <sys/param.h>
|
|
|
|
#include <termios.h>
|
|
#include <math.h>
|
|
#include <fcntl.h>
|
|
#include <stdio.h>
|
|
#include <debug.h>
|
|
|
|
/****************************************************************************
|
|
* Pre-processor Definitions
|
|
****************************************************************************/
|
|
|
|
#define WTGAHRS2_ACCEL_IDX 0
|
|
#define WTGAHRS2_GYRO_IDX 1
|
|
#define WTGAHRS2_MAG_IDX 2
|
|
#define WTGAHRS2_BARO_IDX 3
|
|
#define WTGAHRS2_GNSS_IDX 4
|
|
#define WTGAHRS2_MAX_IDX 5
|
|
|
|
#define WTGAHRS2_GNSS0_MASK (1 << 0) /* Time */
|
|
#define WTGAHRS2_GNSS1_MASK (1 << 1) /* Longitude, Latitude */
|
|
#define WTGAHRS2_GNSS2_MASK (1 << 2) /* Ground speed, Height, Yaw */
|
|
#define WTGAHRS2_GNSS_MASK (7 << 0)
|
|
|
|
#define WTGAHRS2_GNSS0_INFO 0x50
|
|
#define WTGAHRS2_ACCEL_INFO 0x51
|
|
#define WTGAHRS2_GYRO_INFO 0x52
|
|
#define WTGAHRS2_MAG_INFO 0x54
|
|
#define WTGAHRS2_BARO_INFO 0x56
|
|
#define WTGAHRS2_GNSS1_INFO 0x57
|
|
#define WTGAHRS2_GNSS2_INFO 0x58
|
|
|
|
#define WTGAHRS2_RSP_HEADER 0x55
|
|
#define WTGAHRS2_RSP_LENGTH 11
|
|
#define WTGAHRS2_CMD_LENGTH 5
|
|
|
|
/****************************************************************************
|
|
* Private Types
|
|
****************************************************************************/
|
|
|
|
struct wtgahrs2_sensor_s
|
|
{
|
|
struct sensor_lowerhalf_s lower;
|
|
uint32_t interval;
|
|
uint64_t last_update;
|
|
bool enable;
|
|
};
|
|
|
|
struct wtgahrs2_dev_s
|
|
{
|
|
struct wtgahrs2_sensor_s dev[WTGAHRS2_MAX_IDX];
|
|
struct file file;
|
|
|
|
struct sensor_gnss gnss;
|
|
unsigned char gnss_mask;
|
|
};
|
|
|
|
/****************************************************************************
|
|
* Private
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Private Function Prototypes
|
|
****************************************************************************/
|
|
|
|
static int wtgahrs2_activate(FAR struct sensor_lowerhalf_s *lower,
|
|
FAR struct file *filep, bool sw);
|
|
static int wtgahrs2_set_interval(FAR struct sensor_lowerhalf_s *lower,
|
|
FAR struct file *filep,
|
|
FAR uint32_t *interval);
|
|
|
|
/****************************************************************************
|
|
* Private Data
|
|
****************************************************************************/
|
|
|
|
/* in microseconds */
|
|
|
|
static const uint32_t g_wtgahrs2_interval[] =
|
|
{
|
|
10000000, /* 0.1 hz */
|
|
2000000, /* 0.5 hz */
|
|
1000000, /* 1 hz */
|
|
500000, /* 2 hz */
|
|
200000, /* 5 hz */
|
|
100000, /* 10 hz */
|
|
50000, /* 20 hz */
|
|
20000, /* 50 hz */
|
|
10000, /* 100 hz */
|
|
5000, /* 200 hz */
|
|
};
|
|
|
|
static const uint8_t g_wtgahrs2_unlock[] =
|
|
{
|
|
0xff, 0xaa, 0x69, 0x88, 0xb5
|
|
};
|
|
|
|
static const uint8_t g_wtgahrs2_odr_200hz[] =
|
|
{
|
|
0xff, 0xaa, 0x03, 0x0b, 0x00
|
|
};
|
|
|
|
static const uint8_t g_wtgahrs2_enable_sensor[] =
|
|
{
|
|
0xff, 0xaa, 0x02, 0xd7, 0x05
|
|
};
|
|
|
|
static const struct sensor_ops_s g_wtgahrs2_ops =
|
|
{
|
|
.activate = wtgahrs2_activate,
|
|
.set_interval = wtgahrs2_set_interval,
|
|
.batch = NULL,
|
|
};
|
|
|
|
/****************************************************************************
|
|
* Private Functions
|
|
****************************************************************************/
|
|
|
|
static void wtgahrs2_sendcmd(FAR struct wtgahrs2_dev_s *rtdata,
|
|
const void *cmd)
|
|
{
|
|
file_write(&rtdata->file, cmd, WTGAHRS2_CMD_LENGTH);
|
|
nxsig_usleep(10000);
|
|
}
|
|
|
|
static int wtgahrs2_activate(FAR struct sensor_lowerhalf_s *lower,
|
|
FAR struct file *filep, bool sw)
|
|
{
|
|
FAR struct wtgahrs2_sensor_s *dev = (FAR struct wtgahrs2_sensor_s *)lower;
|
|
dev->enable = sw;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int wtgahrs2_set_interval(FAR struct sensor_lowerhalf_s *lower,
|
|
FAR struct file *filep,
|
|
FAR uint32_t *interval)
|
|
{
|
|
FAR struct wtgahrs2_sensor_s *dev = (FAR struct wtgahrs2_sensor_s *)lower;
|
|
int idx = 0;
|
|
|
|
for (; idx < nitems(g_wtgahrs2_interval) - 1; idx++)
|
|
{
|
|
if (*interval >= g_wtgahrs2_interval[idx])
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
*interval = g_wtgahrs2_interval[idx];
|
|
dev->interval = *interval;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static void wtgahrs2_accel_data(FAR struct wtgahrs2_dev_s *rtdata,
|
|
FAR unsigned char *buffer)
|
|
{
|
|
FAR struct wtgahrs2_sensor_s *dev = &rtdata->dev[WTGAHRS2_ACCEL_IDX];
|
|
FAR struct sensor_lowerhalf_s *lower = &dev->lower;
|
|
uint64_t now = sensor_get_timestamp();
|
|
struct sensor_accel accel;
|
|
|
|
if (!dev->enable || now - dev->last_update < dev->interval)
|
|
{
|
|
return;
|
|
}
|
|
|
|
dev->last_update = now;
|
|
|
|
accel.timestamp = now;
|
|
accel.x = (short)(buffer[1] << 8 | buffer[0]) * (16 * 9.8f / 32768);
|
|
accel.y = (short)(buffer[3] << 8 | buffer[2]) * (16 * 9.8f / 32768);
|
|
accel.z = (short)(buffer[5] << 8 | buffer[4]) * (16 * 9.8f / 32768);
|
|
accel.temperature = (short)(buffer[7] << 8 | buffer[6]) / 100.0f;
|
|
|
|
lower->push_event(lower->priv, &accel, sizeof(accel));
|
|
sninfo("Accel: %.3fm/s^2 %.3fm/s^2 %.3fm/s^2, t:%.1f\n",
|
|
accel.x, accel.y, accel.z, accel.temperature);
|
|
}
|
|
|
|
static void wtgahrs2_gyro_data(FAR struct wtgahrs2_dev_s *rtdata,
|
|
FAR unsigned char *buffer)
|
|
{
|
|
FAR struct wtgahrs2_sensor_s *dev = &rtdata->dev[WTGAHRS2_GYRO_IDX];
|
|
FAR struct sensor_lowerhalf_s *lower = &dev->lower;
|
|
uint64_t now = sensor_get_timestamp();
|
|
struct sensor_gyro gyro;
|
|
|
|
if (!dev->enable || now - dev->last_update < dev->interval)
|
|
{
|
|
return;
|
|
}
|
|
|
|
dev->last_update = now;
|
|
|
|
gyro.timestamp = now;
|
|
gyro.x = (short)(buffer[1] << 8 | buffer[0]) * (2000 * M_PI / 180 / 32768);
|
|
gyro.y = (short)(buffer[3] << 8 | buffer[2]) * (2000 * M_PI / 180 / 32768);
|
|
gyro.z = (short)(buffer[5] << 8 | buffer[4]) * (2000 * M_PI / 180 / 32768);
|
|
gyro.temperature = (short)(buffer[7] << 8 | buffer[6]) / 100.0f;
|
|
|
|
lower->push_event(lower->priv, &gyro, sizeof(gyro));
|
|
sninfo("Gyro: %.3frad/s %.3frad/s %.3frad/s, t:%.1f\n",
|
|
gyro.x, gyro.y, gyro.z, gyro.temperature);
|
|
}
|
|
|
|
static void wtgahrs2_mag_data(FAR struct wtgahrs2_dev_s *rtdata,
|
|
FAR unsigned char *buffer)
|
|
{
|
|
FAR struct wtgahrs2_sensor_s *dev = &rtdata->dev[WTGAHRS2_MAG_IDX];
|
|
FAR struct sensor_lowerhalf_s *lower = &dev->lower;
|
|
uint64_t now = sensor_get_timestamp();
|
|
struct sensor_mag mag;
|
|
|
|
if (!dev->enable || now - dev->last_update < dev->interval)
|
|
{
|
|
return;
|
|
}
|
|
|
|
dev->last_update = now;
|
|
|
|
mag.timestamp = now;
|
|
mag.x = (short)(buffer[1] << 8 | buffer[0]) * 0.16f;
|
|
mag.y = (short)(buffer[3] << 8 | buffer[2]) * 0.16f;
|
|
mag.z = (short)(buffer[5] << 8 | buffer[4]) * 0.16f;
|
|
mag.temperature = (short)(buffer[7] << 8 | buffer[6]) / 100.0f;
|
|
|
|
lower->push_event(lower->priv, &mag, sizeof(mag));
|
|
sninfo("Mag: %.3fuT %.3fuT %.3fuT, t:%.1f\n",
|
|
mag.x, mag.y, mag.z, mag.temperature);
|
|
}
|
|
|
|
static void wtgahrs2_baro_data(FAR struct wtgahrs2_dev_s *rtdata,
|
|
FAR unsigned char *buffer)
|
|
{
|
|
FAR struct wtgahrs2_sensor_s *dev = &rtdata->dev[WTGAHRS2_BARO_IDX];
|
|
FAR struct sensor_lowerhalf_s *lower = &dev->lower;
|
|
uint64_t now = sensor_get_timestamp();
|
|
struct sensor_baro baro;
|
|
|
|
if (!dev->enable || now - dev->last_update < dev->interval)
|
|
{
|
|
return;
|
|
}
|
|
|
|
dev->last_update = now;
|
|
|
|
baro.timestamp = now;
|
|
baro.pressure = (long)(buffer[3] << 24 | buffer[2] << 16 |
|
|
buffer[1] << 8 | buffer[0]) / 100.0f;
|
|
baro.temperature = NAN;
|
|
|
|
lower->push_event(lower->priv, &baro, sizeof(baro));
|
|
sninfo("Pressure : %.3fhPa\n", baro.pressure);
|
|
}
|
|
|
|
static void wtgahrs2_gnss_data(FAR struct wtgahrs2_dev_s *rtdata,
|
|
FAR unsigned char *buffer, int info_type)
|
|
{
|
|
FAR struct wtgahrs2_sensor_s *dev = &rtdata->dev[WTGAHRS2_GNSS_IDX];
|
|
FAR struct sensor_lowerhalf_s *lower = &dev->lower;
|
|
uint64_t now = sensor_get_timestamp();
|
|
|
|
if (!dev->enable || now - dev->last_update < dev->interval)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (rtdata->gnss_mask == 0)
|
|
{
|
|
dev->last_update = now;
|
|
}
|
|
|
|
switch (info_type)
|
|
{
|
|
case WTGAHRS2_GNSS0_INFO:
|
|
rtdata->gnss_mask |= WTGAHRS2_GNSS0_MASK;
|
|
break;
|
|
|
|
case WTGAHRS2_GNSS1_INFO:
|
|
rtdata->gnss_mask |= WTGAHRS2_GNSS1_MASK;
|
|
rtdata->gnss.longitude = (buffer[3] << 8
|
|
| buffer[2] << 8
|
|
| buffer[1] << 8
|
|
| buffer[0]) / 10000000.0f;
|
|
rtdata->gnss.latitude = (buffer[7] << 8
|
|
| buffer[6] << 8
|
|
| buffer[5] << 8
|
|
| buffer[4]) / 10000000.0f;
|
|
break;
|
|
|
|
case WTGAHRS2_GNSS2_INFO:
|
|
rtdata->gnss_mask |= WTGAHRS2_GNSS2_MASK;
|
|
rtdata->gnss.altitude = (float)(buffer[1] << 8 | buffer[0]) / 10.0f;
|
|
rtdata->gnss.ground_speed = (float)(buffer[7] << 8 | buffer[6] << 8
|
|
| buffer[5] << 8 | buffer[4]) / 3600.0f;
|
|
break;
|
|
}
|
|
|
|
if (rtdata->gnss_mask == WTGAHRS2_GNSS_MASK)
|
|
{
|
|
rtdata->gnss_mask = 0;
|
|
lower->push_event(lower->priv, &rtdata->gnss, sizeof(rtdata->gnss));
|
|
sninfo("Time : %" PRIu64 " utc_time: %" PRIu64 "\n",
|
|
rtdata->gnss.timestamp, rtdata->gnss.time_utc);
|
|
sninfo("GNSS longitude : %fdegree, latitude:%fdegree\n",
|
|
rtdata->gnss.longitude, rtdata->gnss.latitude);
|
|
sninfo("GNSS speed: %fm/s, altitude: %fm\n",
|
|
rtdata->gnss.ground_speed, rtdata->gnss.altitude);
|
|
}
|
|
}
|
|
|
|
static bool wtgahrs2_process_data(FAR struct wtgahrs2_dev_s *rtdata,
|
|
FAR unsigned char *buffer)
|
|
{
|
|
unsigned char sum = 0;
|
|
int i;
|
|
|
|
/* calculate sum and verify checksum */
|
|
|
|
for (i = 0; i < WTGAHRS2_RSP_LENGTH - 1; i++)
|
|
{
|
|
sum += buffer[i];
|
|
}
|
|
|
|
if (sum != buffer[WTGAHRS2_RSP_LENGTH - 1])
|
|
{
|
|
return false;
|
|
}
|
|
|
|
switch (buffer[1])
|
|
{
|
|
case WTGAHRS2_ACCEL_INFO:
|
|
wtgahrs2_accel_data(rtdata, &buffer[2]);
|
|
break;
|
|
|
|
case WTGAHRS2_GYRO_INFO:
|
|
wtgahrs2_gyro_data(rtdata, &buffer[2]);
|
|
break;
|
|
|
|
case WTGAHRS2_MAG_INFO:
|
|
wtgahrs2_mag_data(rtdata, &buffer[2]);
|
|
break;
|
|
|
|
case WTGAHRS2_BARO_INFO:
|
|
wtgahrs2_baro_data(rtdata, &buffer[2]);
|
|
break;
|
|
|
|
case WTGAHRS2_GNSS0_INFO:
|
|
case WTGAHRS2_GNSS1_INFO:
|
|
case WTGAHRS2_GNSS2_INFO:
|
|
wtgahrs2_gnss_data(rtdata, &buffer[2], buffer[1]);
|
|
break;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static int wtgahrs2_thread(int argc, FAR char *argv[])
|
|
{
|
|
FAR struct wtgahrs2_dev_s *rtdata = (FAR struct wtgahrs2_dev_s *)
|
|
(uintptr_t)strtoul(argv[1], NULL, 16);
|
|
unsigned char buffer[8 * WTGAHRS2_RSP_LENGTH];
|
|
ssize_t count = 0;
|
|
ssize_t pos;
|
|
|
|
while (1)
|
|
{
|
|
count += file_read(&rtdata->file, buffer + count,
|
|
sizeof(buffer) - count);
|
|
for (pos = 0; pos < count; pos++)
|
|
{
|
|
if (buffer[pos] != WTGAHRS2_RSP_HEADER)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if (count - pos < WTGAHRS2_RSP_LENGTH)
|
|
{
|
|
memmove(buffer, buffer + pos, count - pos);
|
|
break;
|
|
}
|
|
|
|
if (wtgahrs2_process_data(rtdata, &buffer[pos]))
|
|
{
|
|
pos += WTGAHRS2_RSP_LENGTH - 1;
|
|
}
|
|
}
|
|
|
|
count -= pos;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Public Functions
|
|
****************************************************************************/
|
|
|
|
int wtgahrs2_initialize(FAR const char *path, int devno)
|
|
{
|
|
FAR struct wtgahrs2_dev_s *rtdata;
|
|
FAR struct wtgahrs2_sensor_s *tmp;
|
|
struct termios opt;
|
|
FAR char *argv[2];
|
|
char arg1[32];
|
|
int ret;
|
|
|
|
if (!path)
|
|
{
|
|
snerr("Invalid path for serial interface\n");
|
|
return -EINVAL;
|
|
}
|
|
|
|
rtdata = kmm_zalloc(sizeof(struct wtgahrs2_dev_s));
|
|
if (!rtdata)
|
|
{
|
|
snerr("Memory cannot be allocated for wtgahrs2\n");
|
|
return -ENOMEM;
|
|
}
|
|
|
|
/* Open serial tty port and set baud rate */
|
|
|
|
ret = file_open(&rtdata->file, path, O_RDWR | O_CLOEXEC);
|
|
if (ret < 0)
|
|
{
|
|
snerr("Failed to open wtgahrs2 serial:%s, err:%d", path, ret);
|
|
goto open_err;
|
|
}
|
|
|
|
file_ioctl(&rtdata->file, TCGETS, &opt);
|
|
cfmakeraw(&opt);
|
|
|
|
#ifdef CONFIG_SERIAL_TERMIOS
|
|
cfsetispeed(&opt, B115200);
|
|
cfsetospeed(&opt, B115200);
|
|
#endif
|
|
|
|
file_ioctl(&rtdata->file, TCSETS, &opt);
|
|
|
|
/* Accelerometer register */
|
|
|
|
tmp = &rtdata->dev[WTGAHRS2_ACCEL_IDX];
|
|
tmp->lower.ops = &g_wtgahrs2_ops;
|
|
tmp->lower.type = SENSOR_TYPE_ACCELEROMETER;
|
|
tmp->lower.nbuffer = 1;
|
|
ret = sensor_register(&tmp->lower, devno);
|
|
if (ret < 0)
|
|
{
|
|
goto accel_err;
|
|
}
|
|
|
|
/* Gyroscope register */
|
|
|
|
tmp = &rtdata->dev[WTGAHRS2_GYRO_IDX];
|
|
tmp->lower.ops = &g_wtgahrs2_ops;
|
|
tmp->lower.type = SENSOR_TYPE_GYROSCOPE;
|
|
tmp->lower.nbuffer = 1;
|
|
ret = sensor_register(&tmp->lower, devno);
|
|
if (ret < 0)
|
|
{
|
|
goto gyro_err;
|
|
}
|
|
|
|
/* Magnetic register */
|
|
|
|
tmp = &rtdata->dev[WTGAHRS2_MAG_IDX];
|
|
tmp->lower.ops = &g_wtgahrs2_ops;
|
|
tmp->lower.type = SENSOR_TYPE_MAGNETIC_FIELD;
|
|
tmp->lower.nbuffer = 1;
|
|
ret = sensor_register(&tmp->lower, devno);
|
|
if (ret < 0)
|
|
{
|
|
goto mag_err;
|
|
}
|
|
|
|
/* Barometer register */
|
|
|
|
tmp = &rtdata->dev[WTGAHRS2_BARO_IDX];
|
|
tmp->lower.ops = &g_wtgahrs2_ops;
|
|
tmp->lower.type = SENSOR_TYPE_BAROMETER;
|
|
tmp->lower.nbuffer = 1;
|
|
ret = sensor_register(&tmp->lower, devno);
|
|
if (ret < 0)
|
|
{
|
|
goto baro_err;
|
|
}
|
|
|
|
/* GNSS register */
|
|
|
|
tmp = &rtdata->dev[WTGAHRS2_GNSS_IDX];
|
|
tmp->lower.ops = &g_wtgahrs2_ops;
|
|
tmp->lower.type = SENSOR_TYPE_GNSS;
|
|
tmp->lower.nbuffer = 1;
|
|
ret = sensor_register(&tmp->lower, devno);
|
|
if (ret < 0)
|
|
{
|
|
goto gnss_err;
|
|
}
|
|
|
|
/* Set sensor default attributes and enter unlock mode */
|
|
|
|
wtgahrs2_sendcmd(rtdata, g_wtgahrs2_unlock);
|
|
|
|
/* Set sensor default odr 200hz */
|
|
|
|
wtgahrs2_sendcmd(rtdata, g_wtgahrs2_odr_200hz);
|
|
|
|
/* Enable all sensor */
|
|
|
|
wtgahrs2_sendcmd(rtdata, g_wtgahrs2_enable_sensor);
|
|
|
|
snprintf(arg1, sizeof(arg1), "%p", rtdata);
|
|
argv[0] = arg1;
|
|
argv[1] = NULL;
|
|
|
|
ret = kthread_create("wtgahrs2_thread", SCHED_PRIORITY_DEFAULT,
|
|
CONFIG_DEFAULT_TASK_STACKSIZE,
|
|
wtgahrs2_thread, argv);
|
|
if (ret < 0)
|
|
{
|
|
goto thr_err;
|
|
}
|
|
|
|
return ret;
|
|
|
|
thr_err:
|
|
sensor_unregister(&rtdata->dev[WTGAHRS2_GNSS_IDX].lower, devno);
|
|
gnss_err:
|
|
sensor_unregister(&rtdata->dev[WTGAHRS2_BARO_IDX].lower, devno);
|
|
baro_err:
|
|
sensor_unregister(&rtdata->dev[WTGAHRS2_MAG_IDX].lower, devno);
|
|
mag_err:
|
|
sensor_unregister(&rtdata->dev[WTGAHRS2_GYRO_IDX].lower, devno);
|
|
gyro_err:
|
|
sensor_unregister(&rtdata->dev[WTGAHRS2_ACCEL_IDX].lower, devno);
|
|
accel_err:
|
|
file_close(&rtdata->file);
|
|
open_err:
|
|
kmm_free(rtdata);
|
|
return ret;
|
|
}
|