a basic mission uploader

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1794 f9c3cf11-9bcb-44bc-f272-b75c42450872
2011-03-20 21:56:48 +00:00 · 2011-03-20 21:56:48 +00:00 · ed202447e6
parent bb285a248b
commit ed202447e6
2 changed files with 140 additions and 0 deletions
--- a/libraries/GCS_SIMPLE/GCS_SIMPLE.cpp
+++ 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 <stdint.h>
+
+// 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;
+}
+
--- a/libraries/GCS_SIMPLE/GCS_SIMPLE.h
+++ 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 <GCS_MAVLink.h>    // 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