dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Another rewrite: most of the polling, reading and writing is now inside the GPS classes

This commit is contained in:
Julian Oes 2013-02-08 11:05:57 -08:00
parent a88b9f4eef
commit df6cf142e7
8 changed files with 583 additions and 567 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
apps/drivers/drv_gps.h
View File

@ -49,7 +49,8 @@
#define GPS_DEVICE_PATH "/dev/gps"
typedef enum {
GPS_DRIVER_MODE_UBX = 0,
GPS_DRIVER_MODE_NONE = 0,
GPS_DRIVER_MODE_UBX,
GPS_DRIVER_MODE_MTK,
GPS_DRIVER_MODE_NMEA,
} gps_driver_mode_t;

62
apps/drivers/gps/gps.cpp
View File

@ -36,10 +36,7 @@
* Driver for the GPS on a serial port
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
#include <nuttx/clock.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdio.h>
@ -54,30 +51,24 @@
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <nuttx/config.h>
#include <nuttx/arch.h>
#include <nuttx/clock.h>
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/i2c.h>
#include <systemlib/perf_counter.h>
#include <systemlib/scheduling_priorities.h>
#include <systemlib/err.h>
#include <drivers/drv_gps.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
//#include "ubx.h"
#include "ubx.h"
#include "mtk.h"
#define SEND_BUFFER_LENGTH 100
#define TIMEOUT 1000000 //1s
#define NUMBER_OF_TRIES 5
#define CONFIG_TIMEOUT 2000000
#define TIMEOUT_5HZ 400
#define RATE_MEASUREMENT_PERIOD 5000000
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@ -170,7 +161,7 @@ GPS::GPS(const char* uart_path) :
_task_should_exit(false),
_healthy(false),
_mode_changed(false),
_mode(GPS_DRIVER_MODE_MTK),
_mode(GPS_DRIVER_MODE_UBX),
_Helper(nullptr),
_report_pub(-1),
_rate(0.0f)
@ -279,12 +270,11 @@ GPS::task_main()
}
switch (_mode) {
// case GPS_DRIVER_MODE_UBX:
// _Helper = new UBX();
// break;
case GPS_DRIVER_MODE_UBX:
_Helper = new UBX(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_MTK:
printf("try new mtk\n");
_Helper = new MTK();
_Helper = new MTK(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_NMEA:
//_Helper = new NMEA(); //TODO: add NMEA
@ -292,11 +282,11 @@ GPS::task_main()
default:
break;
}
// XXX unlock/lock makes sense?
unlock();
if (_Helper->configure(_serial_fd, _baudrate) == 0) {
while (_Helper->receive(_serial_fd, _report) > 0 && !_task_should_exit) {
if (_Helper->configure(_baudrate) == 0) {
unlock();
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
@ -307,7 +297,7 @@ GPS::task_main()
last_rate_count++;
/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > 5000000) {
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
last_rate_measurement = hrt_absolute_time();
last_rate_count = 0;
@ -323,8 +313,26 @@ GPS::task_main()
_healthy = false;
_rate = 0.0f;
}
lock();
}
lock();
/* select next mode */
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_mode = GPS_DRIVER_MODE_MTK;
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_UBX;
break;
// case GPS_DRIVER_MODE_NMEA:
// _mode = GPS_DRIVER_MODE_UBX;
// break;
default:
break;
}
}
debug("exiting");

2
apps/drivers/gps/gps_helper.cpp
View File

@ -57,6 +57,8 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
case 115200: speed = B115200; break;
warnx("try baudrate: %d\n", speed);
default:
warnx("ERROR: Unsupported baudrate: %d\n", baud);
return -EINVAL;

4
apps/drivers/gps/gps_helper.h
View File

@ -44,8 +44,8 @@
class GPS_Helper
{
public:
virtual int configure(const int &fd, unsigned &baud) = 0;
virtual int receive(const int &fd, struct vehicle_gps_position_s &gps_position) = 0;
virtual int configure(unsigned &baud) = 0;
virtual int receive(unsigned timeout) = 0;
int set_baudrate(const int &fd, unsigned baud);
};

69
apps/drivers/gps/mtk.cpp
View File

@ -47,7 +47,9 @@
#include "mtk.h"
MTK::MTK() :
MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd),
_gps_position(gps_position),
_mtk_revision(0)
{
decode_init();
@ -58,40 +60,40 @@ MTK::~MTK()
}
int
MTK::configure(const int &fd, unsigned &baudrate)
MTK::configure(unsigned &baudrate)
{
/* set baudrate first */
if (GPS_Helper::set_baudrate(fd, MTK_BAUDRATE) != 0)
if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0)
return -1;
baudrate = MTK_BAUDRATE;
/* Write config messages, don't wait for an answer */
if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
warnx("mtk: config write failed");
return -1;
}
usleep(10000);
if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
warnx("mtk: config write failed");
return -1;
}
usleep(10000);
if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) {
if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
warnx("mtk: config write failed");
return -1;
}
usleep(10000);
if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) {
if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
warnx("mtk: config write failed");
return -1;
}
usleep(10000);
if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
warnx("mtk: config write failed");
return -1;
}
@ -100,16 +102,19 @@ MTK::configure(const int &fd, unsigned &baudrate)
}
int
MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
MTK::receive(unsigned timeout)
{
/* poll descriptor */
pollfd fds[1];
fds[0].fd = fd;
fds[0].fd = _fd;
fds[0].events = POLLIN;
uint8_t buf[32];
gps_mtk_packet_t packet;
/* timeout additional to poll */
uint64_t time_started = hrt_absolute_time();
int j = 0;
ssize_t count = 0;
@ -120,9 +125,13 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
/* pass received bytes to the packet decoder */
while (j < count) {
if (parse_char(buf[j], packet) > 0) {
handle_message(packet, gps_position);
handle_message(packet);
return 1;
}
/* in case we keep trying but only get crap from GPS */
if (time_started + timeout*1000 < hrt_absolute_time() ) {
return -1;
}
j++;
}
/* everything is read */
@ -130,7 +139,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
}
/* then poll for new data */
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), MTK_TIMEOUT_5HZ);
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
if (ret < 0) {
/* something went wrong when polling */
@ -148,7 +157,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
* won't block even on a blocking device. If more bytes are
* available, we'll go back to poll() again...
*/
count = ::read(fd, buf, sizeof(buf));
count = ::read(_fd, buf, sizeof(buf));
}
}
}
@ -212,26 +221,26 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
}
void
MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position)
MTK::handle_message(gps_mtk_packet_t &packet)
{
if (_mtk_revision == 16) {
gps_position.lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
gps_position.lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
} else if (_mtk_revision == 19) {
gps_position.lat = packet.latitude; // both degrees*1e7
gps_position.lon = packet.longitude; // both degrees*1e7
_gps_position->lat = packet.latitude; // both degrees*1e7
_gps_position->lon = packet.longitude; // both degrees*1e7
} else {
warnx("mtk: unknown revision");
gps_position.lat = 0;
gps_position.lon = 0;
_gps_position->lat = 0;
_gps_position->lon = 0;
}
gps_position.alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
gps_position.fix_type = packet.fix_type;
gps_position.eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
gps_position.epv_m = 0.0; //unknown in mtk custom mode
gps_position.vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
gps_position.cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
gps_position.satellites_visible = packet.satellites;
_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
_gps_position->fix_type = packet.fix_type;
_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
_gps_position->epv_m = 0.0; //unknown in mtk custom mode
_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
_gps_position->satellites_visible = packet.satellites;
/* convert time and date information to unix timestamp */
struct tm timeinfo; //TODO: test this conversion
@ -250,9 +259,9 @@ MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps
timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
time_t epoch = mktime(&timeinfo);
gps_position.time_gps_usec = epoch * 1e6; //TODO: test this
gps_position.time_gps_usec += timeinfo_conversion_temp * 1e3;
gps_position.timestamp_position = gps_position.timestamp_time = hrt_absolute_time();
_gps_position->time_gps_usec = epoch * 1e6; //TODO: test this
_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
return;
}

10
apps/drivers/gps/mtk.h
View File

@ -85,10 +85,10 @@ typedef struct {
class MTK : public GPS_Helper
{
public:
MTK();
MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
~MTK();
int receive(const int &fd, struct vehicle_gps_position_s &gps_position);
int configure(const int &fd, unsigned &baudrate);
int receive(unsigned timeout);
int configure(unsigned &baudrate);
private:
/**
@ -99,7 +99,7 @@ private:
/**
* Handle the package once it has arrived
*/
void handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position);
void handle_message(gps_mtk_packet_t &packet);
/**
* Reset the parse state machine for a fresh start
@ -111,6 +111,8 @@ private:
*/
void add_byte_to_checksum(uint8_t);
int _fd;
struct vehicle_gps_position_s *_gps_position;
mtk_decode_state_t _decode_state;
uint8_t _mtk_revision;
uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE];

968
apps/drivers/gps/ubx.cpp
View File

File diff suppressed because it is too large Load Diff

32
apps/drivers/gps/ubx.h
View File

@ -86,6 +86,8 @@
#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_MAX_PAYLOAD_LENGTH 500
// ************
/** the structures of the binary packets */
#pragma pack(push, 1)
@ -337,35 +339,49 @@ typedef enum {
class UBX : public GPS_Helper
{
public:
UBX();
UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
~UBX();
void reset(void);
void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate);
void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated);
int receive(unsigned timeout);
int configure(unsigned &baudrate);
private:
/**
* Parse the binary MTK packet
*/
int parse_char(uint8_t b);
/**
* Handle the package once it has arrived
*/
int handle_message(void);
/**
* Reset the parse state machine for a fresh start
*/
void decodeInit(void);
void decode_init(void);
/**
* While parsing add every byte (except the sync bytes) to the checksum
*/
void addByteToChecksum(uint8_t);
void add_byte_to_checksum(uint8_t);
/**
* Add the two checksum bytes to an outgoing message
*/
void addChecksumToMessage(uint8_t* message, const unsigned length);
void add_checksum_to_message(uint8_t* message, const unsigned length);
/**
* Helper to send a config packet
*/
void sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length);
void send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
int _fd;
struct vehicle_gps_position_s *_gps_position;
ubx_config_state_t _config_state;
bool _waiting_for_ack;
uint8_t _clsID_needed;
uint8_t _msgID_needed;
ubx_decode_state_t _decode_state;
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
unsigned _rx_count;