xiaoxiang781216 commented on code in PR #16605: URL: https://github.com/apache/nuttx/pull/16605#discussion_r2167389549
########## drivers/sensors/l86xxx_uorb.c: ########## @@ -0,0 +1,754 @@ +/**************************************************************************** + * 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 + #define L86_XXX_BAUD_RATE 4800 +#elif CONFIG_L86_XXX_BAUD == 9600 + #define L86_XXX_BAUD_RATE 9600 +#elif CONFIG_L86_XXX_BAUD == 14400 + #define L86_XXX_BAUD_RATE 14400 +#elif CONFIG_L86_XXX_BAUD == 19200 + #define L86_XXX_BAUD_RATE 19200 +#elif CONFIG_L86_XXX_BAUD == 38400 + #define L86_XXX_BAUD_RATE 38400 +#elif CONFIG_L86_XXX_BAUD == 57600 + #define L86_XXX_BAUD_RATE 57600 +#elif CONFIG_L86_XXX_BAUD == 115200 + #define L86_XXX_BAUD_RATE 115200 +#else + #error "Invalid baud rate. Supported baud rates are: 4800, 5600, 14400, 19200, 38400, 57600, 115200" +#endif + +#ifdef CONFIG_L86_XXX_FIX_INT +#define L86_XXX_FIX_INT CONFIG_L86_XXX_FIX_INT +#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 */ +#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 char calculate_checksum(char *data, int len); +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 void 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 + ****************************************************************************/ + +/**************************************************************************** + * Name: calculate_checksum + * + * Description: + * Calculate checksum of PMTK command. + * + * Arguments: + * data - Char pointer to calculate checksum for + * len - Length of char string + * + * Returns: + * 1-byte checksum value to be interpreted as a hex byte + ****************************************************************************/ + +static char calculate_checksum(char *data, int len) +{ + char ret = 0; + for (int i = 0; i < len; ++i) + { + ret = ret ^ *(data + i); + } + + return ret; +} + +/**************************************************************************** + * Name: set_baud_rate + * + * Description: + * Sets baud rate of UART interface + * + * Arguments: + * dev - Pointer L86-XXX priv struct + * br - Baud rate + * + * Returns: + * negative number - Command failed during writing + * 3 - Command succeeded, baud rate changed + ****************************************************************************/ + +static int set_baud_rate(l86xxx_dev_s *dev, int br) +{ + struct termios opt; + int err; + file_ioctl(&dev->uart, TCGETS, &opt); + cfmakeraw(&opt); + switch (br) + { + case 4800: + { + cfsetispeed(&opt, 4800); + cfsetospeed(&opt, 4800); + break; + } + + case 9600: + { + cfsetispeed(&opt, 9600); + cfsetospeed(&opt, 9600); + break; + } + + case 14400: + { + cfsetispeed(&opt, 14400); + cfsetospeed(&opt, 14400); + break; + } + + case 19200: + { + cfsetispeed(&opt, 19200); + cfsetospeed(&opt, 19200); + break; + } + + case 38400: + { + cfsetispeed(&opt, 38400); + cfsetospeed(&opt, 38400); + break; + } + + case 57600: + { + cfsetispeed(&opt, 57600); + cfsetospeed(&opt, 57600); + break; + } + + case 115200: + { + cfsetispeed(&opt, 115200); + cfsetospeed(&opt, 115200); + break; + } + } + + err = file_ioctl(&dev->uart, TCSETS, &opt); + if (err < 0) + { + snwarn("Couldn't change baud rate of U(S)ART interface: %d\n", err); + return err; + } + + /* Wait for module to update */ + + for (int i = 0; i < 5; ++i) + { + read_line(dev); + } + + return 3; +} + +/**************************************************************************** + * 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 + * negative number - Command failed during writing + * 0 - Invalid packet + * 1 - Unsupported packet type + * 2 - Valid packet, but action failed + * 3 - Valid packet, action succeeded + ****************************************************************************/ + +static int send_command(l86xxx_dev_s *dev, + L86XXX_PMTK_COMMAND cmd, unsigned long arg) +{ + char buf[50]; + int bw1; + 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, 50, "$PMTK%d", cmd); + break; + } + + case CMD_STANDBY_MODE: + { + bw1 = snprintf(buf, 50, "$PMTK%d,%d", cmd, (int)arg); + break; + } + + case SET_NMEA_BAUDRATE: + { + bw1 = snprintf(buf, 50, "$PMTK%d,%d", cmd, (int)arg); + break; + } + + case SET_POS_FIX: + { + bw1 = snprintf(buf, 50, "$PMTK%d,%d", cmd, (int)arg); + break; + } + + case FR_MODE: + { + bw1 = snprintf(buf, 50, "$PMTK%d,%d", cmd, (int)arg); + break; + } + + default: + break; + } + + char checksum = calculate_checksum(buf + 1, bw1 - 1); + int bw2 = snprintf(buf + bw1, 50 - bw1, "*%02X\r\n", checksum); + sninfo("Sending command: %s to L86", buf); + int 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 3; + } + + /* 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 */ + int 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 + */ + + memset(buf, '\0', 50); + snprintf(buf, 50, "$PMTK001,%d", cmd); /* ACK message will be $PMTK001,<cmd num>,<flag> */ + 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); + return dev->buffer[13] - '0'; +} + +static void read_line(l86xxx_dev_s *dev) +{ + memset(dev->buffer, '\0', MINMEA_MAX_LENGTH); + int line_len = 0; + char next_char; + do + { + file_read(&dev->uart, &next_char, 1); + if (next_char != '\r' && next_char != '\n') + { + dev->buffer[line_len++] = next_char; + } + } + while (next_char != '\r' && next_char != '\n' + && line_len < MINMEA_MAX_LENGTH); + dev->buffer[line_len] = '\0'; +} + +/**************************************************************************** + * Name: l86xxx_control + * + * Description: + * Send commands to the l86xxx GNSS module + ****************************************************************************/ + +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); + return send_command(dev, (L86XXX_PMTK_COMMAND)cmd, arg); +} + +/**************************************************************************** + * Name: nau7802_activate + ****************************************************************************/ + +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; + 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; + send_command(dev, CMD_STANDBY_MODE, 0); + } + + return 0; +} + +/**************************************************************************** + * Name: l86xxx_set_interval + * + * Description: + * Set position fix interval of L86-XXX GNSS module + * + * Returns: + * -1 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 -1; + } + + 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; + + /* 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) + { + continue; + } + } + + /* Mutex required because some commands send ACKS */ + + nxmutex_lock(&dev->devlock); + read_line(dev); + + /* Parse line based on NMEA sentence type */ + + switch (minmea_sentence_id(dev->buffer, false)) + { + /* Time data is obtained from RMC sentence */ + + case MINMEA_SENTENCE_RMC: Review Comment: the similar code already done in gnss upperhalf, you can reuse the common code to simplify this driver -- This is an automated message from the Apache Git Service. To respond to the message, please log on to GitHub and use the URL above to go to the specific comment. To unsubscribe, e-mail: commits-unsubscr...@nuttx.apache.org For queries about this service, please contact Infrastructure at: us...@infra.apache.org
