diff --git a/radio/src/gps.cpp b/radio/src/gps.cpp index c590d6f5142..e822a7b1e23 100644 --- a/radio/src/gps.cpp +++ b/radio/src/gps.cpp @@ -38,28 +38,15 @@ */ #include "opentx.h" +#include "gps.h" +#include "gps_nmea.h" +#include "gps_ubx.h" #include 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 - } - return tmp; -} + const int baudrates_count = 5; + const uint32_t baudrates[] = {9600, 57600, 115200, 19200, 38400}; + static uint8_t current_rate = 0; -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; + auto setBaudrate = gpsSerialDrv->setBaudrate; + if (setBaudrate == nullptr) return; + setBaudrate(gpsSerialCtx, baudrates[++current_rate % baudrates_count]); +} -bool gpsNewFrameNMEA(char c) +static void autodetectProtocol(uint8_t 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); - } - } + 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; } - 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; - + if (gpsNewFrameUBX(c, true)) { + gpsProtocol = GPS_PROTOCOL_UBX; + state = 0; + return; } - 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++; - } + uint32_t new_time = get_tmr10ms(); + if (new_time - time > 20) { + // No message received + changeBaudrate(); + time = new_time; } - checksum_param = 0; - break; - default: - if (offset < 15) - string[offset++] = c; - if (!checksum_param) - parity ^= c; } - return frameOK; } -bool gpsNewFrame(uint8_t c) +static void detectDisconnected(bool has_frame) { - return gpsNewFrameNMEA(c); + static tmr10ms_t time = 0; + + if (has_frame) { + time = get_tmr10ms(); + } else if (time > 0 && get_tmr10ms() - time > 500) { + gpsProtocol = GPS_PROTOCOL_AUTO; + time = get_tmr10ms(); + } } + 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); } diff --git a/radio/src/gps.h b/radio/src/gps.h index 0de7aa79005..dfed2ea8f4d 100644 --- a/radio/src/gps.h +++ b/radio/src/gps.h @@ -22,6 +22,7 @@ #ifndef _GPS_H_ #define _GPS_H_ +#include "opentx.h" #include 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_ diff --git a/radio/src/gps_nmea.cpp b/radio/src/gps_nmea.cpp new file mode 100644 index 00000000000..9aab3c27511 --- /dev/null +++ b/radio/src/gps_nmea.cpp @@ -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 . + */ + +#include "opentx.h" +#include "gps.h" +#include + +/* 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; +} diff --git a/radio/src/gps_nmea.h b/radio/src/gps_nmea.h new file mode 100644 index 00000000000..7acf7f01590 --- /dev/null +++ b/radio/src/gps_nmea.h @@ -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 + +bool gpsNewFrameNMEA(char c); + +void gpsSendFrameNMEA(const char * frame); + +#endif // _GPS_NMEA_H_ diff --git a/radio/src/gps_ubx.cpp b/radio/src/gps_ubx.cpp new file mode 100644 index 00000000000..1d3890dad90 --- /dev/null +++ b/radio/src/gps_ubx.cpp @@ -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 . + */ + +#include + +#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; +} diff --git a/radio/src/gps_ubx.h b/radio/src/gps_ubx.h new file mode 100644 index 00000000000..52136516651 --- /dev/null +++ b/radio/src/gps_ubx.h @@ -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 + +bool gpsNewFrameUBX(uint8_t c, bool detect); + +#endif // _GPS_UBX_H_ diff --git a/radio/src/serial.cpp b/radio/src/serial.cpp index b4382af1865..f1330ccfa83 100644 --- a/radio/src/serial.cpp +++ b/radio/src/serial.cpp @@ -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 diff --git a/radio/src/targets/horus/CMakeLists.txt b/radio/src/targets/horus/CMakeLists.txt index 6bfe8554006..c9424dd3896 100644 --- a/radio/src/targets/horus/CMakeLists.txt +++ b/radio/src/targets/horus/CMakeLists.txt @@ -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() diff --git a/radio/src/targets/simu/CMakeLists.txt b/radio/src/targets/simu/CMakeLists.txt index 8b63fe4c68e..7d6e20a9e48 100644 --- a/radio/src/targets/simu/CMakeLists.txt +++ b/radio/src/targets/simu/CMakeLists.txt @@ -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 $ diff --git a/radio/src/targets/simu/rs232-linux.c b/radio/src/targets/simu/rs232-linux.c new file mode 100644 index 00000000000..c8e629a135a --- /dev/null +++ b/radio/src/targets/simu/rs232-linux.c @@ -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 +#define __USE_MISC // For CRTSCTS +#include +#include +#include + +#define __USE_SVID // For strdup +#include +#include +#include + +/*****************************************************************************/ +/** 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 diff --git a/radio/src/targets/simu/rs232-win.c b/radio/src/targets/simu/rs232-win.c new file mode 100644 index 00000000000..bfc50c9c95c --- /dev/null +++ b/radio/src/targets/simu/rs232-win.c @@ -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 +#include + +/*****************************************************************************/ +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 diff --git a/radio/src/targets/simu/rs232.h b/radio/src/targets/simu/rs232.h new file mode 100644 index 00000000000..ab90cd796c4 --- /dev/null +++ b/radio/src/targets/simu/rs232.h @@ -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 + #include + +/*****************************************************************************/ + /* Doxywizard specific */ + /** + * \mainpage RS232 + * \section intro_sec C / C++ RS232 cross-platform serial library + * Version 0.21 - 11/10/2015 + * + * Supported platforms: + * - Windows (XP / Win7, possibly 8 and 10) + * - Linux + * - MacOS X + * + * Copyright (c) 2013-2015 Frédéric Meslin, Florent Touchard
+ * Email: fredericmeslin@hotmail.com
+ * Website: www.fredslab.net
+ * Twitter: \@marzacdev
+ */ + +/*****************************************************************************/ + /** + * \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 diff --git a/radio/src/targets/simu/simpgmspace.cpp b/radio/src/targets/simu/simpgmspace.cpp index d1f3aaedcaa..50635fc9002 100644 --- a/radio/src/targets/simu/simpgmspace.cpp +++ b/radio/src/targets/simu/simpgmspace.cpp @@ -36,6 +36,9 @@ #include #include #include +#ifdef SIMU_COM_PORT + #include "rs232.h" +#endif #if !defined (_MSC_VER) || defined (__GNUC__) #include @@ -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,