Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ubx: add optional spectrum analyzer support (UBX-MON-SPAN) #115

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
+103 −1
Open

ubx: add optional spectrum analyzer support (UBX-MON-SPAN) #115

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e73cb45
ubx: add optional spectrum analyzer support (UBX-MON-SPAN)
dagar Sep 16, 2022
0728850
Apply suggestions from code review
dagar Oct 25, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 14 additions & 0 deletions src/gps_helper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,14 @@ enum class GPSCallbackType {
*/
gotRelativePositionMessage,

/**
* Got a spectrum message from the device.
* data1: pointer to the message
* data2: message length
* return: ignored
*/
gotSpectrumMessage,

/**
* message about current survey-in status
* data1: points to a SurveyInStatus struct
Expand Down Expand Up @@ -285,6 +293,12 @@ class GPSHelper
_callback(GPSCallbackType::gotRelativePositionMessage, &gnss_relative, sizeof(sensor_gnss_relative_s), _callback_user);
}

/** got a spectrum message from the device */
void gotSpectrumMessage(sensor_gnss_spectrum_s &gnss_spectrum)
{
_callback(GPSCallbackType::gotSpectrumMessage, &gnss_spectrum, sizeof(sensor_gnss_spectrum_s), _callback_user);
}

void setClock(timespec &t)
{
_callback(GPSCallbackType::setClock, &t, 0, _callback_user);
Expand Down
65 changes: 64 additions & 1 deletion src/ubx.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,12 +69,13 @@

GPSDriverUBX::GPSDriverUBX(Interface gpsInterface, GPSCallbackPtr callback, void *callback_user,
sensor_gps_s *gps_position, satellite_info_s *satellite_info, uint8_t dynamic_model,
float heading_offset, int32_t uart2_baudrate, UBXMode mode) :
bool spectrum_analyzer, float heading_offset, int32_t uart2_baudrate, UBXMode mode) :
GPSBaseStationSupport(callback, callback_user),
_interface(gpsInterface),
_gps_position(gps_position),
_satellite_info(satellite_info),
_dyn_model(dynamic_model),
_spectrum_analyzer(spectrum_analyzer),
_mode(mode),
_heading_offset(heading_offset),
_uart2_baudrate(uart2_baudrate)
Expand Down Expand Up @@ -776,6 +777,24 @@ int GPSDriverUBX::configureDevice(const GNSSSystemsMask &gnssSystems, const int3

}

// UBX_MSG_MON_SPAN
{
cfg_valset_msg_size = initCfgValset();

uint8_t value = _spectrum_analyzer ? 1 : 0;
cfgValset<uint8_t>(UBX_CFG_KEY_MSGOUT_UBX_MON_SPAN_UART1, value, cfg_valset_msg_size);

if (!sendMessage(UBX_MSG_CFG_VALSET, (uint8_t *)&_buf, cfg_valset_msg_size)) {
fprintf(stderr, "configuring UBX_MSG_MON_SPAN FAILED\n");
dagar marked this conversation as resolved.
Show resolved Hide resolved
return -1;
}

if (waitForAck(UBX_MSG_CFG_VALSET, UBX_CONFIG_TIMEOUT, true) < 0) {
fprintf(stderr, "configuring UBX_MSG_MON_SPAN FAILED\n");
return -1;
}
}

return 0;
}

Expand Down Expand Up @@ -1373,6 +1392,18 @@ GPSDriverUBX::payloadRxInit()

break;

case UBX_MSG_MON_SPAN:
if ((_rx_payload_length < 4 + sizeof(ubx_payload_rx_mon_span_t::ubx_payload_rx_mon_span_block_t)) ||
(_rx_payload_length - 4) % sizeof(ubx_payload_rx_mon_span_t::ubx_payload_rx_mon_span_block_t) != 0) {

_rx_state = UBX_RXMSG_ERROR_LENGTH;

} else if (!_configured) {
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
}

break;

case UBX_MSG_ACK_ACK:
if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) {
_rx_state = UBX_RXMSG_ERROR_LENGTH;
Expand Down Expand Up @@ -2150,6 +2181,38 @@ GPSDriverUBX::payloadRxDone()
ret = 1;
break;

case UBX_MSG_MON_SPAN:
UBX_TRACE_RXMSG("Rx MON-SPAN");

if (_spectrum_analyzer) {
const uint64_t timestamp_sample = gps_absolute_time(); // TODO: adjust with delay estimate

static constexpr int max_rf_blocks = sizeof(ubx_payload_rx_mon_span_t::block)
/ sizeof(ubx_payload_rx_mon_span_t::ubx_payload_rx_mon_span_block_t);

int rf_blocks = _buf.payload_rx_mon_span.numRfBlocks;

if (rf_blocks > max_rf_blocks) {
rf_blocks = max_rf_blocks;
}

for (int i = 0; i < rf_blocks; i++) {
sensor_gnss_spectrum_s gnss_spectrum{};
gnss_spectrum.timestamp_sample = timestamp_sample;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

static_assert(sizeof(gnss_spectrum.spectrum) == sizeof(_buf.payload_rx_mon_span.block[0].spectrum), "size mismatch");

memcpy(gnss_spectrum.spectrum, _buf.payload_rx_mon_span.block[i].spectrum,
sizeof(ubx_payload_rx_mon_span_t::ubx_payload_rx_mon_span_block_t::spectrum));
gnss_spectrum.spectrum_span_hz = _buf.payload_rx_mon_span.block[i].span;
gnss_spectrum.resolution_hz = _buf.payload_rx_mon_span.block[i].res;
gnss_spectrum.center_frequency_hz = _buf.payload_rx_mon_span.block[i].center;
gnss_spectrum.programmable_gain_amplifier_db = _buf.payload_rx_mon_span.block[i].pga;

gotSpectrumMessage(gnss_spectrum);
}
}

ret = 1;
break;

case UBX_MSG_ACK_ACK:
UBX_TRACE_RXMSG("Rx ACK-ACK");

Expand Down
25 changes: 25 additions & 0 deletions src/ubx.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,6 +109,7 @@
#define UBX_ID_CFG_VALDEL 0x8C
#define UBX_ID_MON_VER 0x04
#define UBX_ID_MON_HW 0x09 // deprecated in protocol version >= 27 -> use MON_RF
#define UBX_ID_MON_SPAN 0x31
#define UBX_ID_MON_RF 0x38

/* UBX ID for RTCM3 output messages */
Expand Down Expand Up @@ -160,6 +161,7 @@
#define UBX_MSG_CFG_VALDEL ((UBX_CLASS_CFG) | UBX_ID_CFG_VALDEL << 8)
#define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
#define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
#define UBX_MSG_MON_SPAN ((UBX_CLASS_MON) | UBX_ID_MON_SPAN << 8)
#define UBX_MSG_MON_RF ((UBX_CLASS_MON) | UBX_ID_MON_RF << 8)
#define UBX_MSG_RTCM3_1005 ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_1005 << 8)
#define UBX_MSG_RTCM3_1077 ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_1077 << 8)
Expand Down Expand Up @@ -353,6 +355,8 @@
#define UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART1 0x2091008e
#define UBX_CFG_KEY_MSGOUT_UBX_NAV_RELPOSNED_UART2 0x2091008f

#define UBX_CFG_KEY_MSGOUT_UBX_MON_SPAN_UART1 0x2091038c

#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_0_UART1 0x209102ff
#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE4072_1_UART1 0x20910382
#define UBX_CFG_KEY_MSGOUT_RTCM_3X_TYPE1077_UART1 0x209102cd
Expand Down Expand Up @@ -661,6 +665,24 @@ typedef struct {
ubx_payload_rx_mon_rf_block_t block[1]; ///< only read out the first block
} ubx_payload_rx_mon_rf_t;

/* Rx MON-SPAN */
typedef struct {
uint8_t version;
uint8_t numRfBlocks; /**< Number of RF blocks included */
uint8_t reserved0[2];

struct ubx_payload_rx_mon_span_block_t {
uint8_t spectrum[256]; /**< dB Spectrum data (number of points = span/res) */
uint32_t span; /**< Hz Spectrum span */
uint32_t res; /**< Hz Resolution of the spectrum */
uint32_t center; /**< Hz Center of spectrum span */
uint8_t pga; /**< dB Programmable gain amplifier */
uint8_t reserved1[3]; /**< Reserved */
};

ubx_payload_rx_mon_span_block_t block[2]; ///< handle up to 2 blocks
} ubx_payload_rx_mon_span_t;

/* Rx MON-VER Part 1 */
typedef struct {
uint8_t swVersion[30];
Expand Down Expand Up @@ -854,6 +876,7 @@ typedef union {
ubx_payload_rx_mon_hw_ubx6_t payload_rx_mon_hw_ubx6;
ubx_payload_rx_mon_hw_ubx7_t payload_rx_mon_hw_ubx7;
ubx_payload_rx_mon_rf_t payload_rx_mon_rf;
ubx_payload_rx_mon_span_t payload_rx_mon_span;
ubx_payload_rx_mon_ver_part1_t payload_rx_mon_ver_part1;
ubx_payload_rx_mon_ver_part2_t payload_rx_mon_ver_part2;
ubx_payload_rx_ack_ack_t payload_rx_ack_ack;
Expand Down Expand Up @@ -920,6 +943,7 @@ class GPSDriverUBX : public GPSBaseStationSupport
GPSDriverUBX(Interface gpsInterface, GPSCallbackPtr callback, void *callback_user,
sensor_gps_s *gps_position, satellite_info_s *satellite_info,
uint8_t dynamic_model = 7,
bool spectrum_analyzer = false,
float heading_offset = 0.f,
int32_t uart2_baudrate = 57600,
UBXMode mode = UBXMode::Normal);
Expand Down Expand Up @@ -1092,6 +1116,7 @@ class GPSDriverUBX : public GPSBaseStationSupport
uint8_t _rx_ck_a{0};
uint8_t _rx_ck_b{0};
uint8_t _dyn_model{7}; ///< ublox Dynamic platform model default 7: airborne with <2g acceleration
bool _spectrum_analyzer{false};
dagar marked this conversation as resolved.
Show resolved Hide resolved

uint16_t _ack_waiting_msg{0};
uint16_t _rx_msg{};
Expand Down