Browse Source

GPS/Drivers: Update submodule, fix RTK for F9P

This updates the GPS driver submodule to latest main. This includes a
fix for RTK for the uBlox F9P where a config failed and prevent proper
init of the GPS module.

Signed-off-by: Julian Oes <julian@oes.ch>
QGC4.4
Julian Oes 1 year ago
parent
commit
9699856df3
No known key found for this signature in database
GPG Key ID: F0ED380FEA56DE41
  1. 3
      qgroundcontrol.pro
  2. 2
      src/GPS/Drivers
  3. 2
      src/GPS/GPSManager.cc
  4. 4
      src/GPS/GPSPositionMessage.h
  5. 4
      src/GPS/GPSProvider.cc
  6. 5
      src/GPS/definitions.h
  7. 49
      src/GPS/sensor_gnss_relative.h
  8. 95
      src/GPS/sensor_gps.h

3
qgroundcontrol.pro

@ -818,7 +818,8 @@ HEADERS += \ @@ -818,7 +818,8 @@ HEADERS += \
src/GPS/RTCM/RTCMMavlink.h \
src/GPS/definitions.h \
src/GPS/satellite_info.h \
src/GPS/vehicle_gps_position.h \
src/GPS/sensor_gps.h \
src/GPS/sensor_gnss_relative.h \
src/Joystick/JoystickSDL.h \
src/RunGuard.h \
}

2
src/GPS/Drivers

@ -1 +1 @@ @@ -1 +1 @@
Subproject commit c873a63ef4ba2205a6050e5c2a21726ef7055663
Subproject commit 915024ccc54c7190d8e5648b68b38d0f0ea0d7cb

2
src/GPS/GPSManager.cc

@ -91,7 +91,7 @@ void GPSManager::disconnectGPS(void) @@ -91,7 +91,7 @@ void GPSManager::disconnectGPS(void)
void GPSManager::GPSPositionUpdate(GPSPositionMessage msg)
{
qCDebug(RTKGPSLog) << QString("GPS: got position update: alt=%1, long=%2, lat=%3").arg(msg.position_data.alt).arg(msg.position_data.lon).arg(msg.position_data.lat);
qCDebug(RTKGPSLog) << QString("GPS: got position update: alt=%1, long=%2, lat=%3").arg(msg.position_data.altitude_msl_m).arg(msg.position_data.longitude_deg).arg(msg.position_data.latitude_deg);
}
void GPSManager::GPSSatelliteUpdate(GPSSatelliteMessage msg)
{

4
src/GPS/GPSPositionMessage.h

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
* (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.
@ -10,7 +10,7 @@ @@ -10,7 +10,7 @@
#pragma once
#include "vehicle_gps_position.h"
#include "sensor_gps.h"
#include "satellite_info.h"
#include <QMetaType>

4
src/GPS/GPSProvider.cc

@ -228,6 +228,10 @@ int GPSProvider::callback(GPSCallbackType type, void *data1, int data2) @@ -228,6 +228,10 @@ int GPSProvider::callback(GPSCallbackType type, void *data1, int data2)
case GPSCallbackType::setClock:
/* do nothing */
break;
case GPSCallbackType::gotRelativePositionMessage:
/* do nothing */
break;
}
return 0;

5
src/GPS/definitions.h

@ -47,7 +47,8 @@ @@ -47,7 +47,8 @@
#define GPS_WARN(...) qWarning(__VA_ARGS__)
#define GPS_ERR(...) qCritical(__VA_ARGS__)
#include "vehicle_gps_position.h"
#include "sensor_gps.h"
#include "sensor_gnss_relative.h"
#include "satellite_info.h"
#define M_DEG_TO_RAD (M_PI / 180.0)
@ -55,6 +56,8 @@ @@ -55,6 +56,8 @@
#define M_DEG_TO_RAD_F 0.01745329251994f
#define M_RAD_TO_DEG_F 57.2957795130823f
#define M_PI_2_F M_PI
#include <QThread>
class Sleeper : public QThread

49
src/GPS/vehicle_gps_position.h → src/GPS/sensor_gnss_relative.h

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* 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
@ -31,42 +31,37 @@ @@ -31,42 +31,37 @@
*
****************************************************************************/
/* Auto-generated by genmsg_cpp from file /home/beat/px4/src/Firmware/msg/vehicle_gps_position.msg */
/* Auto-generated by genmsg_cpp from file /home/julianoes/src/upstream/PX4-Autopilot/msg/SensorGnssRelative.msg */
#pragma once
#include <stdint.h>
/*
* This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/vehicle_gps_position.msg
* This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/SensorGnssRelative.msg
* and was manually copied here.
*/
struct sensor_gps_s {
struct sensor_gnss_relative_s {
uint64_t timestamp;
uint64_t timestamp_sample;
uint64_t time_utc_usec;
int32_t lat;
int32_t lon;
int32_t alt;
int32_t alt_ellipsoid;
uint16_t automatic_gain_control;
uint8_t jamming_state;
float s_variance_m_s;
float c_variance_rad;
float eph;
float epv;
float hdop;
float vdop;
int32_t noise_per_ms;
int32_t jamming_indicator;
float vel_m_s;
float vel_n_m_s;
float vel_e_m_s;
float vel_d_m_s;
float cog_rad;
int32_t timestamp_time_relative;
uint32_t device_id;
float position[3];
float position_accuracy[3];
float heading;
uint8_t fix_type;
bool vel_ned_valid;
uint8_t satellites_used;
float heading_accuracy;
float position_length;
float accuracy_length;
uint16_t reference_station_id;
bool gnss_fix_ok;
bool differential_solution;
bool relative_position_valid;
bool carrier_solution_floating;
bool carrier_solution_fixed;
bool moving_base_mode;
bool reference_position_miss;
bool reference_observations_miss;
bool heading_valid;
bool relative_position_normalized;
};

95
src/GPS/sensor_gps.h

@ -0,0 +1,95 @@ @@ -0,0 +1,95 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/* Auto-generated by genmsg_cpp from file /home/julianoes/src/upstream/PX4-Autopilot/msg/SensorGps.msg */
#pragma once
#include <stdint.h>
/*
* This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/SensorGps.msg
* and was manually copied here.
*/
struct sensor_gps_s {
uint64_t timestamp;
uint64_t timestamp_sample;
double latitude_deg;
double longitude_deg;
double altitude_msl_m;
double altitude_ellipsoid_m;
uint64_t time_utc_usec;
uint32_t device_id;
float s_variance_m_s;
float c_variance_rad;
float eph;
float epv;
float hdop;
float vdop;
int32_t noise_per_ms;
int32_t jamming_indicator;
float vel_m_s;
float vel_n_m_s;
float vel_e_m_s;
float vel_d_m_s;
float cog_rad;
int32_t timestamp_time_relative;
float heading;
float heading_offset;
float heading_accuracy;
float rtcm_injection_rate;
uint16_t automatic_gain_control;
uint8_t fix_type;
uint8_t jamming_state;
uint8_t spoofing_state;
bool vel_ned_valid;
uint8_t satellites_used;
uint8_t selected_rtcm_instance;
bool rtcm_crc_failed;
uint8_t rtcm_msg_used;
uint8_t _padding0[2]; // required for logger
static constexpr uint8_t JAMMING_STATE_UNKNOWN = 0;
static constexpr uint8_t JAMMING_STATE_OK = 1;
static constexpr uint8_t JAMMING_STATE_WARNING = 2;
static constexpr uint8_t JAMMING_STATE_CRITICAL = 3;
static constexpr uint8_t SPOOFING_STATE_UNKNOWN = 0;
static constexpr uint8_t SPOOFING_STATE_NONE = 1;
static constexpr uint8_t SPOOFING_STATE_INDICATED = 2;
static constexpr uint8_t SPOOFING_STATE_MULTIPLE = 3;
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;
};
Loading…
Cancel
Save