EliasJRH commented on code in PR #16605: URL: https://github.com/apache/nuttx/pull/16605#discussion_r2164771310
########## 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'; Review Comment: This was used to report back the success code from the GNSS module, this will get replaced with the new error values -- 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
