luchianmihai/walnux
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

drivers/sensors/l86xxx: Use uORB GNSS lower-half

Browse source 
Changes the implementation of the L86xxx driver to use the
gnss_lowerhalf driver implementation, simplifying this driver's logic.
It also fixes the `read_line` function which would early return on `\r`,
as well as adding some retries for response checking and better mutex
locking.

Signed-off-by: Matteo Golin <matteo.golin@gmail.com>
This commit is contained in:
Matteo Golin 2025-07-25 20:21:38 -04:00 • committed by Alin Jerpelea
parent a8ac255386
commit b0760dceb6
2 changed files with 84 additions and 180 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
drivers/sensors/Kconfig
View file

@ -841,7 +841,7 @@ endif # SENSORS_KXTJ9
config SENSORS_L86_XXX
bool "Quectel L86-XXX GNSS support"
default n
depends on SERIAL && STANDARD_SERIAL && UORB && GNSSUTILS_MINMEA_LIB
depends on SERIAL && STANDARD_SERIAL && UORB && SENSORS_GNSS
---help---
Enable driver support for the L86-XXX series of GNSS modules.

262
drivers/sensors/l86xxx_uorb.c
View file

@ -40,8 +40,6 @@
#include <stdbool.h>
#include <stdint.h>
#include <unistd.h>
#include <math.h>
#include <time.h>
#include <termios.h>
#include <nuttx/fs/fs.h>
@ -51,7 +49,7 @@
#include <nuttx/semaphore.h>
#include <nuttx/signal.h>
#include <nuttx/wqueue.h>
#include <nuttx/sensors/sensor.h>
#include <nuttx/sensors/gnss.h>
#include <minmea/minmea.h>
#include <nuttx/sensors/l86xxx.h>
@ -80,7 +78,8 @@
/* Helper to get array length */
#define MINMEA_MAX_LENGTH 256
#define MINMEA_MAX_LENGTH 256
#define L86_ACK_RETRIES 20
/****************************************************************************
* Private Data Types
@ -91,12 +90,11 @@
typedef struct
{
FAR struct file uart; /* UART interface */
struct sensor_lowerhalf_s lower; /* UORB lower-half */
struct gnss_lowerhalf_s lower; /* uORB GNSS lower-half */
mutex_t devlock; /* Exclusive access */
sem_t run; /* Start/stop collection thread */
bool enabled; /* If module has started */
char buffer[MINMEA_MAX_LENGTH]; /* Buffer for UART interface */
int bufbytes;
#ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS
int16_t crefs; /* Number of open references */
#endif
@ -106,13 +104,14 @@ typedef struct
* Private Function Prototypes
****************************************************************************/
static int l86xxx_control(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep, int cmd, unsigned long arg);
static int l86xxx_activate(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep, bool enable);
static int l86xxx_set_interval(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
FAR uint32_t *period_us);
static int l86xxx_control(FAR struct gnss_lowerhalf_s *lower,
FAR struct file *filep, int cmd,
unsigned long arg);
static int l86xxx_activate(FAR struct gnss_lowerhalf_s *lower,
FAR struct file *filep, bool enable);
static int l86xxx_set_interval(FAR struct gnss_lowerhalf_s *lower,
FAR struct file *filep,
FAR uint32_t *period_us);
#ifdef CONFIG_SERIAL_TERMIOS
static int set_baud_rate(l86xxx_dev_s *dev, int br);
#endif
@ -124,7 +123,7 @@ static int read_line(l86xxx_dev_s *dev);
* Private Data
****************************************************************************/
static const struct sensor_ops_s g_sensor_ops =
static const struct gnss_ops_s g_gnss_ops =
{
.control = l86xxx_control,
.activate = l86xxx_activate,
@ -237,14 +236,15 @@ static int set_baud_rate(l86xxx_dev_s *dev, int br)
static int send_command(l86xxx_dev_s *dev,
L86XXX_PMTK_COMMAND cmd, unsigned long arg)
{
char buf[50];
char buf[64];
int bw1;
int bw2;
int err;
int ret;
int i = 0;
ssize_t len;
uint8_t checksum;
nxmutex_lock(&dev->devlock);
switch (cmd)
{
case CMD_HOT_START:
@ -285,10 +285,10 @@ static int send_command(l86xxx_dev_s *dev,
}
sninfo("Sending command: %s to L86", buf);
checksum = minmea_checksum(buf);
bw2 = snprintf(buf + bw1, sizeof(buf) - bw1, "*%02X\r\n", checksum);
nxmutex_lock(&dev->devlock);
err = file_write(&dev->uart, buf, bw1 + bw2);
if (err < 0)
{
@ -338,29 +338,31 @@ static int send_command(l86xxx_dev_s *dev,
* 3 = Valid packet, action succeeded
*/
memset(buf, '\0', 50);
snprintf(buf, 50, "$PMTK001,%d", cmd);
len = snprintf(buf, sizeof(buf), "$PMTK001,%d", cmd);
sninfo("Waiting for ACK from L86...\n");
read_line(dev);
if (strncmp(buf, dev->buffer, strlen(buf)) == 0)
do
{
sninfo("ACK received!\n");
read_line(dev);
i++;
}
else
while (strncmp(buf, dev->buffer, len) != 0 && i < L86_ACK_RETRIES);
if (i >= L86_ACK_RETRIES)
{
snerr("Did not get ACK!\n");
nxmutex_unlock(&dev->devlock);
return -EIO;
}
sninfo("ACK received!\n");
ret = dev->buffer[13] - '0';
nxmutex_unlock(&dev->devlock);
/* Flag num is always in position 13 of ack, subtract by '0'
to obtain return val
*/
ret = dev->buffer[13] - '0';
if (ret == 1)
{
return -ENOSYS;
@ -395,10 +397,12 @@ static int send_command(l86xxx_dev_s *dev,
static int read_line(l86xxx_dev_s *dev)
{
memset(dev->buffer, '\0', sizeof(dev->buffer));
int line_len = 0;
int err;
char next_char;
memset(dev->buffer, '\0', sizeof(dev->buffer));
do
{
err = file_read(&dev->uart, &next_char, 1);
@ -408,13 +412,13 @@ static int read_line(l86xxx_dev_s *dev)
return err;
}
if (next_char != '\r' && next_char != '\n')
if (next_char != '\n')
{
dev->buffer[line_len++] = next_char;
}
}
while (next_char != '\r' && next_char != '\n'
&& line_len < sizeof(dev->buffer));
while (next_char != '\n' && line_len < sizeof(dev->buffer));
dev->buffer[line_len] = '\0';
return line_len;
}
@ -430,8 +434,8 @@ static int read_line(l86xxx_dev_s *dev)
* else return value from send_command
****************************************************************************/
static int l86xxx_control(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep, int cmd, unsigned long arg)
static int l86xxx_control(FAR struct gnss_lowerhalf_s *lower,
FAR struct file *filep, int cmd, unsigned long arg)
{
FAR l86xxx_dev_s *dev = container_of(lower, FAR l86xxx_dev_s, lower);
L86XXX_PMTK_COMMAND pmtk_cmd;
@ -485,8 +489,8 @@ static int l86xxx_control(FAR struct sensor_lowerhalf_s *lower,
* Return value from send_command
****************************************************************************/
static int l86xxx_activate(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep, bool enable)
static int l86xxx_activate(FAR struct gnss_lowerhalf_s *lower,
FAR struct file *filep, bool enable)
{
FAR l86xxx_dev_s *dev = container_of(lower, FAR l86xxx_dev_s, lower);
@ -520,18 +524,20 @@ static int l86xxx_activate(FAR struct sensor_lowerhalf_s *lower,
* -EINVAL if invalid interval, else return value from send_command
****************************************************************************/
static int l86xxx_set_interval(FAR struct sensor_lowerhalf_s *lower,
FAR struct file *filep,
FAR uint32_t *period_us)
static int l86xxx_set_interval(FAR struct gnss_lowerhalf_s *lower,
FAR struct file *filep,
FAR uint32_t *period_us)
{
FAR l86xxx_dev_s *dev = container_of(lower, FAR l86xxx_dev_s, lower);
int fix_interval = *period_us;
int ret;
if (fix_interval < 100 || fix_interval > 10000)
{
return -EINVAL;
}
int ret = send_command(dev, SET_POS_FIX, fix_interval);
ret = send_command(dev, SET_POS_FIX, fix_interval);
return ret;
}
@ -546,10 +552,8 @@ static int l86xxx_thread(int argc, FAR char *argv[])
{
FAR l86xxx_dev_s *dev =
(FAR l86xxx_dev_s *)((uintptr_t)strtoul(argv[1], NULL, 16));
struct sensor_gnss gps;
memset(&gps, 0, sizeof(gps));
int err;
int bw;
ssize_t bw;
/* Read full line of NMEA output */
@ -569,7 +573,9 @@ static int l86xxx_thread(int argc, FAR char *argv[])
}
}
/* Mutex required because some commands send ACKS */
/* Mutex required because some commands send ACKS, don't want those to
* interrupt reading data.
*/
err = nxmutex_lock(&dev->devlock);
if (err < 0)
@ -578,130 +584,22 @@ static int l86xxx_thread(int argc, FAR char *argv[])
return err;
}
bw = read_line(dev);
bw = file_read(&dev->uart, dev->buffer, sizeof(dev->buffer));
/* Parse line based on NMEA sentence type */
if (bw <= 0)
{
snerr("No data on UART: %d\n", bw);
nxmutex_unlock(&dev->devlock);
continue;
}
/* Send data read to the lower half for parsing. Does not need to be a
* full NMEA sentence to be handled.
*/
if (bw > 0)
{
switch (minmea_sentence_id(dev->buffer, false))
{
/* Time data is obtained from RMC sentence */
case MINMEA_SENTENCE_RMC:
{
struct minmea_sentence_rmc frame;
struct tm tm;
if (minmea_check(dev->buffer, false) &&
minmea_parse_rmc(&frame, dev->buffer))
{
gps.timestamp = sensor_get_timestamp();
minmea_getdatetime(&tm, &frame.date, &frame.time);
gps.time_utc = mktime(&tm);
}
break;
}
/* Velocity data is obtained from VTG sentence */
case MINMEA_SENTENCE_VTG:
{
struct minmea_sentence_vtg frame;
if (minmea_parse_vtg(&frame, dev->buffer))
{
gps.ground_speed = minmea_tofloat(&frame.speed_kph) * 3.6; /* Convert speed in kph to mps */
gps.course = minmea_tofloat(&frame.true_track_degrees);
}
break;
}
/* 3D positional data is obtained from GGA sentence */
case MINMEA_SENTENCE_GGA:
{
struct minmea_sentence_gga frame;
if (minmea_parse_gga(&frame, dev->buffer))
{
gps.latitude = minmea_tocoord(&frame.latitude);
gps.longitude = minmea_tocoord(&frame.longitude);
gps.altitude = minmea_tofloat(&frame.altitude);
gps.altitude_ellipsoid = minmea_tofloat(&frame.height);
}
break;
}
/* Precision dilution and satellite data is obtained from
* GSA sentence
*/
case MINMEA_SENTENCE_GSA:
{
struct minmea_sentence_gsa frame;
if (minmea_parse_gsa(&frame, dev->buffer))
{
gps.hdop = minmea_tofloat(&frame.hdop);
gps.pdop = minmea_tofloat(&frame.pdop);
gps.vdop = minmea_tofloat(&frame.vdop);
uint32_t sats = 0;
for (int i = 0; i < 12; ++i)
{
if (frame.sats[i] != 0)
{
++sats;
}
}
gps.satellites_used = sats;
}
break;
}
/* GSV and GLL data are transmitted by the l86-XXX but do not
provide additional information. Since GLL is always the last
message transmitted, events will be pushed whenever that
sentence is read
*/
case MINMEA_SENTENCE_GLL:
{
dev->lower.push_event(dev->lower.priv, &gps, sizeof(gps));
}
/* All remaining sentences are not transmitted by the module */
case MINMEA_SENTENCE_GSV:
case MINMEA_SENTENCE_GBS:
case MINMEA_SENTENCE_GST:
case MINMEA_SENTENCE_ZDA:
{
break;
}
case MINMEA_INVALID:
{
snerr("Read invalid NMEA statement: %s\n", dev->buffer);
break;
}
case MINMEA_UNKNOWN:
{
/* Message could be non-standard NMEA message,
in that case just ignore
*/
if (strncmp("$GPTXT", dev->buffer, strlen("$GPTXT")) == 0)
{
break;
}
snwarn("Read unknown NMEA statement: %s %d\n",
dev->buffer, bw);
break;
}
}
dev->lower.push_data(dev->lower.priv, dev->buffer, bw, true);
}
nxmutex_unlock(&dev->devlock);
@ -730,6 +628,7 @@ int l86xxx_register(FAR const char *uartpath, int devno)
{
FAR l86xxx_dev_s *priv = NULL;
int err;
uint32_t nbuffers[SENSOR_GNSS_IDX_GNSS_MAX];
char *buf;
FAR char *argv[2];
char arg1[32];
@ -791,23 +690,29 @@ int l86xxx_register(FAR const char *uartpath, int devno)
if (err < 0)
{
snwarn("Couldn't set baud rate of device: %d\n", err);
goto close_file;
}
#endif
#endif
err = send_command(priv, SET_POS_FIX, CONFIG_L86_XXX_FIX_INT);
if (err < 0)
{
snwarn("Couldn't set position fix interval, %d\n", err);
goto close_file;
}
/* Register UORB Sensor */
priv->lower.ops = &g_sensor_ops;
priv->lower.type = SENSOR_TYPE_GNSS;
priv->lower.ops = &g_gnss_ops;
priv->lower.priv = priv;
priv->enabled = false;
err = sensor_register(&priv->lower, devno);
nbuffers[SENSOR_GNSS_IDX_GNSS] = 1;
nbuffers[SENSOR_GNSS_IDX_GNSS_SATELLITE] = 1;
nbuffers[SENSOR_GNSS_IDX_GNSS_MEASUREMENT] = 1;
nbuffers[SENSOR_GNSS_IDX_GNSS_CLOCK] = 1;
nbuffers[SENSOR_GNSS_IDX_GNSS_GEOFENCE] = 1;
err =
gnss_register(&priv->lower, devno, nbuffers, SENSOR_GNSS_IDX_GNSS_MAX);
if (err < 0)
{
snerr("Failed to register L86-XXX driver: %d\n", err);
@ -832,21 +737,20 @@ int l86xxx_register(FAR const char *uartpath, int devno)
L86_XXX_BAUD_RATE,
CONFIG_L86_XXX_FIX_INT);
return 0;
/* Cleanup items on error */
if (err < 0)
{
sensor_unreg:
sensor_unregister(&priv->lower, devno);
close_file:
file_close(&priv->uart);
destroy_sem:
nxsem_destroy(&priv->run);
destroy_mutex:
nxmutex_destroy(&priv->devlock);
free_mem:
kmm_free(priv);
}
sensor_unreg:
gnss_unregister(&priv->lower, devno);
close_file:
file_close(&priv->uart);
destroy_sem:
nxsem_destroy(&priv->run);
destroy_mutex:
nxmutex_destroy(&priv->devlock);
free_mem:
kmm_free(priv);
return err;
}