diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index b4fd67ce4..eeb4e1a4a 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -36,11 +36,14 @@ #include #include "NMEA.h" +#include "UBX.h" + #include "gpsposition.h" #include "homelocation.h" #include "gpstime.h" #include "gpssatellites.h" +#include "gpsvelocity.h" #include "WorldMagModel.h" #include "CoordinateConversions.h" #include "hwsettings.h" @@ -137,6 +140,7 @@ int32_t GPSInitialize(void) if (gpsPort && gpsEnabled) { GPSPositionInitialize(); + GPSVelocityInitialize(); #if !defined(PIOS_GPS_MINIMAL) GPSTimeInitialize(); GPSSatellitesInitialize(); @@ -167,10 +171,14 @@ static void gpsTask(void *parameters) portTickType xDelay = 100 / portTICK_RATE_MS; uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; GPSPositionData GpsData; + UBXPacket *ubx = (UBXPacket *)gps_rx_buffer; uint8_t rx_count = 0; - bool start_flag = false; +// bool start_flag = false; bool found_cr = false; + enum proto_states {START,NMEA,UBX_SY2,UBX_CLASS,UBX_ID,UBX_LEN1, + UBX_LEN2,UBX_PAYLOAD,UBX_CHK1,UBX_CHK2}; + enum proto_states proto_state = START; int32_t gpsRxOverflow = 0; numUpdates = 0; @@ -184,6 +192,7 @@ static void gpsTask(void *parameters) while (1) { uint8_t c; + // NMEA or SINGLE-SENTENCE GPS mode // This blocks the task until there is something on the buffer @@ -191,22 +200,94 @@ static void gpsTask(void *parameters) { // detect start while acquiring stream - if (!start_flag && (c == '$')) + switch (proto_state) { - start_flag = true; - found_cr = false; - rx_count = 0; + case START: // detect protocol + switch (c) + { + case UBX_SYNC1: // first UBX sync char found + proto_state = UBX_SY2; + continue; + case '$': // NMEA identifier found + proto_state = NMEA; + found_cr = false; + rx_count = 0; + break; + default: + continue; + } + break; + case UBX_SY2: + if (c == UBX_SYNC2) // second UBX sync char found + { + proto_state = UBX_CLASS; + found_cr = false; + rx_count = 0; + } + else + { + proto_state = START; // reset state + } + continue; + case UBX_CLASS: + ubx->header.class = c; + proto_state = UBX_ID; + continue; + case UBX_ID: + ubx->header.id = c; + proto_state = UBX_LEN1; + continue; + case UBX_LEN1: + ubx->header.len = c; + proto_state = UBX_LEN2; + continue; + case UBX_LEN2: + ubx->header.len += (c << 8); + if ((sizeof (UBXHeader)) + ubx->header.len > NMEA_MAX_PACKET_LENGTH) + { + gpsRxOverflow++; + proto_state = START; + found_cr = false; + rx_count = 0; + } + else + { + proto_state = UBX_PAYLOAD; + } + continue; + case UBX_PAYLOAD: + if (rx_count < ubx->header.len) + { + ubx->payload.payload[rx_count] = c; + if (++rx_count == ubx->header.len) + proto_state = UBX_CHK1; + } + else + proto_state = START; + continue; + case UBX_CHK1: + ubx->header.ck_a = c; + proto_state = UBX_CHK2; + continue; + case UBX_CHK2: + ubx->header.ck_b = c; + if (checksum_ubx_message(ubx)) + { + parse_ubx_message(ubx); + } + proto_state = START; + continue; + case NMEA: + break; } - else - if (!start_flag) - continue; - + + if (rx_count >= NMEA_MAX_PACKET_LENGTH) { // The buffer is already full and we haven't found a valid NMEA sentence. // Flush the buffer and note the overflow event. gpsRxOverflow++; - start_flag = false; + proto_state = START; found_cr = false; rx_count = 0; } @@ -230,7 +311,7 @@ static void gpsTask(void *parameters) gps_rx_buffer[rx_count-2] = 0; // prepare to parse next sentence - start_flag = false; + proto_state = START; found_cr = false; rx_count = 0; // Our rxBuffer must look like this now: diff --git a/flight/Modules/GPS/UBX.c b/flight/Modules/GPS/UBX.c new file mode 100644 index 000000000..19f42081b --- /dev/null +++ b/flight/Modules/GPS/UBX.c @@ -0,0 +1,97 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GSPModule GPS Module + * @brief Process GPS information (UBX binary format) + * @{ + * + * @file UBX.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief GPS module, handles GPS and NMEA stream + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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. + * + * 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. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" +#include "pios.h" +#include "UBX.h" +#include "gpsvelocity.h" + +bool checksum_ubx_message (UBXPacket *ubx) +{ + int i; + uint8_t ck_a, ck_b; + + ck_a = ubx->header.class; + ck_b = ck_a; + + ck_a += ubx->header.id; + ck_b += ck_a; + + ck_a += ubx->header.len & 0xff; + ck_b += ck_a; + + ck_a += ubx->header.len >> 8; + ck_b += ck_a; + + + + for (i = 0; i < ubx->header.len; i++) + { + ck_a += ubx->payload.payload[i]; + ck_b += ck_a; + } + + if (ubx->header.ck_a == ck_a && + ubx->header.ck_b == ck_b) + { + return true; + } + else + return false; + +} + +void parse_ubx_nav_velned (UBXPayload payload) +{ + GPSVelocityData GpsVelocity; + GPSVelocityGet(&GpsVelocity); + + GpsVelocity.North = (float)payload.nav_velned.velN/100.0; + GpsVelocity.East = (float)payload.nav_velned.velE/100.0; + GpsVelocity.Down = (float)payload.nav_velned.velD/100.0; + + GPSVelocitySet(&GpsVelocity); +} + +void parse_ubx_message (UBXPacket *ubx) +{ + switch (ubx->header.class) + { + case UBX_CLASS_NAV: + switch (ubx->header.id) + { + case UBX_ID_VELNED: + parse_ubx_nav_velned (ubx->payload); + } + break; + } +} + + diff --git a/flight/Modules/GPS/inc/UBX.h b/flight/Modules/GPS/inc/UBX.h new file mode 100644 index 000000000..21cd60ce4 --- /dev/null +++ b/flight/Modules/GPS/inc/UBX.h @@ -0,0 +1,80 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GSPModule GPS Module + * @brief Process GPS information + * @{ + * + * @file UBX.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief GPS module, handles GPS and NMEA stream + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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. + * + * 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. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef UBX_H +#define UBX_H +#include "openpilot.h" + +#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters +#define UBX_SYNC2 0x62 + +// From u-blox6 receiver protocol specification + +// Messages classes +#define UBX_CLASS_NAV 0x01 + +// Message IDs +#define UBX_ID_VELNED 0x12 + +// private structures + +typedef struct { + uint32_t iTOW; // ms GPS Millisecond Time of Week + int32_t velN; // cm/s NED north velocity + int32_t velE; // cm/s NED east velocity + int32_t velD; // cm/s NED down velocity + uint32_t speed; // cm/s Speed (3-D) + uint32_t gSpeed; // cm/s Ground Speed (2-D) + int32_t heading; // 1e-5 *deg Heading of motion 2-D + uint32_t sAcc; // cm/s Speed Accuracy Estimate + uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate +} UBX_NAV_VELNED; + +typedef union { // add more message types later + uint8_t payload[0]; + UBX_NAV_VELNED nav_velned; +} UBXPayload; + +typedef struct { + uint8_t class; + uint8_t id; + uint16_t len; + uint8_t ck_a; + uint8_t ck_b; +} UBXHeader; + +typedef struct { + UBXHeader header; + UBXPayload payload; +} UBXPacket; + +bool checksum_ubx_message(UBXPacket *); +void parse_ubx_message(UBXPacket *); +#endif /* UBX_H */ diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index c1601d9f6..b2080bc31 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -47,6 +47,7 @@ UAVOBJSRCFILENAMES += gcstelemetrystats UAVOBJSRCFILENAMES += gpsposition UAVOBJSRCFILENAMES += gpssatellites UAVOBJSRCFILENAMES += gpstime +UAVOBJSRCFILENAMES += gpsvelocity UAVOBJSRCFILENAMES += guidancesettings UAVOBJSRCFILENAMES += homelocation UAVOBJSRCFILENAMES += i2cstats diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 95115b248..75f4e31e7 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -54,6 +54,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/gpsposition.h \ $$UAVOBJECT_SYNTHETICS/gpstime.h \ $$UAVOBJECT_SYNTHETICS/gpssatellites.h \ + $$UAVOBJECT_SYNTHETICS/gpsvelocity.h \ $$UAVOBJECT_SYNTHETICS/positionactual.h \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \ $$UAVOBJECT_SYNTHETICS/homelocation.h \ @@ -117,6 +118,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/gpsposition.cpp \ $$UAVOBJECT_SYNTHETICS/gpstime.cpp \ $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \ + $$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \ $$UAVOBJECT_SYNTHETICS/positionactual.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \ $$UAVOBJECT_SYNTHETICS/homelocation.cpp \ diff --git a/shared/uavobjectdefinition/gpsvelocity.xml b/shared/uavobjectdefinition/gpsvelocity.xml new file mode 100644 index 000000000..7d57ccacc --- /dev/null +++ b/shared/uavobjectdefinition/gpsvelocity.xml @@ -0,0 +1,12 @@ + + + Raw GPS data from @ref GPSModule. Should only be used by @ref AHRSCommsModule. + + + + + + + + +