mirror of https://github.com/ArduPilot/ardupilot
Revert "AP_GPS: GSOF robust config parsing"
This reverts commit bf9881579c.
This commit is contained in:
Peter Barker
Peter Barker
parent
8b7652d1db
commit
2e41cf810c
|
|
@ -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); \
|
||||
|
|
@ -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);
|
||||
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
|
||||
|
|
@ -81,27 +78,19 @@ AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state,
|
|||
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);
|
||||
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++;
|
||||
}
|
||||
if (!gsof_configured) {
|
||||
Debug("Failed to requestGSOF()");
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ret = false;
|
||||
|
|
@ -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
|
||||
|
||||
populateOutgoingTransNumber(buffer);
|
||||
populateChecksum(buffer, sizeof(buffer));
|
||||
buffer[4] = packetcount++;
|
||||
|
||||
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)
|
||||
{
|
||||
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
|
||||
};
|
||||
}; // checksum
|
||||
|
||||
populateOutgoingTransNumber(buffer);
|
||||
populateChecksum(buffer, sizeof(buffer));
|
||||
buffer[4] = packetcount++;
|
||||
|
||||
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;
|
||||
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
|
||||
|
|
@ -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];
|
||||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -57,34 +56,16 @@ private:
|
|||
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.
|
||||
|
||||
// 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.
|
||||
// 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);
|
||||
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;
|
||||
|
|
@ -121,27 +102,12 @@ private:
|
|||
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
|
||||
|
|
|
|||
Loading…
Reference in New Issue