From a3e25139520ca1cedeb9e5e3c66fd161f7d2266e Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 9 Jun 2012 23:38:50 +0200 Subject: [PATCH 01/32] Increased stack size and com receive buffer to support SVINFO message. Fix NOGPS condition for UBX. --- flight/Modules/GPS/GPS.c | 91 +++++++++++++++++++++++++++++++++++++--- 1 file changed, 86 insertions(+), 5 deletions(-) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 716e47aa3..791dab046 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -61,13 +61,17 @@ static float GravityAccel(float latitude, float longitude, float altitude); // Private constants #define GPS_TIMEOUT_MS 500 + +#if defined(REVOLUTION) +#define NMEA_MAX_PACKET_LENGTH MAX_SVINFO_MSG_SIZE +#else #define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed // same as in COM buffer - +#endif #ifdef PIOS_GPS_SETS_HOMELOCATION // Unfortunately need a good size stack for the WMM calculation - #define STACK_SIZE_BYTES 800 + #define STACK_SIZE_BYTES 950 #else #define STACK_SIZE_BYTES 650 #endif @@ -193,9 +197,86 @@ static void gpsTask(void *parameters) // detect start while acquiring stream if (!start_flag && (c == '$')) { - 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)) // message complete and valid + { + parse_ubx_message(ubx, &GpsData); + timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + timeOfLastUpdateMs = timeNowMs; + timeOfLastCommandMs = timeNowMs; + } + proto_state = START; + continue; + case NMEA: + break; } else if (!start_flag) From 8c184ec8f2150851d7c10c3d10128d4c715a0cd2 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 9 Jun 2012 23:41:36 +0200 Subject: [PATCH 02/32] Add additional UBX messages NAV-DOP, NAV-SOL, NAV-STATUS, NAV-SVINFO, NAV-UTC --- flight/Modules/GPS/inc/UBX.h | 217 +++++++++++++++++++++++++++++++++++ 1 file changed, 217 insertions(+) create mode 100644 flight/Modules/GPS/inc/UBX.h diff --git a/flight/Modules/GPS/inc/UBX.h b/flight/Modules/GPS/inc/UBX.h new file mode 100644 index 000000000..5177f8321 --- /dev/null +++ b/flight/Modules/GPS/inc/UBX.h @@ -0,0 +1,217 @@ +/** + ****************************************************************************** + * @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_POSLLH 0x02 +#define UBX_ID_STATUS 0x03 +#define UBX_ID_DOP 0x04 +#define UBX_ID_SOL 0x06 +#define UBX_ID_VELNED 0x12 +#define UBX_ID_TIMEUTC 0x21 +#define UBX_ID_SVINFO 0x30 + +// private structures + +// Geodetic Position Solution +typedef struct { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + int32_t lon; // Longitude (deg*1e-7) + int32_t lat; // Latitude (deg*1e-7) + int32_t height; // Height above Ellipoid (mm) + int32_t hMSL; // Height abobe mean sea level (mm) + uint32_t hAcc; // Horizontal Accuracy Estimate (mm) + uint32_t vAcc; // Vertical Accuracy Estimate (mm) +} UBX_NAV_POSLLH; + +// Receiver Navigation Status + +#define STATUS_GPSFIX_NOFIX 0x00 +#define STATUS_GPSFIX_DRONLY 0x01 +#define STATUS_GPSFIX_2DFIX 0x02 +#define STATUS_GPSFIX_3DFIX 0x03 +#define STATUS_GPSFIX_GPSDR 0x04 +#define STATUS_GPSFIX_TIMEONLY 0x05 + +#define STATUS_FLAGS_GPSFIX_OK (1 << 0) +#define STATUS_FLAGS_DIFFSOLN (1 << 1) +#define STATUS_FLAGS_WKNSET (1 << 2) +#define STATUS_FLAGS_TOWSET (1 << 3) + +typedef struct { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint8_t gpsFix; // GPS fix type + uint8_t flags; // Navigation Status Flags + uint8_t fixStat; // Fix Status Information + uint8_t flags2; // Additional navigation output information + uint32_t ttff; // Time to first fix (ms) + uint32_t msss; // Milliseconds since startup/reset (ms) +} UBX_NAV_STATUS; + +// Dilution of precision +typedef struct { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + 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 +} UBX_NAV_DOP; + +// Navigation solution + +typedef struct { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + int32_t fTOW; // fracional nanoseconds (ns) + int16_t week; // GPS week + uint8_t gpsFix; // GPS fix type + uint8_t flags; // Fix status flags + int32_t ecefX; // ECEF X coordinate (cm) + int32_t ecefY; // ECEF Y coordinate (cm) + int32_t ecefZ; // ECEF Z coordinate (cm) + uint32_t pAcc; // 3D Position Accuracy Estimate (cm) + int32_t ecefVX; // ECEF X coordinate (cm/s) + int32_t ecefVY; // ECEF Y coordinate (cm/s) + int32_t ecefVZ; // ECEF Z coordinate (cm/s) + uint32_t sAcc; // Speed Accuracy Estimate + uint16_t pDOP; // Position DOP + uint8_t reserved1; // Reserved + uint8_t numSV; // Number of SVs used in Nav Solution + uint32_t reserved2; // Reserved +} UBX_NAV_SOL; + +// North/East/Down velocity + +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; + +// UTC Time Solution + +#define TIMEUTC_VALIDTOW (1 << 0) +#define TIMEUTC_VALIDWKN (1 << 1) +#define TIMEUTC_VALIDUTC (1 << 2) + +typedef struct { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint32_t tAcc; // Time Accuracy Estimate (ns) + int32_t nano; // Nanoseconds of second + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + uint8_t valid; // Validity Flags +} UBX_NAV_TIMEUTC; + +// Space Vehicle (SV) Information + +// Single SV information block + +#define SVUSED (1 << 0) // This SV is used for navigation +#define DIFFCORR (1 << 1) // Differential correction available +#define ORBITAVAIL (1 << 2) // Orbit information available +#define ORBITEPH (1 << 3) // Orbit information is Ephemeris +#define UNHEALTHY (1 << 4) // SV is unhealthy +#define ORBITALM (1 << 5) // Orbit information is Almanac Plus +#define ORBITAOP (1 << 6) // Orbit information is AssistNow Autonomous +#define SMOOTHED (1 << 7) // Carrier smoothed pseudoranges used + +typedef struct { + uint8_t chn; // Channel number + uint8_t svid; // Satellite ID + uint8_t flags; // Misc SV information + uint8_t quality; // Misc quality indicators + uint8_t cno; // Carrier to Noise Ratio (dbHz) + int8_t elev; // Elevation (integer degrees) + int16_t azim; // Azimuth (integer degrees) + int32_t prRes; // Pseudo range residual (cm) +} UBX_NAV_SVINFO_SV; + +// SV information message +#define MAX_SVINFO_MSG_SIZE 208 + +typedef struct { + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint8_t numCh; // Number of channels + uint8_t globalFlags; // + uint16_t reserved2; // Reserved + UBX_NAV_SVINFO_SV sv[16]; // Repeated 'numCh' times +} UBX_NAV_SVINFO; + +typedef union { + uint8_t payload[0]; + UBX_NAV_POSLLH nav_posllh; + UBX_NAV_STATUS nav_status; + UBX_NAV_DOP nav_dop; + UBX_NAV_SOL nav_sol; + UBX_NAV_VELNED nav_velned; + UBX_NAV_TIMEUTC nav_timeutc; + UBX_NAV_SVINFO nav_svinfo; +} 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 *); +bool parse_ubx_message(UBXPacket *, GPSPositionData *); +#endif /* UBX_H */ From 29fd85f0e24fd8b31cf5b04c9d0e1aab93cf4d52 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 9 Jun 2012 23:49:03 +0200 Subject: [PATCH 03/32] Process additional UBX messages. All information needed to fill the GPS UAVObjects can now come from UBX protocol alone. Mixture of NMEA and UBX protocols still is possible but not recommended. --- flight/Modules/GPS/UBX.c | 278 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 278 insertions(+) create mode 100644 flight/Modules/GPS/UBX.c diff --git a/flight/Modules/GPS/UBX.c b/flight/Modules/GPS/UBX.c new file mode 100644 index 000000000..99e2a88b1 --- /dev/null +++ b/flight/Modules/GPS/UBX.c @@ -0,0 +1,278 @@ +/** + ****************************************************************************** + * @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 "gpsvelocity.h" +#include "gpssatellites.h" +#include "gpsposition.h" +#include "gpstime.h" +#include "UBX.h" + + +// Keep track of various GPS messages needed to make up a single UAVO update +// time-of-week timestamp is used to correlate matching messages + +#define POSLLH_RECEIVED (1 << 0) +#define STATUS_RECEIVED (1 << 1) +#define DOP_RECEIVED (1 << 2) +#define VELNED_RECEIVED (1 << 3) +#define SOL_RECEIVED (1 << 4) +#define ALL_RECEIVED (SOL_RECEIVED | VELNED_RECEIVED | DOP_RECEIVED | POSLLH_RECEIVED) +#define NONE_RECEIVED 0 + +static struct msgtracker{ + uint32_t currentTOW; // TOW of the message set currently in progress + uint8_t msg_received; // Flag received messages + } msgtracker; + +// Check if a message belongs to the current data set and register it as 'received' +bool check_msgtracker (uint32_t tow, uint8_t msg_flag) +{ + if ((tow > msgtracker.currentTOW) || // start of a new message set + (tow < 60000 && msgtracker.currentTOW > (7*24-1)*3600*1000)) // TOW wrap around + { + msgtracker.currentTOW = tow; + msgtracker.msg_received = NONE_RECEIVED; + } + else + { + if (tow < msgtracker.currentTOW) // message outdated (don't process) + return false; + } + + msgtracker.msg_received |= msg_flag; + return true; +} + +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_posllh (UBXPayload payload, GPSPositionData *gpsposition) +{ + if (check_msgtracker(payload.nav_posllh.iTOW, POSLLH_RECEIVED)) + { + gpsposition->Altitude = (float)payload.nav_posllh.hMSL*0.001f; + gpsposition->GeoidSeparation = (float)(payload.nav_posllh.height - + payload.nav_posllh.hMSL)*0.001f; + gpsposition->Latitude = payload.nav_posllh.lat; + gpsposition->Longitude = payload.nav_posllh.lon; + } +} + +void parse_ubx_nav_status (UBXPayload payload, GPSPositionData *gpsposition) +{ + if (check_msgtracker(payload.nav_status.iTOW, STATUS_RECEIVED)) + { + switch (payload.nav_status.gpsFix) + { + case STATUS_GPSFIX_2DFIX: + gpsposition->Status = GPSPOSITION_STATUS_FIX2D; + break; + case STATUS_GPSFIX_3DFIX: + gpsposition->Status = GPSPOSITION_STATUS_FIX3D; + break; + default: gpsposition->Status = GPSPOSITION_STATUS_NOFIX; + } + } +} + +void parse_ubx_nav_sol (UBXPayload payload, GPSPositionData *gpsposition) +{ + if (check_msgtracker(payload.nav_dop.iTOW, SOL_RECEIVED)) + { + gpsposition->Satellites = (float)payload.nav_sol.numSV; + switch (payload.nav_sol.gpsFix) + { + case STATUS_GPSFIX_2DFIX: + gpsposition->Status = GPSPOSITION_STATUS_FIX2D; + break; + case STATUS_GPSFIX_3DFIX: + gpsposition->Status = GPSPOSITION_STATUS_FIX3D; + break; + default: gpsposition->Status = GPSPOSITION_STATUS_NOFIX; + } + } +} + +void parse_ubx_nav_dop (UBXPayload payload, GPSPositionData *gpsposition) +{ + if (check_msgtracker(payload.nav_dop.iTOW, DOP_RECEIVED)) + { + gpsposition->HDOP = (float)payload.nav_dop.hDOP * 0.01f; + gpsposition->VDOP = (float)payload.nav_dop.vDOP * 0.01f; + gpsposition->PDOP = (float)payload.nav_dop.pDOP * 0.01f; + } +} + +void parse_ubx_nav_velned (UBXPayload payload, GPSPositionData *gpsposition) +{ + GPSVelocityData GpsVelocity; + + GpsVelocity.North = (float)payload.nav_velned.velN/100.0f; + GpsVelocity.East = (float)payload.nav_velned.velE/100.0f; + GpsVelocity.Down = (float)payload.nav_velned.velD/100.0f; + + GPSVelocitySet(&GpsVelocity); + + if (check_msgtracker(payload.nav_velned.iTOW, VELNED_RECEIVED)) + { + gpsposition->Groundspeed = (float)payload.nav_velned.gSpeed * 0.01f; + gpsposition->Heading = (float)payload.nav_velned.heading * 1.0e-5f; + } + +} + +void parse_ubx_nav_timeutc (UBXPayload payload) +{ + if (!(payload.nav_timeutc.valid & TIMEUTC_VALIDUTC)) + return; + + GPSTimeData gpst; + + gpst.Year = payload.nav_timeutc.year; + gpst.Month = payload.nav_timeutc.month; + gpst.Day = payload.nav_timeutc.day; + gpst.Hour = payload.nav_timeutc.hour; + gpst.Minute = payload.nav_timeutc.min; + gpst.Second = payload.nav_timeutc.sec; + + GPSTimeSet(&gpst); +} + +void parse_ubx_nav_svinfo (UBXPayload payload) +{ + GPSSatellitesData svdata; + uint8_t chan; + svdata.SatsInView = 0; + for (chan = 0; chan < payload.nav_svinfo.numCh; chan++) + { + if ((payload.nav_svinfo.sv[chan].elev > 0) && // some unhealthy SV report negative elevation + (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM)) + { + svdata.Azimuth[svdata.SatsInView] = (float)payload.nav_svinfo.sv[chan].azim; + svdata.Elevation[svdata.SatsInView] = (float)payload.nav_svinfo.sv[chan].elev; + svdata.PRN[svdata.SatsInView] = payload.nav_svinfo.sv[chan].svid; + svdata.SNR[svdata.SatsInView] = payload.nav_svinfo.sv[chan].cno; + svdata.SatsInView++; + } + + } + // fill remaining slots (if any) + for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) + { + svdata.Azimuth[chan] = (float)0.0f; + svdata.Elevation[chan] = (float)0.0f; + svdata.PRN[chan] = 0; + svdata.SNR[chan] = 0; + } + + GPSSatellitesSet(&svdata); +} + +// UBX message parser +// returns true on valid position updates + +bool parse_ubx_message (UBXPacket *ubx, GPSPositionData *gpsposition) +{ +#if defined(REVOLUTION) + + switch (ubx->header.class) + { + case UBX_CLASS_NAV: + switch (ubx->header.id) + { + case UBX_ID_POSLLH: + parse_ubx_nav_posllh (ubx->payload, gpsposition); + break; + case UBX_ID_STATUS: + parse_ubx_nav_status (ubx->payload, gpsposition); + break; + case UBX_ID_DOP: + parse_ubx_nav_dop (ubx->payload, gpsposition); + break; + case UBX_ID_SOL: + parse_ubx_nav_sol (ubx->payload, gpsposition); + break; + case UBX_ID_VELNED: + parse_ubx_nav_velned (ubx->payload, gpsposition); + break; + case UBX_ID_TIMEUTC: + parse_ubx_nav_timeutc (ubx->payload); + break; + case UBX_ID_SVINFO: + parse_ubx_nav_svinfo (ubx->payload); + break; + } + break; + } + if (msgtracker.msg_received == ALL_RECEIVED) + { + GPSPositionSet(gpsposition); + msgtracker.msg_received = NONE_RECEIVED; + return true; + } + return false; +#endif +} + + From 04241723983996919377711063e5e3a45be5bf5b Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 28 Jun 2012 09:02:14 +0200 Subject: [PATCH 04/32] Better separation between UBX and NMEA parser Make GPS protocol a user selectable option Support for UBX protocol on CopterControl --- flight/CopterControl/Makefile | 2 + flight/CopterControl/System/inc/pios_config.h | 3 +- flight/Modules/GPS/GPS.c | 260 ++++----------- flight/Modules/GPS/NMEA.c | 99 +++++- flight/Modules/GPS/UBX.c | 305 +++++++++++------- flight/Modules/GPS/inc/GPS.h | 99 +++--- flight/Modules/GPS/inc/NMEA.h | 5 +- flight/Modules/GPS/inc/UBX.h | 155 ++++----- flight/Revolution/System/inc/pios_config.h | 2 + flight/Revolution/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 4 + 11 files changed, 507 insertions(+), 428 deletions(-) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 82334e56a..7c5498806 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -199,6 +199,8 @@ SRC += $(OPUAVSYNTHDIR)/attitudesettings.c SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c SRC += $(OPUAVSYNTHDIR)/cameradesired.c SRC += $(OPUAVSYNTHDIR)/gpsposition.c +SRC += $(OPUAVSYNTHDIR)/gpsvelocity.c +SRC += $(OPUAVSYNTHDIR)/gpssettings.c SRC += $(OPUAVSYNTHDIR)/hwsettings.c SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c SRC += $(OPUAVSYNTHDIR)/receiveractivity.c diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index e4b70e931..bcf0b0b23 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -58,7 +58,8 @@ #define PIOS_INCLUDE_TELEMETRY_RF #define PIOS_INCLUDE_GPS #define PIOS_GPS_MINIMAL - +#define PIOS_INCLUDE_GPS_NMEA_PARSER /* Include the NMEA protocol parser */ +#define PIOS_INCLUDE_GPS_UBX_PARSER /* Include the UBX protocol parser */ #define PIOS_INCLUDE_SERVO #define PIOS_INCLUDE_SPI #define PIOS_INCLUDE_SYS diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 791dab046..15bfa4457 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -33,7 +33,7 @@ #include "openpilot.h" #include "GPS.h" -#include +//#include #include "NMEA.h" @@ -41,6 +41,8 @@ #include "homelocation.h" #include "gpstime.h" #include "gpssatellites.h" +#include "gpsvelocity.h" +#include "gpssettings.h" #include "WorldMagModel.h" #include "CoordinateConversions.h" #include "hwsettings.h" @@ -62,19 +64,17 @@ static float GravityAccel(float latitude, float longitude, float altitude); #define GPS_TIMEOUT_MS 500 -#if defined(REVOLUTION) -#define NMEA_MAX_PACKET_LENGTH MAX_SVINFO_MSG_SIZE -#else -#define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed -// same as in COM buffer -#endif #ifdef PIOS_GPS_SETS_HOMELOCATION // Unfortunately need a good size stack for the WMM calculation - #define STACK_SIZE_BYTES 950 + #define STACK_SIZE_BYTES 750 +#else +#if defined(PIOS_GPS_MINIMAL) + #define STACK_SIZE_BYTES 500 #else #define STACK_SIZE_BYTES 650 -#endif +#endif // PIOS_GPS_MINIMAL +#endif // PIOS_GPS_SETS_HOMELOCATION #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) @@ -124,6 +124,7 @@ int32_t GPSStart(void) int32_t GPSInitialize(void) { gpsPort = PIOS_COM_GPS; + uint8_t gpsProtocol; #ifdef MODULE_GPS_BUILTIN gpsEnabled = true; @@ -141,6 +142,7 @@ int32_t GPSInitialize(void) if (gpsPort && gpsEnabled) { GPSPositionInitialize(); + GPSVelocityInitialize(); #if !defined(PIOS_GPS_MINIMAL) GPSTimeInitialize(); GPSSatellitesInitialize(); @@ -150,7 +152,19 @@ int32_t GPSInitialize(void) #endif updateSettings(); - gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); + if (gpsPort && gpsEnabled) { + GPSSettingsInitialize(); + GPSSettingsDataProtocolGet(&gpsProtocol); + switch (gpsProtocol) { + case GPSSETTINGS_DATAPROTOCOL_NMEA: + gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); + break; + case GPSSETTINGS_DATAPROTOCOL_UBX: + gps_rx_buffer = pvPortMalloc(sizeof(UBXPacket)); + break; + default: + gps_rx_buffer = NULL; + } PIOS_Assert(gps_rx_buffer); return 0; @@ -169,14 +183,13 @@ MODULE_INITCALL(GPSInitialize, GPSStart) static void gpsTask(void *parameters) { portTickType xDelay = 100 / portTICK_RATE_MS; - uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; - GPSPositionData GpsData; - - uint8_t rx_count = 0; - bool start_flag = false; - bool found_cr = false; - int32_t gpsRxOverflow = 0; - + uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + + GPSPositionData gpsposition; + uint8_t gpsProtocol; + + GPSSettingsDataProtocolGet(&gpsProtocol); + numUpdates = 0; numChecksumErrors = 0; numParsingErrors = 0; @@ -184,202 +197,65 @@ static void gpsTask(void *parameters) timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; + GPSPositionGet(&gpsposition); // Loop forever while (1) { uint8_t c; - // NMEA or SINGLE-SENTENCE GPS mode // This blocks the task until there is something on the buffer while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) { - - // detect start while acquiring stream - if (!start_flag && (c == '$')) - { - 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; - } + int res; + switch (gpsProtocol) { +#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) + case GPSSETTINGS_DATAPROTOCOL_NMEA: + res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition); 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)) // message complete and valid - { - parse_ubx_message(ubx, &GpsData); - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - timeOfLastUpdateMs = timeNowMs; - timeOfLastCommandMs = timeNowMs; - } - proto_state = START; - continue; - case NMEA: +#endif +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) + case GPSSETTINGS_DATAPROTOCOL_UBX: + res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition); + break; +#endif + default: + res = NO_PARSER; // this should not happen 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; - found_cr = false; - rx_count = 0; - } - else - { - gps_rx_buffer[rx_count] = c; - rx_count++; - } - - // look for ending '\r\n' sequence - if (!found_cr && (c == '\r') ) - found_cr = true; - else - if (found_cr && (c != '\n') ) - found_cr = false; // false end flag - else - if (found_cr && (c == '\n') ) - { - // The NMEA functions require a zero-terminated string - // As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n - gps_rx_buffer[rx_count-2] = 0; - // prepare to parse next sentence - start_flag = false; - found_cr = false; - rx_count = 0; - // Our rxBuffer must look like this now: - // [0] = '$' - // ... = zero or more bytes of sentence payload - // [end_pos - 1] = '\r' - // [end_pos] = '\n' - // - // Prepare to consume the sentence from the buffer - - // Validate the checksum over the sentence - if (!NMEA_checksum(&gps_rx_buffer[1])) - { // Invalid checksum. May indicate dropped characters on Rx. - //PIOS_DEBUG_PinHigh(2); - ++numChecksumErrors; - //PIOS_DEBUG_PinLow(2); - } - else - { // Valid checksum, use this packet to update the GPS position - if (!NMEA_update_position(&gps_rx_buffer[1])) { - //PIOS_DEBUG_PinHigh(2); - ++numParsingErrors; - //PIOS_DEBUG_PinLow(2); - } - else - ++numUpdates; - - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - timeOfLastUpdateMs = timeNowMs; - timeOfLastCommandMs = timeNowMs; - } + if (res == PARSER_COMPLETE) { + timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; + timeOfLastUpdateMs = timeNowMs; + timeOfLastCommandMs = timeNowMs; } } // Check for GPS timeout timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) - { // we have not received any valid GPS sentences for a while. + if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) { + // we have not received any valid GPS sentences for a while. // either the GPS is not plugged in or a hardware problem or the GPS has locked up. - - GPSPositionGet(&GpsData); - GpsData.Status = GPSPOSITION_STATUS_NOGPS; - GPSPositionSet(&GpsData); + uint8_t status = GPSPOSITION_STATUS_NOGPS; + GPSPositionStatusSet(&status); AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); - - } - else - { // we appear to be receiving GPS sentences OK, we've had an update - - GPSPositionGet(&GpsData); - -#ifdef PIOS_GPS_SETS_HOMELOCATION - HomeLocationData home; - HomeLocationGet(&home); - - if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE)) - setHomeLocation(&GpsData); -#endif - + } else { + // we appear to be receiving GPS sentences OK, we've had an update //criteria for GPS-OK taken from this post... //http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 - if ((GpsData.PDOP < 3.5) && (GpsData.Satellites >= 7)) + if ((gpsposition.PDOP < 3.5) && (gpsposition.Satellites >= 7) && + (gpsposition.Status == GPSPOSITION_STATUS_FIX3D)) { AlarmsClear(SYSTEMALARMS_ALARM_GPS); - else - if (GpsData.Status == GPSPOSITION_STATUS_FIX3D) - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); - else - AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); +#ifdef PIOS_GPS_SETS_HOMELOCATION + HomeLocationData home; + HomeLocationGet(&home); + + if (home.Set == HOMELOCATION_SET_FALSE) + setHomeLocation(&gpsposition); +#endif + } else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D) + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); + else + AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); } } diff --git a/flight/Modules/GPS/NMEA.c b/flight/Modules/GPS/NMEA.c index 59593b807..3faf29337 100644 --- a/flight/Modules/GPS/NMEA.c +++ b/flight/Modules/GPS/NMEA.c @@ -30,10 +30,15 @@ #include "openpilot.h" #include "pios.h" + +#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) + +#include "gpsposition.h" #include "NMEA.h" #include "gpsposition.h" #include "gpstime.h" #include "gpssatellites.h" +#include "GPS.h" //#define ENABLE_DEBUG_MSG ///< define to enable debug-messages #define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is ued for debug-messages @@ -75,6 +80,9 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); #endif //PIOS_GPS_MINIMAL +static uint32_t numUpdates; +static uint32_t numChecksumErrors; +static uint32_t numParsingErrors; static struct nmea_parser nmea_parsers[] = { { @@ -111,6 +119,88 @@ static struct nmea_parser nmea_parsers[] = { #endif //PIOS_GPS_MINIMAL }; +int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData) +{ + static uint8_t rx_count = 0; + static bool start_flag = false; + static bool found_cr = false; + + // detect start while acquiring stream + if (!start_flag && (c == '$')) // NMEA identifier found + { + start_flag = true; + found_cr = false; + rx_count = 0; + } + else + if (!start_flag) + return PARSER_ERROR; + + 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; + found_cr = false; + rx_count = 0; + return PARSER_OVERRUN; + } + else + { + gps_rx_buffer[rx_count] = c; + rx_count++; + } + + // look for ending '\r\n' sequence + if (!found_cr && (c == '\r') ) + found_cr = true; + else + if (found_cr && (c != '\n') ) + found_cr = false; // false end flag + else + if (found_cr && (c == '\n') ) + { + // The NMEA functions require a zero-terminated string + // As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n + gps_rx_buffer[rx_count-2] = 0; + + // prepare to parse next sentence + start_flag = false; + found_cr = false; + rx_count = 0; + // Our rxBuffer must look like this now: + // [0] = '$' + // ... = zero or more bytes of sentence payload + // [end_pos - 1] = '\r' + // [end_pos] = '\n' + // + // Prepare to consume the sentence from the buffer + + // Validate the checksum over the sentence + if (!NMEA_checksum(&gps_rx_buffer[1])) + { // Invalid checksum. May indicate dropped characters on Rx. + //PIOS_DEBUG_PinHigh(2); + ++numChecksumErrors; + //PIOS_DEBUG_PinLow(2); + return PARSER_ERROR; + } + else + { // Valid checksum, use this packet to update the GPS position + if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) { + //PIOS_DEBUG_PinHigh(2); + ++numParsingErrors; + //PIOS_DEBUG_PinLow(2); + } + else + ++numUpdates; + + return PARSER_COMPLETE; + } + } + return PARSER_INCOMPLETE; +} + static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix) { if (!prefix) { @@ -352,7 +442,7 @@ bool NMEA_update_position(char *nmea_sentence) // All is fine :) Update object if data has changed - if (gpsDataUpdated) { + if (gpsDataUpdated || (GpsData->Status == GPSPOSITION_STATUS_NOFIX)) { #ifdef DEBUG_MGSID_IN DEBUG_MSG("U"); #endif @@ -400,6 +490,12 @@ static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch return false; } + + // check for invalid GPS fix + if (param[6][0] == '0') { + // return false; + GpsData->Status = GPSPOSITION_STATUS_NOFIX; // treat invalid fix as NOFIX + } // get number of satellites used in GPS solution GpsData->Satellites = atoi(param[7]); @@ -669,3 +765,4 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch return true; } +#endif // PIOS_INCLUDE_GPS_NMEA_PARSER diff --git a/flight/Modules/GPS/UBX.c b/flight/Modules/GPS/UBX.c index 99e2a88b1..de97b9e3a 100644 --- a/flight/Modules/GPS/UBX.c +++ b/flight/Modules/GPS/UBX.c @@ -30,11 +30,96 @@ #include "openpilot.h" #include "pios.h" -#include "gpsvelocity.h" -#include "gpssatellites.h" -#include "gpsposition.h" -#include "gpstime.h" + +#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) + #include "UBX.h" +#include "gps.h" + +static uint32_t gpsRxReceived = 0; +static uint32_t gpsRxChkSumError = 0; +static uint32_t gpsRxOverflow = 0; + +// parse incoming character stream for messages in UBX binary format + +int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData) +{ + enum proto_states {START,UBX_SY2,UBX_CLASS,UBX_ID,UBX_LEN1, + UBX_LEN2,UBX_PAYLOAD,UBX_CHK1,UBX_CHK2,FINISHED}; + static enum proto_states proto_state = START; + static uint8_t rx_count = 0; + UBXPacket *ubx = (UBXPacket *)gps_rx_buffer; + + switch (proto_state) { + case START: // detect protocol + if (c == UBX_SYNC1) // first UBX sync char found + proto_state = UBX_SY2; + break; + case UBX_SY2: + if (c == UBX_SYNC2) // second UBX sync char found + proto_state = UBX_CLASS; + else + proto_state = START; // reset state + break; + case UBX_CLASS: + ubx->header.class = c; + proto_state = UBX_ID; + break; + case UBX_ID: + ubx->header.id = c; + proto_state = UBX_LEN1; + break; + case UBX_LEN1: + ubx->header.len = c; + proto_state = UBX_LEN2; + break; + case UBX_LEN2: + ubx->header.len += (c << 8); + if (ubx->header.len > sizeof(UBXPayload)) { + gpsRxOverflow++; + proto_state = START; + } else { + rx_count = 0; + proto_state = UBX_PAYLOAD; + } + break; + 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 { + gpsRxOverflow++; + proto_state = START; + } + break; + case UBX_CHK1: + ubx->header.ck_a = c; + proto_state = UBX_CHK2; + break; + case UBX_CHK2: + ubx->header.ck_b = c; + if (checksum_ubx_message(ubx)) { // message complete and valid + parse_ubx_message(ubx, GpsData); + proto_state = FINISHED; + } else { + gpsRxChkSumError++; + proto_state = START; + } + break; + default: break; + } + + if (proto_state == START) + return PARSER_ERROR; // parser couldn't use this byte + else if (proto_state == FINISHED) { + gpsRxReceived++; + proto_state = START; + return PARSER_COMPLETE; // message complete & processed + } + + return PARSER_INCOMPLETE; // message not (yet) complete +} // Keep track of various GPS messages needed to make up a single UAVO update @@ -50,25 +135,21 @@ static struct msgtracker{ uint32_t currentTOW; // TOW of the message set currently in progress - uint8_t msg_received; // Flag received messages + uint8_t msg_received; // keep track of received message types } msgtracker; // Check if a message belongs to the current data set and register it as 'received' bool check_msgtracker (uint32_t tow, uint8_t msg_flag) { - if ((tow > msgtracker.currentTOW) || // start of a new message set - (tow < 60000 && msgtracker.currentTOW > (7*24-1)*3600*1000)) // TOW wrap around - { + + if (tow > msgtracker.currentTOW ? true // start of a new message set + : (msgtracker.currentTOW - tow > 6*24*3600*1000)) { // 6 days, TOW wrap around occured msgtracker.currentTOW = tow; msgtracker.msg_received = NONE_RECEIVED; - } - else - { - if (tow < msgtracker.currentTOW) // message outdated (don't process) + } else if (tow < msgtracker.currentTOW) // message outdated (don't process) return false; - } - msgtracker.msg_received |= msg_flag; + msgtracker.msg_received |= msg_flag; // register reception of this msg type return true; } @@ -89,137 +170,133 @@ bool checksum_ubx_message (UBXPacket *ubx) ck_a += ubx->header.len >> 8; ck_b += ck_a; - - - for (i = 0; i < ubx->header.len; i++) - { + 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_posllh (UBXPayload payload, GPSPositionData *gpsposition) +void parse_ubx_nav_posllh (UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition) { - if (check_msgtracker(payload.nav_posllh.iTOW, POSLLH_RECEIVED)) - { - gpsposition->Altitude = (float)payload.nav_posllh.hMSL*0.001f; - gpsposition->GeoidSeparation = (float)(payload.nav_posllh.height - - payload.nav_posllh.hMSL)*0.001f; - gpsposition->Latitude = payload.nav_posllh.lat; - gpsposition->Longitude = payload.nav_posllh.lon; - } -} - -void parse_ubx_nav_status (UBXPayload payload, GPSPositionData *gpsposition) -{ - if (check_msgtracker(payload.nav_status.iTOW, STATUS_RECEIVED)) - { - switch (payload.nav_status.gpsFix) - { - case STATUS_GPSFIX_2DFIX: - gpsposition->Status = GPSPOSITION_STATUS_FIX2D; - break; - case STATUS_GPSFIX_3DFIX: - gpsposition->Status = GPSPOSITION_STATUS_FIX3D; - break; - default: gpsposition->Status = GPSPOSITION_STATUS_NOFIX; + if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) { + if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { + GpsPosition->Altitude = (float)posllh->hMSL*0.001f; + GpsPosition->GeoidSeparation = (float)(posllh->height - posllh->hMSL)*0.001f; + GpsPosition->Latitude = posllh->lat; + GpsPosition->Longitude = posllh->lon; } } } -void parse_ubx_nav_sol (UBXPayload payload, GPSPositionData *gpsposition) +void parse_ubx_nav_status (UBX_NAV_STATUS *status, GPSPositionData *GpsPosition) { - if (check_msgtracker(payload.nav_dop.iTOW, SOL_RECEIVED)) - { - gpsposition->Satellites = (float)payload.nav_sol.numSV; - switch (payload.nav_sol.gpsFix) - { +#if 0 // we already get this info from NAV_SOL + if (check_msgtracker(status->iTOW, STATUS_RECEIVED)) { + switch (status->gpsFix) { case STATUS_GPSFIX_2DFIX: - gpsposition->Status = GPSPOSITION_STATUS_FIX2D; + GpsPosition->Status = GPSPOSITION_STATUS_FIX2D; break; case STATUS_GPSFIX_3DFIX: - gpsposition->Status = GPSPOSITION_STATUS_FIX3D; + GpsPosition->Status = GPSPOSITION_STATUS_FIX3D; break; - default: gpsposition->Status = GPSPOSITION_STATUS_NOFIX; + default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; } } +#endif } -void parse_ubx_nav_dop (UBXPayload payload, GPSPositionData *gpsposition) +void parse_ubx_nav_sol (UBX_NAV_SOL *sol, GPSPositionData *GpsPosition) { - if (check_msgtracker(payload.nav_dop.iTOW, DOP_RECEIVED)) - { - gpsposition->HDOP = (float)payload.nav_dop.hDOP * 0.01f; - gpsposition->VDOP = (float)payload.nav_dop.vDOP * 0.01f; - gpsposition->PDOP = (float)payload.nav_dop.pDOP * 0.01f; + if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) { + GpsPosition->Satellites = sol->numSV; + + if (sol->flags & STATUS_FLAGS_GPSFIX_OK) { + switch (sol->gpsFix) { + case STATUS_GPSFIX_2DFIX: + GpsPosition->Status = GPSPOSITION_STATUS_FIX2D; + break; + case STATUS_GPSFIX_3DFIX: + GpsPosition->Status = GPSPOSITION_STATUS_FIX3D; + break; + default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; + } + } + else // fix is not valid so we make sure to treat is as NOFIX + GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; } } -void parse_ubx_nav_velned (UBXPayload payload, GPSPositionData *gpsposition) +void parse_ubx_nav_dop (UBX_NAV_DOP *dop, GPSPositionData *GpsPosition) +{ + if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) { + GpsPosition->HDOP = (float)dop->hDOP * 0.01f; + GpsPosition->VDOP = (float)dop->vDOP * 0.01f; + GpsPosition->PDOP = (float)dop->pDOP * 0.01f; + } +} + +void parse_ubx_nav_velned (UBX_NAV_VELNED *velned, GPSPositionData *GpsPosition) { GPSVelocityData GpsVelocity; - GpsVelocity.North = (float)payload.nav_velned.velN/100.0f; - GpsVelocity.East = (float)payload.nav_velned.velE/100.0f; - GpsVelocity.Down = (float)payload.nav_velned.velD/100.0f; - - GPSVelocitySet(&GpsVelocity); - - if (check_msgtracker(payload.nav_velned.iTOW, VELNED_RECEIVED)) - { - gpsposition->Groundspeed = (float)payload.nav_velned.gSpeed * 0.01f; - gpsposition->Heading = (float)payload.nav_velned.heading * 1.0e-5f; + if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) { + if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { + GpsVelocity.North = (float)velned->velN/100.0f; + GpsVelocity.East = (float)velned->velE/100.0f; + GpsVelocity.Down = (float)velned->velD/100.0f; + GPSVelocitySet(&GpsVelocity); + GpsPosition->Groundspeed = (float)velned->gSpeed * 0.01f; + GpsPosition->Heading = (float)velned->heading * 1.0e-5f; + } } - } -void parse_ubx_nav_timeutc (UBXPayload payload) +#if !defined(PIOS_GPS_MINIMAL) +void parse_ubx_nav_timeutc (UBX_NAV_TIMEUTC *timeutc) { - if (!(payload.nav_timeutc.valid & TIMEUTC_VALIDUTC)) + if (!(timeutc->valid & TIMEUTC_VALIDUTC)) return; - GPSTimeData gpst; + GPSTimeData GpsTime; - gpst.Year = payload.nav_timeutc.year; - gpst.Month = payload.nav_timeutc.month; - gpst.Day = payload.nav_timeutc.day; - gpst.Hour = payload.nav_timeutc.hour; - gpst.Minute = payload.nav_timeutc.min; - gpst.Second = payload.nav_timeutc.sec; + GpsTime.Year = timeutc->year; + GpsTime.Month = timeutc->month; + GpsTime.Day = timeutc->day; + GpsTime.Hour = timeutc->hour; + GpsTime.Minute = timeutc->min; + GpsTime.Second = timeutc->sec; - GPSTimeSet(&gpst); + GPSTimeSet(&GpsTime); } +#endif -void parse_ubx_nav_svinfo (UBXPayload payload) +#if !defined(PIOS_GPS_MINIMAL) +void parse_ubx_nav_svinfo (UBX_NAV_SVINFO *svinfo) { - GPSSatellitesData svdata; uint8_t chan; + GPSSatellitesData svdata; + svdata.SatsInView = 0; - for (chan = 0; chan < payload.nav_svinfo.numCh; chan++) - { - if ((payload.nav_svinfo.sv[chan].elev > 0) && // some unhealthy SV report negative elevation - (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM)) - { - svdata.Azimuth[svdata.SatsInView] = (float)payload.nav_svinfo.sv[chan].azim; - svdata.Elevation[svdata.SatsInView] = (float)payload.nav_svinfo.sv[chan].elev; - svdata.PRN[svdata.SatsInView] = payload.nav_svinfo.sv[chan].svid; - svdata.SNR[svdata.SatsInView] = payload.nav_svinfo.sv[chan].cno; + for (chan = 0; chan < svinfo->numCh; chan++) { + if ((svinfo->sv[chan].elev > 0) && // some unhealthy SV report negative elevation + (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM)) { + svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim; + svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev; + svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid; + svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno; svdata.SatsInView++; } } // fill remaining slots (if any) - for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) - { + for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) { svdata.Azimuth[chan] = (float)0.0f; svdata.Elevation[chan] = (float)0.0f; svdata.PRN[chan] = 0; @@ -228,51 +305,51 @@ void parse_ubx_nav_svinfo (UBXPayload payload) GPSSatellitesSet(&svdata); } +#endif // UBX message parser -// returns true on valid position updates +// returns UAVObjectID if a UAVObject structure is ready for further processing -bool parse_ubx_message (UBXPacket *ubx, GPSPositionData *gpsposition) +uint32_t parse_ubx_message (UBXPacket *ubx, GPSPositionData *GpsPosition) { -#if defined(REVOLUTION) + uint32_t id = 0; - switch (ubx->header.class) - { + switch (ubx->header.class) { case UBX_CLASS_NAV: - switch (ubx->header.id) - { + switch (ubx->header.id) { case UBX_ID_POSLLH: - parse_ubx_nav_posllh (ubx->payload, gpsposition); + parse_ubx_nav_posllh (&ubx->payload.nav_posllh, GpsPosition); break; case UBX_ID_STATUS: - parse_ubx_nav_status (ubx->payload, gpsposition); + parse_ubx_nav_status (&ubx->payload.nav_status, GpsPosition); break; case UBX_ID_DOP: - parse_ubx_nav_dop (ubx->payload, gpsposition); + parse_ubx_nav_dop (&ubx->payload.nav_dop, GpsPosition); break; case UBX_ID_SOL: - parse_ubx_nav_sol (ubx->payload, gpsposition); + parse_ubx_nav_sol (&ubx->payload.nav_sol, GpsPosition); break; case UBX_ID_VELNED: - parse_ubx_nav_velned (ubx->payload, gpsposition); + parse_ubx_nav_velned (&ubx->payload.nav_velned, GpsPosition); break; +#if !defined(PIOS_GPS_MINIMAL) case UBX_ID_TIMEUTC: - parse_ubx_nav_timeutc (ubx->payload); + parse_ubx_nav_timeutc (&ubx->payload.nav_timeutc); break; case UBX_ID_SVINFO: - parse_ubx_nav_svinfo (ubx->payload); + parse_ubx_nav_svinfo (&ubx->payload.nav_svinfo); break; +#endif } break; } - if (msgtracker.msg_received == ALL_RECEIVED) - { - GPSPositionSet(gpsposition); + if (msgtracker.msg_received == ALL_RECEIVED) { + GPSPositionSet(GpsPosition); msgtracker.msg_received = NONE_RECEIVED; - return true; + id = GPSPOSITION_OBJID; } - return false; -#endif + return id; } +#endif // PIOS_INCLUDE_GPS_UBX_PARSER diff --git a/flight/Modules/GPS/inc/GPS.h b/flight/Modules/GPS/inc/GPS.h index 86a92d285..250565e0a 100644 --- a/flight/Modules/GPS/inc/GPS.h +++ b/flight/Modules/GPS/inc/GPS.h @@ -1,44 +1,55 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup GSPModule GPS Module - * @brief Process GPS information - * @{ - * - * @file GPS.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Include file of the GPS module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and - * objects. - * @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 GPS_H -#define GPS_H - -int32_t GPSInitialize(void); - -#endif // GPS_H - -/** - * @} - * @} - */ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GSPModule GPS Module + * @brief Process GPS information + * @{ + * + * @file GPS.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Include file of the GPS module. + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and + * objects. + * @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 GPS_H +#define GPS_H + +#include "gpsvelocity.h" +#include "gpssatellites.h" +#include "gpsposition.h" +#include "gpstime.h" + +#define NO_PARSER -3 // no parser available +#define PARSER_OVERRUN -2 // message buffer overrun before completing the message +#define PARSER_ERROR -1 // message unparsable by this parser +#define PARSER_INCOMPLETE 0 // parser needs more data to complete the message +#define PARSER_COMPLETE 1 // parser has received a complete message and finished processing + +int32_t GPSInitialize(void); + +#endif // GPS_H + +/** + * @} + * @} + */ diff --git a/flight/Modules/GPS/inc/NMEA.h b/flight/Modules/GPS/inc/NMEA.h index 6ff6b1195..eb4567560 100644 --- a/flight/Modules/GPS/inc/NMEA.h +++ b/flight/Modules/GPS/inc/NMEA.h @@ -34,7 +34,10 @@ #include #include -extern bool NMEA_update_position(char *nmea_sentence); +#define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed + +extern bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData); extern bool NMEA_checksum(char *nmea_sentence); +extern int parse_nmea_stream(uint8_t, char *, GPSPositionData *); #endif /* NMEA_H */ diff --git a/flight/Modules/GPS/inc/UBX.h b/flight/Modules/GPS/inc/UBX.h index 5177f8321..4df3641be 100644 --- a/flight/Modules/GPS/inc/UBX.h +++ b/flight/Modules/GPS/inc/UBX.h @@ -31,6 +31,7 @@ #ifndef UBX_H #define UBX_H #include "openpilot.h" +#include "gpsposition.h" #define UBX_SYNC1 0xb5 // UBX protocol synchronization characters #define UBX_SYNC2 0x62 @@ -53,13 +54,13 @@ // Geodetic Position Solution typedef struct { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - int32_t lon; // Longitude (deg*1e-7) - int32_t lat; // Latitude (deg*1e-7) - int32_t height; // Height above Ellipoid (mm) - int32_t hMSL; // Height abobe mean sea level (mm) - uint32_t hAcc; // Horizontal Accuracy Estimate (mm) - uint32_t vAcc; // Vertical Accuracy Estimate (mm) + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + int32_t lon; // Longitude (deg*1e-7) + int32_t lat; // Latitude (deg*1e-7) + int32_t height; // Height above Ellipoid (mm) + int32_t hMSL; // Height abobe mean sea level (mm) + uint32_t hAcc; // Horizontal Accuracy Estimate (mm) + uint32_t vAcc; // Vertical Accuracy Estimate (mm) } UBX_NAV_POSLLH; // Receiver Navigation Status @@ -77,61 +78,61 @@ typedef struct { #define STATUS_FLAGS_TOWSET (1 << 3) typedef struct { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - uint8_t gpsFix; // GPS fix type - uint8_t flags; // Navigation Status Flags - uint8_t fixStat; // Fix Status Information - uint8_t flags2; // Additional navigation output information - uint32_t ttff; // Time to first fix (ms) - uint32_t msss; // Milliseconds since startup/reset (ms) + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint8_t gpsFix; // GPS fix type + uint8_t flags; // Navigation Status Flags + uint8_t fixStat; // Fix Status Information + uint8_t flags2; // Additional navigation output information + uint32_t ttff; // Time to first fix (ms) + uint32_t msss; // Milliseconds since startup/reset (ms) } UBX_NAV_STATUS; // Dilution of precision typedef struct { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - 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 + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + 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 } UBX_NAV_DOP; // Navigation solution typedef struct { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - int32_t fTOW; // fracional nanoseconds (ns) - int16_t week; // GPS week - uint8_t gpsFix; // GPS fix type - uint8_t flags; // Fix status flags - int32_t ecefX; // ECEF X coordinate (cm) - int32_t ecefY; // ECEF Y coordinate (cm) - int32_t ecefZ; // ECEF Z coordinate (cm) - uint32_t pAcc; // 3D Position Accuracy Estimate (cm) - int32_t ecefVX; // ECEF X coordinate (cm/s) - int32_t ecefVY; // ECEF Y coordinate (cm/s) - int32_t ecefVZ; // ECEF Z coordinate (cm/s) - uint32_t sAcc; // Speed Accuracy Estimate - uint16_t pDOP; // Position DOP - uint8_t reserved1; // Reserved - uint8_t numSV; // Number of SVs used in Nav Solution - uint32_t reserved2; // Reserved + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + int32_t fTOW; // fracional nanoseconds (ns) + int16_t week; // GPS week + uint8_t gpsFix; // GPS fix type + uint8_t flags; // Fix status flags + int32_t ecefX; // ECEF X coordinate (cm) + int32_t ecefY; // ECEF Y coordinate (cm) + int32_t ecefZ; // ECEF Z coordinate (cm) + uint32_t pAcc; // 3D Position Accuracy Estimate (cm) + int32_t ecefVX; // ECEF X coordinate (cm/s) + int32_t ecefVY; // ECEF Y coordinate (cm/s) + int32_t ecefVZ; // ECEF Z coordinate (cm/s) + uint32_t sAcc; // Speed Accuracy Estimate + uint16_t pDOP; // Position DOP + uint8_t reserved1; // Reserved + uint8_t numSV; // Number of SVs used in Nav Solution + uint32_t reserved2; // Reserved } UBX_NAV_SOL; // North/East/Down velocity 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 + 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; // UTC Time Solution @@ -141,51 +142,51 @@ typedef struct { #define TIMEUTC_VALIDUTC (1 << 2) typedef struct { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - uint32_t tAcc; // Time Accuracy Estimate (ns) - int32_t nano; // Nanoseconds of second + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint32_t tAcc; // Time Accuracy Estimate (ns) + int32_t nano; // Nanoseconds of second uint16_t year; uint8_t month; uint8_t day; uint8_t hour; uint8_t min; uint8_t sec; - uint8_t valid; // Validity Flags + uint8_t valid; // Validity Flags } UBX_NAV_TIMEUTC; // Space Vehicle (SV) Information // Single SV information block -#define SVUSED (1 << 0) // This SV is used for navigation -#define DIFFCORR (1 << 1) // Differential correction available -#define ORBITAVAIL (1 << 2) // Orbit information available -#define ORBITEPH (1 << 3) // Orbit information is Ephemeris -#define UNHEALTHY (1 << 4) // SV is unhealthy -#define ORBITALM (1 << 5) // Orbit information is Almanac Plus -#define ORBITAOP (1 << 6) // Orbit information is AssistNow Autonomous -#define SMOOTHED (1 << 7) // Carrier smoothed pseudoranges used +#define SVUSED (1 << 0) // This SV is used for navigation +#define DIFFCORR (1 << 1) // Differential correction available +#define ORBITAVAIL (1 << 2) // Orbit information available +#define ORBITEPH (1 << 3) // Orbit information is Ephemeris +#define UNHEALTHY (1 << 4) // SV is unhealthy +#define ORBITALM (1 << 5) // Orbit information is Almanac Plus +#define ORBITAOP (1 << 6) // Orbit information is AssistNow Autonomous +#define SMOOTHED (1 << 7) // Carrier smoothed pseudoranges used typedef struct { - uint8_t chn; // Channel number - uint8_t svid; // Satellite ID - uint8_t flags; // Misc SV information - uint8_t quality; // Misc quality indicators - uint8_t cno; // Carrier to Noise Ratio (dbHz) - int8_t elev; // Elevation (integer degrees) - int16_t azim; // Azimuth (integer degrees) - int32_t prRes; // Pseudo range residual (cm) + uint8_t chn; // Channel number + uint8_t svid; // Satellite ID + uint8_t flags; // Misc SV information + uint8_t quality; // Misc quality indicators + uint8_t cno; // Carrier to Noise Ratio (dbHz) + int8_t elev; // Elevation (integer degrees) + int16_t azim; // Azimuth (integer degrees) + int32_t prRes; // Pseudo range residual (cm) } UBX_NAV_SVINFO_SV; // SV information message -#define MAX_SVINFO_MSG_SIZE 208 +#define MAX_SVS 16 typedef struct { - uint32_t iTOW; // GPS Millisecond Time of Week (ms) - uint8_t numCh; // Number of channels - uint8_t globalFlags; // - uint16_t reserved2; // Reserved - UBX_NAV_SVINFO_SV sv[16]; // Repeated 'numCh' times + uint32_t iTOW; // GPS Millisecond Time of Week (ms) + uint8_t numCh; // Number of channels + uint8_t globalFlags; // + uint16_t reserved2; // Reserved + UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times } UBX_NAV_SVINFO; typedef union { @@ -195,8 +196,10 @@ typedef union { UBX_NAV_DOP nav_dop; UBX_NAV_SOL nav_sol; UBX_NAV_VELNED nav_velned; +#if !defined(PIOS_GPS_MINIMAL) UBX_NAV_TIMEUTC nav_timeutc; UBX_NAV_SVINFO nav_svinfo; +#endif } UBXPayload; typedef struct { @@ -213,5 +216,7 @@ typedef struct { } UBXPacket; bool checksum_ubx_message(UBXPacket *); -bool parse_ubx_message(UBXPacket *, GPSPositionData *); +uint32_t parse_ubx_message(UBXPacket *, GPSPositionData *); +int parse_ubx_stream(uint8_t, char *, GPSPositionData *); + #endif /* UBX_H */ diff --git a/flight/Revolution/System/inc/pios_config.h b/flight/Revolution/System/inc/pios_config.h index 3043ea630..cc7c67747 100644 --- a/flight/Revolution/System/inc/pios_config.h +++ b/flight/Revolution/System/inc/pios_config.h @@ -96,6 +96,8 @@ #define PIOS_TELEM_PRIORITY_QUEUE /* Enable a priority queue in telemetry */ #define PIOS_QUATERNION_STABILIZATION /* Stabilization options */ //#define PIOS_GPS_SETS_HOMELOCATION /* GPS options */ +#define PIOS_INCLUDE_GPS_NMEA_PARSER /* Include the NMEA protocol parser */ +#define PIOS_INCLUDE_GPS_UBX_PARSER /* Include the UBX protocol parser */ /* Alarm Thresholds */ #define HEAP_LIMIT_WARNING 4000 diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index d91eb9c17..52063f748 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -46,6 +46,7 @@ UAVOBJSRCFILENAMES += flighttelemetrystats UAVOBJSRCFILENAMES += gcstelemetrystats UAVOBJSRCFILENAMES += gpsposition UAVOBJSRCFILENAMES += gpssatellites +UAVOBJSRCFILENAMES += gpssettings UAVOBJSRCFILENAMES += gpstime UAVOBJSRCFILENAMES += guidancesettings UAVOBJSRCFILENAMES += homelocation diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index bfe2342b3..720d36ebd 100755 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -52,6 +52,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/gpsposition.h \ $$UAVOBJECT_SYNTHETICS/gpstime.h \ $$UAVOBJECT_SYNTHETICS/gpssatellites.h \ + $$UAVOBJECT_SYNTHETICS/gpssettings.h \ + $$UAVOBJECT_SYNTHETICS/gpsvelocity.h \ $$UAVOBJECT_SYNTHETICS/positionactual.h \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \ $$UAVOBJECT_SYNTHETICS/homelocation.h \ @@ -112,6 +114,8 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/gpsposition.cpp \ $$UAVOBJECT_SYNTHETICS/gpstime.cpp \ $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \ + $$UAVOBJECT_SYNTHETICS/gpssettings.cpp \ + $$UAVOBJECT_SYNTHETICS/gpsvelocity.cpp \ $$UAVOBJECT_SYNTHETICS/positionactual.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \ $$UAVOBJECT_SYNTHETICS/homelocation.cpp \ From 94b610c8821a2dadbd143c2fd3ebe40638e58217 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 28 Jun 2012 10:27:03 +0200 Subject: [PATCH 05/32] Added missing gpssettings.xml --- shared/uavobjectdefinition/gpssettings.xml | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 shared/uavobjectdefinition/gpssettings.xml diff --git a/shared/uavobjectdefinition/gpssettings.xml b/shared/uavobjectdefinition/gpssettings.xml new file mode 100644 index 000000000..5c38f43fe --- /dev/null +++ b/shared/uavobjectdefinition/gpssettings.xml @@ -0,0 +1,10 @@ + + + Settings for the GPS + + + + + + + From 7a7b64c9cfa210738d266fac9cca8bc675807f4e Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 30 Jun 2012 21:18:12 +0200 Subject: [PATCH 06/32] Report SVs even if sat position is unknown. This way, received satellites are reported early during cold start which gives better feedback to the user. --- flight/Modules/GPS/UBX.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/flight/Modules/GPS/UBX.c b/flight/Modules/GPS/UBX.c index de97b9e3a..bb45848a6 100644 --- a/flight/Modules/GPS/UBX.c +++ b/flight/Modules/GPS/UBX.c @@ -285,8 +285,7 @@ void parse_ubx_nav_svinfo (UBX_NAV_SVINFO *svinfo) svdata.SatsInView = 0; for (chan = 0; chan < svinfo->numCh; chan++) { - if ((svinfo->sv[chan].elev > 0) && // some unhealthy SV report negative elevation - (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM)) { + if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) { svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim; svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev; svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid; From 6751a4cd8b336f686cae52eb39d829c99c38cf87 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 30 Jun 2012 21:22:57 +0200 Subject: [PATCH 07/32] Satellite constellation display shouldn't display satellites with negative elevation angle. --- .../gpsdisplay/gpsconstellationwidget.cpp | 368 +++++++++--------- 1 file changed, 184 insertions(+), 184 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp index d6bddb291..8ae569095 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp @@ -1,184 +1,184 @@ -/** - ****************************************************************************** - * - * @file gpsconstellationwidget.cpp - * @author Edouard Lafargue Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup GPSGadgetPlugin GPS Gadget Plugin - * @{ - * @brief A widget which displays the GPS constellation - *****************************************************************************/ -/* - * 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 "gpsconstellationwidget.h" - -#include -#include - -/* - * Initialize the widget - */ -GpsConstellationWidget::GpsConstellationWidget(QWidget *parent) : QGraphicsView(parent) -{ - - // Create a layout, add a QGraphicsView and put the SVG inside. - // The constellation widget looks like this: - // |--------------------| - // | | - // | | - // | Constellation | - // | | - // | | - // | | - // |--------------------| - - - setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); - setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); - - QSvgRenderer *renderer = new QSvgRenderer(); - renderer->load(QString(":/gpsgadget/images/gpsEarth.svg")); - - world = new QGraphicsSvgItem(); - world->setSharedRenderer(renderer); - world->setElementId("map"); - - scene = new QGraphicsScene(this); - scene->addItem(world); - scene->setSceneRect(world->boundingRect()); - setScene(scene); - - // Now create 'maxSatellites' satellite icons which we will move around on the map: - for (int i=0; i < MAX_SATTELITES;i++) { - satellites[i][0] = 0; - satellites[i][1] = 0; - satellites[i][2] = 0; - satellites[i][3] = 0; - - satIcons[i] = new QGraphicsSvgItem(world); - satIcons[i]->setSharedRenderer(renderer); - satIcons[i]->setElementId("sat-notSeen"); - satIcons[i]->hide(); - - satTexts[i] = new QGraphicsSimpleTextItem("##",satIcons[i]); - satTexts[i]->setBrush(QColor("Black")); - satTexts[i]->setFont(QFont("Courier")); - } -} - -GpsConstellationWidget::~GpsConstellationWidget() -{ - delete scene; - scene = 0; - - //delete renderer; - //renderer = 0; -} - -void GpsConstellationWidget::showEvent(QShowEvent *event) -{ - Q_UNUSED(event) - // Thit fitInView method should only be called now, once the - // widget is shown, otherwise it cannot compute its values and - // the result is usually a ahrsbargraph that is way too small. - fitInView(world, Qt::KeepAspectRatio); - // Scale, can't use fitInView since that doesn't work until we're shown. - // qreal factor = height()/world->boundingRect().height(); -// world->setScale(factor); - // setSceneRect(world->boundingRect()); - -} - -void GpsConstellationWidget::resizeEvent(QResizeEvent* event) -{ - Q_UNUSED(event); - fitInView(world, Qt::KeepAspectRatio); -} - -void GpsConstellationWidget::updateSat(int index, int prn, int elevation, int azimuth, int snr) -{ - if (index >= MAX_SATTELITES) { - // A bit of error checking never hurts. - return; - } - - // TODO: add range checking - satellites[index][0] = prn; - satellites[index][1] = elevation; - satellites[index][2] = azimuth; - satellites[index][3] = snr; - - if (prn) { - QPointF opd = polarToCoord(elevation,azimuth); - opd += QPointF(-satIcons[index]->boundingRect().center().x(), - -satIcons[index]->boundingRect().center().y()); - satIcons[index]->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false); - if (snr) { - satIcons[index]->setElementId("satellite"); - } else { - satIcons[index]->setElementId("sat-notSeen"); - } - satIcons[index]->show(); - - QRectF iconRect = satIcons[index]->boundingRect(); - QString prnString = QString().number(prn); - if(prnString.length() == 1) { - prnString = "0" + prnString; - } - satTexts[index]->setText(prnString); - QRectF textRect = satTexts[index]->boundingRect(); - - QTransform matrix; - qreal scale = 0.70 * (iconRect.width() / textRect.width()); - matrix.translate(iconRect.width()/2, iconRect.height()/2); - matrix.scale(scale,scale); - matrix.translate(-textRect.width()/2,-textRect.height()/2); - satTexts[index]->setTransform(matrix,false); - } else { - satIcons[index]->hide(); - } - -} - -/** - Converts the elevation/azimuth to X/Y coordinates on the map - - */ -QPointF GpsConstellationWidget::polarToCoord(int elevation, int azimuth) -{ - double x; - double y; - double rad_elevation; - double rad_azimuth; - - - rad_elevation = M_PI*elevation/180; - rad_azimuth = M_PI*azimuth/180; - - x = cos(rad_elevation)*sin(rad_azimuth); - y = -cos(rad_elevation)*cos(rad_azimuth); - - x = world->boundingRect().width()/2 * x; - y = world->boundingRect().height()/2 * y; - - x = (world->boundingRect().width() / 2) + x; - y = (world->boundingRect().height() / 2) + y; - - return QPointF(x,y); - -} +/** + ****************************************************************************** + * + * @file gpsconstellationwidget.cpp + * @author Edouard Lafargue Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup GPSGadgetPlugin GPS Gadget Plugin + * @{ + * @brief A widget which displays the GPS constellation + *****************************************************************************/ +/* + * 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 "gpsconstellationwidget.h" + +#include +#include + +/* + * Initialize the widget + */ +GpsConstellationWidget::GpsConstellationWidget(QWidget *parent) : QGraphicsView(parent) +{ + + // Create a layout, add a QGraphicsView and put the SVG inside. + // The constellation widget looks like this: + // |--------------------| + // | | + // | | + // | Constellation | + // | | + // | | + // | | + // |--------------------| + + + setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + + QSvgRenderer *renderer = new QSvgRenderer(); + renderer->load(QString(":/gpsgadget/images/gpsEarth.svg")); + + world = new QGraphicsSvgItem(); + world->setSharedRenderer(renderer); + world->setElementId("map"); + + scene = new QGraphicsScene(this); + scene->addItem(world); + scene->setSceneRect(world->boundingRect()); + setScene(scene); + + // Now create 'maxSatellites' satellite icons which we will move around on the map: + for (int i=0; i < MAX_SATTELITES;i++) { + satellites[i][0] = 0; + satellites[i][1] = 0; + satellites[i][2] = 0; + satellites[i][3] = 0; + + satIcons[i] = new QGraphicsSvgItem(world); + satIcons[i]->setSharedRenderer(renderer); + satIcons[i]->setElementId("sat-notSeen"); + satIcons[i]->hide(); + + satTexts[i] = new QGraphicsSimpleTextItem("##",satIcons[i]); + satTexts[i]->setBrush(QColor("Black")); + satTexts[i]->setFont(QFont("Courier")); + } +} + +GpsConstellationWidget::~GpsConstellationWidget() +{ + delete scene; + scene = 0; + + //delete renderer; + //renderer = 0; +} + +void GpsConstellationWidget::showEvent(QShowEvent *event) +{ + Q_UNUSED(event) + // Thit fitInView method should only be called now, once the + // widget is shown, otherwise it cannot compute its values and + // the result is usually a ahrsbargraph that is way too small. + fitInView(world, Qt::KeepAspectRatio); + // Scale, can't use fitInView since that doesn't work until we're shown. + // qreal factor = height()/world->boundingRect().height(); +// world->setScale(factor); + // setSceneRect(world->boundingRect()); + +} + +void GpsConstellationWidget::resizeEvent(QResizeEvent* event) +{ + Q_UNUSED(event); + fitInView(world, Qt::KeepAspectRatio); +} + +void GpsConstellationWidget::updateSat(int index, int prn, int elevation, int azimuth, int snr) +{ + if (index >= MAX_SATTELITES) { + // A bit of error checking never hurts. + return; + } + + // TODO: add range checking + satellites[index][0] = prn; + satellites[index][1] = elevation; + satellites[index][2] = azimuth; + satellites[index][3] = snr; + + if (prn && elevation >= 0) { + QPointF opd = polarToCoord(elevation,azimuth); + opd += QPointF(-satIcons[index]->boundingRect().center().x(), + -satIcons[index]->boundingRect().center().y()); + satIcons[index]->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false); + if (snr) { + satIcons[index]->setElementId("satellite"); + } else { + satIcons[index]->setElementId("sat-notSeen"); + } + satIcons[index]->show(); + + QRectF iconRect = satIcons[index]->boundingRect(); + QString prnString = QString().number(prn); + if(prnString.length() == 1) { + prnString = "0" + prnString; + } + satTexts[index]->setText(prnString); + QRectF textRect = satTexts[index]->boundingRect(); + + QTransform matrix; + qreal scale = 0.70 * (iconRect.width() / textRect.width()); + matrix.translate(iconRect.width()/2, iconRect.height()/2); + matrix.scale(scale,scale); + matrix.translate(-textRect.width()/2,-textRect.height()/2); + satTexts[index]->setTransform(matrix,false); + } else { + satIcons[index]->hide(); + } + +} + +/** + Converts the elevation/azimuth to X/Y coordinates on the map + + */ +QPointF GpsConstellationWidget::polarToCoord(int elevation, int azimuth) +{ + double x; + double y; + double rad_elevation; + double rad_azimuth; + + + rad_elevation = M_PI*elevation/180; + rad_azimuth = M_PI*azimuth/180; + + x = cos(rad_elevation)*sin(rad_azimuth); + y = -cos(rad_elevation)*cos(rad_azimuth); + + x = world->boundingRect().width()/2 * x; + y = world->boundingRect().height()/2 * y; + + x = (world->boundingRect().width() / 2) + x; + y = (world->boundingRect().height() / 2) + y; + + return QPointF(x,y); + +} From e2a35d1f4c0971c672f60c91bf6d7a4f44e84fa1 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Mon, 2 Jul 2012 23:21:15 +0200 Subject: [PATCH 08/32] Changed GPS port to be rx/tx instead of rx only --- flight/Revolution/System/pios_board.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c index 705463186..f90327f36 100644 --- a/flight/Revolution/System/pios_board.c +++ b/flight/Revolution/System/pios_board.c @@ -252,6 +252,7 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; #define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 #define PIOS_COM_GPS_RX_BUF_LEN 32 +#define PIOS_COM_GPS_TX_BUF_LEN 32 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 @@ -543,7 +544,7 @@ void PIOS_Board_Init(void) { break; case HWSETTINGS_RV_GPSPORT_GPS: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); + PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); break; case HWSETTINGS_RV_GPSPORT_COMAUX: From f1b846593fadae575d517c10a5457dbe3c96ba14 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 10 Jul 2012 11:40:55 +0200 Subject: [PATCH 09/32] Fixed line endings --- flight/Modules/GPS/inc/GPS.h | 110 +++--- .../gpsdisplay/gpsconstellationwidget.cpp | 368 +++++++++--------- 2 files changed, 239 insertions(+), 239 deletions(-) diff --git a/flight/Modules/GPS/inc/GPS.h b/flight/Modules/GPS/inc/GPS.h index 250565e0a..fad5e8eed 100644 --- a/flight/Modules/GPS/inc/GPS.h +++ b/flight/Modules/GPS/inc/GPS.h @@ -1,55 +1,55 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup GSPModule GPS Module - * @brief Process GPS information - * @{ - * - * @file GPS.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Include file of the GPS module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and - * objects. - * @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 GPS_H -#define GPS_H - -#include "gpsvelocity.h" -#include "gpssatellites.h" -#include "gpsposition.h" -#include "gpstime.h" - -#define NO_PARSER -3 // no parser available -#define PARSER_OVERRUN -2 // message buffer overrun before completing the message -#define PARSER_ERROR -1 // message unparsable by this parser -#define PARSER_INCOMPLETE 0 // parser needs more data to complete the message -#define PARSER_COMPLETE 1 // parser has received a complete message and finished processing - -int32_t GPSInitialize(void); - -#endif // GPS_H - -/** - * @} - * @} - */ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup GSPModule GPS Module + * @brief Process GPS information + * @{ + * + * @file GPS.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Include file of the GPS module. + * As with all modules only the initialize function is exposed all other + * interactions with the module take place through the event queue and + * objects. + * @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 GPS_H +#define GPS_H + +#include "gpsvelocity.h" +#include "gpssatellites.h" +#include "gpsposition.h" +#include "gpstime.h" + +#define NO_PARSER -3 // no parser available +#define PARSER_OVERRUN -2 // message buffer overrun before completing the message +#define PARSER_ERROR -1 // message unparsable by this parser +#define PARSER_INCOMPLETE 0 // parser needs more data to complete the message +#define PARSER_COMPLETE 1 // parser has received a complete message and finished processing + +int32_t GPSInitialize(void); + +#endif // GPS_H + +/** + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp index 8ae569095..e70f2ed72 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp @@ -1,184 +1,184 @@ -/** - ****************************************************************************** - * - * @file gpsconstellationwidget.cpp - * @author Edouard Lafargue Copyright (C) 2010. - * @addtogroup GCSPlugins GCS Plugins - * @{ - * @addtogroup GPSGadgetPlugin GPS Gadget Plugin - * @{ - * @brief A widget which displays the GPS constellation - *****************************************************************************/ -/* - * 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 "gpsconstellationwidget.h" - -#include -#include - -/* - * Initialize the widget - */ -GpsConstellationWidget::GpsConstellationWidget(QWidget *parent) : QGraphicsView(parent) -{ - - // Create a layout, add a QGraphicsView and put the SVG inside. - // The constellation widget looks like this: - // |--------------------| - // | | - // | | - // | Constellation | - // | | - // | | - // | | - // |--------------------| - - - setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); - setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); - - QSvgRenderer *renderer = new QSvgRenderer(); - renderer->load(QString(":/gpsgadget/images/gpsEarth.svg")); - - world = new QGraphicsSvgItem(); - world->setSharedRenderer(renderer); - world->setElementId("map"); - - scene = new QGraphicsScene(this); - scene->addItem(world); - scene->setSceneRect(world->boundingRect()); - setScene(scene); - - // Now create 'maxSatellites' satellite icons which we will move around on the map: - for (int i=0; i < MAX_SATTELITES;i++) { - satellites[i][0] = 0; - satellites[i][1] = 0; - satellites[i][2] = 0; - satellites[i][3] = 0; - - satIcons[i] = new QGraphicsSvgItem(world); - satIcons[i]->setSharedRenderer(renderer); - satIcons[i]->setElementId("sat-notSeen"); - satIcons[i]->hide(); - - satTexts[i] = new QGraphicsSimpleTextItem("##",satIcons[i]); - satTexts[i]->setBrush(QColor("Black")); - satTexts[i]->setFont(QFont("Courier")); - } -} - -GpsConstellationWidget::~GpsConstellationWidget() -{ - delete scene; - scene = 0; - - //delete renderer; - //renderer = 0; -} - -void GpsConstellationWidget::showEvent(QShowEvent *event) -{ - Q_UNUSED(event) - // Thit fitInView method should only be called now, once the - // widget is shown, otherwise it cannot compute its values and - // the result is usually a ahrsbargraph that is way too small. - fitInView(world, Qt::KeepAspectRatio); - // Scale, can't use fitInView since that doesn't work until we're shown. - // qreal factor = height()/world->boundingRect().height(); -// world->setScale(factor); - // setSceneRect(world->boundingRect()); - -} - -void GpsConstellationWidget::resizeEvent(QResizeEvent* event) -{ - Q_UNUSED(event); - fitInView(world, Qt::KeepAspectRatio); -} - -void GpsConstellationWidget::updateSat(int index, int prn, int elevation, int azimuth, int snr) -{ - if (index >= MAX_SATTELITES) { - // A bit of error checking never hurts. - return; - } - - // TODO: add range checking - satellites[index][0] = prn; - satellites[index][1] = elevation; - satellites[index][2] = azimuth; - satellites[index][3] = snr; - - if (prn && elevation >= 0) { - QPointF opd = polarToCoord(elevation,azimuth); - opd += QPointF(-satIcons[index]->boundingRect().center().x(), - -satIcons[index]->boundingRect().center().y()); - satIcons[index]->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false); - if (snr) { - satIcons[index]->setElementId("satellite"); - } else { - satIcons[index]->setElementId("sat-notSeen"); - } - satIcons[index]->show(); - - QRectF iconRect = satIcons[index]->boundingRect(); - QString prnString = QString().number(prn); - if(prnString.length() == 1) { - prnString = "0" + prnString; - } - satTexts[index]->setText(prnString); - QRectF textRect = satTexts[index]->boundingRect(); - - QTransform matrix; - qreal scale = 0.70 * (iconRect.width() / textRect.width()); - matrix.translate(iconRect.width()/2, iconRect.height()/2); - matrix.scale(scale,scale); - matrix.translate(-textRect.width()/2,-textRect.height()/2); - satTexts[index]->setTransform(matrix,false); - } else { - satIcons[index]->hide(); - } - -} - -/** - Converts the elevation/azimuth to X/Y coordinates on the map - - */ -QPointF GpsConstellationWidget::polarToCoord(int elevation, int azimuth) -{ - double x; - double y; - double rad_elevation; - double rad_azimuth; - - - rad_elevation = M_PI*elevation/180; - rad_azimuth = M_PI*azimuth/180; - - x = cos(rad_elevation)*sin(rad_azimuth); - y = -cos(rad_elevation)*cos(rad_azimuth); - - x = world->boundingRect().width()/2 * x; - y = world->boundingRect().height()/2 * y; - - x = (world->boundingRect().width() / 2) + x; - y = (world->boundingRect().height() / 2) + y; - - return QPointF(x,y); - -} +/** + ****************************************************************************** + * + * @file gpsconstellationwidget.cpp + * @author Edouard Lafargue Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup GPSGadgetPlugin GPS Gadget Plugin + * @{ + * @brief A widget which displays the GPS constellation + *****************************************************************************/ +/* + * 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 "gpsconstellationwidget.h" + +#include +#include + +/* + * Initialize the widget + */ +GpsConstellationWidget::GpsConstellationWidget(QWidget *parent) : QGraphicsView(parent) +{ + + // Create a layout, add a QGraphicsView and put the SVG inside. + // The constellation widget looks like this: + // |--------------------| + // | | + // | | + // | Constellation | + // | | + // | | + // | | + // |--------------------| + + + setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff); + + QSvgRenderer *renderer = new QSvgRenderer(); + renderer->load(QString(":/gpsgadget/images/gpsEarth.svg")); + + world = new QGraphicsSvgItem(); + world->setSharedRenderer(renderer); + world->setElementId("map"); + + scene = new QGraphicsScene(this); + scene->addItem(world); + scene->setSceneRect(world->boundingRect()); + setScene(scene); + + // Now create 'maxSatellites' satellite icons which we will move around on the map: + for (int i=0; i < MAX_SATTELITES;i++) { + satellites[i][0] = 0; + satellites[i][1] = 0; + satellites[i][2] = 0; + satellites[i][3] = 0; + + satIcons[i] = new QGraphicsSvgItem(world); + satIcons[i]->setSharedRenderer(renderer); + satIcons[i]->setElementId("sat-notSeen"); + satIcons[i]->hide(); + + satTexts[i] = new QGraphicsSimpleTextItem("##",satIcons[i]); + satTexts[i]->setBrush(QColor("Black")); + satTexts[i]->setFont(QFont("Courier")); + } +} + +GpsConstellationWidget::~GpsConstellationWidget() +{ + delete scene; + scene = 0; + + //delete renderer; + //renderer = 0; +} + +void GpsConstellationWidget::showEvent(QShowEvent *event) +{ + Q_UNUSED(event) + // Thit fitInView method should only be called now, once the + // widget is shown, otherwise it cannot compute its values and + // the result is usually a ahrsbargraph that is way too small. + fitInView(world, Qt::KeepAspectRatio); + // Scale, can't use fitInView since that doesn't work until we're shown. + // qreal factor = height()/world->boundingRect().height(); +// world->setScale(factor); + // setSceneRect(world->boundingRect()); + +} + +void GpsConstellationWidget::resizeEvent(QResizeEvent* event) +{ + Q_UNUSED(event); + fitInView(world, Qt::KeepAspectRatio); +} + +void GpsConstellationWidget::updateSat(int index, int prn, int elevation, int azimuth, int snr) +{ + if (index >= MAX_SATTELITES) { + // A bit of error checking never hurts. + return; + } + + // TODO: add range checking + satellites[index][0] = prn; + satellites[index][1] = elevation; + satellites[index][2] = azimuth; + satellites[index][3] = snr; + + if (prn && elevation >= 0) { + QPointF opd = polarToCoord(elevation,azimuth); + opd += QPointF(-satIcons[index]->boundingRect().center().x(), + -satIcons[index]->boundingRect().center().y()); + satIcons[index]->setTransform(QTransform::fromTranslate(opd.x(),opd.y()), false); + if (snr) { + satIcons[index]->setElementId("satellite"); + } else { + satIcons[index]->setElementId("sat-notSeen"); + } + satIcons[index]->show(); + + QRectF iconRect = satIcons[index]->boundingRect(); + QString prnString = QString().number(prn); + if(prnString.length() == 1) { + prnString = "0" + prnString; + } + satTexts[index]->setText(prnString); + QRectF textRect = satTexts[index]->boundingRect(); + + QTransform matrix; + qreal scale = 0.70 * (iconRect.width() / textRect.width()); + matrix.translate(iconRect.width()/2, iconRect.height()/2); + matrix.scale(scale,scale); + matrix.translate(-textRect.width()/2,-textRect.height()/2); + satTexts[index]->setTransform(matrix,false); + } else { + satIcons[index]->hide(); + } + +} + +/** + Converts the elevation/azimuth to X/Y coordinates on the map + + */ +QPointF GpsConstellationWidget::polarToCoord(int elevation, int azimuth) +{ + double x; + double y; + double rad_elevation; + double rad_azimuth; + + + rad_elevation = M_PI*elevation/180; + rad_azimuth = M_PI*azimuth/180; + + x = cos(rad_elevation)*sin(rad_azimuth); + y = -cos(rad_elevation)*cos(rad_azimuth); + + x = world->boundingRect().width()/2 * x; + y = world->boundingRect().height()/2 * y; + + x = (world->boundingRect().width() / 2) + x; + y = (world->boundingRect().height() / 2) + y; + + return QPointF(x,y); + +} From 10351e61d40dd09526fa7fcaa2031578e0277387 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 10 Jul 2012 12:10:17 +0200 Subject: [PATCH 10/32] Revert "Changed GPS port to be rx/tx instead of rx only" This reverts commit e2a35d1f4c0971c672f60c91bf6d7a4f44e84fa1. --- flight/Revolution/System/pios_board.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/flight/Revolution/System/pios_board.c b/flight/Revolution/System/pios_board.c index f90327f36..705463186 100644 --- a/flight/Revolution/System/pios_board.c +++ b/flight/Revolution/System/pios_board.c @@ -252,7 +252,6 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; #define PIOS_COM_TELEM_RF_TX_BUF_LEN 512 #define PIOS_COM_GPS_RX_BUF_LEN 32 -#define PIOS_COM_GPS_TX_BUF_LEN 32 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 @@ -544,7 +543,7 @@ void PIOS_Board_Init(void) { break; case HWSETTINGS_RV_GPSPORT_GPS: - PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); + PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); break; case HWSETTINGS_RV_GPSPORT_COMAUX: From 19a9cd03c2609593efefd10fcc4a396c9b84c0bd Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 10 Jul 2012 16:23:10 +0200 Subject: [PATCH 11/32] Introduced GPSVelocity UAVObject Conflicts: ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro --- flight/Revolution/UAVObjects.inc | 1 + shared/uavobjectdefinition/gpsvelocity.xml | 12 ++++++++++++ 2 files changed, 13 insertions(+) create mode 100644 shared/uavobjectdefinition/gpsvelocity.xml diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index 52063f748..462acfc34 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -48,6 +48,7 @@ UAVOBJSRCFILENAMES += gpsposition UAVOBJSRCFILENAMES += gpssatellites UAVOBJSRCFILENAMES += gpssettings UAVOBJSRCFILENAMES += gpstime +UAVOBJSRCFILENAMES += gpsvelocity UAVOBJSRCFILENAMES += guidancesettings UAVOBJSRCFILENAMES += homelocation UAVOBJSRCFILENAMES += i2cstats 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. + + + + + + + + + From 57769814368397ab93f0e19db8e52cb7baa01db1 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 10 Jul 2012 16:25:58 +0200 Subject: [PATCH 12/32] Deleted obsolete Libraries/inc/NMEA.h that conflicts with Modules/GPS/inc/NMEA.h --- flight/Libraries/inc/NMEA.h | 42 ------------------------------------- 1 file changed, 42 deletions(-) delete mode 100644 flight/Libraries/inc/NMEA.h diff --git a/flight/Libraries/inc/NMEA.h b/flight/Libraries/inc/NMEA.h deleted file mode 100644 index c3f8b66d9..000000000 --- a/flight/Libraries/inc/NMEA.h +++ /dev/null @@ -1,42 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup GSPModule GPS Module - * @brief Process GPS information - * @{ - * - * @file NMEA.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 NMEA_H -#define NMEA_H - -#include -#include - -#define NMEA_MAX_PACKET_LENGTH 96 - -extern bool NMEA_update_position(char *nmea_sentence); -extern bool NMEA_checksum(char *nmea_sentence); - -#endif /* NMEA_H */ From d0b7dc001230487ff18e07f89195d4450c7e538d Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 10 Jul 2012 16:38:00 +0200 Subject: [PATCH 13/32] Added some pieces that had been lost during rebase --- flight/Modules/GPS/GPS.c | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 15bfa4457..8c320d5b7 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -35,8 +35,6 @@ //#include -#include "NMEA.h" - #include "gpsposition.h" #include "homelocation.h" #include "gpstime.h" @@ -47,6 +45,9 @@ #include "CoordinateConversions.h" #include "hwsettings.h" +#include "NMEA.h" +#include "UBX.h" + // **************** // Private functions @@ -140,6 +141,17 @@ int32_t GPSInitialize(void) gpsEnabled = false; #endif +#if defined(REVOLUTION) + // Revolution expects these objects to always be defined. Not doing so will fail some + // queue connections in navigation + GPSPositionInitialize(); + GPSVelocityInitialize(); + GPSTimeInitialize(); + GPSSatellitesInitialize(); + HomeLocationInitialize(); + updateSettings(); + +#else if (gpsPort && gpsEnabled) { GPSPositionInitialize(); GPSVelocityInitialize(); @@ -151,6 +163,8 @@ int32_t GPSInitialize(void) HomeLocationInitialize(); #endif updateSettings(); + } +#endif if (gpsPort && gpsEnabled) { GPSSettingsInitialize(); @@ -295,14 +309,6 @@ static void setHomeLocation(GPSPositionData * gpsData) // Compute home ECEF coordinates and the rotation matrix into NED double LLA[3] = { ((double)home.Latitude) / 10e6, ((double)home.Longitude) / 10e6, ((double)home.Altitude) }; - double ECEF[3]; - RneFromLLA(LLA, (float (*)[3])home.RNE); - LLA2ECEF(LLA, ECEF); - // TODO: Currently UAVTalk only supports float but these conversions use double - // need to find out if they require that precision and if so extend UAVTAlk - home.ECEF[0] = (int32_t) (ECEF[0] * 100); - home.ECEF[1] = (int32_t) (ECEF[1] * 100); - home.ECEF[2] = (int32_t) (ECEF[2] * 100); // Compute magnetic flux direction at home location if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) >= 0) From feb063dda709f40466ce9a50ec35cf62e199c17f Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 10 Jul 2012 16:40:58 +0200 Subject: [PATCH 14/32] Removed ECEF and RNE flields to be compatible with Revo --- shared/uavobjectdefinition/homelocation.xml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/shared/uavobjectdefinition/homelocation.xml b/shared/uavobjectdefinition/homelocation.xml index 893af7e30..a0503517f 100644 --- a/shared/uavobjectdefinition/homelocation.xml +++ b/shared/uavobjectdefinition/homelocation.xml @@ -5,10 +5,8 @@ - - - + From a77a367df2997c6fcec8d7e9ed9a818725f4e187 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 10 Jul 2012 17:35:35 +0200 Subject: [PATCH 15/32] Pass GPSPositionData as a function argument instead of using a global variable. --- flight/Modules/GPS/NMEA.c | 34 ++++++++++++++++++++-------------- 1 file changed, 20 insertions(+), 14 deletions(-) diff --git a/flight/Modules/GPS/NMEA.c b/flight/Modules/GPS/NMEA.c index 3faf29337..0bb8ad7f7 100644 --- a/flight/Modules/GPS/NMEA.c +++ b/flight/Modules/GPS/NMEA.c @@ -35,7 +35,6 @@ #include "gpsposition.h" #include "NMEA.h" -#include "gpsposition.h" #include "gpstime.h" #include "gpssatellites.h" #include "GPS.h" @@ -324,6 +323,10 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool PIOS_DEBUG_Assert(nmea_latlon); PIOS_DEBUG_Assert(latlon); + if (*nmea_latlon == '\0') { /* empty lat/lon field */ + return false; + } + if (!NMEA_parse_real(&num_DDDMM, &num_m, &units, nmea_latlon)) { return false; } @@ -375,7 +378,7 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool * \return true if the sentence was successfully parsed * \return false if any errors were encountered with the parsing */ -bool NMEA_update_position(char *nmea_sentence) +bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) { char* p = nmea_sentence; char* params[MAX_NB_PARAMS]; @@ -429,12 +432,10 @@ bool NMEA_update_position(char *nmea_sentence) #ifdef DEBUG_MGSID_IN DEBUG_MSG("%s %d ", params[0], parser->cnt); #endif - // Send the message to then parser and get it update the GpsData - GPSPositionData GpsData; - GPSPositionGet(&GpsData); - bool gpsDataUpdated; + // Send the message to the parser and get it update the GpsData + bool gpsDataUpdated = false; - if (!parser->handler(&GpsData, &gpsDataUpdated, params, nbParams)) { + if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) { // Parse failed DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]); return false; @@ -446,7 +447,7 @@ bool NMEA_update_position(char *nmea_sentence) #ifdef DEBUG_MGSID_IN DEBUG_MSG("U"); #endif - GPSPositionSet(&GpsData); + GPSPositionSet(GpsData); } #ifdef DEBUG_MGSID_IN @@ -527,7 +528,7 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch DEBUG_MSG(" DateOfFix=%s\n\n", param[9]); #endif - *gpsDataUpdated = true; + *gpsDataUpdated = false; #if !defined(PIOS_GPS_MINIMAL) GPSTimeData gpst; @@ -540,6 +541,11 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch gpst.Hour = (int)hms / 10000; #endif //PIOS_GPS_MINIMAL + // don't process void sentences + if (param[2][0] == 'V') { + return false; + } + // get latitude [DDMM.mmmmm] [N|S] if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[3], param[4][0] == 'S')) { return false; @@ -551,7 +557,7 @@ static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, ch } // get speed in knots - GpsData->Groundspeed = NMEA_real_to_float(param[7]) * 0.51444; // to m/s + GpsData->Groundspeed = NMEA_real_to_float(param[7]) * 0.51444f; // to m/s // get True course GpsData->Heading = NMEA_real_to_float(param[8]); @@ -585,10 +591,10 @@ static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, ch DEBUG_MSG(" GroundSpeed=%s %s\n", param[5], param[6]); #endif - *gpsDataUpdated = true; + *gpsDataUpdated = false; GpsData->Heading = NMEA_real_to_float(param[1]); - GpsData->Groundspeed = NMEA_real_to_float(param[5]) * 0.51444; // to m/s + GpsData->Groundspeed = NMEA_real_to_float(param[5]) * 0.51444f; // to m/s return true; } @@ -651,7 +657,7 @@ static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, ch uint8_t nbSentences = atoi(param[1]); uint8_t currSentence = atoi(param[2]); - *gpsDataUpdated = true; + *gpsDataUpdated = false; if (nbSentences < 1 || nbSentences > 8 || currSentence < 1 || currSentence > nbSentences) return false; @@ -735,7 +741,7 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch DEBUG_MSG(" VDOP=%s\n", param[17]); #endif - *gpsDataUpdated = true; + *gpsDataUpdated = false; switch (atoi(param[2])) { case 1: From 0a7fa8dc5f58050b5171a3317dceab9a7d09b7ba Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Tue, 10 Jul 2012 23:06:42 +0200 Subject: [PATCH 16/32] Revert "Removed ECEF and RNE flields to be compatible with Revo" This reverts commit feb063dda709f40466ce9a50ec35cf62e199c17f. --- shared/uavobjectdefinition/homelocation.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/homelocation.xml b/shared/uavobjectdefinition/homelocation.xml index a0503517f..893af7e30 100644 --- a/shared/uavobjectdefinition/homelocation.xml +++ b/shared/uavobjectdefinition/homelocation.xml @@ -5,8 +5,10 @@ + + - + From 19c0f6a88a6b474a76618f07f8d00e9063f3313f Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Wed, 11 Jul 2012 00:40:15 +0200 Subject: [PATCH 17/32] HomeLocation UAVOject on CC still has ECEF so make this part confitional. --- flight/Modules/GPS/GPS.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 8c320d5b7..69cc278ef 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -309,7 +309,17 @@ static void setHomeLocation(GPSPositionData * gpsData) // Compute home ECEF coordinates and the rotation matrix into NED double LLA[3] = { ((double)home.Latitude) / 10e6, ((double)home.Longitude) / 10e6, ((double)home.Altitude) }; +#if !defined(REVOLUTION) + double ECEF[3]; + RneFromLLA(LLA, (float (*)[3])home.RNE); + LLA2ECEF(LLA, ECEF); + // TODO: Currently UAVTalk only supports float but these conversions use double + // need to find out if they require that precision and if so extend UAVTAlk + home.ECEF[0] = (int32_t) (ECEF[0] * 100); + home.ECEF[1] = (int32_t) (ECEF[1] * 100); + home.ECEF[2] = (int32_t) (ECEF[2] * 100); +#endif // !defined (Revolution) // Compute magnetic flux direction at home location if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) >= 0) { // calculations appeared to go OK From 7ef7b621d77de161a900c7bf1fe878a212374c7b Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Wed, 11 Jul 2012 23:37:03 +0200 Subject: [PATCH 18/32] Reenable PIOS_GPS_SETS_HOMELOCATION on Revo. Change-Id: I5a5e2ec0deb4241faad23f6fead5036b039bfbf9 --- flight/Revolution/System/inc/pios_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Revolution/System/inc/pios_config.h b/flight/Revolution/System/inc/pios_config.h index cc7c67747..5b249bfe5 100644 --- a/flight/Revolution/System/inc/pios_config.h +++ b/flight/Revolution/System/inc/pios_config.h @@ -95,7 +95,7 @@ #define PIOS_INCLUDE_INITCALL /* Include init call structures */ #define PIOS_TELEM_PRIORITY_QUEUE /* Enable a priority queue in telemetry */ #define PIOS_QUATERNION_STABILIZATION /* Stabilization options */ -//#define PIOS_GPS_SETS_HOMELOCATION /* GPS options */ +#define PIOS_GPS_SETS_HOMELOCATION /* GPS options */ #define PIOS_INCLUDE_GPS_NMEA_PARSER /* Include the NMEA protocol parser */ #define PIOS_INCLUDE_GPS_UBX_PARSER /* Include the UBX protocol parser */ From 57dc9e6d3963553287b0c646b33c4b66bcd09b06 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Wed, 11 Jul 2012 23:53:34 +0200 Subject: [PATCH 19/32] Removed ECEF calculation from SetHomeLocation. --- flight/Modules/GPS/GPS.c | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 69cc278ef..8c320d5b7 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -309,17 +309,7 @@ static void setHomeLocation(GPSPositionData * gpsData) // Compute home ECEF coordinates and the rotation matrix into NED double LLA[3] = { ((double)home.Latitude) / 10e6, ((double)home.Longitude) / 10e6, ((double)home.Altitude) }; -#if !defined(REVOLUTION) - double ECEF[3]; - RneFromLLA(LLA, (float (*)[3])home.RNE); - LLA2ECEF(LLA, ECEF); - // TODO: Currently UAVTalk only supports float but these conversions use double - // need to find out if they require that precision and if so extend UAVTAlk - home.ECEF[0] = (int32_t) (ECEF[0] * 100); - home.ECEF[1] = (int32_t) (ECEF[1] * 100); - home.ECEF[2] = (int32_t) (ECEF[2] * 100); -#endif // !defined (Revolution) // Compute magnetic flux direction at home location if (WMM_GetMagVector(LLA[0], LLA[1], LLA[2], gps.Month, gps.Day, gps.Year, &home.Be[0]) >= 0) { // calculations appeared to go OK From e6f62f5806cb2f4f824f1d1e34b867afc6693d00 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Fri, 13 Jul 2012 13:22:27 +0200 Subject: [PATCH 20/32] Deleted commented-out code --- flight/Modules/GPS/GPS.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 8c320d5b7..3f22b2af8 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -33,8 +33,6 @@ #include "openpilot.h" #include "GPS.h" -//#include - #include "gpsposition.h" #include "homelocation.h" #include "gpstime.h" From 6ae9db8cb7f72e3c4bdc2633855fad0cc2d9bb47 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Fri, 13 Jul 2012 14:40:11 +0200 Subject: [PATCH 21/32] Removed typedefs for structures to be consistent with style guide. --- flight/Modules/GPS/inc/UBX.h | 62 ++++++++++++++++++------------------ 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/flight/Modules/GPS/inc/UBX.h b/flight/Modules/GPS/inc/UBX.h index 4df3641be..4eb823910 100644 --- a/flight/Modules/GPS/inc/UBX.h +++ b/flight/Modules/GPS/inc/UBX.h @@ -53,7 +53,7 @@ // private structures // Geodetic Position Solution -typedef struct { +struct UBX_NAV_POSLLH { uint32_t iTOW; // GPS Millisecond Time of Week (ms) int32_t lon; // Longitude (deg*1e-7) int32_t lat; // Latitude (deg*1e-7) @@ -61,7 +61,7 @@ typedef struct { int32_t hMSL; // Height abobe mean sea level (mm) uint32_t hAcc; // Horizontal Accuracy Estimate (mm) uint32_t vAcc; // Vertical Accuracy Estimate (mm) -} UBX_NAV_POSLLH; +}; // Receiver Navigation Status @@ -77,7 +77,7 @@ typedef struct { #define STATUS_FLAGS_WKNSET (1 << 2) #define STATUS_FLAGS_TOWSET (1 << 3) -typedef struct { +struct UBX_NAV_STATUS { uint32_t iTOW; // GPS Millisecond Time of Week (ms) uint8_t gpsFix; // GPS fix type uint8_t flags; // Navigation Status Flags @@ -85,10 +85,10 @@ typedef struct { uint8_t flags2; // Additional navigation output information uint32_t ttff; // Time to first fix (ms) uint32_t msss; // Milliseconds since startup/reset (ms) -} UBX_NAV_STATUS; +}; // Dilution of precision -typedef struct { +struct UBX_NAV_DOP { uint32_t iTOW; // GPS Millisecond Time of Week (ms) uint16_t gDOP; // Geometric DOP uint16_t pDOP; // Position DOP @@ -97,11 +97,11 @@ typedef struct { uint16_t hDOP; // Horizontal DOP uint16_t nDOP; // Northing DOP uint16_t eDOP; // Easting DOP -} UBX_NAV_DOP; +}; // Navigation solution -typedef struct { +struct UBX_NAV_SOL { uint32_t iTOW; // GPS Millisecond Time of Week (ms) int32_t fTOW; // fracional nanoseconds (ns) int16_t week; // GPS week @@ -119,11 +119,11 @@ typedef struct { uint8_t reserved1; // Reserved uint8_t numSV; // Number of SVs used in Nav Solution uint32_t reserved2; // Reserved -} UBX_NAV_SOL; +}; // North/East/Down velocity -typedef struct { +struct UBX_NAV_VELNED { 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 @@ -133,7 +133,7 @@ typedef struct { 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; +}; // UTC Time Solution @@ -141,7 +141,7 @@ typedef struct { #define TIMEUTC_VALIDWKN (1 << 1) #define TIMEUTC_VALIDUTC (1 << 2) -typedef struct { +struct UBX_NAV_TIMEUTC { uint32_t iTOW; // GPS Millisecond Time of Week (ms) uint32_t tAcc; // Time Accuracy Estimate (ns) int32_t nano; // Nanoseconds of second @@ -152,7 +152,7 @@ typedef struct { uint8_t min; uint8_t sec; uint8_t valid; // Validity Flags -} UBX_NAV_TIMEUTC; +}; // Space Vehicle (SV) Information @@ -167,7 +167,7 @@ typedef struct { #define ORBITAOP (1 << 6) // Orbit information is AssistNow Autonomous #define SMOOTHED (1 << 7) // Carrier smoothed pseudoranges used -typedef struct { +struct UBX_NAV_SVINFO_SV { uint8_t chn; // Channel number uint8_t svid; // Satellite ID uint8_t flags; // Misc SV information @@ -176,47 +176,47 @@ typedef struct { int8_t elev; // Elevation (integer degrees) int16_t azim; // Azimuth (integer degrees) int32_t prRes; // Pseudo range residual (cm) -} UBX_NAV_SVINFO_SV; +}; // SV information message #define MAX_SVS 16 -typedef struct { +struct UBX_NAV_SVINFO { uint32_t iTOW; // GPS Millisecond Time of Week (ms) uint8_t numCh; // Number of channels uint8_t globalFlags; // uint16_t reserved2; // Reserved - UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times -} UBX_NAV_SVINFO; + struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times +}; typedef union { uint8_t payload[0]; - UBX_NAV_POSLLH nav_posllh; - UBX_NAV_STATUS nav_status; - UBX_NAV_DOP nav_dop; - UBX_NAV_SOL nav_sol; - UBX_NAV_VELNED nav_velned; + struct UBX_NAV_POSLLH nav_posllh; + struct UBX_NAV_STATUS nav_status; + struct UBX_NAV_DOP nav_dop; + struct UBX_NAV_SOL nav_sol; + struct UBX_NAV_VELNED nav_velned; #if !defined(PIOS_GPS_MINIMAL) - UBX_NAV_TIMEUTC nav_timeutc; - UBX_NAV_SVINFO nav_svinfo; + struct UBX_NAV_TIMEUTC nav_timeutc; + struct UBX_NAV_SVINFO nav_svinfo; #endif } UBXPayload; -typedef struct { +struct UBXHeader { uint8_t class; uint8_t id; uint16_t len; uint8_t ck_a; uint8_t ck_b; -} UBXHeader; +}; -typedef struct { - UBXHeader header; +struct UBXPacket { + struct UBXHeader header; UBXPayload payload; -} UBXPacket; +}; -bool checksum_ubx_message(UBXPacket *); -uint32_t parse_ubx_message(UBXPacket *, GPSPositionData *); +bool checksum_ubx_message(struct UBXPacket *); +uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionData *); int parse_ubx_stream(uint8_t, char *, GPSPositionData *); #endif /* UBX_H */ From 7207d2bb989b5c2dd47066cbf2182cd2a668ad74 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Fri, 13 Jul 2012 14:43:26 +0200 Subject: [PATCH 22/32] Fixed typos in comments. --- flight/Modules/GPS/inc/UBX.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/Modules/GPS/inc/UBX.h b/flight/Modules/GPS/inc/UBX.h index 4eb823910..449cb3ca4 100644 --- a/flight/Modules/GPS/inc/UBX.h +++ b/flight/Modules/GPS/inc/UBX.h @@ -57,8 +57,8 @@ struct UBX_NAV_POSLLH { uint32_t iTOW; // GPS Millisecond Time of Week (ms) int32_t lon; // Longitude (deg*1e-7) int32_t lat; // Latitude (deg*1e-7) - int32_t height; // Height above Ellipoid (mm) - int32_t hMSL; // Height abobe mean sea level (mm) + 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) }; @@ -103,7 +103,7 @@ struct UBX_NAV_DOP { struct UBX_NAV_SOL { uint32_t iTOW; // GPS Millisecond Time of Week (ms) - int32_t fTOW; // fracional nanoseconds (ns) + int32_t fTOW; // fractional nanoseconds (ns) int16_t week; // GPS week uint8_t gpsFix; // GPS fix type uint8_t flags; // Fix status flags From bc7defe21fff2b99fa861ad6ded8216b6c062e99 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Fri, 13 Jul 2012 14:48:11 +0200 Subject: [PATCH 23/32] Changed usage of UBX structures to follow changed structure declarations. (see commit e6f62f) --- flight/Modules/GPS/GPS.c | 2 +- flight/Modules/GPS/UBX.c | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 3f22b2af8..c5c341ed6 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -172,7 +172,7 @@ int32_t GPSInitialize(void) gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); break; case GPSSETTINGS_DATAPROTOCOL_UBX: - gps_rx_buffer = pvPortMalloc(sizeof(UBXPacket)); + gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket)); break; default: gps_rx_buffer = NULL; diff --git a/flight/Modules/GPS/UBX.c b/flight/Modules/GPS/UBX.c index bb45848a6..4329bbc69 100644 --- a/flight/Modules/GPS/UBX.c +++ b/flight/Modules/GPS/UBX.c @@ -48,7 +48,7 @@ int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData) UBX_LEN2,UBX_PAYLOAD,UBX_CHK1,UBX_CHK2,FINISHED}; static enum proto_states proto_state = START; static uint8_t rx_count = 0; - UBXPacket *ubx = (UBXPacket *)gps_rx_buffer; + struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer; switch (proto_state) { case START: // detect protocol @@ -153,7 +153,7 @@ bool check_msgtracker (uint32_t tow, uint8_t msg_flag) return true; } -bool checksum_ubx_message (UBXPacket *ubx) +bool checksum_ubx_message (struct UBXPacket *ubx) { int i; uint8_t ck_a, ck_b; @@ -183,7 +183,7 @@ bool checksum_ubx_message (UBXPacket *ubx) } -void parse_ubx_nav_posllh (UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition) +void parse_ubx_nav_posllh (struct UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition) { if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) { if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) { @@ -195,7 +195,7 @@ void parse_ubx_nav_posllh (UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition) } } -void parse_ubx_nav_status (UBX_NAV_STATUS *status, GPSPositionData *GpsPosition) +void parse_ubx_nav_status (struct UBX_NAV_STATUS *status, GPSPositionData *GpsPosition) { #if 0 // we already get this info from NAV_SOL if (check_msgtracker(status->iTOW, STATUS_RECEIVED)) { @@ -212,7 +212,7 @@ void parse_ubx_nav_status (UBX_NAV_STATUS *status, GPSPositionData *GpsPosition) #endif } -void parse_ubx_nav_sol (UBX_NAV_SOL *sol, GPSPositionData *GpsPosition) +void parse_ubx_nav_sol (struct UBX_NAV_SOL *sol, GPSPositionData *GpsPosition) { if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) { GpsPosition->Satellites = sol->numSV; @@ -233,7 +233,7 @@ void parse_ubx_nav_sol (UBX_NAV_SOL *sol, GPSPositionData *GpsPosition) } } -void parse_ubx_nav_dop (UBX_NAV_DOP *dop, GPSPositionData *GpsPosition) +void parse_ubx_nav_dop (struct UBX_NAV_DOP *dop, GPSPositionData *GpsPosition) { if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) { GpsPosition->HDOP = (float)dop->hDOP * 0.01f; @@ -242,7 +242,7 @@ void parse_ubx_nav_dop (UBX_NAV_DOP *dop, GPSPositionData *GpsPosition) } } -void parse_ubx_nav_velned (UBX_NAV_VELNED *velned, GPSPositionData *GpsPosition) +void parse_ubx_nav_velned (struct UBX_NAV_VELNED *velned, GPSPositionData *GpsPosition) { GPSVelocityData GpsVelocity; @@ -309,7 +309,7 @@ void parse_ubx_nav_svinfo (UBX_NAV_SVINFO *svinfo) // UBX message parser // returns UAVObjectID if a UAVObject structure is ready for further processing -uint32_t parse_ubx_message (UBXPacket *ubx, GPSPositionData *GpsPosition) +uint32_t parse_ubx_message (struct UBXPacket *ubx, GPSPositionData *GpsPosition) { uint32_t id = 0; From a22a11d9732758f62e38a6c822c30b61af000672 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 19 Jul 2012 13:05:55 +0200 Subject: [PATCH 24/32] Cleanups: - removed unused nav_status parser function - enum coding style change --- flight/Modules/GPS/UBX.c | 35 +++++++++++++---------------------- 1 file changed, 13 insertions(+), 22 deletions(-) diff --git a/flight/Modules/GPS/UBX.c b/flight/Modules/GPS/UBX.c index 4329bbc69..8f947b70a 100644 --- a/flight/Modules/GPS/UBX.c +++ b/flight/Modules/GPS/UBX.c @@ -44,8 +44,19 @@ static uint32_t gpsRxOverflow = 0; int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData) { - enum proto_states {START,UBX_SY2,UBX_CLASS,UBX_ID,UBX_LEN1, - UBX_LEN2,UBX_PAYLOAD,UBX_CHK1,UBX_CHK2,FINISHED}; + enum proto_states { + START, + UBX_SY2, + UBX_CLASS, + UBX_ID, + UBX_LEN1, + UBX_LEN2, + UBX_PAYLOAD, + UBX_CHK1, + UBX_CHK2, + FINISHED + }; + static enum proto_states proto_state = START; static uint8_t rx_count = 0; struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer; @@ -195,23 +206,6 @@ void parse_ubx_nav_posllh (struct UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPo } } -void parse_ubx_nav_status (struct UBX_NAV_STATUS *status, GPSPositionData *GpsPosition) -{ -#if 0 // we already get this info from NAV_SOL - if (check_msgtracker(status->iTOW, STATUS_RECEIVED)) { - switch (status->gpsFix) { - case STATUS_GPSFIX_2DFIX: - GpsPosition->Status = GPSPOSITION_STATUS_FIX2D; - break; - case STATUS_GPSFIX_3DFIX: - GpsPosition->Status = GPSPOSITION_STATUS_FIX3D; - break; - default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX; - } - } -#endif -} - void parse_ubx_nav_sol (struct UBX_NAV_SOL *sol, GPSPositionData *GpsPosition) { if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) { @@ -319,9 +313,6 @@ uint32_t parse_ubx_message (struct UBXPacket *ubx, GPSPositionData *GpsPosition) case UBX_ID_POSLLH: parse_ubx_nav_posllh (&ubx->payload.nav_posllh, GpsPosition); break; - case UBX_ID_STATUS: - parse_ubx_nav_status (&ubx->payload.nav_status, GpsPosition); - break; case UBX_ID_DOP: parse_ubx_nav_dop (&ubx->payload.nav_dop, GpsPosition); break; From 926e3d93b77311bef46f20413abf4d1d7c9adfa2 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 19 Jul 2012 20:14:51 +0200 Subject: [PATCH 25/32] Removed unused variables --- flight/Modules/GPS/GPS.c | 7 ------- 1 file changed, 7 deletions(-) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index c5c341ed6..73f8e34e5 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -89,9 +89,6 @@ static char* gps_rx_buffer; static uint32_t timeOfLastCommandMs; static uint32_t timeOfLastUpdateMs; -static uint32_t numUpdates; -static uint32_t numChecksumErrors; -static uint32_t numParsingErrors; // **************** /** @@ -202,10 +199,6 @@ static void gpsTask(void *parameters) GPSSettingsDataProtocolGet(&gpsProtocol); - numUpdates = 0; - numChecksumErrors = 0; - numParsingErrors = 0; - timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; From 307626d1de4e7993e7cab53adaa4d5d41aaef655 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 19 Jul 2012 20:46:44 +0200 Subject: [PATCH 26/32] Fixed typo --- flight/Modules/GPS/NMEA.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Modules/GPS/NMEA.c b/flight/Modules/GPS/NMEA.c index 0bb8ad7f7..5948b8cf3 100644 --- a/flight/Modules/GPS/NMEA.c +++ b/flight/Modules/GPS/NMEA.c @@ -47,7 +47,7 @@ // Debugging #ifdef ENABLE_DEBUG_MSG //#define DEBUG_MSG_IN ///< define to display the incoming NMEA messages -//#define DEBUG_PARS ///< define to display the incoming NMEA messages split into its parameters +//#define DEBUG_PARMS ///< define to display the incoming NMEA messages split into its parameters //#define DEBUG_MGSID_IN ///< define to display the the names of the incoming NMEA messages //#define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages //#define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages From b6ebee403dbab11e7b154a9f1d4b3a2d3c6d446b Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 19 Jul 2012 20:48:35 +0200 Subject: [PATCH 27/32] Removed reference to AHRSCommsModule --- shared/uavobjectdefinition/gpsvelocity.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/gpsvelocity.xml b/shared/uavobjectdefinition/gpsvelocity.xml index 7d57ccacc..8673463a8 100644 --- a/shared/uavobjectdefinition/gpsvelocity.xml +++ b/shared/uavobjectdefinition/gpsvelocity.xml @@ -1,6 +1,6 @@ - Raw GPS data from @ref GPSModule. Should only be used by @ref AHRSCommsModule. + Raw GPS data from @ref GPSModule. From 15fa42058eb08bc18aab8afe1fa9223358470f06 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 19 Jul 2012 22:00:24 +0200 Subject: [PATCH 28/32] Unified statistics counters between UBX and NMEA parser and moved them to GPS.c. --- flight/Modules/GPS/GPS.c | 6 ++++-- flight/Modules/GPS/NMEA.c | 14 +++++--------- flight/Modules/GPS/UBX.c | 14 +++++--------- flight/Modules/GPS/inc/GPS.h | 7 +++++++ flight/Modules/GPS/inc/NMEA.h | 3 ++- flight/Modules/GPS/inc/UBX.h | 4 +++- 6 files changed, 26 insertions(+), 22 deletions(-) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 73f8e34e5..93ea12262 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -90,6 +90,8 @@ static char* gps_rx_buffer; static uint32_t timeOfLastCommandMs; static uint32_t timeOfLastUpdateMs; +static struct GPS_RX_STATS gpsRxStats; + // **************** /** * Initialise the gps module @@ -215,12 +217,12 @@ static void gpsTask(void *parameters) switch (gpsProtocol) { #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) case GPSSETTINGS_DATAPROTOCOL_NMEA: - res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition); + res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats); break; #endif #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) case GPSSETTINGS_DATAPROTOCOL_UBX: - res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition); + res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats); break; #endif default: diff --git a/flight/Modules/GPS/NMEA.c b/flight/Modules/GPS/NMEA.c index 5948b8cf3..5cffc8483 100644 --- a/flight/Modules/GPS/NMEA.c +++ b/flight/Modules/GPS/NMEA.c @@ -79,10 +79,6 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); #endif //PIOS_GPS_MINIMAL -static uint32_t numUpdates; -static uint32_t numChecksumErrors; -static uint32_t numParsingErrors; - static struct nmea_parser nmea_parsers[] = { { .prefix = "GPGGA", @@ -118,7 +114,7 @@ static struct nmea_parser nmea_parsers[] = { #endif //PIOS_GPS_MINIMAL }; -int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData) +int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) { static uint8_t rx_count = 0; static bool start_flag = false; @@ -139,7 +135,7 @@ int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData) { // The buffer is already full and we haven't found a valid NMEA sentence. // Flush the buffer and note the overflow event. -// gpsRxOverflow++; + gpsRxStats->gpsRxOverflow++; start_flag = false; found_cr = false; rx_count = 0; @@ -180,7 +176,7 @@ int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData) if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum. May indicate dropped characters on Rx. //PIOS_DEBUG_PinHigh(2); - ++numChecksumErrors; + gpsRxStats->gpsRxChkSumError++; //PIOS_DEBUG_PinLow(2); return PARSER_ERROR; } @@ -188,11 +184,11 @@ int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData) { // Valid checksum, use this packet to update the GPS position if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) { //PIOS_DEBUG_PinHigh(2); - ++numParsingErrors; + gpsRxStats->gpsRxParserError++; //PIOS_DEBUG_PinLow(2); } else - ++numUpdates; + gpsRxStats->gpsRxReceived++;; return PARSER_COMPLETE; } diff --git a/flight/Modules/GPS/UBX.c b/flight/Modules/GPS/UBX.c index 8f947b70a..1bf9a0979 100644 --- a/flight/Modules/GPS/UBX.c +++ b/flight/Modules/GPS/UBX.c @@ -36,13 +36,9 @@ #include "UBX.h" #include "gps.h" -static uint32_t gpsRxReceived = 0; -static uint32_t gpsRxChkSumError = 0; -static uint32_t gpsRxOverflow = 0; - // parse incoming character stream for messages in UBX binary format -int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData) +int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats) { enum proto_states { START, @@ -87,7 +83,7 @@ int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData) case UBX_LEN2: ubx->header.len += (c << 8); if (ubx->header.len > sizeof(UBXPayload)) { - gpsRxOverflow++; + gpsRxStats->gpsRxOverflow++; proto_state = START; } else { rx_count = 0; @@ -100,7 +96,7 @@ int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData) if (++rx_count == ubx->header.len) proto_state = UBX_CHK1; } else { - gpsRxOverflow++; + gpsRxStats->gpsRxOverflow++; proto_state = START; } break; @@ -114,7 +110,7 @@ int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData) parse_ubx_message(ubx, GpsData); proto_state = FINISHED; } else { - gpsRxChkSumError++; + gpsRxStats->gpsRxChkSumError++; proto_state = START; } break; @@ -124,7 +120,7 @@ int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData) if (proto_state == START) return PARSER_ERROR; // parser couldn't use this byte else if (proto_state == FINISHED) { - gpsRxReceived++; + gpsRxStats->gpsRxReceived++; proto_state = START; return PARSER_COMPLETE; // message complete & processed } diff --git a/flight/Modules/GPS/inc/GPS.h b/flight/Modules/GPS/inc/GPS.h index fad5e8eed..6e243c21f 100644 --- a/flight/Modules/GPS/inc/GPS.h +++ b/flight/Modules/GPS/inc/GPS.h @@ -45,6 +45,13 @@ #define PARSER_INCOMPLETE 0 // parser needs more data to complete the message #define PARSER_COMPLETE 1 // parser has received a complete message and finished processing +struct GPS_RX_STATS { + uint16_t gpsRxReceived; + uint16_t gpsRxChkSumError; + uint16_t gpsRxOverflow; + uint16_t gpsRxParserError; +}; + int32_t GPSInitialize(void); #endif // GPS_H diff --git a/flight/Modules/GPS/inc/NMEA.h b/flight/Modules/GPS/inc/NMEA.h index eb4567560..b6c88bd27 100644 --- a/flight/Modules/GPS/inc/NMEA.h +++ b/flight/Modules/GPS/inc/NMEA.h @@ -33,11 +33,12 @@ #include #include +#include "gps.h" #define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed extern bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData); extern bool NMEA_checksum(char *nmea_sentence); -extern int parse_nmea_stream(uint8_t, char *, GPSPositionData *); +extern int parse_nmea_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *); #endif /* NMEA_H */ diff --git a/flight/Modules/GPS/inc/UBX.h b/flight/Modules/GPS/inc/UBX.h index 449cb3ca4..6296a335a 100644 --- a/flight/Modules/GPS/inc/UBX.h +++ b/flight/Modules/GPS/inc/UBX.h @@ -32,6 +32,8 @@ #define UBX_H #include "openpilot.h" #include "gpsposition.h" +#include "gps.h" + #define UBX_SYNC1 0xb5 // UBX protocol synchronization characters #define UBX_SYNC2 0x62 @@ -217,6 +219,6 @@ struct UBXPacket { bool checksum_ubx_message(struct UBXPacket *); uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionData *); -int parse_ubx_stream(uint8_t, char *, GPSPositionData *); +int parse_ubx_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *); #endif /* UBX_H */ From d3962f330327094fc43e5aaf88c3e8f6a510914e Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 19 Jul 2012 22:35:19 +0200 Subject: [PATCH 29/32] Removed message counter from nmea_parsers structure and made it const to save some RAM. --- flight/Modules/GPS/NMEA.c | 18 +++++------------- 1 file changed, 5 insertions(+), 13 deletions(-) diff --git a/flight/Modules/GPS/NMEA.c b/flight/Modules/GPS/NMEA.c index 5cffc8483..f4f394438 100644 --- a/flight/Modules/GPS/NMEA.c +++ b/flight/Modules/GPS/NMEA.c @@ -67,7 +67,6 @@ struct nmea_parser { const char *prefix; bool(*handler) (GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - uint32_t cnt; }; static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); @@ -79,37 +78,31 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); #endif //PIOS_GPS_MINIMAL -static struct nmea_parser nmea_parsers[] = { +const static struct nmea_parser nmea_parsers[] = { { .prefix = "GPGGA", .handler = nmeaProcessGPGGA, - .cnt = 0, }, { .prefix = "GPVTG", .handler = nmeaProcessGPVTG, - .cnt = 0, }, { .prefix = "GPGSA", .handler = nmeaProcessGPGSA, - .cnt = 0, }, { .prefix = "GPRMC", .handler = nmeaProcessGPRMC, - .cnt = 0, }, #if !defined(PIOS_GPS_MINIMAL) { .prefix = "GPZDA", .handler = nmeaProcessGPZDA, - .cnt = 0, }, { .prefix = "GPGSV", .handler = nmeaProcessGPGSV, - .cnt = 0, }, #endif //PIOS_GPS_MINIMAL }; @@ -196,14 +189,14 @@ int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, return PARSER_INCOMPLETE; } -static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix) +const static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix) { if (!prefix) { return (NULL); } for (uint8_t i = 0; i < NELEMENTS(nmea_parsers); i++) { - struct nmea_parser *parser = &nmea_parsers[i]; + const struct nmea_parser *parser = &nmea_parsers[i]; /* Use strcmp to check for exact equality over the entire prefix */ if (!strcmp(prefix, parser->prefix)) { @@ -416,7 +409,7 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) #endif // The first parameter is the message name, lets see if we find a parser for it - struct nmea_parser *parser; + const struct nmea_parser *parser; parser = NMEA_find_parser_by_prefix(params[0]); if (!parser) { // No parser found @@ -424,9 +417,8 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) return false; } - parser->cnt++; #ifdef DEBUG_MGSID_IN - DEBUG_MSG("%s %d ", params[0], parser->cnt); + DEBUG_MSG("%s %d ", params[0]); #endif // Send the message to the parser and get it update the GpsData bool gpsDataUpdated = false; From 6db2c949a3cdb3a71ecd68537cb616d009a52ed5 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Fri, 20 Jul 2012 00:23:05 +0200 Subject: [PATCH 30/32] Report GPS NO_FIX status only once per message set and not with every single message. --- flight/Modules/GPS/NMEA.c | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/flight/Modules/GPS/NMEA.c b/flight/Modules/GPS/NMEA.c index f4f394438..6a25b1fed 100644 --- a/flight/Modules/GPS/NMEA.c +++ b/flight/Modules/GPS/NMEA.c @@ -426,12 +426,15 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) { // Parse failed DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]); + if (gpsDataUpdated && (GpsData->Status == GPSPOSITION_STATUS_NOFIX)) { + GPSPositionSet(GpsData); + } return false; } // All is fine :) Update object if data has changed - if (gpsDataUpdated || (GpsData->Status == GPSPOSITION_STATUS_NOFIX)) { + if (gpsDataUpdated) { #ifdef DEBUG_MGSID_IN DEBUG_MSG("U"); #endif @@ -469,6 +472,13 @@ static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch *gpsDataUpdated = true; + // check for invalid GPS fix + // do this first to make sure we get this information, even if later checks exit + // this function early + if (param[6][0] == '0') { + GpsData->Status = GPSPOSITION_STATUS_NOFIX; // treat invalid fix as NOFIX + } + // get latitude [DDMM.mmmmm] [N|S] if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[2], param[3][0] == 'S')) { return false; @@ -479,12 +489,6 @@ static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch return false; } - - // check for invalid GPS fix - if (param[6][0] == '0') { - // return false; - GpsData->Status = GPSPOSITION_STATUS_NOFIX; // treat invalid fix as NOFIX - } // get number of satellites used in GPS solution GpsData->Satellites = atoi(param[7]); From bcee75a27029fed7543350c2e1c9fe9996883762 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Fri, 20 Jul 2012 11:56:40 +0200 Subject: [PATCH 31/32] Added comment to explain the gpsDataUpdated flag --- flight/Modules/GPS/NMEA.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/flight/Modules/GPS/NMEA.c b/flight/Modules/GPS/NMEA.c index 6a25b1fed..150949e8e 100644 --- a/flight/Modules/GPS/NMEA.c +++ b/flight/Modules/GPS/NMEA.c @@ -421,6 +421,10 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData) DEBUG_MSG("%s %d ", params[0]); #endif // Send the message to the parser and get it update the GpsData + // Information from various different NMEA messages are temporarily + // cumulated in the GpsData structure. An actual GPSPosition update + // is triggered by GGA messages only. This message type sets the + // gpsDataUpdated flag to request this. bool gpsDataUpdated = false; if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) { From d4a401ce3000e18538dcae43d8cf5ad006a003d4 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Sat, 21 Jul 2012 07:58:00 +0200 Subject: [PATCH 32/32] Removed Revolution specific code --- flight/Modules/GPS/GPS.c | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 93ea12262..5851ac10d 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -138,17 +138,6 @@ int32_t GPSInitialize(void) gpsEnabled = false; #endif -#if defined(REVOLUTION) - // Revolution expects these objects to always be defined. Not doing so will fail some - // queue connections in navigation - GPSPositionInitialize(); - GPSVelocityInitialize(); - GPSTimeInitialize(); - GPSSatellitesInitialize(); - HomeLocationInitialize(); - updateSettings(); - -#else if (gpsPort && gpsEnabled) { GPSPositionInitialize(); GPSVelocityInitialize(); @@ -161,7 +150,6 @@ int32_t GPSInitialize(void) #endif updateSettings(); } -#endif if (gpsPort && gpsEnabled) { GPSSettingsInitialize();