diff --git a/libraries/GCS_SIMPLE/GCS_SIMPLE.cpp b/libraries/GCS_SIMPLE/GCS_SIMPLE.cpp new file mode 100644 index 0000000000..40b1b144db --- /dev/null +++ b/libraries/GCS_SIMPLE/GCS_SIMPLE.cpp @@ -0,0 +1,90 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega. +// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com +// +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. +// +// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1" +// + +#include "GPS_SIMPLE.h" +#include + +// Public Methods ////////////////////////////////////////////////////////////// +GPS_SIMPLE::GPS_SIMPLE(Stream *s) : _port(s) +{ +} + +// Process bytes available from the stream +// +// The stream is assumed to contain only our custom message. If it +// contains other messages, and those messages contain the preamble bytes, +// it is possible for this code to become de-synchronised. Without +// buffering the entire message and re-processing it from the top, +// this is unavoidable. +// +// The lack of a standard header length field makes it impossible to skip +// unrecognised messages. +// + +bool +GPS_SIMPLE::read(void) +{ +/* + uint8_t data; + int numc; + bool parsed = false; + + numc = _port->available(); + + for (int i = 0; i < numc; i++){ + // Process bytes received + + // read the next byte + data = _port->read(); + + restart: + switch(_step){ + case 0: + if(52 == data){ + _step++; + _payload_counter = 0; + } + break; + + case 1: + if (68 == data) { + _step++; + break; + } + _step = 0; + goto restart; + + case 2: + length = data; + _step++; + break; + + case 3: + id = data; + _step++; + break; + + case 4: + _buffer.bytes[_payload_counter++] = data; + if (_payload_counter == sizeof(_buffer)){ + _step = 0; + parsed = true; + } + break; + } + } + return parsed; + */ + return true; +} + diff --git a/libraries/GCS_SIMPLE/GCS_SIMPLE.h b/libraries/GCS_SIMPLE/GCS_SIMPLE.h new file mode 100644 index 0000000000..a477a68809 --- /dev/null +++ b/libraries/GCS_SIMPLE/GCS_SIMPLE.h @@ -0,0 +1,50 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// DIYDrones Custom Mediatek GPS driver for ArduPilot and ArduPilotMega. +// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com +// +// This library is free software; you can redistribute it and / or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. +// +// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1" +// +// Note - see GPS_SIMPLE16.h for firmware 1.6 and later. +// +#ifndef GPS_SIMPLE_h +#define GPS_SIMPLE_h + +#include // MAVLink GCS definitions + +class GPS_SIMPLE { +public: + GPS_SIMPLE(Stream *s); + virtual bool read(void); + Stream *_port; ///< port the GPS is attached to + +private: +#pragma pack(1) + struct diyd_mtk_msg { + int8_t id; + int8_t p1; + int32_t altitude; + int32_t latitude; + int32_t longitude; + }; +#pragma pack(pop) + + // State machine state + uint8_t _step; + uint8_t _payload_counter; + uint8_t length; + uint8_t id; + + // Receive buffer + union { + diyd_mtk_msg msg; + uint8_t bytes[]; + } _buffer; +}; + +#endif // GPS_SIMPLE_H