From 0fceb76493cb4e7ea5bb65176e1fd6d70f179b4c Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Fri, 11 Sep 2015 21:42:58 +0800 Subject: [PATCH] AP_GPS_GSOF: add trimble gsof driver --- libraries/AP_GPS/AP_GPS.cpp | 7 +- libraries/AP_GPS/AP_GPS.h | 2 + libraries/AP_GPS/AP_GPS_GSOF.cpp | 353 ++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_GSOF.h | 92 ++++++++ libraries/AP_GPS/GPS_detect_state.h | 3 +- 5 files changed, 453 insertions(+), 4 deletions(-) create mode 100644 libraries/AP_GPS/AP_GPS_GSOF.cpp create mode 100644 libraries/AP_GPS/AP_GPS_GSOF.h diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 1cf47a9013..cc82142c73 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -145,7 +145,7 @@ const uint32_t AP_GPS::_baudrates[] PROGMEM = {4800U, 38400U, 115200U, 57600U, 9 // initialisation blobs to send to the GPS to try to get it into the // right mode -const prog_char AP_GPS::_initialisation_blob[] PROGMEM = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY ; +const prog_char AP_GPS::_initialisation_blob[] PROGMEM = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY; const prog_char AP_GPS::_initialisation_raw_blob[] PROGMEM = UBLOX_SET_BINARY_RAW_BAUD MTK_SET_BINARY SIRF_SET_BINARY; /* @@ -216,10 +216,13 @@ AP_GPS::detect_instance(uint8_t instance) state[instance].hdop = 9999; #if GPS_RTK_AVAILABLE - // by default the sbf gps outputs no data on its port, until configured. + // by default the sbf/trimble gps outputs no data on its port, until configured. if (_type[instance] == GPS_TYPE_SBF) { hal.console->print_P(PSTR(" SBF ")); new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]); + } else if ((_type[instance] == GPS_TYPE_GSOF)) { + hal.console->print_P(PSTR(" GSOF ")); + new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]); } #endif // GPS_RTK_AVAILABLE diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 318bf2a324..df8b07b0cc 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -90,6 +90,7 @@ public: GPS_TYPE_SBP = 8, GPS_TYPE_PX4 = 9, GPS_TYPE_SBF = 10, + GPS_TYPE_GSOF = 11, }; /// GPS status codes @@ -435,5 +436,6 @@ private: #include "AP_GPS_SBP.h" #include "AP_GPS_PX4.h" #include "AP_GPS_SBF.h" +#include "AP_GPS_GSOF.h" #endif // __AP_GPS_H__ diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp new file mode 100644 index 0000000000..f1f6d3096f --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_GSOF.cpp @@ -0,0 +1,353 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +// +// Trimble GPS driver for ArduPilot. +// Code by Michael Oborne +// + +#include "AP_GPS.h" +#include "AP_GPS_GSOF.h" +#include + +#if GPS_RTK_AVAILABLE + +extern const AP_HAL::HAL& hal; + +#define gsof_DEBUGGING 0 + +#if gsof_DEBUGGING +# define Debug(fmt, args ...) \ +do { \ + hal.console->printf("%s:%d: " fmt "\n", \ + __FUNCTION__, __LINE__, \ + ## args); \ + hal.scheduler->delay(1); \ +} while(0) +#else +# define Debug(fmt, args ...) +#endif + +AP_GPS_GSOF::AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state, + AP_HAL::UARTDriver *_port) : + AP_GPS_Backend(_gps, _state, _port) +{ + gsof_msg.gsof_state = gsof_msg_parser_t::STARTTX; + + requestBaud(); + + uint32_t now = hal.scheduler->millis(); + gsofmsg_time = now + 110; +} + +// Process all bytes available from the stream +// +bool +AP_GPS_GSOF::read(void) +{ + uint32_t now = hal.scheduler->millis(); + + if (gsofmsgreq_index < (sizeof(gsofmsgreq))) { + if (now > gsofmsg_time) { + requestGSOF(gsofmsgreq[gsofmsgreq_index]); + gsofmsg_time = now + 110; + gsofmsgreq_index++; + } + } + + bool ret = false; + while (port->available() > 0) { + uint8_t temp = port->read(); + ret |= parse(temp); + } + + return ret; +} + +bool +AP_GPS_GSOF::parse(uint8_t temp) +{ + switch (gsof_msg.gsof_state) + { + default: + case gsof_msg_parser_t::STARTTX: + if (temp == GSOF_STX) + { + gsof_msg.starttx = temp; + gsof_msg.gsof_state = gsof_msg_parser_t::STATUS; + gsof_msg.read = 0; + gsof_msg.checksumcalc = 0; + } + break; + case gsof_msg_parser_t::STATUS: + gsof_msg.status = temp; + gsof_msg.gsof_state = gsof_msg_parser_t::PACKETTYPE; + gsof_msg.checksumcalc += temp; + break; + case gsof_msg_parser_t::PACKETTYPE: + gsof_msg.packettype = temp; + gsof_msg.gsof_state = gsof_msg_parser_t::LENGTH; + gsof_msg.checksumcalc += temp; + break; + case gsof_msg_parser_t::LENGTH: + gsof_msg.length = temp; + gsof_msg.gsof_state = gsof_msg_parser_t::DATA; + gsof_msg.checksumcalc += temp; + break; + case gsof_msg_parser_t::DATA: + gsof_msg.data[gsof_msg.read] = temp; + gsof_msg.read++; + gsof_msg.checksumcalc += temp; + if (gsof_msg.read >= gsof_msg.length) + { + gsof_msg.gsof_state = gsof_msg_parser_t::CHECKSUM; + } + break; + case gsof_msg_parser_t::CHECKSUM: + gsof_msg.checksum = temp; + gsof_msg.gsof_state = gsof_msg_parser_t::ENDTX; + if (gsof_msg.checksum == gsof_msg.checksumcalc) + { + return process_message(); + } + break; + case gsof_msg_parser_t::ENDTX: + gsof_msg.endtx = temp; + gsof_msg.gsof_state = gsof_msg_parser_t::STARTTX; + break; + } + + return false; +} + +void +AP_GPS_GSOF::requestBaud() +{ + uint8_t buffer[19] = {0x02,0x00,0x64,0x0d,0x00,0x00,0x00, // application file record + 0x03, 0x00, 0x01, 0x00, // file control information block + 0x02, 0x04, 0x00, 0x07, 0x00,0x00, // serial port baud format + 0x00,0x03 + }; // checksum + + buffer[4] = packetcount++; + + 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)); +} + +void +AP_GPS_GSOF::requestGSOF(uint8_t messagetype) +{ + 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,0x00,0x01,0x00,0x01,0x00, // output message record + 0x00,0x03 + }; // checksum + + buffer[4] = packetcount++; + buffer[17] = messagetype; + + uint8_t checksum = 0; + for (uint8_t a = 1; a < (sizeof(buffer) - 1); a++) { + checksum += buffer[a]; + } + + buffer[19] = checksum; + + port->write((const uint8_t*)buffer, sizeof(buffer)); +} + +double +AP_GPS_GSOF::SwapDouble(uint8_t* src, uint32_t pos) +{ + union { + double d; + char bytes[sizeof(double)]; + } doubleu; + doubleu.bytes[0] = src[pos + 7]; + doubleu.bytes[1] = src[pos + 6]; + doubleu.bytes[2] = src[pos + 5]; + doubleu.bytes[3] = src[pos + 4]; + doubleu.bytes[4] = src[pos + 3]; + doubleu.bytes[5] = src[pos + 2]; + doubleu.bytes[6] = src[pos + 1]; + doubleu.bytes[7] = src[pos + 0]; + + return doubleu.d; +} + +float +AP_GPS_GSOF::SwapFloat(uint8_t* src, uint32_t pos) +{ + union { + float f; + char bytes[sizeof(float)]; + } floatu; + floatu.bytes[0] = src[pos + 3]; + floatu.bytes[1] = src[pos + 2]; + floatu.bytes[2] = src[pos + 1]; + floatu.bytes[3] = src[pos + 0]; + + return floatu.f; +} + +uint32_t +AP_GPS_GSOF::SwapUint32(uint8_t* src, uint32_t pos) +{ + union { + uint32_t u; + char bytes[sizeof(uint32_t)]; + } uint32u; + uint32u.bytes[0] = src[pos + 3]; + uint32u.bytes[1] = src[pos + 2]; + uint32u.bytes[2] = src[pos + 1]; + uint32u.bytes[3] = src[pos + 0]; + + return uint32u.u; +} + +uint16_t +AP_GPS_GSOF::SwapUint16(uint8_t* src, uint32_t pos) +{ + union { + uint16_t u; + char bytes[sizeof(uint16_t)]; + } uint16u; + uint16u.bytes[0] = src[pos + 1]; + uint16u.bytes[1] = src[pos + 0]; + + return uint16u.u; +} + +bool +AP_GPS_GSOF::process_message(void) +{ + //http://www.trimble.com/EC_ReceiverHelp/V4.19/en/GSOFmessages_Overview.htm + + if (gsof_msg.packettype == 0x40) { // GSOF + uint8_t trans_number = gsof_msg.data[0]; + uint8_t pageidx = gsof_msg.data[1]; + uint8_t maxpageidx = gsof_msg.data[2]; + + //Debug("GSOF page: " + pageidx + " of " + maxpageidx); + + int valid = 0; + + // want 1 2 8 9 12 + for (uint32_t a = 3; a < gsof_msg.length; a++) + { + uint8_t output_type = gsof_msg.data[a]; + a++; + uint8_t output_length = gsof_msg.data[a]; + a++; + //Debug("GSOF type: " + output_type + " len: " + output_length); + + if (output_type == 1) // pos time + { + state.time_week_ms = SwapUint32(gsof_msg.data, a); + state.time_week = SwapUint16(gsof_msg.data, a + 4); + state.num_sats = gsof_msg.data[a + 6]; + uint8_t posf1 = gsof_msg.data[a + 7]; + uint8_t posf2 = gsof_msg.data[a + 8]; + + //Debug("POSTIME: " + posf1 + " " + posf2); + + if ((posf1 & 1) == 1) + { + state.status = AP_GPS::GPS_OK_FIX_3D; + if ((posf2 & 1) == 1) + { + state.status = AP_GPS::GPS_OK_FIX_3D_DGPS; + if ((posf2 & 4) == 4) + { + state.status = AP_GPS::GPS_OK_FIX_3D_RTK; + } + } + } + else + { + state.status = AP_GPS::NO_FIX; + } + valid++; + } + else if (output_type == 2) // position + { + state.location.lat = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a)) * 1e7); + state.location.lng = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a + 8)) * 1e7); + state.location.alt = (int32_t)(SwapDouble(gsof_msg.data, a + 16) * 1e2); + + state.last_gps_time_ms = state.time_week_ms; + + valid++; + } + else if (output_type == 8) // velocity + { + uint8_t vflag = gsof_msg.data[a]; + if ((vflag & 1) == 1) + { + state.ground_speed = SwapFloat(gsof_msg.data, a + 1); + state.ground_course_cd = (int32_t)(ToDeg(SwapFloat(gsof_msg.data, a + 5)) * 100); + fill_3d_velocity(); + state.velocity.z = -SwapFloat(gsof_msg.data, a + 9); + state.have_vertical_velocity = true; + } + valid++; + } + else if (output_type == 9) //dop + { + state.hdop = (uint16_t)(SwapFloat(gsof_msg.data, a + 4) * 100); + valid++; + } + else if (output_type == 12) // position sigma + { + state.horizontal_accuracy = (SwapFloat(gsof_msg.data, a + 4) + SwapFloat(gsof_msg.data, a + 8)) / 2; + state.vertical_accuracy = SwapFloat(gsof_msg.data, a + 16); + state.have_horizontal_accuracy = true; + state.have_vertical_accuracy = true; + valid++; + } + + a += output_length-1u; + } + + if (valid == 5) { + return true; + } else { + state.status = AP_GPS::NO_FIX; + } + } + + return false; +} + +void +AP_GPS_GSOF::inject_data(uint8_t *data, uint8_t len) +{ + + if (port->txspace() > len) { + last_injected_data_ms = hal.scheduler->millis(); + port->write(data, len); + } else { + Debug("GSOF: Not enough TXSPACE"); + } +} +#endif // GPS_RTK_AVAILABLE diff --git a/libraries/AP_GPS/AP_GPS_GSOF.h b/libraries/AP_GPS/AP_GPS_GSOF.h new file mode 100644 index 0000000000..32e5084fe6 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_GSOF.h @@ -0,0 +1,92 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +// +// Trimble GPS driver for ArduPilot. +// Code by Michael Oborne +// + +#ifndef __AP_GPS_GSOF_H__ +#define __AP_GPS_GSOF_H__ + +#include "AP_GPS.h" + +class AP_GPS_GSOF : public AP_GPS_Backend +{ +public: + AP_GPS_GSOF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); + + AP_GPS::GPS_Status highest_supported_status(void) { + return AP_GPS::GPS_OK_FIX_3D_RTK; + } + + // Methods + bool read(); + + void inject_data(uint8_t *data, uint8_t len); + +private: + + bool parse(uint8_t temp); + bool process_message(); + void requestBaud(); + void requestGSOF(uint8_t messagetype); + double SwapDouble(uint8_t* src, uint32_t pos); + float SwapFloat(uint8_t* src, uint32_t pos); + uint32_t SwapUint32(uint8_t* src, uint32_t pos); + uint16_t SwapUint16(uint8_t* src, uint32_t pos); + + + struct gsof_msg_parser_t + { + enum + { + STARTTX = 0, + STATUS, + PACKETTYPE, + LENGTH, + DATA, + CHECKSUM, + ENDTX + } gsof_state; + + uint8_t starttx; + uint8_t status; + uint8_t packettype; + uint8_t length; + uint8_t data[256]; + uint8_t checksum; + uint8_t endtx; + + uint16_t read; + uint8_t checksumcalc; + } gsof_msg; + + static const uint8_t GSOF_STX = 0x02; + static const uint8_t GSOF_ETX = 0x03; + + uint8_t packetcount = 0; + + uint32_t gsofmsg_time = 0; + uint8_t gsofmsgreq_index = 0; + uint8_t gsofmsgreq[5] = {1,2,8,9,12}; + + uint32_t last_hdop = 9999; + uint32_t crc_error_counter = 0; + uint32_t last_injected_data_ms = 0; +}; + +#endif // __AP_GPS_GSOF_H__ diff --git a/libraries/AP_GPS/GPS_detect_state.h b/libraries/AP_GPS/GPS_detect_state.h index 4678fea52c..47fdc0a017 100644 --- a/libraries/AP_GPS/GPS_detect_state.h +++ b/libraries/AP_GPS/GPS_detect_state.h @@ -67,5 +67,4 @@ struct SBP_detect_state { uint8_t msg_len; uint16_t crc_so_far; uint16_t crc; -}; - +}; \ No newline at end of file