Merged in Oblivium/librepilot/LP-536_GNSS_improvements (pull request #444)

LP-536 GNSS improvements

Approved-by: Lalanne Laurent <f5soh@free.fr>
Approved-by: Brian Webb <webbbn@gmail.com>
Approved-by: Alessio Morale <alessiomorale@gmail.com>
Approved-by: Philippe Renon <philippe_renon@yahoo.fr>
Approved-by: Jan NIJS <dr.oblivium@gmail.com>

Author: dr-oblivium

flight/modules/GPS/GPS.c

@@ -105,30 +105,52 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev);
 // the new location with Set = true.
 #define GPS_HOMELOCATION_SET_DELAY 5000
 
-#define GPS_LOOP_DELAY_MS          6
+// PIOS serial port receive buffer for GPS is set to 32 bytes for the minimal and 128 bytes for the full version.
+// GPS_READ_BUFFER is defined a few lines below in this file.
+//
+// 57600 bps = 5760 bytes per second
+//
+// For 32 bytes buffer: this is a maximum of 5760/32 = 180 buffers per second
+// that is 1/180 = 0.0056 seconds per packet
+// We must never wait more than 5ms since packet was last drained or it may overflow
+//
+// For 128 bytes buffer: this is a maximum of 5760/128 = 45 buffers per second
+// that is 1/45 = 0.022 seconds per packet
+// We must never wait more than 22ms since packet was last drained or it may overflow
+
+// There are two delays in play for the GPS task:
+// - GPS_BLOCK_ON_NO_DATA_MS is the time to block while waiting to receive serial data from the GPS
+// - GPS_LOOP_DELAY_MS is used for a context switch initiated by calling vTaskDelayUntil() to let other tasks run
+//
+// The delayMs time is not that critical. It should not be too high so that maintenance actions within this task
+// are run often enough.
+// GPS_LOOP_DELAY_MS on the other hand, should be less then 5.55 ms. A value set too high will cause data to be dropped.
+
+#define GPS_LOOP_DELAY_MS        5
+#define GPS_BLOCK_ON_NO_DATA_MS  20
 
 #ifdef PIOS_GPS_SETS_HOMELOCATION
 // Unfortunately need a good size stack for the WMM calculation
-        #define STACK_SIZE_BYTES   1024
+        #define STACK_SIZE_BYTES 1024
 #else
 #if defined(PIOS_GPS_MINIMAL)
-        #define GPS_READ_BUFFER    32
+        #define GPS_READ_BUFFER  32
 
 #ifdef PIOS_INCLUDE_GPS_NMEA_PARSER
-        #define STACK_SIZE_BYTES   580 // NMEA
-#else
-        #define STACK_SIZE_BYTES   440 // UBX
+        #define STACK_SIZE_BYTES 580 // NMEA
+#else // PIOS_INCLUDE_GPS_NMEA_PARSER
+        #define STACK_SIZE_BYTES 440 // UBX
 #endif // PIOS_INCLUDE_GPS_NMEA_PARSER
-#else
-        #define STACK_SIZE_BYTES   650
+#else // PIOS_GPS_MINIMAL
+        #define STACK_SIZE_BYTES 650
 #endif // PIOS_GPS_MINIMAL
 #endif // PIOS_GPS_SETS_HOMELOCATION
 
 #ifndef GPS_READ_BUFFER
-#define GPS_READ_BUFFER            128
+#define GPS_READ_BUFFER          128
 #endif
 
-#define TASK_PRIORITY              (tskIDLE_PRIORITY + 1)
+#define TASK_PRIORITY            (tskIDLE_PRIORITY + 1)
 
 // ****************
 // Private variables
@@ -284,14 +306,7 @@ MODULE_INITCALL(GPSInitialize, GPSStart);
 
 static void gpsTask(__attribute__((unused)) void *parameters)
 {
-    // 57600 baud = 5760 bytes per second
-    // PIOS serial buffers appear to be 32 bytes long
-    // that is a maximum of 5760/32 = 180 buffers per second
-    // that is 1/180 = 0.005555556 seconds per packet
-    // we must never wait more than 5ms since packet was last drained or it may overflow
-    // 100ms is way slow too, considering we do everything possible to make the sensor data as contemporary as possible
-    portTickType xDelay = 5 / portTICK_RATE_MS;
-    uint32_t timeNowMs  = xTaskGetTickCount() * portTICK_RATE_MS;
+    uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
 
 #ifdef PIOS_GPS_SETS_HOMELOCATION
     portTickType homelocationSetDelay = 0;
@@ -353,8 +368,8 @@ static void gpsTask(__attribute__((unused)) void *parameters)
 
             uint16_t cnt;
             int res;
-            // This blocks the task until there is something on the buffer (or 100ms? passes)
-            cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, xDelay);
+            // This blocks the task until there is something in the buffer (or GPS_BLOCK_ON_NO_DATA_MS passes)
+            cnt = PIOS_COM_ReceiveBuffer(gpsPort, c, GPS_READ_BUFFER, GPS_BLOCK_ON_NO_DATA_MS);
             res = PARSER_INCOMPLETE;
             if (cnt > 0) {
                 PERF_TIMED_SECTION_START(counterParse);
@@ -416,7 +431,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
                 //
                 // if (the fix is good) {
                 if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) &&
-                    (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
+                    ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) || (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS)) &&
                     (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
                     AlarmsClear(SYSTEMALARMS_ALARM_GPS);
 #ifdef PIOS_GPS_SETS_HOMELOCATION
@@ -436,7 +451,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
                     }
 #endif
                     // else if (we are at least getting what might be usable GPS data to finish a flight with) {
-                } else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
+                } else if (((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) || (gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS)) &&
                            (gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
                     AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
                     // else data is probably not good enough to fly

flight/modules/GPS/UBX.c

@@ -173,8 +173,35 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
 #if defined(PIOS_GPS_MINIMAL)
                 restart_state = RESTART_NO_ERROR;
 #else
-                restart_state = RESTART_WITH_ERROR;
+                /*  UBX-NAV-SVINFO (id 30) and UBX-NAV-SAT (id 35) packets are NOT critical to the navigation.
+                    Their only use is to update the nice GPS constellation widget in the GCS.
+                    These packets become very large when a lot of SV (Space Vehicles) are tracked. (8 + 12 * <number of SV>) bytes
+
+                    In the case of 3 simultaneously enabled GNSS, it is easy to reach the currently defined tracking limit of 32 SV.
+                    The memory taken up by this is 8 + 12 * 32 = 392 bytes
+
+                    The NEO-M8N has been seen to send out information for more than 32 SV. This causes overflow errors.
+                    We will ignore these informational packets if they become too large.
+                    The downside of this is no more constellation updates in the GCS when we reach the limit.
+
+                    An alternative fix could be to increment the maximum number of satellites: MAX_SVS and UBX_CFG_GNSS_NUMCH_VER8 in UBX.h
+                    This would use extra memory for little gain. Both fixes can be combined.
+
+                    Tests indicate that, once we reach this amount of tracked SV, the NEO-M8N module positioning output
+                    becomes jittery (in time) and therefore less accurate.
+
+                    The recommendation is to limit the maximum number of simultaneously used GNSS to a value of 2.
+                    This will help keep the number of tracked satellites in line.
+                 */
+                if ((ubx->header.class == 0x01) && ((ubx->header.id == 0x30) || (ubx->header.id == 0x35))) {
+                    restart_state = RESTART_NO_ERROR;
+                } else {
+                    restart_state = RESTART_WITH_ERROR;
+                }
 #endif
+                // We won't see the end of the packet. Which means it is useless to do any further processing.
+                // Therefore scan for the start of the next packet.
+                proto_state = START;
                 break;
             } else {
                 if (ubx->header.len == 0) {
@@ -337,7 +364,7 @@ static void parse_ubx_nav_sol(struct UBXPacket *ubx, GPSPositionSensorData *GpsP
                 GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_FIX2D;
                 break;
             case STATUS_GPSFIX_3DFIX:
-                GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_FIX3D;
+                GpsPosition->Status = (sol->flags & STATUS_FLAGS_DIFFSOLN) ? GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS : GPSPOSITIONSENSOR_STATUS_FIX3D;
                 break;
             default: GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
             }
@@ -399,10 +426,18 @@ static void parse_ubx_nav_pvt(struct UBXPacket *ubx, GPSPositionSensorData *GpsP
     GpsPosition->Longitude       = pvt->lon;
     GpsPosition->Satellites      = pvt->numSV;
     GpsPosition->PDOP = pvt->pDOP * 0.01f;
+
     if (pvt->flags & PVT_FLAGS_GNSSFIX_OK) {
-        GpsPosition->Status = pvt->fixType == PVT_FIX_TYPE_3D ?
-                              GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D;
-    } else {
+        switch (pvt->fixType) {
+        case PVT_FIX_TYPE_2D:
+            GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_FIX2D;
+            break;
+        case PVT_FIX_TYPE_3D:
+            GpsPosition->Status = (pvt->flags & PVT_FLAGS_DIFFSOLN) ? GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS : GPSPOSITIONSENSOR_STATUS_FIX3D;
+            break;
+        default: GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
+        }
+    } else { // fix is not valid so we make sure to treat is as NOFIX
         GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
     }
 

flight/modules/Osd/OsdEtStd/OsdEtStd.c

@@ -450,7 +450,7 @@ static void Run(void)
         // positionData.Status, positionData.Satellites, (uint32_t)positionData.Altitude);
 
         // GPS Status
-        if (positionData.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) {
+        if ((positionData.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) || (positionData.Status == GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS)) {
             msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_FIX;
         } else {
             msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_NOFIX;

flight/modules/StateEstimation/filterlla.c

@@ -104,7 +104,7 @@ static filterResult filter(__attribute__((unused)) stateFilter *self, stateEstim
 
         // check if we have a valid GPS signal (not checked by StateEstimation istelf)
         if ((gpsdata.PDOP < this->settings.MaxPDOP) && (gpsdata.Satellites >= this->settings.MinSatellites) &&
-            (gpsdata.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
+            ((gpsdata.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) || (gpsdata.Status == GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS)) &&
             (gpsdata.Latitude != 0 || gpsdata.Longitude != 0)) {
             int32_t LLAi[3] = {
                 gpsdata.Latitude,

flight/modules/UAVOFrSKYSensorHubBridge/UAVOFrSKYSensorHubBridge.c

@@ -382,6 +382,7 @@ static void uavoFrSKYSensorHubBridgeTask(__attribute__((unused)) void *parameter
                 status = 300;
                 break;
             case GPSPOSITIONSENSOR_STATUS_FIX3D:
+            case GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS:
                 if (hl_set == HOMELOCATION_SET_TRUE) {
                     status = 500;
                 } else {
@@ -419,7 +420,8 @@ static void uavoFrSKYSensorHubBridgeTask(__attribute__((unused)) void *parameter
             msg_length += frsky_pack_temperature_02((vdop * 256 + hdop), shub_global->serial_buf + msg_length);
 
             if (gpsPosData.Status == GPSPOSITIONSENSOR_STATUS_FIX2D ||
-                gpsPosData.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) {
+                gpsPosData.Status == GPSPOSITIONSENSOR_STATUS_FIX3D ||
+                gpsPosData.Status == GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS) {
                 msg_length += frsky_pack_gps(
                     gpsPosData.Heading,
                     gpsPosData.Latitude,

flight/modules/UAVOHottBridge/uavohottbridge.c

@@ -357,6 +357,7 @@ uint16_t build_GPS_message(struct hott_gps_message *msg)
         msg->gps_fix_char = '2';
         break;
     case GPSPOSITIONSENSOR_STATUS_FIX3D:
+    case GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS:
         msg->gps_fix_char = '3';
         break;
     default:

flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c

@@ -320,6 +320,7 @@ static void mavlink_send_position()
             gps_fix_type = 2;
             break;
         case GPSPOSITIONSENSOR_STATUS_FIX3D:
+        case GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS:
             gps_fix_type = 3;
             break;
         }

ground/gcs/src/plugins/gpsdisplay/flatearthwidget.cpp

@@ -0,0 +1,162 @@
+/**
+ ******************************************************************************
+ *
+ * @file       flatearthwidget.cpp
+ * @author     The LibrePilot Team, http://www.librepilot.org Copyright (C) 2017.
+ *             Edouard Lafargue Copyright (C) 2010.
+ * @addtogroup GCSPlugins GCS Plugins
+ * @{
+ * @addtogroup FlatEarthWidget FlatEarth Widget Plugin
+ * @{
+ * @brief      A widget for visualizing a location on a flat map of the world.
+ *****************************************************************************/
+/*
+ * 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "flatearthwidget.h"
+#include <QtSvg/QSvgRenderer>
+#include <QtSvg/QGraphicsSvgItem>
+#include <math.h> /* fabs */
+
+#define MARKER_SIZE 40.0
+
+FlatEarthWidget::FlatEarthWidget(QWidget *parent) : QGraphicsView(parent)
+{
+    // Disable scrollbars to prevent an "always updated loop"
+    this->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
+    this->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
+
+    // Show the whole world by default
+    zoomedin   = false;
+
+    // Set initial location
+    oldLat     = oldLon = 0;
+
+    // Draw the earth
+    earthScene = new QGraphicsScene(this);
+    QPixmap earthpix(":/gpsgadget/images/flatEarth.jpg");
+    earthPixmapItem = earthScene->addPixmap(earthpix);
+    this->setScene(earthScene);
+
+    // Draw the marker
+    marker = new QGraphicsSvgItem();
+    QSvgRenderer *renderer = new QSvgRenderer();
+    renderer->load(QString(":/gpsgadget/images/marker.svg"));
+    marker->setSharedRenderer(renderer);
+    earthScene->addItem(marker);
+}
+
+FlatEarthWidget::~FlatEarthWidget()
+{
+    delete earthScene;
+    earthScene = NULL;
+}
+
+/*
+ * Refresh the map on first showing
+ */
+void FlatEarthWidget::showEvent(QShowEvent *event)
+{
+    Q_UNUSED(event)
+
+    refreshMap();
+}
+
+/*
+ * Refresh the map while resizing the widget
+ */
+void FlatEarthWidget::resizeEvent(QResizeEvent *event)
+{
+    Q_UNUSED(event)
+
+    refreshMap();
+}
+
+/*
+ * Toggle zoom state on mouse click event
+ */
+void FlatEarthWidget::mousePressEvent(QMouseEvent *event)
+{
+    Q_UNUSED(event)
+
+    // toggle zoom state and refresh the map
+    zoomedin = !zoomedin;
+    refreshMap();
+}
+
+/*
+ * Update the position of the marker on the map
+ */
+void FlatEarthWidget::setPosition(double lat, double lon, bool forceUpdate)
+{
+    /* Only perform this expensive graphics operation if the position has changed enough
+       to be visible on the map. Or if an update is requested by force.
+     */
+    if (fabs(oldLat - lat) > 0.1 || fabs(oldLon - lon) > 0.1 || forceUpdate) {
+        double wscale = this->sceneRect().width() / 360;
+        double hscale = this->sceneRect().height() / 180;
+        QPointF opd   = QPointF((lon + 180) * wscale - marker->boundingRect().width() * marker->scale() / 2,
+                                (90 - lat) * hscale - marker->boundingRect().height() * marker->scale() / 2);
+        marker->setTransform(QTransform::fromTranslate(opd.x(), opd.y()), false);
+
+        // Center the map on the uav position
+        if (zoomedin) {
+            this->centerOn(marker);
+        }
+    }
+    oldLat = lat;
+    oldLon = lon;
+}
+
+/*
+ * Update the scale of the map
+ */
+void FlatEarthWidget::refreshMap(void)
+{
+    double markerScale;
+
+    if (zoomedin) {
+        // Hide the marker
+        marker->setVisible(false);
+
+        // Zoom the map to it's native resolution
+        this->resetTransform();
+
+        // Center the map on the uav position
+        this->centerOn(marker);
+
+        // Rescale the marker dimensions to keep the same appearance in both zoom levels
+        markerScale = MARKER_SIZE / 100.0 * (double)this->rect().width() / (double)(earthPixmapItem->boundingRect().width());
+        marker->setScale(markerScale);
+
+        // Show the marker again
+        marker->setVisible(true);
+    } else {
+        // Hide the marker
+        marker->setVisible(false);
+
+        // Fit the whole world
+        this->fitInView(earthPixmapItem, Qt::KeepAspectRatio);
+
+        // Rescale the marker dimensions to keep the same appearance in both zoom levels
+        markerScale = MARKER_SIZE / 100.0;
+        marker->setScale(markerScale);
+
+        // Show the marker again
+        marker->setVisible(true);
+    }
+    // force an update of the marker position
+    this->setPosition(oldLat, oldLon, true);
+}