Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Revert "AP_GPS: GSOF robust config parsing" 

This reverts commit bf9881579c.
This commit is contained in:
Peter Barker 2023-11-28 16:02:44 +11:00 committed by Peter Barker
parent 8b7652d1db
commit 2e41cf810c
2 changed files with 40 additions and 202 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

192
libraries/AP_GPS/AP_GPS_GSOF.cpp
View File

@ -36,15 +36,12 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
// Set this to 1 to enable debug messages
#define gsof_DEBUGGING 0 #define gsof_DEBUGGING 0
#if gsof_DEBUGGING #if gsof_DEBUGGING
# define Debug(fmt, args ...) \ # define Debug(fmt, args ...) \
do { \ do { \
hal.console->printf("%u %s:%s:%d: " fmt "\n", \ hal.console->printf("%s:%d: " fmt "\n", \
AP_HAL::millis(), \
__FILE__, \
__FUNCTION__, __LINE__, \ __FUNCTION__, __LINE__, \
## args); \ ## args); \
hal.scheduler->delay(1); \ hal.scheduler->delay(1); \
@ -70,10 +67,10 @@ AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "GSOF instance %d has invalid COM port setting of %d", state.instance, com_port); GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "GSOF instance %d has invalid COM port setting of %d", state.instance, com_port);
return; return;
} }
const HW_Baud baud = HW_Baud::BAUD115K; requestBaud(static_cast<HW_Port>(com_port));
// Start by disabling output during config
[[maybe_unused]] const auto output_disabled = disableOutput(static_cast<HW_Port>(com_port)); const uint32_t now = AP_HAL::millis();
is_baud_configured = requestBaud(static_cast<HW_Port>(com_port), baud); gsofmsg_time = now + 110;
} }
// Process all bytes available from the stream // Process all bytes available from the stream
@ -81,27 +78,19 @@ AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
bool bool
AP_GPS_GSOF::read(void) AP_GPS_GSOF::read(void)
{ {
if (!is_baud_configured) { const uint32_t now = AP_HAL::millis();
// Debug("Baud not configured, not ready to read()");
return false;
}
bool gsof_configured = true; if (gsofmsgreq_index < (sizeof(gsofmsgreq))) {
static_assert(sizeof(gsofmsgreq) != 0, "gsofmsgreq is not empty");
while (gsofmsgreq_index < (sizeof(gsofmsgreq))) {
const auto com_port = gps._com_port[state.instance].get(); const auto com_port = gps._com_port[state.instance].get();
if (!validate_com_port(com_port)) { if (!validate_com_port(com_port)) {
// The user parameter for COM port is not a valid GSOF port // The user parameter for COM port is not a valid GSOF port
return false; return false;
} }
if (now > gsofmsg_time) {
gsof_configured &= requestGSOF(gsofmsgreq[gsofmsgreq_index], static_cast<HW_Port>(com_port), Output_Rate::FREQ_10_HZ); requestGSOF(gsofmsgreq[gsofmsgreq_index], static_cast<HW_Port>(com_port), Output_Rate::FREQ_10_HZ);
gsofmsgreq_index++; gsofmsg_time = now + 110;
} gsofmsgreq_index++;
if (!gsof_configured) { }
Debug("Failed to requestGSOF()");
return false;
} }
bool ret = false; bool ret = false;
@ -172,152 +161,46 @@ AP_GPS_GSOF::parse(const uint8_t temp)
return false; return false;
} }
bool void
AP_GPS_GSOF::requestBaud(const HW_Port portIndex, const HW_Baud baudRate) AP_GPS_GSOF::requestBaud(const HW_Port portindex)
{ {
Debug("Requesting baud on port %u", (uint8_t)portIndex);
// GSOF is supported on the following bauds:
// 2400, 4800, 9600, 19200, 38400, 115200, 230400
// This packet is not documented in the API.
uint8_t buffer[19] = {0x02,0x00,0x64,0x0d,0x00,0x00,0x00, // application file record uint8_t buffer[19] = {0x02,0x00,0x64,0x0d,0x00,0x00,0x00, // application file record
0x03, 0x00, 0x01, 0x00, // file control information block 0x03, 0x00, 0x01, 0x00, // file control information block
0x02, 0x04, static_cast<uint8_t>(portIndex), static_cast<uint8_t>(baudRate), 0x00,0x00, // serial port baud format 0x02, 0x04, static_cast<uint8_t>(portindex), 0x07, 0x00,0x00, // serial port baud format
0x00,0x03 0x00,0x03
}; }; // checksum
populateOutgoingTransNumber(buffer); buffer[4] = packetcount++;
populateChecksum(buffer, sizeof(buffer));
return requestResponse(buffer, sizeof(buffer)); uint8_t checksum = 0;
for (uint8_t a = 1; a < (sizeof(buffer) - 1); a++) {
checksum += buffer[a];
}
buffer[17] = checksum;
port->write((const uint8_t*)buffer, sizeof(buffer));
} }
bool void
AP_GPS_GSOF::requestGSOF(const uint8_t messageType, const HW_Port portIndex, const Output_Rate rateHz) AP_GPS_GSOF::requestGSOF(const uint8_t messageType, const HW_Port portIndex, const Output_Rate rateHz)
{ {
Debug("Requesting gsof #%u on port %u", messageType, (uint8_t)portIndex);
// This packet is not documented in the API.
uint8_t buffer[21] = {0x02,0x00,0x64,0x0f,0x00,0x00,0x00, // application file record uint8_t buffer[21] = {0x02,0x00,0x64,0x0f,0x00,0x00,0x00, // application file record
0x03,0x00,0x01,0x00, // file control information block 0x03,0x00,0x01,0x00, // file control information block
0x07,0x06,0x0a,static_cast<uint8_t>(portIndex),static_cast<uint8_t>(rateHz),0x00,messageType,0x00, // output message record 0x07,0x06,0x0a,static_cast<uint8_t>(portIndex),static_cast<uint8_t>(rateHz),0x00,messageType,0x00, // output message record
0x00,0x03 0x00,0x03
}; }; // checksum
populateOutgoingTransNumber(buffer); buffer[4] = packetcount++;
populateChecksum(buffer, sizeof(buffer));
return requestResponse(buffer, sizeof(buffer));
}
bool
AP_GPS_GSOF::disableOutput(const HW_Port portIndex) {
Debug("Disabling output on on port %u", (uint8_t)portIndex);
switch(portIndex) {
case HW_Port::COM1:
{
constexpr uint8_t cmd[8] = {
STX,
0x00,
0x51,
0x02,
0x0A,
0x01,
0x5E,
ETX
};
// The checksum is hard-coded.
// No need to calculate it.
return requestResponse(cmd, sizeof(cmd));
}
case HW_Port::COM2:
{
constexpr uint8_t cmd[8] = {
STX,
0x00,
0x51,
0x02,
0x0A,
0x02,
0x5F,
ETX
};
return requestResponse(cmd, sizeof(cmd));
}
}
return false;
}
bool
AP_GPS_GSOF::requestResponse(const uint8_t* buf, const uint8_t len) {
if (!port->is_initialized()) {
Debug("Port not initialized");
return false;
}
for (int attempt = 0; attempt <= configuration_attempts; attempt++) {
// Clear input buffer before doing configuration
if (!port->discard_input()) {
Debug("Failed to discard input");
return false;
};
port->write((const uint8_t*)buf, len);
constexpr uint16_t expected_bytes = 1;
// TODO use wait_timeout instead once HAL_SITL has it implemented.
const auto start_wait = AP_HAL::millis();
auto now = start_wait;
while (now - start_wait <= configuration_wait_time_ms) {
if (port->available() >= expected_bytes) {
break;
}
constexpr uint16_t delay_us = 100;
hal.scheduler->delay_microseconds(delay_us);
now = AP_HAL::millis();
}
const auto available_bytes = port->available();
if (available_bytes != expected_bytes) {
Debug("Didn't get expected bytes, got %u bytes back", available_bytes);
return false;
}
uint8_t resp_code;
if(port->read(resp_code) && resp_code == ACK) {
Debug("Got ack");
return true;
} else {
Debug("Didn't get ACK, got 0x%02x", resp_code);
}
}
return false;
}
void
AP_GPS_GSOF::populateChecksum(uint8_t* buf, const uint8_t len)
{
// The buffer is of size len.
// If ETX is not element buf[len-1], the buf/len numbers are wrong.
// Same problem if the len is too small for a valid packet.
if (len <= 3 || buf[len - 1] != ETX) {
return;
}
// The checksum field is at buf[len - 2]
uint8_t checksum = 0; uint8_t checksum = 0;
const uint8_t payload_size = len - 2; for (uint8_t a = 1; a < (sizeof(buffer) - 1); a++) {
for (uint8_t i = 1; i < payload_size; i++) { checksum += buffer[a];
checksum += buf[i];
} }
buf[len -2] = checksum; buffer[19] = checksum;
}
void port->write((const uint8_t*)buffer, sizeof(buffer));
AP_GPS_GSOF::populateOutgoingTransNumber(uint8_t* buf) {
buf[4] = packetOutboundTransNumber++;
} }
double double
@ -376,7 +259,6 @@ AP_GPS_GSOF::process_message(void)
if (msg.packettype == 0x40) { // GSOF if (msg.packettype == 0x40) { // GSOF
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25 // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
#if gsof_DEBUGGING #if gsof_DEBUGGING
//trans_number is functionally the sequence number.
const uint8_t trans_number = msg.data[0]; const uint8_t trans_number = msg.data[0];
const uint8_t pageidx = msg.data[1]; const uint8_t pageidx = msg.data[1];
const uint8_t maxpageidx = msg.data[2]; const uint8_t maxpageidx = msg.data[2];
@ -478,16 +360,6 @@ AP_GPS_GSOF::process_message(void)
return false; return false;
} }
bool
AP_GPS_GSOF::validate_baud(const uint8_t baud) const {
switch(baud) {
case static_cast<uint8_t>(HW_Baud::BAUD230K):
case static_cast<uint8_t>(HW_Baud::BAUD115K):
return true;
default:
return false;
}
}
bool bool
AP_GPS_GSOF::validate_com_port(const uint8_t com_port) const { AP_GPS_GSOF::validate_com_port(const uint8_t com_port) const {
switch(com_port) { switch(com_port) {

50
libraries/AP_GPS/AP_GPS_GSOF.h
View File

@ -16,7 +16,6 @@
// //
// Trimble GPS driver for ArduPilot. // Trimble GPS driver for ArduPilot.
// Code by Michael Oborne // Code by Michael Oborne
// Maintained by Ryan Friedman
// https://receiverhelp.trimble.com/oem-gnss/index.html#Welcome.html?TocPath=_____1 // https://receiverhelp.trimble.com/oem-gnss/index.html#Welcome.html?TocPath=_____1
#pragma once #pragma once
@ -57,35 +56,17 @@ private:
FREQ_100_HZ = 16, FREQ_100_HZ = 16,
}; };
// A subset of the supported baud rates in the GSOF protocol that are useful.
// These values are not documented in the API.
// The matches the GPS_GSOF_BAUD parameter.
enum class HW_Baud {
BAUD115K = 0x07,
BAUD230K = 0x0B,
};
bool parse(const uint8_t temp) WARN_IF_UNUSED; bool parse(const uint8_t temp) WARN_IF_UNUSED;
bool process_message() WARN_IF_UNUSED; bool process_message() WARN_IF_UNUSED;
// Send a request to the GPS to configure its baud rate on a certain (serial) port.
// Note - these request functions expect an ACK from the device.
// If the device is already sending serial traffic, there is no mechanism to prevent this.
// According to the manufacturer, the best approach is to switch to ethernet.
// Use one port for configuration data and another for stream data to prevent conflict.
// Because there is only one TTL serial interface exposed, the approach here is using retry logic.
bool requestBaud(const HW_Port portIndex, const HW_Baud baudRate) WARN_IF_UNUSED;
// Send a request to the GPS to enable a message type on the port.
bool requestGSOF(const uint8_t messageType, const HW_Port portIndex, const Output_Rate rateHz) WARN_IF_UNUSED;
bool disableOutput(const HW_Port portIndex) WARN_IF_UNUSED;
// Generic handler for sending command and checking return code. // Send a request to the GPS to set the baud rate on the specified port.
// Make sure to disableOutput() before doing configuration. // Note - these request functions currently ignore the ACK from the device.
bool requestResponse(const uint8_t* buf, const uint8_t len) WARN_IF_UNUSED; // If the device is already sending serial traffic, there is no mechanism to prevent conflict.
// According to the manufacturer, the best approach is to switch to ethernet.
void requestBaud(const HW_Port portIndex);
// Send a request to the GPS to enable a message type on the port at the specified rate.
void requestGSOF(const uint8_t messageType, const HW_Port portIndex, const Output_Rate rateHz);
static void populateChecksum(uint8_t* buf, const uint8_t len);
void populateOutgoingTransNumber(uint8_t* buf);
double SwapDouble(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED; double SwapDouble(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
float SwapFloat(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED; float SwapFloat(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
uint32_t SwapUint32(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED; uint32_t SwapUint32(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
@ -121,27 +102,12 @@ private:
uint8_t checksumcalc; uint8_t checksumcalc;
} msg; } msg;
static const uint8_t STX = 0x02; static const uint8_t STX = 0x02;
static const uint8_t ETX = 0x03; static const uint8_t ETX = 0x03;
static const uint8_t ACK = 0x06;
static const uint8_t NACK = 0x15;
// How long to wait from sending configuration data for a response. uint8_t packetcount;
// This assumes delay is the same regardless of baud rate.
static const uint8_t configuration_wait_time_ms {5};
// How many attempts to attempt configuration.
// Raising this makes the initialization more immune to data conflicts with streamed data.
// Raising it too high will trigger the watchdog.
// Lowering this makes the driver quicker to return from initialization calls.
static const uint8_t configuration_attempts {3};
// The counter for number of outgoing packets
uint8_t packetOutboundTransNumber;
uint32_t gsofmsg_time; uint32_t gsofmsg_time;
uint8_t gsofmsgreq_index; uint8_t gsofmsgreq_index;
const uint8_t gsofmsgreq[5] = {1,2,8,9,12}; const uint8_t gsofmsgreq[5] = {1,2,8,9,12};
bool is_baud_configured {false};
bool is_configured {false};
}; };
#endif #endif