drivers/sensors: add Quectel L86-XXX GNSS uORB sensor driver
This commit is contained in:
Elias Hawa
Alan C. Assis
parent
3f65182699
commit
0ce9e82d73
8 changed files with 1150 additions and 1 deletions
|
|
@ -33,3 +33,4 @@ general interface.
|
|||
sensors/sht4x.rst
|
||||
sensors/lsm6dso32.rst
|
||||
sensors/lis2mdl.rst
|
||||
sensors/l86xxx.rst
|
||||
|
|
|
|||
177
Documentation/components/drivers/special/sensors/l86xxx.rst
Normal file
177
Documentation/components/drivers/special/sensors/l86xxx.rst
Normal file
|
|
@ -0,0 +1,177 @@
|
|||
=======
|
||||
L86-XXX
|
||||
=======
|
||||
|
||||
.. tags:: experimental
|
||||
|
||||
This driver provides support for the L86-XXX family of GNSS modules by
|
||||
Quectel via the :doc:`uorb </components/drivers/special/sensors/sensors_uorb>` interface.
|
||||
Functionality for this driver was tested using the Quectel L86-M33.
|
||||
|
||||
.. warning::
|
||||
This driver only contains preliminary support for a handful of proprietary
|
||||
'PMTK' commands There is no support for the entire suite of commands yet.
|
||||
This driver also does not use the standard uORD GNSS upper half driver and
|
||||
should eventually be modified such that it does. CONSIDER THIS DRIVER EXPERIMENTAL.
|
||||
|
||||
Application Programming Interface
|
||||
=================================
|
||||
|
||||
To register the device for use, you will need to enable the standard upper half
|
||||
serial drivers (``CONFIG_STANDARD_SERIAL``), since the L86-XXX driver requires
|
||||
the path to the UART interface the module is connected to. You will also need to
|
||||
ensure that the baud rate of the UART interface is set to 9600, which is the default
|
||||
baud rate of the L86-XXX series of GNSS modules.
|
||||
|
||||
The driver supports changing the default baud rate and update rate of the GNSS module.
|
||||
As a result, you will also need to enable serial TERMIOS support (``CONFIG_SERIAL_TERMIOS``).
|
||||
The baud rate and update rate of the GNSS module can be configured using the ``L86_XXX_BAUD`` and ``L86_XXX_FIX_INT`` options respectively.
|
||||
Note that a faster update rate will require a higher baud rate to support it and the supported baud rates for the L86-XXX series of GNSS modules are:
|
||||
* 4800
|
||||
* 9600
|
||||
* 14400
|
||||
* 19200
|
||||
* 38400
|
||||
* 57600
|
||||
* 115200
|
||||
The baud rate and update rates of the module are changed at registration time.
|
||||
|
||||
.. code-block:: c
|
||||
|
||||
#if defined(CONFIG_SENSORS_L86_XXX)
|
||||
#include <nuttx/sensors/l86xxx.h>
|
||||
|
||||
/* Register L86-M33 on USART3 */
|
||||
|
||||
ret = l86xxx_register("/dev/l86m33", "/dev/ttyS2", 0);
|
||||
if (ret < 0) {
|
||||
syslog(LOG_ERR, "Failed to register L86-M33: %d\n", ret);
|
||||
}
|
||||
#endif
|
||||
|
||||
Once the driver is registered, it starts a thread that continuously reads raw output from the specified UART device and
|
||||
parses the output according to `NMEA <https://en.wikipedia.org/wiki/NMEA_0183>`_ standards using the
|
||||
`minmea <https://github.com/kosma/minmea>`_ library included in NuttX. The driver populates the ``sensor_gnss`` struct
|
||||
and pushes it to the appropriate event once all NMEA messages in its sequence have been read.
|
||||
|
||||
|
||||
**uORB commands**
|
||||
-----------------
|
||||
The driver implements the ``orb_activate``, ``orb_set_interval`` and, ``orb_ioctl`` operations to interact with the device.
|
||||
The latter is used to send proprietary 'PMTK' commands which are documented further below.
|
||||
|
||||
**Activate**
|
||||
|
||||
There are 4 modes that the L86-XXX GNSS modules can be in. Those are "Full On Mode", "Standby Mode", "Backup Mode", "Periodic Mode" and, "AlwaysLocateTM Mode".
|
||||
Calling ``orb_activate`` with ``enable`` set to false will enter the module into "Standby Mode".
|
||||
In "Standby Mode", the module doesn't output any NMEA messages but the internal core and I/O power domain are still active.
|
||||
|
||||
The module can be re-enabled by calling ``orb_activate`` with ``enable`` set to true, which will hot start the module OR by
|
||||
sending any 'PMTK' command.
|
||||
|
||||
**Set interval**
|
||||
|
||||
The L86-XXX GNSS modules support interval rates from 1Hz to 10Hz (100ms - 10000ms). When using ``orb_set_interval``, be aware that
|
||||
increasing the interval of the module may also require and increase in baud rate. An example of how this is performed can be found in
|
||||
source code of this driver in the register function.
|
||||
|
||||
Any interval rate outside of the supported range will result in a failed call to this function.
|
||||
|
||||
**Control**
|
||||
|
||||
The ``orb_ioctl`` interface allows one to send proprietary 'PMTK' commands to the L86-XXX GNSS module. It effectively works
|
||||
as a wrapper for the command framework outlined by Quectel. The return value of calls to ``orb_ioctl`` follow this pattern:
|
||||
|
||||
* -EINVAL - Invalid packet
|
||||
* -ENOSYS - Unsupported packet type
|
||||
* -EIO - Valid packet, but action failed
|
||||
* 0 - Valid packet, action succeeded
|
||||
* Other - Command failed during writing
|
||||
|
||||
The supported commands are their arguments are listed below.
|
||||
|
||||
``SNIOC_HOT_START``
|
||||
-------------------
|
||||
Used to "Hot start" the GNSS module. Normally hot start means the GNSS module was powered down for less
|
||||
than 3 hours (RTC must be alive) and its ephemeris is still valid. As there is no need for downloading
|
||||
ephemeris, it is the fastest startup method.
|
||||
|
||||
.. code-block:: c
|
||||
|
||||
orb_ioctl(sensor, SNIOC_HOT_START);
|
||||
|
||||
``SNIOC_WARM_START``
|
||||
--------------------
|
||||
Used to "Warm start" the GNSS module. Warm start means the GNSS module has approximate information of time,
|
||||
position and coarse data on satellite positions, but it needs to download ephemeris until it can get a fix.
|
||||
|
||||
.. code-block:: c
|
||||
|
||||
orb_ioctl(sensor, SNIOC_WARM_START);
|
||||
|
||||
``SNIOC_COLD_START``
|
||||
--------------------
|
||||
Used to "Cold start" the GNSS module. Using this message will force the GNSS module to be restarted without
|
||||
any prior location information, including time, position, almanacs and ephemeris data.
|
||||
|
||||
.. code-block:: c
|
||||
|
||||
orb_ioctl(sensor, SNIOC_COLD_START);
|
||||
|
||||
``SNIOC_FULL_COLD_START``
|
||||
-------------------------
|
||||
Used to "Full cold start" the GNSS module. This is effectively the same as a cold restart, but additionally
|
||||
clears system and user configurations. In other words, this resets the GNSS module to its factory settings.
|
||||
When full-cold started, the GNSS module has no information on its last location.
|
||||
|
||||
.. code-block:: c
|
||||
|
||||
orb_ioctl(sensor, SNIOC_FULL_COLD_START);
|
||||
|
||||
``SNIOC_SET_INTERVAL``
|
||||
----------------------
|
||||
Used to modify the position fix interval of the GNSS module. The argument is an integer between 100 and 10000, default value is 1000.
|
||||
|
||||
.. code-block:: c
|
||||
|
||||
orb_ioctl(sensor, SNIOC_SET_INTERVAL, 1000);
|
||||
|
||||
``SNIOC_SET_BAUD``
|
||||
------------------
|
||||
.. note::
|
||||
|
||||
This feature requires termios support to be enabled(``CONFIG_SERIAL_TERMIOS``)
|
||||
|
||||
Used to modify the baud rate of the GNSS module. The argument is an integer representing a supported baud rate, default value is 9600.
|
||||
Upon sending this command, the baud rate of the UART interface used to communicate with the module is also modified.
|
||||
Supported baud rates for the L86-XXX series of GNSS modules are:
|
||||
|
||||
* 4800
|
||||
* 9600
|
||||
* 14400
|
||||
* 19200
|
||||
* 38400
|
||||
* 57600
|
||||
* 115200
|
||||
|
||||
.. code-block:: c
|
||||
|
||||
orb_ioctl(sensor, SNIOC_SET_BAUD, 9600);
|
||||
|
||||
``SNIOC_SET_OPERATIONAL_MODE``
|
||||
------------------------------
|
||||
Used to set the navigation mode of the GNSS module. The argument is an ``L86XXX_OPERATIONAL_MODE`` enum:
|
||||
|
||||
* NORMAL - For general purpose
|
||||
* FITNESS - For instances in which low-speed movements (<5 m/s>) will affect calculations
|
||||
* AVIATION - For high-dynamic purposes that the large-acceleration movement will have more effect on the position calculation
|
||||
* BALLOON - For high-altitude balloon purposes that vertical movement will have more effect on the position calculation
|
||||
* STANDBY - Used to enter standby mode for power saving
|
||||
|
||||
Default mode is NORMAL
|
||||
|
||||
.. code-block:: c
|
||||
|
||||
orb_ioctl(sensor, SNIOC_SET_OPERATIONAL_MODE, NORMAL);
|
||||
|
||||
|
||||
|
|
@ -37,6 +37,10 @@ if(CONFIG_SENSORS)
|
|||
list(APPEND SRCS nau7802.c)
|
||||
endif()
|
||||
|
||||
if(CONFIG_SENSORS_L86_XXX)
|
||||
list(APPEND SRCS l86xxx_uorb.c)
|
||||
endif()
|
||||
|
||||
if(CONFIG_SENSORS_GNSS)
|
||||
set_source_files_properties(
|
||||
gnss_uorb.c DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/..
|
||||
|
|
|
|||
|
|
@ -838,6 +838,35 @@ config KXTJ9_I2C_BUS_SPEED
|
|||
|
||||
endif # SENSORS_KXTJ9
|
||||
|
||||
config SENSORS_L86_XXX
|
||||
bool "Quectel L86-XXX GNSS support"
|
||||
default n
|
||||
depends on SERIAL && STANDARD_SERIAL && UORB
|
||||
---help---
|
||||
Enable driver support for the L86-XXX series of GNSS modules.
|
||||
|
||||
config SENSORS_L86_XXX_THREAD_STACKSIZE
|
||||
int "Stack size for L86XXX module collection thread"
|
||||
default 10000
|
||||
depends on SENSORS_L86_XXX
|
||||
|
||||
config L86_XXX_BAUD
|
||||
int "Quectel L86-XXX baud rate"
|
||||
default 9600
|
||||
depends on SENSORS_L86_XXX && SERIAL_TERMIOS
|
||||
range 4800 115200
|
||||
---help---
|
||||
Supported values are: 4800, 9600, 14400, 19200, 38400, 57600 and 115200
|
||||
|
||||
config L86_XXX_FIX_INT
|
||||
int "Quectel L86-XXX fix interval rate"
|
||||
default 1000
|
||||
depends on SENSORS_L86_XXX && SERIAL_TERMIOS
|
||||
range 100 10000
|
||||
---help---
|
||||
Rate in ms at which module obtains satellite fix. Supported values
|
||||
are integers between 100 10000
|
||||
|
||||
config SENSORS_LIS2DH
|
||||
bool "STMicro LIS2DH device support"
|
||||
default n
|
||||
|
|
|
|||
|
|
@ -34,11 +34,14 @@ ifeq ($(CONFIG_SENSORS_RPMSG),y)
|
|||
CSRCS += sensor_rpmsg.c
|
||||
endif
|
||||
|
||||
|
||||
ifeq ($(CONFIG_SENSORS_NAU7802),y)
|
||||
CSRCS += nau7802.c
|
||||
endif
|
||||
|
||||
ifeq ($(CONFIG_SENSORS_L86_XXX),y)
|
||||
CSRCS += l86xxx_uorb.c
|
||||
endif
|
||||
|
||||
ifeq ($(CONFIG_SENSORS_GNSS),y)
|
||||
CSRCS += gnss_uorb.c
|
||||
endif
|
||||
|
|
|
|||
840
drivers/sensors/l86xxx_uorb.c
Normal file
840
drivers/sensors/l86xxx_uorb.c
Normal file
|
|
@ -0,0 +1,840 @@
|
|||
/****************************************************************************
|
||||
* drivers/sensors/l86xxx_uorb.c
|
||||
*
|
||||
* NOTE: EXPERIMENTAL DRIVER
|
||||
*
|
||||
* Contributed by Carleton University InSpace
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* 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/nuttx.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <unistd.h>
|
||||
#include <math.h>
|
||||
#include <time.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include <nuttx/fs/fs.h>
|
||||
#include <nuttx/kmalloc.h>
|
||||
#include <nuttx/kthread.h>
|
||||
#include <nuttx/mutex.h>
|
||||
#include <nuttx/semaphore.h>
|
||||
#include <nuttx/signal.h>
|
||||
#include <nuttx/wqueue.h>
|
||||
#include <nuttx/sensors/sensor.h>
|
||||
#include <minmea/minmea.h>
|
||||
|
||||
#include <nuttx/sensors/l86xxx.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef CONFIG_SENSORS_L86_XXX_THREAD_STACKSIZE
|
||||
#define CONFIG_SENSORS_L86_XXX_THREAD_STACKSIZE 10000
|
||||
#endif
|
||||
|
||||
#ifndef CONFIG_L86_XXX_BAUD
|
||||
#define CONFIG_L86_XXX_BAUD 9600
|
||||
#endif
|
||||
|
||||
#if CONFIG_L86_XXX_BAUD == 4800 || CONFIG_L86_XXX_BAUD == 9600 || CONFIG_L86_XXX_BAUD == 14400 || CONFIG_L86_XXX_BAUD == 19200 || CONFIG_L86_XXX_BAUD == 38400 || CONFIG_L86_XXX_BAUD == 57600 || CONFIG_L86_XXX_BAUD == 115200
|
||||
#define L86_XXX_BAUD_RATE CONFIG_L86_XXX_BAUD
|
||||
#else
|
||||
#error "Invalid baud rate. Supported baud rates are: 4800, 5600, 14400, 19200, 38400, 57600, 115200"
|
||||
#endif
|
||||
|
||||
#if CONFIG_L86_XXX_FIX_INT < 100 || CONFIG_L86_XXX_FIX_INT > 10000
|
||||
#error "Invalid fix interval rate. Values must be between 100 and 10000"
|
||||
#endif
|
||||
|
||||
/* Helper to get array length */
|
||||
|
||||
#define MINMEA_MAX_LENGTH 256
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data Types
|
||||
****************************************************************************/
|
||||
|
||||
/* GNSS device struct */
|
||||
|
||||
typedef struct
|
||||
{
|
||||
FAR struct file uart; /* UART interface */
|
||||
struct sensor_lowerhalf_s lower; /* UORB 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
|
||||
} l86xxx_dev_s;
|
||||
|
||||
/****************************************************************************
|
||||
* 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 set_baud_rate(l86xxx_dev_s *dev, int br);
|
||||
static int send_command(l86xxx_dev_s *dev,
|
||||
L86XXX_PMTK_COMMAND cmd, unsigned long arg);
|
||||
static int read_line(l86xxx_dev_s *dev);
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
|
||||
static const struct sensor_ops_s g_sensor_ops =
|
||||
{
|
||||
.control = l86xxx_control,
|
||||
.activate = l86xxx_activate,
|
||||
.set_interval = l86xxx_set_interval,
|
||||
};
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
#ifdef CONFIG_SERIAL_TERMIOS
|
||||
/****************************************************************************
|
||||
* Name: set_baud_rate
|
||||
*
|
||||
* Description:
|
||||
* Sets baud rate of UART interface
|
||||
*
|
||||
* Arguments:
|
||||
* dev - Pointer L86-XXX priv struct
|
||||
* br - Baud rate
|
||||
*
|
||||
* Returns:
|
||||
* -EINVAL - Invalid baud rate
|
||||
* 0 - Command succeeded, baud rate changed
|
||||
* Other - Error occurred during ioctl call
|
||||
****************************************************************************/
|
||||
|
||||
static int set_baud_rate(l86xxx_dev_s *dev, int br)
|
||||
{
|
||||
struct termios opt;
|
||||
int err;
|
||||
err = file_ioctl(&dev->uart, TCGETS, &opt);
|
||||
if (err < 0)
|
||||
{
|
||||
snwarn("Couldn't get interface settings: %d\n", err);
|
||||
return err;
|
||||
}
|
||||
|
||||
cfmakeraw(&opt);
|
||||
|
||||
switch (br)
|
||||
{
|
||||
case 4800:
|
||||
case 9600:
|
||||
case 14400:
|
||||
case 19200:
|
||||
case 38400:
|
||||
case 57600:
|
||||
case 115200:
|
||||
{
|
||||
cfsetispeed(&opt, br);
|
||||
cfsetospeed(&opt, br);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
snerr("Invalid baud rate, %ld\n", br);
|
||||
return -EINVAL;
|
||||
}
|
||||
}
|
||||
|
||||
err = file_ioctl(&dev->uart, TCSETS, &opt);
|
||||
if (err < 0)
|
||||
{
|
||||
snerr("Couldn't change baud rate of U(S)ART interface: %d\n", err);
|
||||
return err;
|
||||
}
|
||||
|
||||
/* These calls to read_line will flush out the buffer
|
||||
after the baud rate change
|
||||
*/
|
||||
|
||||
for (int i = 0; i < 5; ++i)
|
||||
{
|
||||
err = read_line(dev);
|
||||
if (err < 0)
|
||||
{
|
||||
snerr("Problem occurred when flushing buffer %d\n", err);
|
||||
return err;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
/****************************************************************************
|
||||
* Name: send_command
|
||||
*
|
||||
* Description:
|
||||
* Sends command L86-XXX GNSS device and waits for acknowledgement
|
||||
* if command supports it.
|
||||
*
|
||||
* Arguments:
|
||||
* dev - Pointer L86-XXX priv struct
|
||||
* cmd - L86XXX_COMMAND enum
|
||||
* arg - Dependent on command type. Could be used for preset
|
||||
* enum, numeric args or struct pointers
|
||||
*
|
||||
* Returns:
|
||||
* Flag defined by device
|
||||
* -EINVAL - Invalid packet
|
||||
* -ENOSYS - Unsupported packet type
|
||||
* -EIO - Valid packet, but action failed
|
||||
* 0 - Valid packet, action succeeded
|
||||
* Other - Command failed during writing
|
||||
****************************************************************************/
|
||||
|
||||
static int send_command(l86xxx_dev_s *dev,
|
||||
L86XXX_PMTK_COMMAND cmd, unsigned long arg)
|
||||
{
|
||||
char buf[50];
|
||||
int bw1;
|
||||
int bw2;
|
||||
int err;
|
||||
int ret;
|
||||
uint8_t checksum;
|
||||
|
||||
nxmutex_lock(&dev->devlock);
|
||||
switch (cmd)
|
||||
{
|
||||
case CMD_HOT_START:
|
||||
case CMD_WARM_START:
|
||||
case CMD_COLD_START:
|
||||
case CMD_FULL_COLD_START:
|
||||
{
|
||||
bw1 = snprintf(buf, sizeof(buf), "$PMTK%d", cmd);
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_STANDBY_MODE:
|
||||
{
|
||||
bw1 = snprintf(buf, sizeof(buf), "$PMTK%d,0", cmd);
|
||||
break;
|
||||
}
|
||||
|
||||
case SET_NMEA_BAUDRATE:
|
||||
{
|
||||
bw1 = snprintf(buf, sizeof(buf), "$PMTK%d,%d", cmd, (int)arg);
|
||||
break;
|
||||
}
|
||||
|
||||
case SET_POS_FIX:
|
||||
{
|
||||
bw1 = snprintf(buf, sizeof(buf), "$PMTK%d,%d", cmd, (int)arg);
|
||||
break;
|
||||
}
|
||||
|
||||
case FR_MODE:
|
||||
{
|
||||
bw1 = snprintf(buf, sizeof(buf), "$PMTK%d,%d", cmd, (int)arg);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
return -ENOSYS;
|
||||
}
|
||||
|
||||
sninfo("Sending command: %s to L86", buf);
|
||||
|
||||
checksum = minmea_checksum(buf);
|
||||
bw2 = snprintf(buf + bw1, sizeof(buf) - bw1, "*%02X\r\n", checksum);
|
||||
|
||||
err = file_write(&dev->uart, buf, bw1 + bw2);
|
||||
if (err < 0)
|
||||
{
|
||||
snerr("Could not send command to device\n");
|
||||
return err;
|
||||
}
|
||||
|
||||
/* These commands do not send ACKs
|
||||
so just return after they've been written
|
||||
*/
|
||||
|
||||
if (cmd == CMD_HOT_START ||
|
||||
cmd == CMD_WARM_START ||
|
||||
cmd == CMD_COLD_START ||
|
||||
cmd == CMD_FULL_COLD_START)
|
||||
{
|
||||
nxmutex_unlock(&dev->devlock);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Setting baud rate also doesn't send an ACK but the interface baud rate
|
||||
needs to be updated
|
||||
*/
|
||||
|
||||
if (cmd == SET_NMEA_BAUDRATE)
|
||||
{
|
||||
nxsig_usleep(20000); /* Should wait for a bit before changing interface baud rate */
|
||||
ret = set_baud_rate(dev, (int)arg);
|
||||
nxmutex_unlock(&dev->devlock);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Some commands will send ACKs,
|
||||
wait for them here before unlocking the mutex
|
||||
*/
|
||||
|
||||
/* ACK message will be $PMTK001,<cmd num>,<flag>
|
||||
flag num indicates success of command
|
||||
0 = Invalid packet
|
||||
1 = Unsupported packet type
|
||||
2 = Valid packet, but action failed
|
||||
3 = Valid packet, action succeeded
|
||||
*/
|
||||
|
||||
memset(buf, '\0', 50);
|
||||
snprintf(buf, 50, "$PMTK001,%d", cmd);
|
||||
sninfo("Waiting for ACK from L86...\n");
|
||||
for (; ; )
|
||||
{
|
||||
read_line(dev);
|
||||
if (strncmp(buf, dev->buffer, strlen(buf)) == 0) break;
|
||||
}
|
||||
|
||||
sninfo("ACK received!\n");
|
||||
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;
|
||||
}
|
||||
else if (ret == 2)
|
||||
{
|
||||
return -EIO;
|
||||
}
|
||||
else if (ret == 3)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
return -EINVAL;
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: read_line
|
||||
*
|
||||
* Description:
|
||||
* Reads line from UART interface and stores in priv buffer
|
||||
*
|
||||
* Arguments:
|
||||
* dev - Pointer L86-XXX priv struct
|
||||
*
|
||||
* Returns:
|
||||
* Negative number if reading from device failed, else number
|
||||
* of bytes read
|
||||
****************************************************************************/
|
||||
|
||||
static int read_line(l86xxx_dev_s *dev)
|
||||
{
|
||||
memset(dev->buffer, '\0', sizeof(dev->buffer));
|
||||
int line_len = 0;
|
||||
int err;
|
||||
char next_char;
|
||||
do
|
||||
{
|
||||
err = file_read(&dev->uart, &next_char, 1);
|
||||
if (err < 0)
|
||||
{
|
||||
snerr("Couldn't read from UART device: %d\n", err);
|
||||
return err;
|
||||
}
|
||||
|
||||
if (next_char != '\r' && next_char != '\n')
|
||||
{
|
||||
dev->buffer[line_len++] = next_char;
|
||||
}
|
||||
}
|
||||
while (next_char != '\r' && next_char != '\n'
|
||||
&& line_len < sizeof(dev->buffer));
|
||||
dev->buffer[line_len] = '\0';
|
||||
return line_len;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: l86xxx_control
|
||||
*
|
||||
* Description:
|
||||
* Send commands to the l86xxx GNSS module
|
||||
*
|
||||
* Returns:
|
||||
* -ENOSYS if ioctl command is not supported,
|
||||
* 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)
|
||||
{
|
||||
FAR l86xxx_dev_s *dev = container_of(lower, FAR l86xxx_dev_s, lower);
|
||||
L86XXX_PMTK_COMMAND pmtk_cmd;
|
||||
switch (cmd)
|
||||
{
|
||||
case SNIOC_HOT_START:
|
||||
pmtk_cmd = CMD_HOT_START;
|
||||
break;
|
||||
case SNIOC_WARM_START:
|
||||
pmtk_cmd = CMD_WARM_START;
|
||||
break;
|
||||
case SNIOC_COLD_START:
|
||||
pmtk_cmd = CMD_COLD_START;
|
||||
break;
|
||||
case SNIOC_FULL_COLD_START:
|
||||
pmtk_cmd = CMD_FULL_COLD_START;
|
||||
break;
|
||||
case SNIOC_SET_INTERVAL:
|
||||
pmtk_cmd = SET_POS_FIX;
|
||||
break;
|
||||
case SNIOC_SET_BAUD:
|
||||
pmtk_cmd = SET_NMEA_BAUDRATE;
|
||||
break;
|
||||
case SNIOC_SET_OPERATIONAL_MODE:
|
||||
if (arg == STANDBY)
|
||||
{
|
||||
pmtk_cmd = CMD_STANDBY_MODE;
|
||||
}
|
||||
else
|
||||
{
|
||||
pmtk_cmd = FR_MODE;
|
||||
}
|
||||
|
||||
break;
|
||||
default:
|
||||
snerr("Unsupported command\n");
|
||||
return -ENOSYS;
|
||||
}
|
||||
|
||||
return send_command(dev, pmtk_cmd, arg);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: l86xxx_activate
|
||||
*
|
||||
* Description:
|
||||
* Puts GNSS module into hot start mode or standby mode depending
|
||||
* on args
|
||||
*
|
||||
* Returns:
|
||||
* Return value from send_command
|
||||
****************************************************************************/
|
||||
|
||||
static int l86xxx_activate(FAR struct sensor_lowerhalf_s *lower,
|
||||
FAR struct file *filep, bool enable)
|
||||
{
|
||||
FAR l86xxx_dev_s *dev = container_of(lower, FAR l86xxx_dev_s, lower);
|
||||
|
||||
/* If not already enabled, start gps */
|
||||
|
||||
if (enable && !dev->enabled)
|
||||
{
|
||||
nxsem_post(&dev->run);
|
||||
dev->enabled = true;
|
||||
return send_command(dev, CMD_HOT_START, (int)NULL);
|
||||
}
|
||||
|
||||
/* If not already disabled, send gps into standby mode */
|
||||
|
||||
else if (!enable && dev->enabled)
|
||||
{
|
||||
dev->enabled = false;
|
||||
return send_command(dev, CMD_STANDBY_MODE, 0);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: l86xxx_set_interval
|
||||
*
|
||||
* Description:
|
||||
* Set position fix interval of L86-XXX GNSS module
|
||||
*
|
||||
* Returns:
|
||||
* -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)
|
||||
{
|
||||
FAR l86xxx_dev_s *dev = container_of(lower, FAR l86xxx_dev_s, lower);
|
||||
int fix_interval = *period_us;
|
||||
if (fix_interval < 100 || fix_interval > 10000)
|
||||
{
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
int ret = send_command(dev, SET_POS_FIX, fix_interval);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: l86xxx_thread
|
||||
*
|
||||
* Description:
|
||||
* Kernel thread to poll the l86xxx
|
||||
****************************************************************************/
|
||||
|
||||
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));
|
||||
dev->enabled = true;
|
||||
int err;
|
||||
int bw;
|
||||
|
||||
/* Read full line of NMEA output */
|
||||
|
||||
for (; ; )
|
||||
{
|
||||
/* If the sensor is disabled
|
||||
* wait until enabled with activate function
|
||||
*/
|
||||
|
||||
if (!dev->enabled)
|
||||
{
|
||||
err = nxsem_wait(&dev->run);
|
||||
if (err < 0)
|
||||
{
|
||||
snerr("Couldn't wait on semaphore\n");
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
/* Mutex required because some commands send ACKS */
|
||||
|
||||
err = nxmutex_lock(&dev->devlock);
|
||||
if (err < 0)
|
||||
{
|
||||
snerr("Couldn't lock mutex\n");
|
||||
return err;
|
||||
}
|
||||
|
||||
bw = read_line(dev);
|
||||
|
||||
/* Parse line based on NMEA sentence type */
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
nxmutex_unlock(&dev->devlock);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: l86xxx_register
|
||||
*
|
||||
* Description:
|
||||
* Register the L86-XXX GNSS device driver.
|
||||
*
|
||||
* Arguments:
|
||||
* uartpath - The path to the UART character driver connected to the
|
||||
* transceiver
|
||||
* devno - The device number to use for the topic (i.e. /dev/mag0)
|
||||
****************************************************************************/
|
||||
|
||||
int l86xxx_register(FAR const char *uartpath, int devno)
|
||||
{
|
||||
FAR l86xxx_dev_s *priv = NULL;
|
||||
int err;
|
||||
char *buf;
|
||||
|
||||
DEBUGASSERT(uartpath != NULL);
|
||||
|
||||
/* Initialize device structure */
|
||||
|
||||
priv = kmm_zalloc(sizeof(l86xxx_dev_s));
|
||||
if (priv == NULL)
|
||||
{
|
||||
snerr("Failed to allocate instance of L86-XXX driver.\n");
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
memset(priv, 0, sizeof(l86xxx_dev_s));
|
||||
|
||||
/* Initialize mutex */
|
||||
|
||||
err = nxmutex_init(&priv->devlock);
|
||||
if (err < 0)
|
||||
{
|
||||
snerr("Failed to initialize mutex for L86-XXX device: %d\n", err);
|
||||
goto free_mem;
|
||||
}
|
||||
|
||||
/* Initialize semaphore */
|
||||
|
||||
err = nxsem_init(&priv->run, 0, 0);
|
||||
if (err < 0)
|
||||
{
|
||||
snerr("Failed to register L86-XXX driver: %d\n", err);
|
||||
goto destroy_mutex;
|
||||
}
|
||||
|
||||
/* Open UART interface for use */
|
||||
|
||||
err = file_open(&priv->uart, uartpath, O_RDWR | O_CLOEXEC);
|
||||
if (err < 0)
|
||||
{
|
||||
wlerr("Failed to open UART interface %s for L86-XXX driver: %d\n",
|
||||
uartpath, err);
|
||||
goto destroy_sem;
|
||||
}
|
||||
|
||||
/* Setup sensor with configured settings */
|
||||
|
||||
sninfo("Waiting for GNSS to start...\n");
|
||||
buf = "$PMTK010,001*2E";
|
||||
for (; ; )
|
||||
{
|
||||
read_line(priv);
|
||||
if (strncmp(buf, priv->buffer, strlen(buf)) == 0) break;
|
||||
}
|
||||
|
||||
sninfo("GNSS module started.\n");
|
||||
|
||||
#ifdef CONFIG_SERIAL_TERMIOS
|
||||
err = send_command(priv, SET_NMEA_BAUDRATE, L86_XXX_BAUD_RATE);
|
||||
if (err < 0)
|
||||
{
|
||||
snwarn("Couldn't set baud rate of device: %d\n", err);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* Register UORB Sensor */
|
||||
|
||||
priv->lower.ops = &g_sensor_ops;
|
||||
priv->lower.type = SENSOR_TYPE_GNSS;
|
||||
|
||||
err = sensor_register(&priv->lower, devno);
|
||||
if (err < 0)
|
||||
{
|
||||
snerr("Failed to register L86-XXX driver: %d\n", err);
|
||||
goto close_file;
|
||||
}
|
||||
|
||||
FAR char *argv[2];
|
||||
char arg1[32];
|
||||
snprintf(arg1, 16, "%p", priv);
|
||||
argv[0] = arg1;
|
||||
argv[1] = NULL;
|
||||
err = kthread_create("l86xxx_thread", SCHED_PRIORITY_DEFAULT,
|
||||
CONFIG_SENSORS_L86_XXX_THREAD_STACKSIZE,
|
||||
l86xxx_thread, argv);
|
||||
|
||||
if (err < 0)
|
||||
{
|
||||
snerr("Failed to create the l86xxx notification kthread\n");
|
||||
goto sensor_unreg;
|
||||
}
|
||||
|
||||
sninfo("Registered L86-XXX driver with kernel polling thread"
|
||||
" with baud rate %d and update rate %d",
|
||||
L86_XXX_BAUD_RATE,
|
||||
CONFIG_L86_XXX_FIX_INT);
|
||||
|
||||
/* 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);
|
||||
}
|
||||
|
||||
return err;
|
||||
}
|
||||
|
|
@ -498,6 +498,19 @@
|
|||
|
||||
#define SNIOC_GET_CALIBVALUE _SNIOC(0x00A3)
|
||||
|
||||
/* Command: SNIOC_SET_BAUD
|
||||
* Description: Sets the baud rate of the sensor.
|
||||
*/
|
||||
|
||||
#define SNIOC_SET_BAUD _SNIOC(0x00A4)
|
||||
|
||||
/* IOCTL commands unique to the L86XXX and other GNSS modules */
|
||||
|
||||
#define SNIOC_HOT_START _SNIOC(0X00A5)
|
||||
#define SNIOC_WARM_START _SNIOC(0X00A6)
|
||||
#define SNIOC_COLD_START _SNIOC(0X00A7)
|
||||
#define SNIOC_FULL_COLD_START _SNIOC(0X00A8)
|
||||
|
||||
/****************************************************************************
|
||||
* Public types
|
||||
****************************************************************************/
|
||||
|
|
|
|||
82
include/nuttx/sensors/l86xxx.h
Normal file
82
include/nuttx/sensors/l86xxx.h
Normal file
|
|
@ -0,0 +1,82 @@
|
|||
/****************************************************************************
|
||||
* include/nuttx/sensors/l86xxx.h
|
||||
*
|
||||
* NOTE: EXPERIMENTAL DRIVER
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef __INCLUDE_NUTTX_SENSORS_L86XXX_H
|
||||
#define __INCLUDE_NUTTX_SENSORS_L86XXX_H
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <nuttx/sensors/ioctl.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-Processor Declarations
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data Types
|
||||
****************************************************************************/
|
||||
|
||||
typedef enum
|
||||
{
|
||||
CMD_HOT_START = 101,
|
||||
CMD_WARM_START = 102,
|
||||
CMD_COLD_START = 103,
|
||||
CMD_FULL_COLD_START = 104,
|
||||
CMD_STANDBY_MODE = 161,
|
||||
SET_POS_FIX = 220,
|
||||
SET_NMEA_BAUDRATE = 251,
|
||||
FR_MODE = 886,
|
||||
} L86XXX_PMTK_COMMAND;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
NORMAL = 0,
|
||||
FITNESS = 1,
|
||||
AVIATION = 2,
|
||||
BALLOON = 3,
|
||||
STANDBY = 4,
|
||||
} L86XXX_OPERATIONAL_MODE;
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: l86xxx_register
|
||||
*
|
||||
* Description:
|
||||
* Register the L86-XXX GNSS device driver.
|
||||
*
|
||||
* Arguments:
|
||||
* uartpath - The path to the UART character driver connected to the
|
||||
* GNSS module
|
||||
* devno - The device number to use for the topic (i.e. /dev/mag0)
|
||||
****************************************************************************/
|
||||
|
||||
int l86xxx_register(FAR const char *uartpath, int devno);
|
||||
|
||||
#endif /* __INCLUDE_NUTTX_SENSORS_L86XXX_H */
|
||||
Loading…
Add table
Reference in a new issue