piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add U-BLOX GPS support (#4689)

Browse Source
This commit is contained in:
David PHAM-VAN
2024-04-08 08:36:45 +10:00
committed by GitHub
parent 9ed9227212
commit 4b8f80af3a
13 changed files with 1536 additions and 262 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/gps.cpp
+75 -249
View File
@@ -38,28 +38,15 @@
*/
#include "opentx.h"
#include "gps.h"
#include "gps_nmea.h"
#include "gps_ubx.h"
#include <ctype.h>
gpsdata_t gpsData;
/* This is a light implementation of a GPS frame decoding
This should work with most of modern GPS devices configured to output 5 frames.
It assumes there are some NMEA GGA frames to decode on the serial bus
Now verifies checksum correctly before applying data
Here we use only the following data :
- latitude
- longitude
- GPS fix is/is not ok
- GPS num sat (4 is enough to be +/- reliable)
// added by Mis
- GPS altitude (for OSD displaying)
- GPS speed (for OSD displaying)
*/
#define NO_FRAME 0
#define FRAME_GGA 1
#define FRAME_RMC 2
static int gpsProtocol = -1;
const etx_serial_driver_t* gpsSerialDrv = nullptr;
void* gpsSerialCtx = nullptr;
#define DIGIT_TO_VAL(_x) (_x - '0')
@@ -104,231 +91,84 @@ uint32_t GPS_coord_to_degrees(const char * coordinateString)
return degrees * 1000000UL + (minutes * 100000UL + fractionalMinutes * 10UL) / 6;
}
// helper functions
uint32_t grab_fields(char * src, uint8_t mult)
static void changeBaudrate()
{
uint32_t i;
uint32_t tmp = 0;
for (i = 0; src[i] != 0; i++) {
if (src[i] == '.') {
i++;
if (mult == 0)
break;
else
src[i + mult] = 0;
}
tmp *= 10;
if (src[i] >= '0' && src[i] <= '9')
tmp += src[i] - '0';
if (i >= 15)
return 0; // out of bounds
const int baudrates_count = 5;
const uint32_t baudrates[] = {9600, 57600, 115200, 19200, 38400};
static uint8_t current_rate = 0;
auto setBaudrate = gpsSerialDrv->setBaudrate;
if (setBaudrate == nullptr) return;
setBaudrate(gpsSerialCtx, baudrates[++current_rate % baudrates_count]);
}
static void autodetectProtocol(uint8_t c)
{
static tmr10ms_t time;
static uint8_t state = 0;
switch (state) {
case 0: // Init
time = get_tmr10ms();
state = 1;
case 1: // Wait for a valid packet
if (gpsNewFrameNMEA(c)) {
gpsProtocol = GPS_PROTOCOL_NMEA;
state = 0;
return;
}
if (gpsNewFrameUBX(c, true)) {
gpsProtocol = GPS_PROTOCOL_UBX;
state = 0;
return;
}
uint32_t new_time = get_tmr10ms();
if (new_time - time > 20) {
// No message received
changeBaudrate();
time = new_time;
}
}
return tmp;
}
typedef struct gpsDataNmea_s
static void detectDisconnected(bool has_frame)
{
uint8_t fix;
int32_t latitude;
int32_t longitude;
uint8_t numSat;
uint16_t altitude;
uint16_t speed;
uint16_t groundCourse;
uint16_t hdop;
uint32_t date;
uint32_t time;
} gpsDataNmea_t;
static tmr10ms_t time = 0;
bool gpsNewFrameNMEA(char c)
{
static gpsDataNmea_t gps_Msg;
uint8_t frameOK = 0;
static uint8_t param = 0, offset = 0, parity = 0;
static char string[15];
static uint8_t checksum_param, gps_frame = NO_FRAME;
switch (c) {
case '$':
param = 0;
offset = 0;
parity = 0;
break;
case ',':
case '*':
string[offset] = 0;
if (param == 0) {
// Frame identification (accept all GPS talkers (GP: GPS, GL:Glonass, GN:combination, etc...))
gps_frame = NO_FRAME;
if (string[0] == 'G' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') {
gps_frame = FRAME_GGA;
}
else if (string[0] == 'G' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') {
gps_frame = FRAME_RMC;
}
else {
// turn off this frame (do this only once a second)
static gtime_t lastGpsCmdSent = 0;
if (string[0] == 'G' && g_rtcTime != lastGpsCmdSent) {
lastGpsCmdSent = g_rtcTime;
char cmd[] = "$PUBX,40,GSV,0,0,0,0";
cmd[9] = string[2];
cmd[10] = string[3];
cmd[11] = string[4];
gpsSendFrame(cmd);
}
}
}
switch (gps_frame) {
case FRAME_GGA: //************* GPGGA FRAME parsing
switch (param) {
case 2:
gps_Msg.latitude = GPS_coord_to_degrees(string);
break;
case 3:
if (string[0] == 'S')
gps_Msg.latitude *= -1;
break;
case 4:
gps_Msg.longitude = GPS_coord_to_degrees(string);
break;
case 5:
if (string[0] == 'W')
gps_Msg.longitude *= -1;
break;
case 6:
if (string[0] > '0') {
gps_Msg.fix = 1;
}
else {
gps_Msg.fix = 0;
}
break;
case 7:
gps_Msg.numSat = grab_fields(string, 0);
break;
case 8:
gps_Msg.hdop = grab_fields(string, 1) * 10;
break;
case 9:
gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis
break;
}
break;
case FRAME_RMC: //************* GPRMC FRAME parsing
switch (param) {
case 1:
gps_Msg.time = grab_fields(string, 0);
break;
case 2:
if (string[0] == 'A') {
gps_Msg.fix = 1;
}
else {
gps_Msg.fix = 0;
}
break;
case 7:
gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis
break;
case 8:
gps_Msg.groundCourse = (grab_fields(string, 1)); // ground course deg * 10
break;
case 9:
gps_Msg.date = grab_fields(string, 0);
break;
}
break;
}
param++;
offset = 0;
if (c == '*')
checksum_param = 1;
else
parity ^= c;
break;
case '\r':
case '\n':
if (checksum_param) { //parity checksum
uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') +
((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
if (checksum == parity) {
gpsData.packetCount++;
switch (gps_frame) {
case FRAME_GGA:
frameOK = 1;
gpsData.fix = gps_Msg.fix;
gpsData.numSat = gps_Msg.numSat;
gpsData.hdop = gps_Msg.hdop;
if (gps_Msg.fix) {
__disable_irq(); // do the atomic update of lat/lon
gpsData.latitude = gps_Msg.latitude;
gpsData.longitude = gps_Msg.longitude;
gpsData.altitude = gps_Msg.altitude;
__enable_irq();
}
break;
case FRAME_RMC:
gpsData.speed = gps_Msg.speed;
gpsData.groundCourse = gps_Msg.groundCourse;
#if defined(RTCLOCK)
// set RTC clock if needed
if (g_eeGeneral.adjustRTC && gps_Msg.fix) {
div_t qr = div(gps_Msg.date, 100);
uint8_t year = qr.rem;
qr = div(qr.quot, 100);
uint8_t mon = qr.rem;
uint8_t day = qr.quot;
qr = div(gps_Msg.time, 100);
uint8_t sec = qr.rem;
qr = div(qr.quot, 100);
uint8_t min = qr.rem;
uint8_t hour = qr.quot;
rtcAdjust(year+2000, mon, day, hour, min, sec);
}
#endif
} // end switch
}
else {
gpsData.errorCount++;
}
}
checksum_param = 0;
break;
default:
if (offset < 15)
string[offset++] = c;
if (!checksum_param)
parity ^= c;
if (has_frame) {
time = get_tmr10ms();
} else if (time > 0 && get_tmr10ms() - time > 500) {
gpsProtocol = GPS_PROTOCOL_AUTO;
time = get_tmr10ms();
}
return frameOK;
}
bool gpsNewFrame(uint8_t c)
{
return gpsNewFrameNMEA(c);
}
void gpsNewData(uint8_t c)
{
if (!gpsNewFrame(c)) {
return;
switch (gpsProtocol) {
case GPS_PROTOCOL_NMEA:
detectDisconnected(gpsNewFrameNMEA(c));
break;
case GPS_PROTOCOL_UBX:
detectDisconnected(gpsNewFrameUBX(c, false));
break;
case GPS_PROTOCOL_AUTO:
autodetectProtocol(c);
break;
}
}
static const etx_serial_driver_t* gpsSerialDrv = nullptr;
static void* gpsSerialCtx = nullptr;
#if defined(DEBUG)
uint8_t gpsTraceEnabled = false;
#endif
void gpsSetSerialDriver(void* ctx, const etx_serial_driver_t* drv)
void gpsSetSerialDriver(void* ctx, const etx_serial_driver_t* drv, int protocol)
{
gpsProtocol = protocol;
gpsSerialCtx = ctx;
gpsSerialDrv = drv;
}
@@ -336,47 +176,33 @@ void gpsSetSerialDriver(void* ctx, const etx_serial_driver_t* drv)
void gpsWakeup()
{
if (!gpsSerialDrv) return;
auto _getByte = gpsSerialDrv->getByte;
if (!_getByte) return;
static tmr10ms_t time = get_tmr10ms();
uint8_t byte;
while (_getByte(gpsSerialCtx, &byte)) {
#if defined(DEBUG)
if (gpsTraceEnabled) {
dbgSerialPutc(byte);
}
#endif
#endif
gpsNewData(byte);
time = get_tmr10ms();
}
}
char hex(uint8_t b) {
return b > 9 ? b + 'A' - 10 : b + '0';
if (get_tmr10ms() - time > 20) {
changeBaudrate();
time = get_tmr10ms();
}
}
void gpsSendFrame(const char * frame)
{
if (!gpsSerialDrv) return;
auto _sendByte = gpsSerialDrv->sendByte;
if (!_sendByte) return;
// send given frame, add checksum and CRLF
uint8_t parity = 0;
TRACE_NOCRLF("gps> %s", frame);
while (*frame) {
if (*frame != '$') parity ^= *frame;
_sendByte(gpsSerialCtx, *frame);
++frame;
switch (gpsProtocol) {
case GPS_PROTOCOL_NMEA:
gpsSendFrameNMEA(frame);
break;
}
_sendByte(gpsSerialCtx, '*');
_sendByte(gpsSerialCtx, hex(parity >> 4));
_sendByte(gpsSerialCtx, hex(parity & 0x0F));
_sendByte(gpsSerialCtx, '\r');
_sendByte(gpsSerialCtx, '\n');
TRACE("*%02x", parity);
}
radio/src/gps.h
+9 -2
View File
@@ -22,6 +22,7 @@
#ifndef _GPS_H_
#define _GPS_H_
#include "opentx.h"
#include <inttypes.h>
struct gpsdata_t
@@ -32,7 +33,7 @@ struct gpsdata_t
uint8_t numSat;
uint32_t packetCount;
uint32_t errorCount;
uint16_t altitude; // altitude in 0.1m
int32_t altitude; // altitude in 0.1m
uint16_t speed; // speed in 0.1m/s
uint16_t groundCourse; // degrees * 10
uint16_t hdop;
@@ -44,8 +45,12 @@ extern gpsdata_t gpsData;
extern uint8_t gpsTraceEnabled;
#endif
#define GPS_PROTOCOL_AUTO 0
#define GPS_PROTOCOL_NMEA 1
#define GPS_PROTOCOL_UBX 2
// Setup GPS serial port
void gpsSetSerialDriver(void* ctx, const etx_serial_driver_t* drv);
void gpsSetSerialDriver(void* ctx, const etx_serial_driver_t* drv, int protocol);
// Periodic processing
void gpsWakeup();
@@ -53,4 +58,6 @@ void gpsWakeup();
// Send a 0-terminated frame
void gpsSendFrame(const char * frame);
uint32_t GPS_coord_to_degrees(const char * coordinateString);
#endif // _GPS_H_
radio/src/gps_nmea.cpp
+300
View File
@@ -0,0 +1,300 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
/*
* This file is based on code from Cleanflight project
* https://github.com/cleanflight/cleanflight
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include "opentx.h"
#include "gps.h"
#include <ctype.h>
/* This is a light implementation of a GPS frame decoding
This should work with most of modern GPS devices configured to output 5 frames.
It assumes there are some NMEA GGA frames to decode on the serial bus
Now verifies checksum correctly before applying data
Here we use only the following data :
- latitude
- longitude
- GPS fix is/is not ok
- GPS num sat (4 is enough to be +/- reliable)
// added by Mis
- GPS altitude (for OSD displaying)
- GPS speed (for OSD displaying)
*/
#define NO_FRAME 0
#define FRAME_GGA 1
#define FRAME_RMC 2
#define DIGIT_TO_VAL(_x) (_x - '0')
extern const etx_serial_driver_t* gpsSerialDrv;
extern void* gpsSerialCtx;
char hex(uint8_t b) {
return b > 9 ? b + 'A' - 10 : b + '0';
}
void gpsSendFrameNMEA(const char * frame)
{
if (!gpsSerialDrv) return;
auto _sendByte = gpsSerialDrv->sendByte;
if (!_sendByte) return;
// send given frame, add checksum and CRLF
uint8_t parity = 0;
TRACE_NOCRLF("gps> %s", frame);
while (*frame) {
if (*frame != '$') parity ^= *frame;
_sendByte(gpsSerialCtx, *frame);
++frame;
}
_sendByte(gpsSerialCtx, '*');
_sendByte(gpsSerialCtx, hex(parity >> 4));
_sendByte(gpsSerialCtx, hex(parity & 0x0F));
_sendByte(gpsSerialCtx, '\r');
_sendByte(gpsSerialCtx, '\n');
TRACE("*%02x", parity);
}
// helper functions
uint32_t grab_fields(char * src, uint8_t mult)
{
uint32_t i;
uint32_t tmp = 0;
for (i = 0; src[i] != 0; i++) {
if (src[i] == '.') {
i++;
if (mult == 0)
break;
else
src[i + mult] = 0;
}
tmp *= 10;
if (src[i] >= '0' && src[i] <= '9')
tmp += src[i] - '0';
if (i >= 15)
return 0; // out of bounds
}
return tmp;
}
typedef struct gpsDataNmea_s
{
uint8_t fix;
int32_t latitude;
int32_t longitude;
uint8_t numSat;
uint16_t altitude;
uint16_t speed;
uint16_t groundCourse;
uint16_t hdop;
uint32_t date;
uint32_t time;
} gpsDataNmea_t;
bool gpsNewFrameNMEA(char c)
{
static gpsDataNmea_t gps_Msg;
uint8_t frameOK = 0;
static uint8_t param = 0, offset = 0, parity = 0;
static char string[15];
static uint8_t checksum_param, gps_frame = NO_FRAME;
switch (c) {
case '$':
param = 0;
offset = 0;
parity = 0;
break;
case ',':
case '*':
string[offset] = 0;
if (param == 0) {
// Frame identification (accept all GPS talkers (GP: GPS, GL:Glonass, GN:combination, etc...))
gps_frame = NO_FRAME;
if (string[0] == 'G' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') {
gps_frame = FRAME_GGA;
}
else if (string[0] == 'G' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') {
gps_frame = FRAME_RMC;
}
else {
// turn off this frame (do this only once a second)
static gtime_t lastGpsCmdSent = 0;
if (string[0] == 'G' && g_rtcTime != lastGpsCmdSent) {
lastGpsCmdSent = g_rtcTime;
char cmd[] = "$PUBX,40,GSV,0,0,0,0";
cmd[9] = string[2];
cmd[10] = string[3];
cmd[11] = string[4];
gpsSendFrameNMEA(cmd);
}
}
}
switch (gps_frame) {
case FRAME_GGA: //************* GPGGA FRAME parsing
switch (param) {
case 2:
gps_Msg.latitude = GPS_coord_to_degrees(string);
break;
case 3:
if (string[0] == 'S')
gps_Msg.latitude *= -1;
break;
case 4:
gps_Msg.longitude = GPS_coord_to_degrees(string);
break;
case 5:
if (string[0] == 'W')
gps_Msg.longitude *= -1;
break;
case 6:
if (string[0] > '0') {
gps_Msg.fix = 1;
}
else {
gps_Msg.fix = 0;
}
break;
case 7:
gps_Msg.numSat = grab_fields(string, 0);
break;
case 8:
gps_Msg.hdop = grab_fields(string, 1) * 10;
break;
case 9:
gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis
break;
}
break;
case FRAME_RMC: //************* GPRMC FRAME parsing
switch (param) {
case 1:
gps_Msg.time = grab_fields(string, 0);
break;
case 2:
if (string[0] == 'A') {
gps_Msg.fix = 1;
}
else {
gps_Msg.fix = 0;
}
break;
case 7:
gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L); // speed in cm/s added by Mis
break;
case 8:
gps_Msg.groundCourse = (grab_fields(string, 1)); // ground course deg * 10
break;
case 9:
gps_Msg.date = grab_fields(string, 0);
break;
}
break;
}
param++;
offset = 0;
if (c == '*')
checksum_param = 1;
else
parity ^= c;
break;
case '\r':
case '\n':
if (checksum_param) { //parity checksum
uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') +
((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
if (checksum == parity) {
gpsData.packetCount++;
switch (gps_frame) {
case FRAME_GGA:
frameOK = 1;
gpsData.fix = gps_Msg.fix;
gpsData.numSat = gps_Msg.numSat;
gpsData.hdop = gps_Msg.hdop;
if (gps_Msg.fix) {
__disable_irq(); // do the atomic update of lat/lon
gpsData.latitude = gps_Msg.latitude;
gpsData.longitude = gps_Msg.longitude;
gpsData.altitude = gps_Msg.altitude;
__enable_irq();
}
break;
case FRAME_RMC:
gpsData.speed = gps_Msg.speed;
gpsData.groundCourse = gps_Msg.groundCourse;
#if defined(RTCLOCK)
// set RTC clock if needed
if (g_eeGeneral.adjustRTC && gps_Msg.fix) {
div_t qr = div(gps_Msg.date, 100);
uint8_t year = qr.rem;
qr = div(qr.quot, 100);
uint8_t mon = qr.rem;
uint8_t day = qr.quot;
qr = div(gps_Msg.time, 100);
uint8_t sec = qr.rem;
qr = div(qr.quot, 100);
uint8_t min = qr.rem;
uint8_t hour = qr.quot;
rtcAdjust(year+2000, mon, day, hour, min, sec);
}
#endif
} // end switch
}
else {
gpsData.errorCount++;
}
}
checksum_param = 0;
break;
default:
if (offset < 15)
string[offset++] = c;
if (!checksum_param)
parity ^= c;
}
return frameOK;
}
radio/src/gps_nmea.h
+31
View File
@@ -0,0 +1,31 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#ifndef _GPS_NMEA_H_
#define _GPS_NMEA_H_
#include <inttypes.h>
bool gpsNewFrameNMEA(char c);
void gpsSendFrameNMEA(const char * frame);
#endif // _GPS_NMEA_H_
radio/src/gps_ubx.cpp
+296
View File
@@ -0,0 +1,296 @@
/*
* Copyright (C) EdgeTX
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* This file is based on code from Betaflight project
* https://github.com/betaflight/betaflight
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <ctype.h>
#include "gps.h"
#include "opentx.h"
extern const etx_serial_driver_t* gpsSerialDrv;
extern void* gpsSerialCtx;
// https://docs.ros.org/en/noetic/api/ublox_msgs/html/msg/NavPVT.html
typedef struct ubxNavPvt_s {
uint32_t time; // GPS Millisecond time of week [ms]
uint16_t year; // Year (UTC)
uint8_t month; // Month, range 1..12 (UTC)
uint8_t day; // Day of month, range 1..31 (UTC)
uint8_t hour; // Hour of day, range 0..23 (UTC)
uint8_t min; // Minute of hour, range 0..59 (UTC)
uint8_t sec; // Seconds of minute, range 0..60 (UTC)
uint8_t valid; // Validity flags
uint32_t tAcc; // time accuracy estimate [ns] (UTC)
int32_t nano; // fraction of a second [ns], range -1e9 .. 1e9 (UTC)
uint8_t fixType; // GNSS fix Type
uint8_t flags; // Fix Status Flags
uint8_t flags2; // Additional Flags
uint8_t numSV; // Number of SVs used in Nav Solution
int32_t lon; // Longitude [deg / 1e-7]
int32_t lat; // Latitude [deg / 1e-7]
int32_t height; // Height above Ellipsoid [mm]
int32_t hMSL; // Height above mean sea level [mm]
uint32_t hAcc; // Horizontal Accuracy Estimate [mm]
uint32_t vAcc; // Vertical Accuracy Estimate [mm]
int32_t velN; // NED north velocity [mm/s]
int32_t velE; // NED east velocity [mm/s]
int32_t velD; // NED down velocity [mm/s]
int32_t gSpeed; // Ground Speed (2-D) [mm/s]
int32_t headMot; // Heading of motion 2-D [deg / 1e-5]
uint32_t sAcc; // Speed Accuracy Estimate [mm/s]
uint32_t headAcc; // Heading Accuracy Estimate (both motion & vehicle)
uint16_t pDOP; // Position DOP [1 / 0.01]
uint8_t flags3; // Additional Flags
uint8_t reserved0[5]; // Reserved
int32_t headVeh; // Heading of vehicle (2-D) [deg / 1e-5]
int16_t magDec; // # Magnetic declination [deg / 1e-2]
uint16_t magAcc; // Magnetic declination accuracy [deg / 1e-2]
} ubxNavPvt_t;
typedef struct ubxNavDop_s {
uint32_t itow; // GPS Millisecond Time of Week
uint16_t gdop; // Geometric DOP
uint16_t pdop; // Position DOP
uint16_t tdop; // Time DOP
uint16_t vdop; // Vertical DOP
uint16_t hdop; // Horizontal DOP
uint16_t ndop; // Northing DOP
uint16_t edop; // Easting DOP
} ubxNavDop_t;
typedef struct ubxCfgMsg_s {
uint8_t msgClass;
uint8_t msgID;
uint8_t rate;
} ubxCfgMsg_t;
static void gpsSendMessage(uint16_t msg_type, uint16_t msg_len,
uint8_t* payload)
{
if (!gpsSerialDrv) return;
auto _sendByte = gpsSerialDrv->sendByte;
if (!_sendByte) return;
uint8_t ck_a, ck_b;
gpsSerialDrv->sendByte(gpsSerialCtx, 0xb5);
gpsSerialDrv->sendByte(gpsSerialCtx, 0x62);
ck_b = ck_a = (msg_type >> 8);
gpsSerialDrv->sendByte(gpsSerialCtx, msg_type >> 8);
ck_b += (ck_a += (msg_type & 0xff));
gpsSerialDrv->sendByte(gpsSerialCtx, msg_type & 0xff);
ck_b += (ck_a += (msg_len & 0xff));
gpsSerialDrv->sendByte(gpsSerialCtx, msg_len & 0xff);
ck_b += (ck_a += (msg_len >> 8));
gpsSerialDrv->sendByte(gpsSerialCtx, msg_len >> 8);
for (int n = 0; n < msg_len; n++) {
ck_b += (ck_a += payload[n]);
}
if (msg_len > 0) {
gpsSerialDrv->sendBuffer(gpsSerialCtx, payload, msg_len);
}
gpsSerialDrv->sendByte(gpsSerialCtx, ck_a);
gpsSerialDrv->sendByte(gpsSerialCtx, ck_b);
}
static void configureGps(bool detect)
{
static int state = 0;
if (detect) {
state = 0;
return;
}
auto txCompleted = gpsSerialDrv->txCompleted;
if (txCompleted && !txCompleted(gpsSerialCtx)) return;
switch (state) {
case 0:
gpsSendMessage(0x0a04, 0, nullptr);
ubxCfgMsg_t val;
val.msgClass = 0x01;
val.msgID = 0x07;
val.rate = 1;
gpsSendMessage(0x0601, sizeof(ubxCfgMsg_t), (uint8_t*)&val);
val.msgID = 0x04;
gpsSendMessage(0x0601, sizeof(ubxCfgMsg_t), (uint8_t*)&val);
state++;
}
}
static void gpsProcessMessage(uint16_t msg_type, uint16_t msg_len,
uint8_t* payload)
{
if (msg_type == 0x0107) {
auto pvt = (ubxNavPvt_t*)payload;
gpsData.fix = pvt->flags & 0x01;
gpsData.numSat = pvt->numSV;
gpsData.speed = pvt->gSpeed / 100; // speed in 0.1m/s
gpsData.groundCourse = pvt->headMot / 10000; // degrees * 10
if (gpsData.fix) {
__disable_irq(); // do the atomic update of lat/lon
gpsData.longitude = pvt->lon / 10; // degrees * 1.000.000
gpsData.latitude = pvt->lat / 10; // degrees * 1.000.000
gpsData.altitude = pvt->hMSL / 100; // altitude in 0.1m
__enable_irq();
}
#if defined(RTCLOCK)
// set RTC clock if needed
if (g_eeGeneral.adjustRTC && (pvt->valid & 0x03) == 0x03) {
rtcAdjust(pvt->year, pvt->month, pvt->day, pvt->hour, pvt->min, pvt->sec);
}
#endif
}
if (msg_type == 0x0104) {
auto dop = (ubxNavDop_t*)payload;
gpsData.hdop = dop->hdop;
}
}
bool gpsNewFrameUBX(uint8_t c, bool detect)
{
configureGps(detect);
const auto BUFFER_SIZE = 100u;
static int state = 0;
static uint16_t msg_type;
static uint16_t msg_len;
static uint16_t msg_pos;
static uint8_t buf[BUFFER_SIZE];
static uint8_t ck_a;
static uint8_t ck_b;
bool result = false;
switch (state) {
case 0: // Search Sync Char 1
if (c == 0xb5) {
state = 1;
}
break;
case 1: // Search Sync Char 2
if (c == 0x62) {
state = 2;
} else {
state = 0;
}
break;
case 2: // Get message class
ck_b = ck_a = c;
msg_type = c << 8;
state = 3;
break;
case 3: // Get message id
ck_b += (ck_a += c);
msg_type |= c;
state = 4;
break;
case 4: // Get length 1
ck_b += (ck_a += c);
msg_len = c;
state = 5;
break;
case 5: // Get length 2
ck_b += (ck_a += c);
msg_len |= c << 8;
msg_pos = 0;
if (msg_len == 0) {
state = 7;
break;
}
if ((msg_type != 0x0107 && msg_type != 0x0104) || msg_len > BUFFER_SIZE) {
state = 9;
break;
}
state = 6;
break;
case 6: // Get payload
ck_b += (ck_a += c);
buf[msg_pos++] = c;
if (msg_pos >= msg_len) {
state = 7;
}
break;
case 7: // Get checksum 1
if (c == ck_a) {
state = 8;
} else {
gpsData.errorCount++;
state = 0;
}
break;
case 8: // Get checksum 2, end of frame
if (c == ck_b) {
gpsProcessMessage(msg_type, msg_len, buf);
gpsData.packetCount++;
result = true;
} else {
gpsData.errorCount++;
}
state = 0;
break;
case 9: // Discard message
ck_b += (ck_a += c);
msg_pos++;
if (msg_pos >= msg_len) {
state = 10;
result = true;
}
break;
case 10: // Get checksum 1 of discarded message
if (c == ck_a) {
state = 11;
} else {
gpsData.errorCount++;
state = 0;
}
break;
case 11: // Get checksum 2 of discarded message
if (c == ck_b) {
// We have a valid UBX message
gpsData.packetCount++;
result = true;
} else {
gpsData.errorCount++;
}
state = 0;
break;
}
return result;
}
radio/src/gps_ubx.h
+23
View File
@@ -0,0 +1,23 @@
/*
* Copyright (C) EdgeTX
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#ifndef _GPS_UBX_H_
#define _GPS_UBX_H_
#include <inttypes.h>
bool gpsNewFrameUBX(uint8_t c, bool detect);
#endif // _GPS_UBX_H_
radio/src/serial.cpp
+1 -1
View File
@@ -242,7 +242,7 @@ static void serialSetCallBacks(int mode, void* ctx, const etx_serial_port_t* por
#if defined(INTERNAL_GPS)
case UART_MODE_GPS:
gpsSetSerialDriver(ctx, drv);
gpsSetSerialDriver(ctx, drv, GPS_PROTOCOL_AUTO);
break;
#endif
radio/src/targets/horus/CMakeLists.txt
+4 -4
View File
@@ -77,7 +77,7 @@ if (PCB STREQUAL X10)
add_definitions(-DRADIO_FAMILY_T16)
set(DEFAULT_INTERNAL_MODULE MULTIMODULE CACHE STRING "Default internal module")
option(BLUETOOTH "Support for bluetooth module" OFF)
option(INTERNAL_GPS "Support for internal NMEA GPS" ON)
option(INTERNAL_GPS "Support for internal GPS" ON)
add_definitions(-DMANUFACTURER_JUMPER)
if (NOT BLUETOOTH)
set(AUX2_SERIAL ON)
@@ -90,7 +90,7 @@ if (PCB STREQUAL X10)
add_definitions(-DRADIO_FAMILY_T16)
set(DEFAULT_INTERNAL_MODULE MULTIMODULE CACHE STRING "Default internal module")
option(BLUETOOTH "Support for bluetooth module" OFF)
option(INTERNAL_GPS "Support for internal NMEA GPS" ON)
option(INTERNAL_GPS "Support for internal GPS" ON)
set(SWSERIALPOWER YES)
set(USB_CHARGER YES)
set(DEFAULT_THEME "DARKBLUE")
@@ -107,7 +107,7 @@ if (PCB STREQUAL X10)
add_definitions(-DRADIO_FAMILY_T16)
set(DEFAULT_INTERNAL_MODULE MULTIMODULE CACHE STRING "Default internal module")
option(BLUETOOTH "Support for bluetooth module" OFF)
option(INTERNAL_GPS "Support for internal NMEA GPS" ON)
option(INTERNAL_GPS "Support for internal GPS" ON)
add_definitions(-DMANUFACTURER_JUMPER)
if (NOT BLUETOOTH)
set(AUX2_SERIAL ON)
@@ -199,7 +199,7 @@ if(DISK_CACHE)
endif()
if(INTERNAL_GPS)
set(SRC ${SRC} gps.cpp)
set(SRC ${SRC} gps.cpp gps_nmea.cpp gps_ubx.cpp)
add_definitions(-DINTERNAL_GPS)
message("-- Internal GPS enabled")
endif()
radio/src/targets/simu/CMakeLists.txt
+16
View File
@@ -20,8 +20,20 @@ set(SIMU_DRIVERS
bt_driver.cpp
timers_driver.cpp
abnormal_reboot.cpp
rs232-linux.c
)
option(SIMU_AUX "Allow the simulator to use the host's RS-232 port" OFF)
set(SIMU_COM_PORT "2" CACHE STRING "Set COM port number to use")
if (SIMU_AUX)
if(WIN32)
set(SIMU_DRIVERS ${SIMU_DRIVERS} rs232-win.c)
else()
set(SIMU_DRIVERS ${SIMU_DRIVERS} rs232-linux.c)
endif()
endif()
set(HW_DESC_JSON ${FLAVOUR}.json)
AddHardwareDefTarget(${HW_DESC_JSON})
AddHWGenTarget(${HW_DESC_JSON} simu_keys simu_keys.inc)
@@ -70,6 +82,10 @@ target_compile_options(simu_drivers PRIVATE ${SIMU_SRC_OPTIONS})
set_property(TARGET simu_drivers PROPERTY POSITION_INDEPENDENT_CODE ON)
if(SIMU_AUX)
target_compile_definitions(simu_drivers PRIVATE -DSIMU_COM_PORT=${SIMU_COM_PORT})
endif()
# Replace SIMU_SRC with the objects from radiolib_native & simu_drivers
set(SIMU_SRC
$<TARGET_OBJECTS:radiolib_native>
radio/src/targets/simu/rs232-linux.c
+261
View File
@@ -0,0 +1,261 @@
/*
Cross-platform serial / RS232 library
Version 0.21, 11/10/2015
-> LINUX and MacOS implementation
-> rs232-linux.c
The MIT License (MIT)
Copyright (c) 2013-2015 Frédéric Meslin, Florent Touchard
Email: fredericmeslin@hotmail.com
Website: www.fredslab.net
Twitter: @marzacdev
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#if defined(__unix__) || defined(__unix) || \
defined(__APPLE__) && defined(__MACH__)
#define _DARWIN_C_SOURCE
#include "rs232.h"
#include <unistd.h>
#define __USE_MISC // For CRTSCTS
#include <termios.h>
#include <fcntl.h>
#include <dirent.h>
#define __USE_SVID // For strdup
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
/*****************************************************************************/
/** Base name for COM devices */
#if defined(__APPLE__) && defined(__MACH__)
static const char * devBases[] = {
"tty."
};
static int noBases = 1;
#else
static const char * devBases[] = {
"ttyACM", "ttyUSB", "rfcomm", "ttyS"
};
static int noBases = 4;
#endif
/*****************************************************************************/
typedef struct {
char * port;
int handle;
} COMDevice;
#define COM_MAXDEVICES 64
static COMDevice comDevices[COM_MAXDEVICES];
static int noDevices = 0;
/*****************************************************************************/
/** Private functions */
void _AppendDevices(const char * base);
int _BaudFlag(int BaudRate);
/*****************************************************************************/
int comEnumerate()
{
for (int i = 0; i < noDevices; i++) {
if (comDevices[i].port) free(comDevices[i].port);
comDevices[i].port = NULL;
}
noDevices = 0;
for (int i = 0; i < noBases; i++)
_AppendDevices(devBases[i]);
return noDevices;
}
void comTerminate()
{
comCloseAll();
for (int i = 0; i < noDevices; i++) {
if (comDevices[i].port) free(comDevices[i].port);
comDevices[i].port = NULL;
}
}
int comGetNoPorts()
{
return noDevices;
}
/*****************************************************************************/
int comFindPort(const char * name)
{
int p;
for (p = 0; p < noDevices; p++)
if (strcmp(name, comDevices[p].port) == 0)
return p;
return -1;
}
const char * comGetInternalName(int index)
{
#define COM_MAXNAME 128
static char name[COM_MAXNAME];
if (index >= noDevices || index < 0)
return NULL;
sprintf(name, "/dev/%s", comDevices[index].port);
return name;
}
const char * comGetPortName(int index) {
if (index >= noDevices || index < 0)
return NULL;
return comDevices[index].port;
}
/*****************************************************************************/
int comOpen(int index, int baudrate)
{
if (index >= noDevices || index < 0)
return 0;
// Close if already open
COMDevice * com = &comDevices[index];
if (com->handle >= 0) comClose(index);
// Open port
printf("Try %s \n", comGetInternalName(index));
int handle = open(comGetInternalName(index), O_RDWR | O_NOCTTY | O_NDELAY);
if (handle < 0)
return 0;
printf("Open %s \n", comGetInternalName(index));
// General configuration
struct termios config;
memset(&config, 0, sizeof(config));
tcgetattr(handle, &config);
config.c_iflag &= ~(INLCR | ICRNL);
config.c_iflag |= IGNPAR | IGNBRK;
config.c_oflag &= ~(OPOST | ONLCR | OCRNL);
config.c_cflag &= ~(PARENB | PARODD | CSTOPB | CSIZE | CRTSCTS);
config.c_cflag |= CLOCAL | CREAD | CS8;
config.c_lflag &= ~(ICANON | ISIG | ECHO);
int flag = _BaudFlag(baudrate);
cfsetospeed(&config, flag);
cfsetispeed(&config, flag);
// Timeouts configuration
config.c_cc[VTIME] = 1;
config.c_cc[VMIN] = 0;
//fcntl(handle, F_SETFL, FNDELAY);
// Validate configuration
if (tcsetattr(handle, TCSANOW, &config) < 0) {
close(handle);
return 0;
}
com->handle = handle;
return 1;
}
void comClose(int index)
{
if (index >= noDevices || index < 0)
return;
COMDevice * com = &comDevices[index];
if (com->handle < 0)
return;
tcdrain(com->handle);
close(com->handle);
com->handle = -1;
}
void comCloseAll()
{
for (int i = 0; i < noDevices; i++)
comClose(i);
}
/*****************************************************************************/
int comWrite(int index, const char * buffer, size_t len)
{
if (index >= noDevices || index < 0)
return 0;
if (comDevices[index].handle <= 0)
return 0;
int res = write(comDevices[index].handle, buffer, len);
if (res < 0)
res = 0;
return res;
}
int comRead(int index, char * buffer, size_t len)
{
if (index >= noDevices || index < 0)
return 0;
if (comDevices[index].handle <= 0)
return 0;
int res = read(comDevices[index].handle, buffer, len);
if (res < 0)
res = 0;
return res;
}
/*****************************************************************************/
int _BaudFlag(int BaudRate)
{
switch(BaudRate)
{
case 50: return B50; break;
case 110: return B110; break;
case 134: return B134; break;
case 150: return B150; break;
case 200: return B200; break;
case 300: return B300; break;
case 600: return B600; break;
case 1200: return B1200; break;
case 1800: return B1800; break;
case 2400: return B2400; break;
case 4800: return B4800; break;
case 9600: return B9600; break;
case 19200: return B19200; break;
case 38400: return B38400; break;
case 57600: return B57600; break;
case 115200: return B115200; break;
case 230400: return B230400; break;
default : return B0; break;
}
}
void _AppendDevices(const char * base)
{
int baseLen = strlen(base);
struct dirent * dp;
// Enumerate devices
DIR * dirp = opendir("/dev");
while ((dp = readdir(dirp)) && noDevices < COM_MAXDEVICES) {
if (strlen(dp->d_name) >= baseLen) {
if (memcmp(base, dp->d_name, baseLen) == 0) {
COMDevice * com = &comDevices[noDevices ++];
com->port = (char *) strdup(dp->d_name);
com->handle = -1;
}
}
}
closedir(dirp);
}
#endif // unix
radio/src/targets/simu/rs232-win.c
+322
View File
@@ -0,0 +1,322 @@
/*
Cross-platform serial / RS232 library
Version 0.21, 11/10/2015
-> WIN32 implementation
-> rs232-win.c
The MIT License (MIT)
Copyright (c) 2013-2015 Frédéric Meslin, Florent Touchard
Email: fredericmeslin@hotmail.com
Website: www.fredslab.net
Twitter: @marzacdev
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifdef _WIN32
#include "rs232.h"
#include <stdio.h>
#include <string.h>
/*****************************************************************************/
typedef int bool;
#define true -1
#define false 0
typedef struct {
int port;
void * handle;
} COMDevice;
/*****************************************************************************/
#define COM_MAXDEVICES 64
static COMDevice comDevices[COM_MAXDEVICES];
static int noDevices = 0;
#define COM_MINDEVNAME 16384
const char * comPtn = "COM???";
/*****************************************************************************/
const char * findPattern(const char * string, const char * pattern, int * value);
const char * portInternalName(int index);
/*****************************************************************************/
typedef struct _COMMTIMEOUTS {
uint32_t ReadIntervalTimeout;
uint32_t ReadTotalTimeoutMultiplier;
uint32_t ReadTotalTimeoutConstant;
uint32_t WriteTotalTimeoutMultiplier;
uint32_t WriteTotalTimeoutConstant;
} COMMTIMEOUTS;
typedef struct _DCB {
uint32_t DCBlength;
uint32_t BaudRate;
uint32_t fBinary :1;
uint32_t fParity :1;
uint32_t fOutxCtsFlow :1;
uint32_t fOutxDsrFlow :1;
uint32_t fDtrControl :2;
uint32_t fDsrSensitivity :1;
uint32_t fTXContinueOnXoff :1;
uint32_t fOutX :1;
uint32_t fInX :1;
uint32_t fErrorChar :1;
uint32_t fNull :1;
uint32_t fRtsControl :2;
uint32_t fAbortOnError :1;
uint32_t fDummy2 :17;
uint16_t wReserved;
uint16_t XonLim;
uint16_t XoffLim;
uint8_t ByteSize;
uint8_t Parity;
uint8_t StopBits;
int8_t XonChar;
int8_t XoffChar;
int8_t ErrorChar;
int8_t EofChar;
int8_t EvtChar;
uint16_t wReserved1;
} DCB;
/*****************************************************************************/
/** Windows system constants */
#define ERROR_INSUFFICIENT_BUFFER 122
#define INVALID_HANDLE_VALUE ((void *) -1)
#define GENERIC_READ 0x80000000
#define GENERIC_WRITE 0x40000000
#define OPEN_EXISTING 3
#define MAX_DWORD 0xFFFFFFFF
/*****************************************************************************/
/** Windows system functions */
void * __stdcall CreateFileA(const char * lpFileName, uint32_t dwDesiredAccess, uint32_t dwShareMode, void * lpSecurityAttributes, uint32_t dwCreationDisposition, uint32_t dwFlagsAndAttributes, void * hTemplateFile);
bool __stdcall WriteFile(void * hFile, const void * lpBuffer, uint32_t nNumberOfBytesToWrite, uint32_t * lpNumberOfBytesWritten, void * lpOverlapped);
bool __stdcall ReadFile(void * hFile, void * lpBuffer, uint32_t nNumberOfBytesToRead, uint32_t * lpNumberOfBytesRead, void * lpOverlapped);
bool __stdcall CloseHandle(void * hFile);
uint32_t __stdcall GetLastError(void);
void __stdcall SetLastError(uint32_t dwErrCode);
uint32_t __stdcall QueryDosDeviceA(const char * lpDeviceName, char * lpTargetPath, uint32_t ucchMax);
bool __stdcall GetCommState(void * hFile, DCB * lpDCB);
bool __stdcall GetCommTimeouts(void * hFile, COMMTIMEOUTS * lpCommTimeouts);
bool __stdcall SetCommState(void * hFile, DCB * lpDCB);
bool __stdcall SetCommTimeouts(void * hFile, COMMTIMEOUTS * lpCommTimeouts);
bool __stdcall SetupComm(void * hFile, uint32_t dwInQueue, uint32_t dwOutQueue);
/*****************************************************************************/
int comEnumerate()
{
// Get devices information text
size_t size = COM_MINDEVNAME;
char * list = (char *) malloc(size);
SetLastError(0);
QueryDosDeviceA(NULL, list, size);
while (GetLastError() == ERROR_INSUFFICIENT_BUFFER) {
size *= 2;
char * nlist = realloc(list, size);
if (!nlist) {
free(list);
return 0;
}
list = nlist;
SetLastError(0);
QueryDosDeviceA(NULL, list, size);
}
// Gather all COM ports
int port;
const char * nlist = findPattern(list, comPtn, &port);
noDevices = 0;
while(port > 0 && noDevices < COM_MAXDEVICES) {
COMDevice * com = &comDevices[noDevices ++];
com->port = port;
com->handle = 0;
nlist = findPattern(nlist, comPtn, &port);
}
free(list);
return noDevices;
}
void comTerminate()
{
comCloseAll();
}
int comGetNoPorts()
{
return noDevices;
}
/*****************************************************************************/
const char * comGetPortName(int index)
{
#define COM_MAXNAME 32
static char name[COM_MAXNAME];
if (index < 0 || index >= noDevices)
return 0;
sprintf(name, "COM%i", comDevices[index].port);
return name;
}
int comFindPort(const char * name)
{
for (int i = 0; i < noDevices; i++)
if (strcmp(name, comGetPortName(i)) == 0)
return i;
return -1;
}
const char * comGetInternalName(int index)
{
#define COM_MAXNAME 32
static char name[COM_MAXNAME];
if (index < 0 || index >= noDevices)
return 0;
sprintf(name, "//./COM%i", comDevices[index].port);
return name;
}
/*****************************************************************************/
int comOpen(int index, int baudrate)
{
DCB config;
COMMTIMEOUTS timeouts;
if (index < 0 || index >= noDevices)
return 0;
// Close if already open
COMDevice * com = &comDevices[index];
if (com->handle) comClose(index);
// Open COM port
void * handle = CreateFileA(comGetInternalName(index), GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
if (handle == INVALID_HANDLE_VALUE)
return 0;
com->handle = handle;
// Prepare read / write timeouts
SetupComm(handle, 64, 64);
timeouts.ReadIntervalTimeout = MAX_DWORD;
timeouts.ReadTotalTimeoutConstant = 0;
timeouts.WriteTotalTimeoutConstant = 0;
timeouts.ReadTotalTimeoutMultiplier = 0;
timeouts.WriteTotalTimeoutMultiplier = 0;
SetCommTimeouts(handle, &timeouts);
// Prepare serial communication format
GetCommState(handle, &config);
config.BaudRate = baudrate;
config.fBinary = true;
config.fParity = 0;
config.fErrorChar = 0;
config.fNull = 0;
config.fAbortOnError = 0;
config.ByteSize = 8;
config.Parity = 0;
config.StopBits = 0;
config.EvtChar = '\n';
// Set the port state
if (SetCommState(handle, &config) == 0) {
CloseHandle(handle);
return 0;
}
return 1;
}
void comClose(int index)
{
if (index < 0 || index >= noDevices)
return;
COMDevice * com = &comDevices[index];
if (!com->handle)
return;
CloseHandle(com->handle);
com->handle = 0;
}
void comCloseAll()
{
for (int i = 0; i < noDevices; i++)
comClose(i);
}
/*****************************************************************************/
int comWrite(int index, const char * buffer, size_t len)
{
if (index < 0 || index >= noDevices)
return 0;
COMDevice * com = &comDevices[index];
uint32_t bytes = 0;
WriteFile(com->handle, buffer, len, &bytes, NULL);
return bytes;
}
int comRead(int index, char * buffer, size_t len)
{
if (index < 0 || index >= noDevices)
return 0;
COMDevice * com = &comDevices[index];
uint32_t bytes = 0;
ReadFile(com->handle, buffer, len, &bytes, NULL);
return bytes;
}
/*****************************************************************************/
const char * findPattern(const char * string, const char * pattern, int * value)
{
char c, n = 0;
const char * sp = string;
const char * pp = pattern;
// Check for the string pattern
while (1) {
c = *sp ++;
if (c == '\0') {
if (*pp == '?') break;
if (*sp == '\0') break;
n = 0;
pp = pattern;
}else{
if (*pp == '?') {
// Expect a digit
if (c >= '0' && c <= '9') {
n = n * 10 + (c - '0');
if (*pp ++ == '\0') break;
}else{
n = 0;
pp = comPtn;
}
}else{
// Expect a character
if (c == *pp) {
if (*pp ++ == '\0') break;
}else{
n = 0;
pp = comPtn;
}
}
}
}
// Return the value
* value = n;
return sp;
}
#endif // _WIN32
radio/src/targets/simu/rs232.h
+155
View File
@@ -0,0 +1,155 @@
/*
Cross-platform serial / RS232 library
Version 0.21, 11/10/2015
-> All platforms header
-> rs232.h
The MIT License (MIT)
Copyright (c) 2013-2015 Frédéric Meslin, Florent Touchard
Email: fredericmeslin@hotmail.com
Website: www.fredslab.net
Twitter: @marzacdev
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef RS232_H
#define RS232_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdlib.h>
/*****************************************************************************/
/* Doxywizard specific */
/**
* \mainpage RS232
* \section intro_sec C / C++ RS232 cross-platform serial library
* <b>Version 0.21 - 11/10/2015</b>
*
* Supported platforms:
* - Windows (XP / Win7, possibly 8 and 10)
* - Linux
* - MacOS X
*
* Copyright (c) 2013-2015 Fr&eacute;d&eacute;ric Meslin, Florent Touchard <br>
* Email: fredericmeslin@hotmail.com <br>
* Website: www.fredslab.net <br>
* Twitter: \@marzacdev <br>
*/
/*****************************************************************************/
/**
* \fn int comEnumerate()
* \brief Enumerate available serial ports (Serial, USB serial, Bluetooth serial)
* \return number of enumerated ports
*/
int comEnumerate();
/**
* \fn int comGetNoPorts()
* \brief Return the number of enumerated ports
* \return number of enumerated ports
*/
int comGetNoPorts();
/**
* \fn int comTerminate()
* \brief Release ports and memory resources used by the library
*/
void comTerminate();
/**
* \fn const char * comGetPortName(int index)
* \brief Get port user-friendly name
* \param[in] index port index
* \return null terminated port name
*/
const char * comGetPortName(int index);
/**
* \fn const char * comGetInternalName(int index)
* \brief Get port operating-system name
* \param[in] index port index
* \return null terminated port name
*/
const char * comGetInternalName(int index);
/**
* \fn int comFindPort(const char * name)
* \brief Try to find a port given its user-friendly name
* \param[in] name port name (case sensitive)
* \return index of found port or -1 if not enumerated
*/
int comFindPort(const char * name);
/*****************************************************************************/
/**
* \fn int comOpen(int index, int baudrate)
* \brief Try to open a port at a specific baudrate
* \brief (No parity, single stop bit, no hardware flow control)
* \param[in] index port index
* \param[in] baudrate port baudrate
* \return 1 if opened, 0 if not available
*/
int comOpen(int index, int baudrate);
/**
* \fn void comClose(int index)
* \brief Close an opened port
* \param[in] index port index
*/
void comClose(int index);
/**
* \fn void comCloseAll()
* \brief Close all opened ports
*/
void comCloseAll();
/*****************************************************************************/
/**
* \fn int comWrite(int index, const char * buffer, size_t len)
* \brief Write data to the port (non-blocking)
* \param[in] index port index
* \param[in] buffer pointer to transmit buffer
* \param[in] len length of transmit buffer in bytes
* \return number of bytes transferred
*/
int comWrite(int index, const char * buffer, size_t len);
/**
* \fn int comRead(int index, const char * buffer, size_t len)
* \brief Read data from the port (non-blocking)
* \param[in] index port index
* \param[in] buffer pointer to receive buffer
* \param[in] len length of receive buffer in bytes
* \return number of bytes transferred
*/
int comRead(int index, char * buffer, size_t len);
#ifdef __cplusplus
}
#endif
#endif // RS232_H
radio/src/targets/simu/simpgmspace.cpp
+43 -6
View File
@@ -36,6 +36,9 @@
#include <errno.h>
#include <stdarg.h>
#include <string>
#ifdef SIMU_COM_PORT
#include "rs232.h"
#endif
#if !defined (_MSC_VER) || defined (__GNUC__)
#include <chrono>
@@ -600,11 +603,45 @@ const etx_serial_port_t UsbSerialPort = { "USB-VCP", nullptr, nullptr };
#endif
#if defined(AUX_SERIAL) || defined(AUX2_SERIAL)
static void* _fake_drv_init(void*, const etx_serial_init*) { return (void*)1; }
static void _fake_drv_fct1(void*) {}
static void _fake_drv_send_byte(void*, uint8_t) {}
static void _fake_drv_send_buffer(void*, const uint8_t*, uint32_t) {}
static int _fake_drv_get_byte(void*, uint8_t*) { return 0; }
static void* _fake_drv_init(void* n, const etx_serial_init* dev) {
#ifdef SIMU_COM_PORT
static bool init = false;
if (!init) {
comEnumerate();
init=true;
}
comOpen(SIMU_COM_PORT, dev->baudrate);
#endif
return (void*)1;
}
static void _fake_drv_fct1(void*) {
#ifdef SIMU_COM_PORT
comClose(SIMU_COM_PORT);
#endif
}
static void _fake_drv_send_byte(void*, uint8_t b) {
#ifdef SIMU_COM_PORT
comWrite(SIMU_COM_PORT, (char*)&b, 1);
#endif
}
static void _fake_drv_send_buffer(void*, const uint8_t* b, uint32_t l) {
#ifdef SIMU_COM_PORT
comWrite(SIMU_COM_PORT, (char*)b, l);
#endif
}
static int _fake_drv_get_byte(void*, uint8_t* b) {
#ifdef SIMU_COM_PORT
return comRead(SIMU_COM_PORT, (char*)b, 1);
#else
return 0;
#endif
}
static void _fake_drv_set_baudrate(void*, uint32_t baudrate) {
#ifdef SIMU_COM_PORT
comClose(SIMU_COM_PORT);
comOpen(SIMU_COM_PORT, baudrate);
#endif
}
static const etx_serial_driver_t _fake_drv = {
.init = _fake_drv_init,
.deinit = _fake_drv_fct1,
@@ -619,7 +656,7 @@ static const etx_serial_driver_t _fake_drv = {
.copyRxBuffer = nullptr,
.clearRxBuffer = nullptr,
.getBaudrate = nullptr,
.setBaudrate = nullptr,
.setBaudrate = _fake_drv_set_baudrate,
.setPolarity = nullptr,
.setHWOption = nullptr,
.setReceiveCb = nullptr,