mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 15:38:29 -04:00
AP_GPS: Builds under AP_HAL. Not tested.
This commit is contained in:
parent
e83504f80d
commit
9ed023aeb4
libraries/AP_GPS
AP_GPS_406.cppAP_GPS_406.hAP_GPS_Auto.cppAP_GPS_Auto.hAP_GPS_HIL.cppAP_GPS_HIL.hAP_GPS_IMU.cppAP_GPS_IMU.hAP_GPS_MTK.cppAP_GPS_MTK.hAP_GPS_MTK16.cppAP_GPS_MTK16.hAP_GPS_MTK_Common.hAP_GPS_NMEA.cppAP_GPS_NMEA.hAP_GPS_None.hAP_GPS_SIRF.cppAP_GPS_SIRF.hAP_GPS_UBLOX.cppAP_GPS_UBLOX.hGPS.cppGPS.h
examples
GPS_AUTO_test
GPS_MTK_test
GPS_UBLOX_test
@ -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);
|
||||
}
|
||||
|
||||
|
@ -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__
|
||||
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
@ -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;
|
||||
|
@ -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,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__
|
||||
|
@ -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();
|
||||
|
@ -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__
|
||||
|
@ -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)
|
||||
{
|
||||
}
|
||||
|
||||
|
@ -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__
|
||||
|
@ -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)
|
||||
{
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
@ -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__
|
||||
|
@ -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__
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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__
|
||||
|
@ -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__
|
||||
|
@ -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)
|
||||
{
|
||||
}
|
||||
|
||||
|
@ -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();
|
||||
|
@ -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;
|
||||
|
@ -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__
|
||||
|
@ -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;
|
||||
|
@ -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
0
libraries/AP_GPS/examples/GPS_AUTO_test/Arduino.h
Normal 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,2 +1 @@
|
||||
BOARD = mega
|
||||
include ../../../AP_Common/Arduino.mk
|
||||
|
0
libraries/AP_GPS/examples/GPS_MTK_test/Arduino.h
Normal file
0
libraries/AP_GPS/examples/GPS_MTK_test/Arduino.h
Normal 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_UBLOX_test/Arduino.h
Normal file
0
libraries/AP_GPS/examples/GPS_UBLOX_test/Arduino.h
Normal 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();
|
||||
|
Loading…
Reference in New Issue
Block a user