diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index b70078105b08b353afa890fa88a23050b3e2a637..fddf246e2d98783398a7bd2f0f0fc0ccc3e2c371 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -200,6 +200,7 @@ INCLUDEPATH += \ src/input \ src/Joystick \ src/FollowMe \ + src/GPS \ src/lib/qmapcontrol \ src/MissionEditor \ src/MissionManager \ @@ -260,6 +261,15 @@ HEADERS += \ src/FlightDisplay/FlightDisplayViewController.h \ src/FlightMap/FlightMapSettings.h \ src/FlightMap/Widgets/ValuesWidgetController.h \ + src/GPS/Drivers/src/gps_helper.h \ + src/GPS/Drivers/src/ubx.h \ + src/GPS/definitions.h \ + src/GPS/vehicle_gps_position.h \ + src/GPS/satellite_info.h \ + src/GPS/RTCM/RTCMMavlink.h \ + src/GPS/GPSManager.h \ + src/GPS/GPSPositionMessage.h \ + src/GPS/GPSProvider.h \ src/GAudioOutput.h \ src/HomePositionManager.h \ src/Joystick/Joystick.h \ @@ -406,6 +416,11 @@ SOURCES += \ src/FlightDisplay/FlightDisplayViewController.cc \ src/FlightMap/FlightMapSettings.cc \ src/FlightMap/Widgets/ValuesWidgetController.cc \ + src/GPS/Drivers/src/gps_helper.cpp \ + src/GPS/Drivers/src/ubx.cpp \ + src/GPS/RTCM/RTCMMavlink.cc \ + src/GPS/GPSManager.cc \ + src/GPS/GPSProvider.cc \ src/GAudioOutput.cc \ src/HomePositionManager.cc \ src/Joystick/Joystick.cc \ diff --git a/src/GPS/GPSManager.cc b/src/GPS/GPSManager.cc new file mode 100644 index 0000000000000000000000000000000000000000..b9fbf52ab2d6202d681fc4333860a9c62a81219b --- /dev/null +++ b/src/GPS/GPSManager.cc @@ -0,0 +1,87 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2016 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL 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. + + QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#include "GPSManager.h" +#include +#include + +GPSManager::GPSManager(QGCApplication* app) + : QGCTool(app) +{ + qRegisterMetaType(); + qRegisterMetaType(); +} + +GPSManager::~GPSManager() +{ + cleanup(); +} + +void GPSManager::setupGPS(const QString& device) +{ + Q_ASSERT(_toolbox); + + cleanup(); + _requestGpsStop = false; + _gpsProvider = new GPSProvider(device, true, _requestGpsStop); + _gpsProvider->start(); + + //create RTCM device + _rtcmMavlink = new RTCMMavlink(*_toolbox); + + connect(_gpsProvider, SIGNAL(RTCMDataUpdate(QByteArray)), _rtcmMavlink, + SLOT(RTCMDataUpdate(QByteArray))); + + //test: connect to position update + connect(_gpsProvider, SIGNAL(positionUpdate(GPSPositionMessage)), this, + SLOT(GPSPositionUpdate(GPSPositionMessage))); + connect(_gpsProvider, SIGNAL(satelliteInfoUpdate(GPSSatelliteMessage)), this, + SLOT(GPSSatelliteUpdate(GPSSatelliteMessage))); + +} + +void GPSManager::GPSPositionUpdate(GPSPositionMessage msg) +{ + qDebug("GPS: got position update: alt=%i, long=%i, lat=%i, time=%" PRIu64, + msg.position_data.alt, msg.position_data.lon, + msg.position_data.lat, msg.position_data.time_utc_usec); +} +void GPSManager::GPSSatelliteUpdate(GPSSatelliteMessage msg) +{ + qDebug("GPS: got satellite info update, %i satellites", (int)msg.satellite_data.count); +} + +void GPSManager::cleanup() +{ + if (_gpsProvider) { + _requestGpsStop = true; + //Note that we need a relatively high timeout to be sure the GPS thread finished. + if (!_gpsProvider->wait(2000)) { + qWarning() << "Failed to wait for GPS thread exit. Consider increasing the timeout"; + } + delete(_gpsProvider); + } + if (_rtcmMavlink) { + delete(_rtcmMavlink); + } +} diff --git a/src/GPS/GPSManager.h b/src/GPS/GPSManager.h new file mode 100644 index 0000000000000000000000000000000000000000..731233e1949e973e5724ad76433a019516abcb80 --- /dev/null +++ b/src/GPS/GPSManager.h @@ -0,0 +1,56 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2016 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL 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. + + QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#pragma once + +#include "GPSProvider.h" +#include "RTCM/RTCMMavlink.h" +#include + +#include +#include + +/** + ** class GPSManager + * handles a GPS provider and RTK + */ +class GPSManager : public QGCTool +{ + Q_OBJECT +public: + GPSManager(QGCApplication* app); + ~GPSManager(); + + void setupGPS(const QString& device); + +private slots: + void GPSPositionUpdate(GPSPositionMessage msg); + void GPSSatelliteUpdate(GPSSatelliteMessage msg); +private: + void cleanup(); + + GPSProvider* _gpsProvider = nullptr; + RTCMMavlink* _rtcmMavlink = nullptr; + + std::atomic_bool _requestGpsStop; ///< signals the thread to quit +}; diff --git a/src/GPS/GPSPositionMessage.h b/src/GPS/GPSPositionMessage.h new file mode 100644 index 0000000000000000000000000000000000000000..782ced8dd3e076cacec5bc3c6383bdc4227435bb --- /dev/null +++ b/src/GPS/GPSPositionMessage.h @@ -0,0 +1,46 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2016 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL 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. + + QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#pragma once + +#include "vehicle_gps_position.h" +#include "satellite_info.h" +#include + +/** + ** struct GPSPositionMessage + * wrapper that can be used for Qt signal/slots + */ +struct GPSPositionMessage +{ + vehicle_gps_position_s position_data; +}; + +Q_DECLARE_METATYPE(GPSPositionMessage); + + +struct GPSSatelliteMessage +{ + satellite_info_s satellite_data; +}; +Q_DECLARE_METATYPE(GPSSatelliteMessage); diff --git a/src/GPS/GPSProvider.cc b/src/GPS/GPSProvider.cc new file mode 100644 index 0000000000000000000000000000000000000000..16f98dc7939265b5861aa8779d4f54041dc0dee2 --- /dev/null +++ b/src/GPS/GPSProvider.cc @@ -0,0 +1,163 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2016 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL 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. + + QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#include "GPSProvider.h" + +#define TIMEOUT_5HZ 500 + +#include + +#include "Drivers/src/ubx.h" +#include "Drivers/src/gps_helper.h" +#include "definitions.h" + + +void GPSProvider::run() +{ + if (_serial) delete _serial; + + _serial = new QSerialPort(); + _serial->setPortName(_device); + if (!_serial->open(QIODevice::ReadWrite)) { + qWarning() << "GPS: Failed to open Serial Device" << _device; + return; + } + _serial->setBaudRate(QSerialPort::Baud9600); + _serial->setDataBits(QSerialPort::Data8); + _serial->setParity(QSerialPort::NoParity); + _serial->setStopBits(QSerialPort::OneStop); + _serial->setFlowControl(QSerialPort::NoFlowControl); + + unsigned int baudrate; + GPSHelper* gpsHelper = nullptr; + + while (!_requestStop) { + + if (gpsHelper) { + delete gpsHelper; + gpsHelper = nullptr; + } + + gpsHelper = new GPSDriverUBX(&callbackEntry, this, &_reportGpsPos, _pReportSatInfo); + + if (gpsHelper->configure(baudrate, GPSHelper::OutputMode::RTCM) == 0) { + + /* reset report */ + memset(&_reportGpsPos, 0, sizeof(_reportGpsPos)); + int helperRet; + + while ((helperRet = gpsHelper->receive(TIMEOUT_5HZ)) > 0 && !_requestStop) { + + if (helperRet & 1) { + publishGPSPosition(); + } + + if (_pReportSatInfo && (helperRet & 2)) { + publishGPSSatellite(); + } + } + if (_serial->error() != QSerialPort::NoError) { + break; + } + } + } + qDebug() << "Exiting GPS thread"; +} + +GPSProvider::GPSProvider(const QString& device, bool enableSatInfo, const std::atomic_bool& requestStop) + : _device(device), _requestStop(requestStop) +{ + if (enableSatInfo) _pReportSatInfo = new satellite_info_s(); +} + +GPSProvider::~GPSProvider() +{ + if (_pReportSatInfo) delete(_pReportSatInfo); + if (_serial) delete _serial; +} + +void GPSProvider::publishGPSPosition() +{ + GPSPositionMessage msg; + msg.position_data = _reportGpsPos; + emit positionUpdate(msg); +} + +void GPSProvider::publishGPSSatellite() +{ + GPSSatelliteMessage msg; + msg.satellite_data = *_pReportSatInfo; + emit satelliteInfoUpdate(msg); +} + +void GPSProvider::gotRTCMData(uint8_t* data, size_t len) +{ + QByteArray message((char*)data, len); + emit RTCMDataUpdate(message); +} + +int GPSProvider::callbackEntry(GPSCallbackType type, void *data1, int data2, void *user) +{ + GPSProvider *gps = (GPSProvider *)user; + return gps->callback(type, data1, data2); +} + +int GPSProvider::callback(GPSCallbackType type, void *data1, int data2) +{ + switch (type) { + case GPSCallbackType::readDeviceData: { + int timeout = *((int *) data1); + if (!_serial->waitForReadyRead(timeout)) + return 0; //timeout + msleep(10); //give some more time to buffer data + return (int)_serial->read((char*) data1, data2); + } + case GPSCallbackType::writeDeviceData: + if (_serial->write((char*) data1, data2) >= 0) { + if (_serial->waitForBytesWritten(-1)) + return data2; + } + return -1; + + case GPSCallbackType::setBaudrate: + return _serial->setBaudRate(data2) ? 0 : -1; + + case GPSCallbackType::gotRTCMMessage: + gotRTCMData((uint8_t*) data1, data2); + break; + + case GPSCallbackType::surveyInStatus: + { + SurveyInStatus* status = (SurveyInStatus*)data1; + qInfo("Survey-in status: %is cur accuracy: %imm valid: %i active: %i", + status->duration, status->mean_accuracy, (int)(status->flags & 1), (int)((status->flags>>1) & 1)); + } + break; + + case GPSCallbackType::setClock: + /* do nothing */ + break; + } + + return 0; +} diff --git a/src/GPS/GPSProvider.h b/src/GPS/GPSProvider.h new file mode 100644 index 0000000000000000000000000000000000000000..58af5ed4645a60423898ecca968d418b1f1cb88a --- /dev/null +++ b/src/GPS/GPSProvider.h @@ -0,0 +1,78 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2016 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL 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. + + QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#pragma once + +#include +#include +#include +#include + +#include + +#include "GPSPositionMessage.h" +#include "Drivers/src/gps_helper.h" + + +/** + ** class GPSProvider + * opens a GPS device and handles the protocol + */ +class GPSProvider : public QThread +{ + Q_OBJECT +public: + GPSProvider(const QString& device, bool enableSatInfo, const std::atomic_bool& requestStop); + ~GPSProvider(); + + /** + * this is called by the callback method + */ + void gotRTCMData(uint8_t *data, size_t len); +signals: + void positionUpdate(GPSPositionMessage message); + void satelliteInfoUpdate(GPSSatelliteMessage message); + void RTCMDataUpdate(QByteArray message); + +protected: + void run(); + +private: + void publishGPSPosition(); + void publishGPSSatellite(); + + /** + * callback from the driver for the platform specific stuff + */ + static int callbackEntry(GPSCallbackType type, void *data1, int data2, void *user); + + int callback(GPSCallbackType type, void *data1, int data2); + + QString _device; + const std::atomic_bool& _requestStop; + + struct vehicle_gps_position_s _reportGpsPos; + struct satellite_info_s *_pReportSatInfo = nullptr; + + QSerialPort *_serial = nullptr; +}; diff --git a/src/GPS/RTCM/RTCMMavlink.cc b/src/GPS/RTCM/RTCMMavlink.cc new file mode 100644 index 0000000000000000000000000000000000000000..9578d74671a85a05fd7003060e71b0d620ecf94d --- /dev/null +++ b/src/GPS/RTCM/RTCMMavlink.cc @@ -0,0 +1,65 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2016 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL 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. + + QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#include "RTCMMavlink.h" + +#include "MultiVehicleManager.h" +#include "MAVLinkProtocol.h" +#include "Vehicle.h" + +#include + +RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox) + : _toolbox(toolbox) +{ + _bandwidthTimer.start(); +} + +void RTCMMavlink::RTCMDataUpdate(QByteArray message) +{ + Q_ASSERT(message.size() <= 110); //mavlink message uses a fixed-size buffer + + /* statistics */ + _bandwidthByteCounter += message.size(); + qint64 elapsed = _bandwidthTimer.elapsed(); + if (elapsed > 1000) { + printf("RTCM bandwidth: %.2f kB/s\n", (float) _bandwidthByteCounter / elapsed * 1000.f / 1024.f); + _bandwidthTimer.restart(); + _bandwidthByteCounter = 0; + } + QmlObjectListModel& vehicles = *_toolbox.multiVehicleManager()->vehicles(); + MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol(); + mavlink_gps_inject_data_t mavlinkRtcmData; + memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_inject_data_t)); + + mavlinkRtcmData.len = message.size(); + memcpy(&mavlinkRtcmData.data, message.data(), message.size()); + + for (int i = 0; i < vehicles.count(); i++) { + Vehicle* vehicle = qobject_cast(vehicles[i]); + mavlink_message_t message; + mavlink_msg_gps_inject_data_encode(mavlinkProtocol->getSystemId(), + mavlinkProtocol->getComponentId(), &message, &mavlinkRtcmData); + vehicle->sendMessage(message); + } +} diff --git a/src/GPS/RTCM/RTCMMavlink.h b/src/GPS/RTCM/RTCMMavlink.h new file mode 100644 index 0000000000000000000000000000000000000000..865302d47adcff313c95af3c6fa8991cb5a6544f --- /dev/null +++ b/src/GPS/RTCM/RTCMMavlink.h @@ -0,0 +1,49 @@ +/*===================================================================== + + QGroundControl Open Source Ground Control Station + + (c) 2009 - 2016 QGROUNDCONTROL PROJECT + + This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL 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. + + QGROUNDCONTROL 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 QGROUNDCONTROL. If not, see . + + ======================================================================*/ + +#pragma once + +#include +#include + +#include "QGCToolbox.h" + +/** + ** class RTCMMavlink + * Receives RTCM updates and sends them via MAVLINK to the device + */ +class RTCMMavlink : public QObject +{ + Q_OBJECT +public: + RTCMMavlink(QGCToolbox& toolbox); + //TODO: API to select device(s)? + +public slots: + void RTCMDataUpdate(QByteArray message); + +private: + QGCToolbox& _toolbox; + QElapsedTimer _bandwidthTimer; + int _bandwidthByteCounter = 0; +}; diff --git a/src/GPS/definitions.h b/src/GPS/definitions.h new file mode 100644 index 0000000000000000000000000000000000000000..b28351e3e5d1529495aabca0d76de981e4f27b62 --- /dev/null +++ b/src/GPS/definitions.h @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file definitions.h + * common platform-specific definitions & abstractions for gps + * @author Beat Küng + */ + +#pragma once + +#include + +#define GPS_INFO(...) qInfo(__VA_ARGS__) +#define GPS_WARN(...) qWarning(__VA_ARGS__) +#define GPS_ERR(...) qCritical(__VA_ARGS__) + +#include "vehicle_gps_position.h" +#include "satellite_info.h" + +#define M_DEG_TO_RAD_F 0.01745329251994f +#define M_RAD_TO_DEG_F 57.2957795130823f + +#include + +class Sleeper : public QThread +{ +public: + static void usleep(unsigned long usecs) { QThread::usleep(usecs); } +}; + +#define usleep Sleeper::usleep + + +typedef uint64_t gps_abstime; + +#include +/** + * Get the current time in us. Function signature: + * uint64_t hrt_absolute_time() + */ +static inline gps_abstime gps_absolute_time() { + //FIXME: is there something with microsecond accuracy? + return QDateTime::currentMSecsSinceEpoch() * 1000; +} + +//timespec is UNIX-specific +#ifdef _WIN32 +struct timespec +{ + time_t tv_sec; + long tv_nsec; +}; +#endif + diff --git a/src/GPS/satellite_info.h b/src/GPS/satellite_info.h new file mode 100644 index 0000000000000000000000000000000000000000..a83bdfecab46920488e2fbaf9de6da048347b5b3 --- /dev/null +++ b/src/GPS/satellite_info.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once +#include + +/* + * This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/satellite_info.msg + * and was manually copied here. + */ + +struct satellite_info_s { + uint64_t timestamp; + uint8_t count; + uint8_t svid[20]; + uint8_t used[20]; + uint8_t elevation[20]; + uint8_t azimuth[20]; + uint8_t snr[20]; +#ifdef __cplusplus + static const uint8_t SAT_INFO_MAX_SATELLITES = 20; + +#endif +}; diff --git a/src/GPS/vehicle_gps_position.h b/src/GPS/vehicle_gps_position.h new file mode 100644 index 0000000000000000000000000000000000000000..e0bbeb8393a28284932b09dc62d528ccdcd113eb --- /dev/null +++ b/src/GPS/vehicle_gps_position.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once +#include + +/* + * This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/vehicle_gps_position.msg + * and was manually copied here. + */ + +struct vehicle_gps_position_s { + uint64_t timestamp_position; + int32_t lat; + int32_t lon; + int32_t alt; + int32_t alt_ellipsoid; + uint64_t timestamp_variance; + float s_variance_m_s; + float c_variance_rad; + uint8_t fix_type; + float eph; + float epv; + float hdop; + float vdop; + int32_t noise_per_ms; + int32_t jamming_indicator; + uint64_t timestamp_velocity; + float vel_m_s; + float vel_n_m_s; + float vel_e_m_s; + float vel_d_m_s; + float cog_rad; + bool vel_ned_valid; + uint64_t timestamp_time; + uint64_t time_utc_usec; + uint8_t satellites_used; +}; diff --git a/src/QGCToolbox.cc b/src/QGCToolbox.cc index fcda047d231c7956f3e6060ed41ba382a5195621..ace2e7c148c0c61bfc350c513d8790e3d5afb25e 100644 --- a/src/QGCToolbox.cc +++ b/src/QGCToolbox.cc @@ -26,6 +26,7 @@ #include "FirmwarePluginManager.h" #include "FlightMapSettings.h" #include "GAudioOutput.h" +#include "GPSManager.h" #include "HomePositionManager.h" #include "JoystickManager.h" #include "LinkManager.h" @@ -59,6 +60,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app) _factSystem = new FactSystem(app); _firmwarePluginManager = new FirmwarePluginManager(app); _flightMapSettings = new FlightMapSettings(app); + _gpsManager = new GPSManager(app); _homePositionManager = new HomePositionManager(app); _imageProvider = new QGCImageProvider(app); _joystickManager = new JoystickManager(app); @@ -75,6 +77,7 @@ QGCToolbox::QGCToolbox(QGCApplication* app) _factSystem->setToolbox(this); _firmwarePluginManager->setToolbox(this); _flightMapSettings->setToolbox(this); + _gpsManager->setToolbox(this); _homePositionManager->setToolbox(this); _imageProvider->setToolbox(this); _joystickManager->setToolbox(this); @@ -85,6 +88,9 @@ QGCToolbox::QGCToolbox(QGCApplication* app) _mapEngineManager->setToolbox(this); _uasMessageHandler->setToolbox(this); _followMe->setToolbox(this); + + //FIXME: make this configurable... + //_gpsManager->setupGPS("ttyACM0"); } QGCToolbox::~QGCToolbox() diff --git a/src/QGCToolbox.h b/src/QGCToolbox.h index a37f124bf922829e5e6f6fd7e2ab267ed27fded7..c52388856108c0de6e70be2872b51c0b791fde1e 100644 --- a/src/QGCToolbox.h +++ b/src/QGCToolbox.h @@ -31,6 +31,7 @@ class FactSystem; class FirmwarePluginManager; class FlightMapSettings; class GAudioOutput; +class GPSManager; class HomePositionManager; class JoystickManager; class FollowMe; @@ -71,6 +72,7 @@ private: FactSystem* _factSystem; FirmwarePluginManager* _firmwarePluginManager; FlightMapSettings* _flightMapSettings; + GPSManager* _gpsManager; HomePositionManager* _homePositionManager; QGCImageProvider* _imageProvider; JoystickManager* _joystickManager;