Skip to content

Commit

Permalink
GPS: Move FactGroup from Vehicle to GPS
Browse files Browse the repository at this point in the history
  • Loading branch information
HTRamsey committed Nov 21, 2024
1 parent 787a407 commit de2ce00
Show file tree
Hide file tree
Showing 12 changed files with 121 additions and 199 deletions.
2 changes: 1 addition & 1 deletion custom-example/qgroundcontrol.qrc
Original file line number Diff line number Diff line change
Expand Up @@ -400,7 +400,7 @@
<file alias="Vehicle/EscStatusFactGroup.json">../src/Vehicle/FactGroups/EscStatusFactGroup.json</file>
<file alias="Vehicle/EstimatorStatusFactGroup.json">../src/Vehicle/FactGroups/EstimatorStatusFactGroup.json</file>
<file alias="Vehicle/GPSFact.json">../src/Vehicle/FactGroups/GPSFact.json</file>
<file alias="Vehicle/GPSRTKFact.json">../src/Vehicle/FactGroups/GPSRTKFact.json</file>
<file alias="Vehicle/GPSRTKFact.json">../src/GPS/GPSRTKFact.json</file>
<file alias="Vehicle/SetpointFact.json">../src/Vehicle/FactGroups/SetpointFact.json</file>
<file alias="Vehicle/LocalPositionFact.json">../src/Vehicle/FactGroups/LocalPositionFact.json</file>
<file alias="Vehicle/LocalPositionSetpointFact.json">../src/Vehicle/FactGroups/LocalPositionFact.json</file>
Expand Down
2 changes: 1 addition & 1 deletion qgroundcontrol.qrc
Original file line number Diff line number Diff line change
Expand Up @@ -402,7 +402,7 @@
<file alias="Vehicle/EscStatusFactGroup.json">src/Vehicle/FactGroups/EscStatusFactGroup.json</file>
<file alias="Vehicle/EstimatorStatusFactGroup.json">src/Vehicle/FactGroups/EstimatorStatusFactGroup.json</file>
<file alias="Vehicle/GPSFact.json">src/Vehicle/FactGroups/GPSFact.json</file>
<file alias="Vehicle/GPSRTKFact.json">src/Vehicle/FactGroups/GPSRTKFact.json</file>
<file alias="Vehicle/GPSRTKFact.json">src/GPS/GPSRTKFact.json</file>
<file alias="Vehicle/SetpointFact.json">src/Vehicle/FactGroups/SetpointFact.json</file>
<file alias="Vehicle/LocalPositionFact.json">src/Vehicle/FactGroups/LocalPositionFact.json</file>
<file alias="Vehicle/LocalPositionSetpointFact.json">src/Vehicle/FactGroups/LocalPositionFact.json</file>
Expand Down
2 changes: 2 additions & 0 deletions src/GPS/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,8 @@ target_sources(GPS
GPSProvider.h
GPSRtk.cc
GPSRtk.h
GPSRTKFactGroup.cc
GPSRTKFactGroup.h
RTCMMavlink.cc
RTCMMavlink.h
satellite_info.h
Expand Down
File renamed without changes.
43 changes: 43 additions & 0 deletions src/GPS/GPSRTKFactGroup.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
/****************************************************************************
*
* (c) 2009-2024 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/

#include "GPSRTKFactGroup.h"
#include "QGCLoggingCategory.h"

QGC_LOGGING_CATEGORY(GPSRTKFactGroupLog, "qgc.gps.gpsrtkfactgroup")

GPSRTKFactGroup::GPSRTKFactGroup(QObject *parent)
: FactGroup(1000, ":/json/Vehicle/GPSRTKFact.json", parent)
, _connected(new Fact(0, _connectedFactName, FactMetaData::valueTypeBool, this))
, _currentDuration(new Fact(0, _currentDurationFactName, FactMetaData::valueTypeDouble, this))
, _currentAccuracy(new Fact(0, _currentAccuracyFactName, FactMetaData::valueTypeDouble, this))
, _currentLatitude(new Fact(0, _currentLatitudeFactName, FactMetaData::valueTypeDouble, this))
, _currentLongitude(new Fact(0, _currentLongitudeFactName, FactMetaData::valueTypeDouble, this))
, _currentAltitude(new Fact(0, _currentAltitudeFactName, FactMetaData::valueTypeFloat, this))
, _valid(new Fact(0, _validFactName, FactMetaData::valueTypeBool, this))
, _active(new Fact(0, _activeFactName, FactMetaData::valueTypeBool, this))
, _numSatellites(new Fact(0, _numSatellitesFactName, FactMetaData::valueTypeInt32, this))
{
// qCDebug(GPSRTKFactGroupLog) << Q_FUNC_INFO << this;

_addFact(_connected, _connectedFactName);
_addFact(_currentDuration, _currentDurationFactName);
_addFact(_currentAccuracy, _currentAccuracyFactName);
_addFact(_currentLatitude, _currentLatitudeFactName);
_addFact(_currentLongitude, _currentLongitudeFactName);
_addFact(_currentAltitude, _currentAltitudeFactName);
_addFact(_valid, _validFactName);
_addFact(_active, _activeFactName);
_addFact(_numSatellites, _numSatellitesFactName);
}

GPSRTKFactGroup::~GPSRTKFactGroup()
{
// qCDebug(GPSRTKFactGroupLog) << Q_FUNC_INFO << this;
}
65 changes: 65 additions & 0 deletions src/GPS/GPSRTKFactGroup.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,65 @@
/****************************************************************************
*
* (c) 2009-2024 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/

#pragma once

#include <QtCore/QLoggingCategory>

#include "FactGroup.h"

Q_DECLARE_LOGGING_CATEGORY(GPSRTKFactGroupLog)

class GPSRTKFactGroup : public FactGroup
{
Q_OBJECT
Q_PROPERTY(Fact *connected READ connected CONSTANT)
Q_PROPERTY(Fact *currentDuration READ currentDuration CONSTANT)
Q_PROPERTY(Fact *currentAccuracy READ currentAccuracy CONSTANT)
Q_PROPERTY(Fact *currentLatitude READ currentLatitude CONSTANT)
Q_PROPERTY(Fact *currentLongitude READ currentLongitude CONSTANT)
Q_PROPERTY(Fact *currentAltitude READ currentAltitude CONSTANT)
Q_PROPERTY(Fact *valid READ valid CONSTANT)
Q_PROPERTY(Fact *active READ active CONSTANT)
Q_PROPERTY(Fact *numSatellites READ numSatellites CONSTANT)

public:
explicit GPSRTKFactGroup(QObject* parent = nullptr);
~GPSRTKFactGroup();

Fact *connected() const { return _connected; }
Fact *currentDuration() const { return _currentDuration; }
Fact *currentAccuracy() const { return _currentAccuracy; }
Fact *currentLatitude() const { return _currentLatitude; }
Fact *currentLongitude() const { return _currentLongitude; }
Fact *currentAltitude() const { return _currentAltitude; }
Fact *valid() const { return _valid; }
Fact *active() const { return _active; }
Fact *numSatellites() const { return _numSatellites; }

private:
const QString _connectedFactName = QStringLiteral("connected");
const QString _currentAccuracyFactName = QStringLiteral("currentAccuracy");
const QString _currentDurationFactName = QStringLiteral("currentDuration");
const QString _currentLatitudeFactName = QStringLiteral("currentLatitude");
const QString _currentLongitudeFactName = QStringLiteral("currentLongitude");
const QString _currentAltitudeFactName = QStringLiteral("currentAltitude");
const QString _validFactName = QStringLiteral("valid");
const QString _activeFactName = QStringLiteral("active");
const QString _numSatellitesFactName = QStringLiteral("numSatellites");

Fact *_connected = nullptr; ///< is an RTK gps connected?
Fact *_currentDuration = nullptr; ///< survey-in status in [s]
Fact *_currentAccuracy = nullptr; ///< survey-in accuracy in [mm]
Fact *_currentLatitude = nullptr; ///< survey-in latitude
Fact *_currentLongitude = nullptr; ///< survey-in latitude
Fact *_currentAltitude = nullptr; ///< survey-in latitude
Fact *_valid = nullptr; ///< survey-in complete?
Fact *_active = nullptr; ///< survey-in active?
Fact *_numSatellites = nullptr; ///< number of satellites
};
33 changes: 0 additions & 33 deletions src/GPS/satellite_info.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,39 +7,6 @@
*
****************************************************************************/

/****************************************************************************
*
* 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.
*
****************************************************************************/

/* https://github.com/PX4/Firmware/blob/master/msg/SatelliteInfo.msg */

#pragma once
Expand Down
33 changes: 0 additions & 33 deletions src/GPS/sensor_gnss_relative.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,39 +7,6 @@
*
****************************************************************************/

/****************************************************************************
*
* Copyright (C) 2013-2022 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.
*
****************************************************************************/

/* https://github.com/PX4/Firmware/blob/master/msg/SensorGnssRelative.msg */

#pragma once
Expand Down
42 changes: 9 additions & 33 deletions src/GPS/sensor_gps.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,39 +7,6 @@
*
****************************************************************************/

/****************************************************************************
*
* Copyright (C) 2013-2022 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.
*
****************************************************************************/

/* https://github.com/PX4/Firmware/blob/master/msg/SensorGps.msg */

#pragma once
Expand All @@ -62,6 +29,14 @@ struct sensor_gps_s

float s_variance_m_s;
float c_variance_rad;

static constexpr uint8_t FIX_TYPE_NONE = 1;
static constexpr uint8_t FIX_TYPE_2D = 2;
static constexpr uint8_t FIX_TYPE_3D = 3;
static constexpr uint8_t FIX_TYPE_RTCM_CODE_DIFFERENTIAL = 4;
static constexpr uint8_t FIX_TYPE_RTK_FLOAT = 5;
static constexpr uint8_t FIX_TYPE_RTK_FIXED = 6;
static constexpr uint8_t FIX_TYPE_EXTRAPOLATED = 8;
uint8_t fix_type;

float eph;
Expand Down Expand Up @@ -106,6 +81,7 @@ struct sensor_gps_s
uint8_t selected_rtcm_instance;

bool rtcm_crc_failed;

static constexpr uint8_t RTCM_MSG_USED_UNKNOWN = 0;
static constexpr uint8_t RTCM_MSG_USED_NOT_USED = 1;
static constexpr uint8_t RTCM_MSG_USED_USED = 2;
Expand Down
2 changes: 0 additions & 2 deletions src/Vehicle/FactGroups/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,8 +1,6 @@
find_package(Qt6 REQUIRED COMPONENTS Core)

qt_add_library(VehicleFactGroups STATIC
GPSRTKFactGroup.cc
GPSRTKFactGroup.h
TerrainFactGroup.cc
TerrainFactGroup.h
VehicleBatteryFactGroup.cc
Expand Down
34 changes: 0 additions & 34 deletions src/Vehicle/FactGroups/GPSRTKFactGroup.cc

This file was deleted.

62 changes: 0 additions & 62 deletions src/Vehicle/FactGroups/GPSRTKFactGroup.h

This file was deleted.

0 comments on commit de2ce00

Please sign in to comment.