diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp index 1effd36754ef2..42616a6af8845 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.cpp +++ b/libraries/AP_GPS/AP_GPS_GSOF.cpp @@ -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(com_port)); - is_baud_configured = requestBaud(static_cast(com_port), baud); + requestBaud(static_cast(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(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(com_port), Output_Rate::FREQ_10_HZ); + gsofmsg_time = now + 110; + gsofmsgreq_index++; + } } 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(portIndex), static_cast(baudRate), 0x00,0x00, // serial port baud format + 0x02, 0x04, static_cast(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(portIndex),static_cast(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 @@ -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(HW_Baud::BAUD230K): - case static_cast(HW_Baud::BAUD115K): - return true; - default: - return false; - } -} bool AP_GPS_GSOF::validate_com_port(const uint8_t com_port) const { switch(com_port) { diff --git a/libraries/AP_GPS/AP_GPS_GSOF.h b/libraries/AP_GPS/AP_GPS_GSOF.h index e44b2137a55c8..a55d91c0a9198 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.h +++ b/libraries/AP_GPS/AP_GPS_GSOF.h @@ -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,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; @@ -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