Skip to content

Commit

Permalink
Revert "AP_GPS: GSOF robust config parsing"
Browse files Browse the repository at this point in the history 
This reverts commit bf98815.
  • Loading branch information
@peterbarker
peterbarker committed Nov 28, 2023
1 parent 8b7652d commit 2e41cf8
Show file tree
Hide file tree
Showing 2 changed files with 43 additions and 205 deletions.
192 changes: 32 additions & 160 deletions libraries/AP_GPS/AP_GPS_GSOF.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,15 +36,12 @@

extern const AP_HAL::HAL& hal;

// Set this to 1 to enable debug messages
#define gsof_DEBUGGING 0

#if gsof_DEBUGGING
# define Debug(fmt, args ...) \
do { \
hal.console->printf("%u %s:%s:%d: " fmt "\n", \
AP_HAL::millis(), \
__FILE__, \
hal.console->printf("%s:%d: " fmt "\n", \
__FUNCTION__, __LINE__, \
## args); \
hal.scheduler->delay(1); \
Expand All @@ -70,38 +67,30 @@ 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);
return;
}
const HW_Baud baud = HW_Baud::BAUD115K;
// Start by disabling output during config
[[maybe_unused]] const auto output_disabled = disableOutput(static_cast<HW_Port>(com_port));
is_baud_configured = requestBaud(static_cast<HW_Port>(com_port), baud);
requestBaud(static_cast<HW_Port>(com_port));

const uint32_t now = AP_HAL::millis();
gsofmsg_time = now + 110;
}

// Process all bytes available from the stream
//
bool
AP_GPS_GSOF::read(void)
{
if (!is_baud_configured) {
// Debug("Baud not configured, not ready to read()");
return false;
}
const uint32_t now = AP_HAL::millis();

bool gsof_configured = true;

static_assert(sizeof(gsofmsgreq) != 0, "gsofmsgreq is not empty");
while (gsofmsgreq_index < (sizeof(gsofmsgreq))) {
if (gsofmsgreq_index < (sizeof(gsofmsgreq))) {
const auto com_port = gps._com_port[state.instance].get();
if (!validate_com_port(com_port)) {
// The user parameter for COM port is not a valid GSOF port
return false;
}

gsof_configured &= requestGSOF(gsofmsgreq[gsofmsgreq_index], static_cast<HW_Port>(com_port), Output_Rate::FREQ_10_HZ);
gsofmsgreq_index++;
}
if (!gsof_configured) {
Debug("Failed to requestGSOF()");
return false;
if (now > gsofmsg_time) {
requestGSOF(gsofmsgreq[gsofmsgreq_index], static_cast<HW_Port>(com_port), Output_Rate::FREQ_10_HZ);
gsofmsg_time = now + 110;
gsofmsgreq_index++;
}
}

bool ret = false;
Expand Down Expand Up @@ -172,152 +161,46 @@ AP_GPS_GSOF::parse(const uint8_t temp)
return false;
}

bool
AP_GPS_GSOF::requestBaud(const HW_Port portIndex, const HW_Baud baudRate)
void
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
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
};
}; // checksum

buffer[4] = packetcount++;

populateOutgoingTransNumber(buffer);
populateChecksum(buffer, sizeof(buffer));
uint8_t checksum = 0;
for (uint8_t a = 1; a < (sizeof(buffer) - 1); a++) {
checksum += buffer[a];
}

return requestResponse(buffer, sizeof(buffer));
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)
{
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
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
0x00,0x03
};

populateOutgoingTransNumber(buffer);
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;
}
}; // checksum

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;
}
buffer[4] = packetcount++;

// The checksum field is at buf[len - 2]
uint8_t checksum = 0;
const uint8_t payload_size = len - 2;
for (uint8_t i = 1; i < payload_size; i++) {
checksum += buf[i];
for (uint8_t a = 1; a < (sizeof(buffer) - 1); a++) {
checksum += buffer[a];
}

buf[len -2] = checksum;
}
buffer[19] = checksum;

void
AP_GPS_GSOF::populateOutgoingTransNumber(uint8_t* buf) {
buf[4] = packetOutboundTransNumber++;
port->write((const uint8_t*)buffer, sizeof(buffer));
}

double
Expand Down Expand Up @@ -376,7 +259,6 @@ AP_GPS_GSOF::process_message(void)
if (msg.packettype == 0x40) { // GSOF
// https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
#if gsof_DEBUGGING
//trans_number is functionally the sequence number.
const uint8_t trans_number = msg.data[0];
const uint8_t pageidx = msg.data[1];
const uint8_t maxpageidx = msg.data[2];
Expand Down Expand Up @@ -478,16 +360,6 @@ AP_GPS_GSOF::process_message(void)
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
AP_GPS_GSOF::validate_com_port(const uint8_t com_port) const {
switch(com_port) {
Expand Down
56 changes: 11 additions & 45 deletions libraries/AP_GPS/AP_GPS_GSOF.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
//
// Trimble GPS driver for ArduPilot.
// Code by Michael Oborne
// Maintained by Ryan Friedman
// https://receiverhelp.trimble.com/oem-gnss/index.html#Welcome.html?TocPath=_____1

#pragma once
Expand Down Expand Up @@ -57,35 +56,17 @@ class AP_GPS_GSOF : public AP_GPS_Backend
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 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.
// Make sure to disableOutput() before doing configuration.
bool requestResponse(const uint8_t* buf, const uint8_t len) WARN_IF_UNUSED;

static void populateChecksum(uint8_t* buf, const uint8_t len);
void populateOutgoingTransNumber(uint8_t* buf);


// Send a request to the GPS to set the baud rate on the specified port.
// Note - these request functions currently ignore the ACK from the device.
// 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);

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;
uint32_t SwapUint32(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
Expand Down Expand Up @@ -121,27 +102,12 @@ class AP_GPS_GSOF : public AP_GPS_Backend
uint8_t checksumcalc;
} msg;


static const uint8_t STX = 0x02;
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.
// 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;

uint8_t packetcount;
uint32_t gsofmsg_time;
uint8_t gsofmsgreq_index;
const uint8_t gsofmsgreq[5] = {1,2,8,9,12};
bool is_baud_configured {false};
bool is_configured {false};
};
#endif

0 comments on commit 2e41cf8

Please sign in to comment.