Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Builds under AP_HAL. Not tested.

This commit is contained in:
Pat Hickey 2012-09-26 22:18:44 -07:00 committed by Andrew Tridgell
parent e83504f80d
commit 9ed023aeb4
32 changed files with 229 additions and 228 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_GPS/AP_GPS_406.cpp
View File

@ -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 <AP_HAL.h>
#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);
}

10
libraries/AP_GPS/AP_GPS_406.h
View File

@ -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 <AP_HAL.h>
#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__

37
libraries/AP_GPS/AP_GPS_Auto.cpp
View File

@ -3,20 +3,21 @@
/// @file AP_GPS_Auto.cpp
/// @brief Simple GPS auto-detection logic.
#include <FastSerial.h>
#include <AP_HAL.h>
#include <AP_Common.h>
#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);
}
}

13
libraries/AP_GPS/AP_GPS_Auto.h
View File

@ -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 <AP_HAL.h>
#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;

8
libraries/AP_GPS/AP_GPS_HIL.cpp
View File

@ -11,15 +11,11 @@
// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
//
#include <AP_HAL.h>
#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)
{
}

9
libraries/AP_GPS/AP_GPS_HIL.h
View File

@ -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 <AP_HAL.h>
#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__

12
libraries/AP_GPS/AP_GPS_IMU.cpp
View File

@ -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 <AP_HAL.h>
#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();

9
libraries/AP_GPS/AP_GPS_IMU.h
View File

@ -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 <AP_HAL.h>
#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__

7
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -11,11 +11,14 @@
// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
//
#include "AP_GPS_MTK.h"
#include <stdint.h>
#include <AP_HAL.h>
#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)
{
}

8
libraries/AP_GPS/AP_GPS_MTK.h
View File

@ -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__

16
libraries/AP_GPS/AP_GPS_MTK16.cpp
View File

@ -10,18 +10,14 @@
//
// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
//
#include <FastSerial.h>
#include <AP_HAL.h>
#include "AP_GPS_MTK16.h"
#include <stdint.h>
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include <wiring.h>
#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)
{
}
@ -223,7 +219,7 @@ 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;
@ -232,7 +228,7 @@ AP_GPS_MTK16::_detect(uint8_t data)
if (ck_b == data) {
return true;
}
Serial.printf("wrong ck_b\n");
hal.console->println_P(PSTR("wrong ck_b"));
break;
}
return false;

9
libraries/AP_GPS/AP_GPS_MTK16.h
View File

@ -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 <AP_HAL.h>
#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__

6
libraries/AP_GPS/AP_GPS_MTK_Common.h
View File

@ -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__

12
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -25,12 +25,12 @@
/// TinyGPS parser by Mikal Hart.
///
#include <FastSerial.h>
#include <AP_Common.h>
#include <avr/pgmspace.h>
#include <ctype.h>
#include <stdint.h>
#include <stdlib.h>
#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;
}

9
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -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 <AP_HAL.h>
#include "GPS.h"
#include <avr/pgmspace.h>
@ -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__

9
libraries/AP_GPS/AP_GPS_None.h
View File

@ -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 <AP_HAL.h>
#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__

2
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -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)
{
}

7
libraries/AP_GPS/AP_GPS_SIRF.h
View File

@ -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 <AP_HAL.h>
#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();

20
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -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 <stdint.h>
#include <AP_HAL.h>
#define UBLOX_DEBUGGING 0
#include <FastSerial.h>
#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 <stdint.h>
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;

9
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -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 <AP_HAL.h>
#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__

33
libraries/AP_GPS/GPS.cpp
View File

@ -1,25 +1,22 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Math.h>
#include <AP_HAL.h>
#include "GPS.h"
extern const AP_HAL::HAL& hal;
#define GPS_DEBUGGING 0
#if GPS_DEBUGGING
#include <FastSerial.h>
# 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 <AP_Common.h>
#include <AP_Math.h>
#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;

19
libraries/AP_GPS/GPS.h
View File

@ -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 <AP_HAL.h>
#include <inttypes.h>
#include <Stream.h>
#include <avr/pgmspace.h>
/// @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__

0
libraries/AP_GPS/examples/GPS_AUTO_test/Arduino.h Normal file
View File

31
libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde
View File

@ -3,24 +3,26 @@
// Test for AP_GPS_AUTO
//
#include <FastSerial.h>
#include <stdlib.h>
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_GPS.h>
#include <AP_Math.h>
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();

1
libraries/AP_GPS/examples/GPS_AUTO_test/Makefile
View File

@ -1,2 +1 @@
BOARD = mega
include ../../../AP_Common/Arduino.mk

0
libraries/AP_GPS/examples/GPS_AUTO_test/nocore.inoflag Normal file
View File

0
libraries/AP_GPS/examples/GPS_MTK_test/Arduino.h Normal file
View File

56
libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde
View File

@ -6,54 +6,54 @@
* Works with Ardupilot Mega Hardware (GPS on Serial Port1)
*/
#include <FastSerial.h>
#include <stdlib.h>
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_GPS.h>
#include <AP_Math.h>
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();

0
libraries/AP_GPS/examples/GPS_MTK_test/nocore.inoflag Normal file
View File

0
libraries/AP_GPS/examples/GPS_UBLOX_test/Arduino.h Normal file
View File

58
libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde
View File

@ -6,58 +6,58 @@
* Works with Ardupilot Mega Hardware (GPS on Serial Port1)
*/
#include <FastSerial.h>
#include <stdlib.h>
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_GPS.h>
#include <AP_Math.h>
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();

0
libraries/AP_GPS/examples/GPS_UBLOX_test/nocore.inoflag Normal file
View File