From 9ed023aeb44e947766a6a484b0b9f16d995d1c6f Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Wed, 26 Sep 2012 22:18:44 -0700 Subject: [PATCH] AP_GPS: Builds under AP_HAL. Not tested. --- libraries/AP_GPS/AP_GPS_406.cpp | 27 +++++---- libraries/AP_GPS/AP_GPS_406.h | 10 ++-- libraries/AP_GPS/AP_GPS_Auto.cpp | 37 ++++++------ libraries/AP_GPS/AP_GPS_Auto.h | 13 ++--- libraries/AP_GPS/AP_GPS_HIL.cpp | 8 +-- libraries/AP_GPS/AP_GPS_HIL.h | 9 +-- libraries/AP_GPS/AP_GPS_IMU.cpp | 12 ++-- libraries/AP_GPS/AP_GPS_IMU.h | 9 +-- libraries/AP_GPS/AP_GPS_MTK.cpp | 7 ++- libraries/AP_GPS/AP_GPS_MTK.h | 8 +-- libraries/AP_GPS/AP_GPS_MTK16.cpp | 36 +++++------- libraries/AP_GPS/AP_GPS_MTK16.h | 9 +-- libraries/AP_GPS/AP_GPS_MTK_Common.h | 6 +- libraries/AP_GPS/AP_GPS_NMEA.cpp | 12 ++-- libraries/AP_GPS/AP_GPS_NMEA.h | 9 +-- libraries/AP_GPS/AP_GPS_None.h | 9 +-- libraries/AP_GPS/AP_GPS_SIRF.cpp | 2 +- libraries/AP_GPS/AP_GPS_SIRF.h | 7 ++- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 20 ++++--- libraries/AP_GPS/AP_GPS_UBLOX.h | 9 +-- libraries/AP_GPS/GPS.cpp | 33 +++++------ libraries/AP_GPS/GPS.h | 19 +++--- .../AP_GPS/examples/GPS_AUTO_test/Arduino.h | 0 .../examples/GPS_AUTO_test/GPS_AUTO_test.pde | 31 +++++----- .../AP_GPS/examples/GPS_AUTO_test/Makefile | 1 - .../examples/GPS_AUTO_test/nocore.inoflag | 0 .../AP_GPS/examples/GPS_MTK_test/Arduino.h | 0 .../examples/GPS_MTK_test/GPS_MTK_test.pde | 56 +++++++++--------- .../examples/GPS_MTK_test/nocore.inoflag | 0 .../AP_GPS/examples/GPS_UBLOX_test/Arduino.h | 0 .../GPS_UBLOX_test/GPS_UBLOX_test.pde | 58 +++++++++---------- .../examples/GPS_UBLOX_test/nocore.inoflag | 0 32 files changed, 229 insertions(+), 228 deletions(-) create mode 100644 libraries/AP_GPS/examples/GPS_AUTO_test/Arduino.h create mode 100644 libraries/AP_GPS/examples/GPS_AUTO_test/nocore.inoflag create mode 100644 libraries/AP_GPS/examples/GPS_MTK_test/Arduino.h create mode 100644 libraries/AP_GPS/examples/GPS_MTK_test/nocore.inoflag create mode 100644 libraries/AP_GPS/examples/GPS_UBLOX_test/Arduino.h create mode 100644 libraries/AP_GPS/examples/GPS_UBLOX_test/nocore.inoflag diff --git a/libraries/AP_GPS/AP_GPS_406.cpp b/libraries/AP_GPS/AP_GPS_406.cpp index 61e3e3f282..d46a5b4a86 100644 --- a/libraries/AP_GPS/AP_GPS_406.cpp +++ b/libraries/AP_GPS/AP_GPS_406.cpp @@ -9,18 +9,16 @@ // License as published by the Free Software Foundation; either // version 2.1 of the License, or (at your option) any later version. -#include "../FastSerial/FastSerial.h" // because we need to change baud rates... ugh. +#include + #include "AP_GPS_406.h" -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif + +extern const AP_HAL::HAL& hal; static const char init_str[] = "$PSRF100,0,57600,8,1,0*37"; // Constructors //////////////////////////////////////////////////////////////// -AP_GPS_406::AP_GPS_406(Stream *s) : AP_GPS_SIRF(s) +AP_GPS_406::AP_GPS_406(AP_HAL::UARTDriver *s) : AP_GPS_SIRF(s) { } @@ -67,21 +65,22 @@ AP_GPS_406::_configure_gps(void) void AP_GPS_406::_change_to_sirf_protocol(void) { - FastSerial *fs = (FastSerial *)_port; // this is a bit grody... + // this is a bit grody... + AP_HAL::UARTDriver *fs = (AP_HAL::UARTDriver*)_port; fs->begin(4800); - delay(300); + hal.scheduler->delay(300); _port->print(init_str); - delay(300); + hal.scheduler->delay(300); fs->begin(9600); - delay(300); + hal.scheduler->delay(300); _port->print(init_str); - delay(300); + hal.scheduler->delay(300); fs->begin(GPS_406_BITRATE); - delay(300); + hal.scheduler->delay(300); _port->print(init_str); - delay(300); + hal.scheduler->delay(300); } diff --git a/libraries/AP_GPS/AP_GPS_406.h b/libraries/AP_GPS/AP_GPS_406.h index 99fd72987b..a01e5619ca 100644 --- a/libraries/AP_GPS/AP_GPS_406.h +++ b/libraries/AP_GPS/AP_GPS_406.h @@ -9,8 +9,10 @@ // version 2.1 of the License, or (at your option) any later version. // -#ifndef AP_GPS_406_h -#define AP_GPS_406_h +#ifndef __AP_GPS_406_H__ +#define __AP_GPS_406_H__ + +#include #include "AP_GPS_SIRF.h" @@ -20,7 +22,7 @@ class AP_GPS_406 : public AP_GPS_SIRF { public: // Methods - AP_GPS_406(Stream *port); + AP_GPS_406(AP_HAL::UARTDriver *port); virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE); private: @@ -28,5 +30,5 @@ private: void _configure_gps(void); }; -#endif +#endif // __AP_HAL_GPS_406_H__ diff --git a/libraries/AP_GPS/AP_GPS_Auto.cpp b/libraries/AP_GPS/AP_GPS_Auto.cpp index b8be5d2eb5..60bc6f6eed 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.cpp +++ b/libraries/AP_GPS/AP_GPS_Auto.cpp @@ -3,20 +3,21 @@ /// @file AP_GPS_Auto.cpp /// @brief Simple GPS auto-detection logic. -#include +#include #include #include "AP_GPS.h" // includes AP_GPS_Auto.h +extern const AP_HAL::HAL& hal; + static const uint32_t baudrates[] PROGMEM = {38400U, 57600U, 9600U, 4800U}; const prog_char AP_GPS_Auto::_mtk_set_binary[] PROGMEM = MTK_SET_BINARY; const prog_char AP_GPS_Auto::_sirf_set_binary[] PROGMEM = SIRF_SET_BINARY; -AP_GPS_Auto::AP_GPS_Auto(FastSerial *s, GPS **gps) : - GPS(s), - _fs(s), +AP_GPS_Auto::AP_GPS_Auto(AP_HAL::UARTDriver *u, GPS **gps) : + GPS(u), _gps(gps) { } @@ -42,21 +43,23 @@ AP_GPS_Auto::read(void) static uint32_t last_baud_change_ms; static uint8_t last_baud; GPS *gps; - uint32_t now = millis(); + uint32_t now = hal.scheduler->millis(); if (now - last_baud_change_ms > 1200) { // its been more than 1.2 seconds without detection on this // GPS - switch to another baud rate - _fs->begin(pgm_read_dword(&baudrates[last_baud]), 256, 16); + uint32_t newbaud = pgm_read_dword(&baudrates[last_baud]); + hal.console->printf_P(PSTR("gps set baud %ld\r\n"), newbaud); + _port->begin(newbaud, 256, 16); last_baud++; last_baud_change_ms = now; if (last_baud == sizeof(baudrates) / sizeof(baudrates[0])) { last_baud = 0; } // write config strings for the types of GPS we support - _send_progstr(_fs, _mtk_set_binary, sizeof(_mtk_set_binary)); - _send_progstr(_fs, AP_GPS_UBLOX::_ublox_set_binary, AP_GPS_UBLOX::_ublox_set_binary_size); - _send_progstr(_fs, _sirf_set_binary, sizeof(_sirf_set_binary)); + _send_progstr(_port, _mtk_set_binary, sizeof(_mtk_set_binary)); + _send_progstr(_port, AP_GPS_UBLOX::_ublox_set_binary, AP_GPS_UBLOX::_ublox_set_binary_size); + _send_progstr(_port, _sirf_set_binary, sizeof(_sirf_set_binary)); } _update_progstr(); @@ -64,7 +67,7 @@ AP_GPS_Auto::read(void) if (NULL != (gps = _detect())) { // configure the detected GPS gps->init(_nav_setting); - Serial.println_P(PSTR("OK")); + hal.console->println_P(PSTR("gps OK")); *_gps = gps; return true; } @@ -80,34 +83,34 @@ AP_GPS_Auto::_detect(void) static uint32_t detect_started_ms; if (detect_started_ms == 0 && _port->available() > 0) { - detect_started_ms = millis(); + detect_started_ms = hal.scheduler->millis(); } while (_port->available() > 0) { uint8_t data = _port->read(); if (AP_GPS_UBLOX::_detect(data)) { - Serial.print_P(PSTR(" ublox ")); + hal.console->println_P(PSTR("gps ublox")); return new AP_GPS_UBLOX(_port); } if (AP_GPS_MTK16::_detect(data)) { - Serial.print_P(PSTR(" MTK16 ")); + hal.console->println_P(PSTR("gps MTK 1.6")); return new AP_GPS_MTK16(_port); } if (AP_GPS_MTK::_detect(data)) { - Serial.print_P(PSTR(" MTK ")); + hal.console->println_P(PSTR("gps MTK 1.0")); return new AP_GPS_MTK(_port); } #if !defined( __AVR_ATmega1280__ ) // save a bit of code space on a 1280 if (AP_GPS_SIRF::_detect(data)) { - Serial.print_P(PSTR(" SIRF ")); + hal.console->println_P(PSTR("gps SIRF")); return new AP_GPS_SIRF(_port); } - if (millis() - detect_started_ms > 5000) { + if (hal.scheduler->millis() - detect_started_ms > 5000) { // prevent false detection of NMEA mode in // a MTK or UBLOX which has booted in NMEA mode if (AP_GPS_NMEA::_detect(data)) { - Serial.print_P(PSTR(" NMEA ")); + hal.console->println_P(PSTR("gps NMEA")); return new AP_GPS_NMEA(_port); } } diff --git a/libraries/AP_GPS/AP_GPS_Auto.h b/libraries/AP_GPS/AP_GPS_Auto.h index 95b922a373..055263d800 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.h +++ b/libraries/AP_GPS/AP_GPS_Auto.h @@ -3,10 +3,10 @@ // Auto-detecting pseudo-GPS driver // -#ifndef AP_GPS_Auto_h -#define AP_GPS_Auto_h +#ifndef __AP_GPS_AUTO_H__ +#define __AP_GPS_AUTO_H__ -#include "../FastSerial/FastSerial.h" +#include #include "GPS.h" class AP_GPS_Auto : public GPS @@ -17,11 +17,11 @@ public: /// @note The stream is expected to be set up and configured for the /// correct bitrate before ::init is called. /// - /// @param port Stream connected to the GPS module. + /// @param port UARTDriver connected to the GPS module. /// @param ptr Pointer to a GPS * that will be fixed up by ::init /// when the GPS type has been detected. /// - AP_GPS_Auto(FastSerial *s, GPS **gps); + AP_GPS_Auto(AP_HAL::UARTDriver *s, GPS **gps); /// Dummy init routine, does nothing virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE); @@ -32,9 +32,6 @@ public: virtual bool read(void); private: - /// Copy of the port, known at construction time to be a real FastSerial port. - FastSerial * _fs; - /// global GPS driver pointer, updated by auto-detection /// GPS ** _gps; diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp index 0435e20697..71f02f0ee8 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.cpp +++ b/libraries/AP_GPS/AP_GPS_HIL.cpp @@ -11,15 +11,11 @@ // GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1" // +#include #include "AP_GPS_HIL.h" -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif // Constructors //////////////////////////////////////////////////////////////// -AP_GPS_HIL::AP_GPS_HIL(Stream *s) : GPS(s) +AP_GPS_HIL::AP_GPS_HIL(AP_HAL::UARTDriver* s) : GPS(s) { } diff --git a/libraries/AP_GPS/AP_GPS_HIL.h b/libraries/AP_GPS/AP_GPS_HIL.h index a396bb5c27..e0ea0f3258 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.h +++ b/libraries/AP_GPS/AP_GPS_HIL.h @@ -9,14 +9,15 @@ // version 2.1 of the License, or (at your option) any later version. // // -#ifndef AP_GPS_HIL_h -#define AP_GPS_HIL_h +#ifndef __AP_GPS_HIL_H__ +#define __AP_GPS_HIL_H__ +#include #include "GPS.h" class AP_GPS_HIL : public GPS { public: - AP_GPS_HIL(Stream *s); + AP_GPS_HIL(AP_HAL::UARTDriver *s); virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE); virtual bool read(void); @@ -37,4 +38,4 @@ private: bool _updated; }; -#endif // AP_GPS_HIL_H +#endif // __AP_GPS_HIL_H__ diff --git a/libraries/AP_GPS/AP_GPS_IMU.cpp b/libraries/AP_GPS/AP_GPS_IMU.cpp index 5cb5d69f46..32fbfd1fb9 100644 --- a/libraries/AP_GPS/AP_GPS_IMU.cpp +++ b/libraries/AP_GPS/AP_GPS_IMU.cpp @@ -27,16 +27,12 @@ * fix : 1: GPS NO fix, 2: 2D fix, 3: 3D fix. * */ -#include "AP_GPS_IMU.h" -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif +#include +#include "AP_GPS_IMU.h" // Constructors //////////////////////////////////////////////////////////////// -AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s) +AP_GPS_IMU::AP_GPS_IMU(AP_HAL::UARTDriver *s) : GPS(s) { } @@ -55,7 +51,7 @@ AP_GPS_IMU::init(enum GPS_Engine_Setting nav_setting) bool AP_GPS_IMU::read(void) { - byte data; + uint8_t data; int16_t numc = 0; numc = _port->available(); diff --git a/libraries/AP_GPS/AP_GPS_IMU.h b/libraries/AP_GPS/AP_GPS_IMU.h index 73c755812a..5c9faa5a5e 100644 --- a/libraries/AP_GPS/AP_GPS_IMU.h +++ b/libraries/AP_GPS/AP_GPS_IMU.h @@ -1,8 +1,9 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -#ifndef AP_GPS_IMU_h -#define AP_GPS_IMU_h +#ifndef __AP_GPS_IMU_H__ +#define __AP_GPS_IMU_H__ +#include #include "GPS.h" #define MAXPAYLOAD 32 @@ -10,7 +11,7 @@ class AP_GPS_IMU : public GPS { public: // Methods - AP_GPS_IMU(Stream *s); + AP_GPS_IMU(AP_HAL::UARTDriver *s); virtual void init(enum GPS_Engine_Setting nav_setting); virtual bool read(void); @@ -45,4 +46,4 @@ private: void checksum(unsigned char data); }; -#endif +#endif // __AP_GPS_IMU_H__ diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index 546426e8f5..f2aae3e2fc 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -11,11 +11,14 @@ // GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1" // -#include "AP_GPS_MTK.h" #include +#include + +#include "AP_GPS_MTK.h" + // Constructors //////////////////////////////////////////////////////////////// -AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s) +AP_GPS_MTK::AP_GPS_MTK(AP_HAL::UARTDriver *s) : GPS(s) { } diff --git a/libraries/AP_GPS/AP_GPS_MTK.h b/libraries/AP_GPS/AP_GPS_MTK.h index 0642b34c07..44a52b10ed 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.h +++ b/libraries/AP_GPS/AP_GPS_MTK.h @@ -12,15 +12,15 @@ // // Note - see AP_GPS_MTK16.h for firmware 1.6 and later. // -#ifndef AP_GPS_MTK_h -#define AP_GPS_MTK_h +#ifndef __AP_GPS_MTK_H__ +#define __AP_GPS_MTK_H__ #include "GPS.h" #include "AP_GPS_MTK_Common.h" class AP_GPS_MTK : public GPS { public: - AP_GPS_MTK(Stream *s); + AP_GPS_MTK(AP_HAL::UARTDriver *s); virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE); virtual bool read(void); static bool _detect(uint8_t ); @@ -71,4 +71,4 @@ private: void _parse_gps(); }; -#endif // AP_GPS_MTK_H +#endif // __AP_GPS_MTK_H__ diff --git a/libraries/AP_GPS/AP_GPS_MTK16.cpp b/libraries/AP_GPS/AP_GPS_MTK16.cpp index 027b0408b3..2fdcdeec03 100644 --- a/libraries/AP_GPS/AP_GPS_MTK16.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK16.cpp @@ -10,18 +10,14 @@ // // GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1" // - -#include +#include #include "AP_GPS_MTK16.h" #include -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include -#endif + +extern const AP_HAL::HAL& hal; // Constructors //////////////////////////////////////////////////////////////// -AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s) +AP_GPS_MTK16::AP_GPS_MTK16(AP_HAL::UARTDriver *s) : GPS(s) { } @@ -166,7 +162,7 @@ restart: } #endif - /* Waiting on clarification of MAVLink protocol! + /* Waiting on clarification of MAVLink protocol! * if(!_offset_calculated && parsed) { * int32_t tempd1 = date; * int32_t day = tempd1/10000; @@ -191,11 +187,11 @@ restart: bool AP_GPS_MTK16::_detect(uint8_t data) { - static uint8_t payload_counter; - static uint8_t step; - static uint8_t ck_a, ck_b; + static uint8_t payload_counter; + static uint8_t step; + static uint8_t ck_a, ck_b; - switch (step) { + switch (step) { case 1: if (PREAMBLE2 == data) { step++; @@ -203,10 +199,10 @@ AP_GPS_MTK16::_detect(uint8_t data) } step = 0; case 0: - ck_b = ck_a = payload_counter = 0; + ck_b = ck_a = payload_counter = 0; if (PREAMBLE1 == data) step++; - break; + break; case 2: if (data == sizeof(struct diyd_mtk_msg)) { step++; @@ -223,17 +219,17 @@ AP_GPS_MTK16::_detect(uint8_t data) case 4: step++; if (ck_a != data) { - Serial.printf("wrong ck_a\n"); + hal.console->println_P(PSTR("wrong ck_a\n")); step = 0; } break; case 5: step = 0; if (ck_b == data) { - return true; + return true; } - Serial.printf("wrong ck_b\n"); - break; - } + hal.console->println_P(PSTR("wrong ck_b")); + break; + } return false; } diff --git a/libraries/AP_GPS/AP_GPS_MTK16.h b/libraries/AP_GPS/AP_GPS_MTK16.h index 9a6b119170..e3510d76ca 100644 --- a/libraries/AP_GPS/AP_GPS_MTK16.h +++ b/libraries/AP_GPS/AP_GPS_MTK16.h @@ -10,15 +10,16 @@ // // GPS configuration : Custom protocol per "Customize Function Specification, 3D Robotics, v1.6" // -#ifndef AP_GPS_MTK16_h -#define AP_GPS_MTK16_h +#ifndef __AP_GPS_MTK16_H__ +#define __AP_GPS_MTK16_H__ +#include #include "GPS.h" #include "AP_GPS_MTK_Common.h" class AP_GPS_MTK16 : public GPS { public: - AP_GPS_MTK16(Stream *s); + AP_GPS_MTK16(AP_HAL::UARTDriver *s); virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE); virtual bool read(void); static bool _detect(uint8_t ); @@ -69,4 +70,4 @@ private: } _buffer; }; -#endif // AP_GPS_MTK16_H +#endif // __AP_GPS_MTK16_H__ diff --git a/libraries/AP_GPS/AP_GPS_MTK_Common.h b/libraries/AP_GPS/AP_GPS_MTK_Common.h index e08ed98b82..fedaa94a29 100644 --- a/libraries/AP_GPS/AP_GPS_MTK_Common.h +++ b/libraries/AP_GPS/AP_GPS_MTK_Common.h @@ -10,8 +10,8 @@ // // Common definitions for MediaTek GPS modules. -#ifndef AP_GPS_MTK_Common_h -#define AP_GPS_MTK_Common_h +#ifndef __AP_GPS_MTK_COMMON_H__ +#define __AP_GPS_MTK_COMMON_H__ #define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n" #define MTK_SET_NMEA "$PGCMD,16,1,1,1,1,1*6B\r\n" @@ -32,4 +32,4 @@ #define WAAS_ON "$PMTK301,2*2E\r\n" #define WAAS_OFF "$PMTK301,0*2C\r\n" -#endif // AP_GPS_MTK_Common_h +#endif // __AP_GPS_MTK_COMMON_H__ diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 5a8ca4e330..f6d603260b 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -25,12 +25,12 @@ /// TinyGPS parser by Mikal Hart. /// -#include #include #include #include #include +#include #include "AP_GPS_NMEA.h" @@ -91,7 +91,7 @@ const char AP_GPS_NMEA::_gpvtg_string[] PROGMEM = "GPVTG"; #define DIGIT_TO_VAL(_x) (_x - '0') // Constructors //////////////////////////////////////////////////////////////// -AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : +AP_GPS_NMEA::AP_GPS_NMEA(AP_HAL::UARTDriver *s) : GPS(s) { } @@ -99,16 +99,14 @@ AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : // Public Methods ////////////////////////////////////////////////////////////// void AP_GPS_NMEA::init(enum GPS_Engine_Setting nav_setting) { - BetterStream *bs = (BetterStream *)_port; - // send the SiRF init strings - bs->print_P((const prog_char_t *)_SiRF_init_string); + _port->print_P((const prog_char_t *)_SiRF_init_string); // send the MediaTek init strings - bs->print_P((const prog_char_t *)_MTK_init_string); + _port->print_P((const prog_char_t *)_MTK_init_string); // send the ublox init strings - bs->print_P((const prog_char_t *)_ublox_init_string); + _port->print_P((const prog_char_t *)_ublox_init_string); idleTimeout = 1200; } diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index 6f7d5654ba..634d797f14 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -39,9 +39,10 @@ /// -#ifndef AP_GPS_NMEA_h -#define AP_GPS_NMEA_h +#ifndef __AP_GPS_NMEA_H__ +#define __AP_GPS_NMEA_H__ +#include #include "GPS.h" #include @@ -53,7 +54,7 @@ class AP_GPS_NMEA : public GPS public: /// Constructor /// - AP_GPS_NMEA(Stream *s); + AP_GPS_NMEA(AP_HAL::UARTDriver* s); /// Perform a (re)initialisation of the GPS; sends the /// protocol configuration messages. @@ -159,4 +160,4 @@ private: //@} }; -#endif +#endif // __AP_GPS_NMEA_H__ diff --git a/libraries/AP_GPS/AP_GPS_None.h b/libraries/AP_GPS/AP_GPS_None.h index bce51a163a..1fbcb0956e 100644 --- a/libraries/AP_GPS/AP_GPS_None.h +++ b/libraries/AP_GPS/AP_GPS_None.h @@ -1,14 +1,15 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -#ifndef AP_GPS_None_h -#define AP_GPS_None_h +#ifndef __AP_GPS_NONE_H__ +#define __AP_GPS_NONE_H__ +#include #include "GPS.h" class AP_GPS_None : public GPS { public: - AP_GPS_None(Stream *s) : GPS(s) { + AP_GPS_None(AP_HAL::UARTDriver* s) : GPS(s) { } virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE) { }; @@ -16,4 +17,4 @@ public: return false; }; }; -#endif +#endif // __AP_GPS_NONE_H__ diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index d11aab44b8..76b64d86b9 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -24,7 +24,7 @@ static uint8_t init_messages[] = { }; // Constructors //////////////////////////////////////////////////////////////// -AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s) +AP_GPS_SIRF::AP_GPS_SIRF(AP_HAL::UARTDriver *s) : GPS(s) { } diff --git a/libraries/AP_GPS/AP_GPS_SIRF.h b/libraries/AP_GPS/AP_GPS_SIRF.h index aa8d13b6fd..5881f73301 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.h +++ b/libraries/AP_GPS/AP_GPS_SIRF.h @@ -8,16 +8,17 @@ // License as published by the Free Software Foundation; either // version 2.1 of the License, or (at your option) any later version. // -#ifndef AP_GPS_SIRF_h -#define AP_GPS_SIRF_h +#ifndef __AP_GPS_SIRF_H__ +#define __AP_GPS_SIRF_H__ +#include #include "GPS.h" #define SIRF_SET_BINARY "$PSRF100,0,38400,8,1,0*3C" class AP_GPS_SIRF : public GPS { public: - AP_GPS_SIRF(Stream *s); + AP_GPS_SIRF(AP_HAL::UARTDriver *s); virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE); virtual bool read(); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 90f258409d..9d593a3722 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -8,10 +8,12 @@ // License as published by the Free Software Foundation; either // version 2.1 of the License, or (at your option) any later version. // +#include + +#include #define UBLOX_DEBUGGING 0 -#include #if UBLOX_DEBUGGING # define Debug(fmt, args ...) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); delay(0); } while(0) @@ -20,14 +22,15 @@ #endif #include "AP_GPS_UBLOX.h" -#include + +extern const AP_HAL::HAL& hal; const prog_char AP_GPS_UBLOX::_ublox_set_binary[] PROGMEM = UBLOX_SET_BINARY; const uint8_t AP_GPS_UBLOX::_ublox_set_binary_size = sizeof(AP_GPS_UBLOX::_ublox_set_binary); // Constructors //////////////////////////////////////////////////////////////// -AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s) +AP_GPS_UBLOX::AP_GPS_UBLOX(AP_HAL::UARTDriver *s) : GPS(s) { } @@ -338,16 +341,17 @@ AP_GPS_UBLOX::_configure_gps(void) { struct ubx_cfg_nav_rate msg; const unsigned baudrates[4] = {9600U, 19200U, 38400U, 57600U}; - FastSerial *_fs = (FastSerial *)_port; // the GPS may be setup for a different baud rate. This ensures // it gets configured correctly for (uint8_t i=0; i<4; i++) { - _fs->begin(baudrates[i]); - _write_progstr_block(_fs, _ublox_set_binary, _ublox_set_binary_size); - while (_fs->tx_pending()) delay(1); + _port->begin(baudrates[i]); + _write_progstr_block(_port, _ublox_set_binary, _ublox_set_binary_size); + while (_port->tx_pending()) { + hal.scheduler->delay(1); + } } - _fs->begin(38400U); + _port->begin(38400U); // ask for navigation solutions every 200ms msg.measure_rate_ms = 200; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 824e05f7a5..7abfb37ea2 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -8,9 +8,10 @@ // License as published by the Free Software Foundation; either // version 2.1 of the License, or (at your option) any later version. // -#ifndef AP_GPS_UBLOX_h -#define AP_GPS_UBLOX_h +#ifndef __AP_GPS_UBLOX_H__ +#define __AP_GPS_UBLOX_H__ +#include #include "GPS.h" /* @@ -27,7 +28,7 @@ class AP_GPS_UBLOX : public GPS { public: // Methods - AP_GPS_UBLOX(Stream *s); + AP_GPS_UBLOX(AP_HAL::UARTDriver *s); virtual void init(enum GPS_Engine_Setting nav_setting = GPS_ENGINE_NONE); virtual bool read(); static bool _detect(uint8_t ); @@ -201,4 +202,4 @@ private: }; -#endif +#endif // __AP_GPS_UBLOX_H__ diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index 3aa9560122..7ba9b7fa8c 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -1,25 +1,22 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -#include + + +#include +#include +#include +#include "GPS.h" + +extern const AP_HAL::HAL& hal; #define GPS_DEBUGGING 0 #if GPS_DEBUGGING - #include - # define Debug(fmt, args ...) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); delay(0); } while(0) + # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(0); } while(0) #else # define Debug(fmt, args ...) #endif -#include -#include -#include "GPS.h" -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif - void GPS::update(void) { @@ -29,7 +26,7 @@ GPS::update(void) // call the GPS driver to process incoming data result = read(); - tnow = millis(); + tnow = hal.scheduler->millis(); // if we did not get a message, and the idle timer has expired, re-init if (!result) { @@ -88,13 +85,13 @@ GPS::setHIL(uint32_t _time, float _latitude, float _longitude, float _altitude, void GPS::_error(const char *msg) { - Serial.println(msg); + hal.console->println(msg); } /// /// write a block of configuration data to a GPS /// -void GPS::_write_progstr_block(Stream *_fs, const prog_char *pstr, uint8_t size) +void GPS::_write_progstr_block(AP_HAL::UARTDriver *_fs, const prog_char *pstr, uint8_t size) { while (size--) { _fs->write(pgm_read_byte(pstr++)); @@ -117,15 +114,15 @@ struct progstr_queue { }; static struct { - FastSerial *fs; + AP_HAL::UARTDriver *fs; uint8_t queue_size; uint8_t idx, next_idx; struct progstr_queue queue[PROGSTR_QUEUE_SIZE]; } progstr_state; -void GPS::_send_progstr(Stream *_fs, const prog_char *pstr, uint8_t size) +void GPS::_send_progstr(AP_HAL::UARTDriver *_fs, const prog_char *pstr, uint8_t size) { - progstr_state.fs = (FastSerial *)_fs; + progstr_state.fs = _fs; struct progstr_queue *q = &progstr_state.queue[progstr_state.next_idx]; q->pstr = pstr; q->size = size; diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index f531b61ac3..b881bdbfe1 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -3,11 +3,12 @@ /// @file GPS.h /// @brief Interface definition for the various GPS drivers. -#ifndef GPS_h -#define GPS_h +#ifndef __GPS_H__ +#define __GPS_H__ + +#include #include -#include #include /// @class GPS @@ -150,16 +151,16 @@ public: protected: - Stream *_port; ///< port the GPS is attached to + AP_HAL::UARTDriver *_port; ///< port the GPS is attached to /// Constructor /// /// @note The stream is expected to be set up and configured for the /// correct bitrate before ::init is called. /// - /// @param s Stream connected to the GPS module. + /// @param u UARTDriver connected to the GPS module. /// - GPS(Stream *s) : _port(s) { + GPS(AP_HAL::UARTDriver *u) : _port(u) { }; /// read from the GPS stream and update properties @@ -202,8 +203,8 @@ protected: enum GPS_Engine_Setting _nav_setting; - void _write_progstr_block(Stream *_fs, const prog_char *pstr, uint8_t size); - void _send_progstr(Stream *_fs, const prog_char *pstr, uint8_t size); + void _write_progstr_block(AP_HAL::UARTDriver *_fs, const prog_char *pstr, uint8_t size); + void _send_progstr(AP_HAL::UARTDriver *_fs, const prog_char *pstr, uint8_t size); void _update_progstr(void); // velocities in cm/s if available from the GPS @@ -265,4 +266,4 @@ GPS::_swapi(const void *bytes) return(u.v); } -#endif +#endif // __GPS_H__ diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/Arduino.h b/libraries/AP_GPS/examples/GPS_AUTO_test/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde index 460b65e538..9574bcbc35 100644 --- a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde +++ b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde @@ -3,24 +3,26 @@ // Test for AP_GPS_AUTO // -#include +#include + #include #include +#include +#include #include #include -FastSerialPort0(Serial); -FastSerialPort1(Serial1); +const AP_HAL::HAL& hal = AP_HAL_AVR_APM2; GPS *gps; -AP_GPS_Auto GPS(&Serial1, &gps); +AP_GPS_Auto GPS(hal.uart1, &gps); #define T6 1000000 #define T7 10000000 // print_latlon - prints an latitude or longitude value held in an int32_t // probably this should be moved to AP_Common -void print_latlon(BetterStream *s, int32_t lat_or_lon) +void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon) { int32_t dec_portion, frac_portion; int32_t abs_lat_or_lon = labs(lat_or_lon); @@ -40,10 +42,10 @@ void print_latlon(BetterStream *s, int32_t lat_or_lon) void setup() { - Serial.begin(115200); - Serial1.begin(38400); + hal.uart0->begin(115200); + hal.uart1->begin(38400); - Serial.println("GPS AUTO library test"); + hal.uart0->println("GPS AUTO library test"); gps = &GPS; gps->init(GPS::GPS_ENGINE_AIRBORNE_2G); } @@ -53,11 +55,11 @@ void loop() gps->update(); if (gps->new_data) { if (gps->fix) { - Serial.print("Lat: "); - print_latlon(&Serial,gps->latitude); - Serial.print(" Lon: "); - print_latlon(&Serial,gps->longitude); - Serial.printf(" Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu STATUS: %u\n", + hal.uart0->print("Lat: "); + print_latlon(hal.uart0,gps->latitude); + hal.uart0->print(" Lon: "); + print_latlon(hal.uart0,gps->longitude); + hal.uart0->printf(" Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu STATUS: %u\n", (float)gps->altitude / 100.0, (float)gps->ground_speed / 100.0, (int)gps->ground_course / 100, @@ -65,9 +67,10 @@ void loop() gps->time, gps->status()); } else { - Serial.println("No fix"); + hal.uart0->println("No fix"); } gps->new_data = false; } } +AP_HAL_MAIN(); diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/Makefile b/libraries/AP_GPS/examples/GPS_AUTO_test/Makefile index 85b4d8856b..d1f40fd90f 100644 --- a/libraries/AP_GPS/examples/GPS_AUTO_test/Makefile +++ b/libraries/AP_GPS/examples/GPS_AUTO_test/Makefile @@ -1,2 +1 @@ -BOARD = mega include ../../../AP_Common/Arduino.mk diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/nocore.inoflag b/libraries/AP_GPS/examples/GPS_AUTO_test/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_GPS/examples/GPS_MTK_test/Arduino.h b/libraries/AP_GPS/examples/GPS_MTK_test/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde b/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde index 3f55e24c19..928a558d55 100644 --- a/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde +++ b/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde @@ -6,54 +6,54 @@ * Works with Ardupilot Mega Hardware (GPS on Serial Port1) */ -#include +#include #include #include +#include +#include #include #include -FastSerialPort0(Serial); -FastSerialPort1(Serial1); +const AP_HAL::HAL& hal = AP_HAL_AVR_APM2; -AP_GPS_MTK16 gps(&Serial1); +AP_GPS_MTK16 gps(hal.uart1); #define T6 1000000 #define T7 10000000 void setup() { - Serial.begin(115200); - Serial1.begin(38400); - stderr = stdout; + hal.uart1->begin(38400); gps.print_errors = true; - Serial.println("GPS MTK library test"); + hal.uart0->println("GPS MTK library test"); gps.init(); // GPS Initialization - delay(1000); } + void loop() { - delay(20); + hal.scheduler->delay(20); gps.update(); if (gps.new_data) { - Serial.print("gps:"); - Serial.print(" Lat:"); - Serial.print((float)gps.latitude / T7, DEC); - Serial.print(" Lon:"); - Serial.print((float)gps.longitude / T7, DEC); - Serial.print(" Alt:"); - Serial.print((float)gps.altitude / 100.0, DEC); - Serial.print(" GSP:"); - Serial.print(gps.ground_speed / 100.0); - Serial.print(" COG:"); - Serial.print(gps.ground_course / 100.0, DEC); - Serial.print(" SAT:"); - Serial.print(gps.num_sats, DEC); - Serial.print(" FIX:"); - Serial.print(gps.fix, DEC); - Serial.print(" TIM:"); - Serial.print(gps.time, DEC); - Serial.println(); + hal.uart0->print("gps:"); + hal.uart0->print(" Lat:"); + hal.uart0->print((float)gps.latitude / T7, DEC); + hal.uart0->print(" Lon:"); + hal.uart0->print((float)gps.longitude / T7, DEC); + hal.uart0->print(" Alt:"); + hal.uart0->print((float)gps.altitude / 100.0, DEC); + hal.uart0->print(" GSP:"); + hal.uart0->print(gps.ground_speed / 100.0); + hal.uart0->print(" COG:"); + hal.uart0->print(gps.ground_course / 100.0, DEC); + hal.uart0->print(" SAT:"); + hal.uart0->print(gps.num_sats, DEC); + hal.uart0->print(" FIX:"); + hal.uart0->print(gps.fix, DEC); + hal.uart0->print(" TIM:"); + hal.uart0->print(gps.time, DEC); + hal.uart0->println(); gps.new_data = 0; // We have readed the data } } +AP_HAL_MAIN(); diff --git a/libraries/AP_GPS/examples/GPS_MTK_test/nocore.inoflag b/libraries/AP_GPS/examples/GPS_MTK_test/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_GPS/examples/GPS_UBLOX_test/Arduino.h b/libraries/AP_GPS/examples/GPS_UBLOX_test/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde b/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde index f847326187..09686744f8 100644 --- a/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde +++ b/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde @@ -6,58 +6,58 @@ * Works with Ardupilot Mega Hardware (GPS on Serial Port1) */ -#include +#include #include #include +#include +#include #include #include -FastSerialPort0(Serial); -FastSerialPort1(Serial1); +const AP_HAL::HAL& hal = AP_HAL_AVR_APM2; -AP_GPS_UBLOX gps(&Serial1); +AP_GPS_UBLOX gps(hal.uart1); #define T6 1000000 #define T7 10000000 void setup() { - Serial.begin(115200); - Serial1.begin(38400); - stderr = stdout; + hal.uart1->begin(38400); gps.print_errors = true; - Serial.println("GPS UBLOX library test"); + hal.uart0->println("GPS UBLOX library test"); gps.init(GPS::GPS_ENGINE_AIRBORNE_2G); // GPS Initialization - delay(1000); } + void loop() { - delay(20); + hal.scheduler->delay(20); gps.update(); if (gps.new_data) { - Serial.print("gps:"); - Serial.print(" Lat:"); - Serial.print((float)gps.latitude / T7, DEC); - Serial.print(" Lon:"); - Serial.print((float)gps.longitude / T7, DEC); - Serial.print(" Alt:"); - Serial.print((float)gps.altitude / 100.0, DEC); - Serial.print(" GSP:"); - Serial.print(gps.ground_speed / 100.0); - Serial.print(" COG:"); - Serial.print(gps.ground_course / 100.0, DEC); - Serial.printf(" VEL: %.2f %.2f %.2f", + hal.uart0->print("gps:"); + hal.uart0->print(" Lat:"); + hal.uart0->print((float)gps.latitude / T7, DEC); + hal.uart0->print(" Lon:"); + hal.uart0->print((float)gps.longitude / T7, DEC); + hal.uart0->print(" Alt:"); + hal.uart0->print((float)gps.altitude / 100.0, DEC); + hal.uart0->print(" GSP:"); + hal.uart0->print(gps.ground_speed / 100.0); + hal.uart0->print(" COG:"); + hal.uart0->print(gps.ground_course / 100.0, DEC); + hal.uart0->printf(" VEL: %.2f %.2f %.2f", gps.velocity_north(), gps.velocity_east(), sqrt(sq(gps.velocity_north())+sq(gps.velocity_east()))); - Serial.print(" SAT:"); - Serial.print(gps.num_sats, DEC); - Serial.print(" FIX:"); - Serial.print(gps.fix, DEC); - Serial.print(" TIM:"); - Serial.print(gps.time, DEC); - Serial.println(); + hal.uart0->print(" SAT:"); + hal.uart0->print(gps.num_sats, DEC); + hal.uart0->print(" FIX:"); + hal.uart0->print(gps.fix, DEC); + hal.uart0->print(" TIM:"); + hal.uart0->print(gps.time, DEC); + hal.uart0->println(); gps.new_data = 0; // We have readed the data } } +AP_HAL_MAIN(); diff --git a/libraries/AP_GPS/examples/GPS_UBLOX_test/nocore.inoflag b/libraries/AP_GPS/examples/GPS_UBLOX_test/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2