Skip to content

Commit

Permalink
Merged with newer, cleaned up code, fixed the checksum error
Browse files Browse the repository at this point in the history
  • Loading branch information
Julian Oes committed Feb 5, 2013
1 parent cb0fd83 commit 039d394
Show file tree
Hide file tree
Showing 5 changed files with 191 additions and 167 deletions.
2 changes: 0 additions & 2 deletions apps/drivers/drv_gps.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,6 @@ typedef enum {
} gps_driver_mode_t;




/*
* ObjDev tag for GPS data.
*/
Expand Down
130 changes: 72 additions & 58 deletions apps/drivers/gps/gps.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@

/**
* @file gps.cpp
* Driver for the GPS on UART6
* Driver for the GPS on a serial port
*/

#include <nuttx/config.h>
Expand Down Expand Up @@ -107,46 +107,41 @@ class GPS : public device::CDev

private:

int _task_should_exit;
int _serial_fd; ///< serial interface to GPS
unsigned _baudrate;
unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES];
uint8_t _send_buffer[SEND_BUFFER_LENGTH];
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
volatile int _task; ///< worker task
bool _task_should_exit; ///< flag to make the main worker task exit
int _serial_fd; ///< serial interface to GPS
unsigned _baudrate; ///< current baudrate
unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to
volatile int _task; ///< worker task
bool _config_needed; ///< flag to signal that configuration of GPS is needed
bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed
bool _mode_changed; ///< flag that the GPS mode has changed
gps_driver_mode_t _mode; ///< current mode
GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper
struct vehicle_gps_position_s _report; ///< uORB topic for gps position
orb_advert_t _report_pub; ///< uORB pub for gps position
float _rate; ///< position update rate

bool _response_received;
bool _config_needed;
bool _baudrate_changed;
bool _mode_changed;
bool _healthy;
gps_driver_mode_t _mode;
unsigned _messages_received;
float _rate; ///< position update rate

GPS_Helper *_Helper; ///< class for either UBX, MTK or NMEA helper

struct vehicle_gps_position_s _report; ///< the current GPS report
orb_advert_t _report_pub; ///< the publication topic, only valid on first valid GPS module message

void recv();

/**
* Try to configure the GPS, handle outgoing communication to the GPS
*/
void config();

/**
* Trampoline to the worker task
*/
static void task_main_trampoline(void *arg);


/**
* Worker task.
* Worker task: main GPS thread that configures the GPS and parses incoming data, always running
*/
void task_main(void);

/**
* Set the module baud rate
* Set the baudrate of the UART to the GPS
*/
int set_baudrate(unsigned baud);
int set_baudrate(unsigned baud);

/**
* Send a reset command to the GPS
Expand Down Expand Up @@ -176,12 +171,10 @@ GPS::GPS() :
_config_needed(true),
_baudrate_changed(false),
_mode_changed(true),
_healthy(false),
_mode(GPS_DRIVER_MODE_UBX),
_messages_received(0),
_Helper(nullptr),
_rate(0.0f),
_report_pub(-1)
_report_pub(-1),
_rate(0.0f)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
Expand Down Expand Up @@ -216,7 +209,7 @@ GPS::init()
if (CDev::init() != OK)
goto out;

/* start the IO interface task */
/* start the GPS driver worker task */
_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);

if (_task < 0) {
Expand Down Expand Up @@ -275,17 +268,22 @@ void
GPS::config()
{
int length = 0;
uint8_t send_buffer[SEND_BUFFER_LENGTH];

_Helper->configure(_config_needed, _baudrate_changed, _baudrate, _send_buffer, length, SEND_BUFFER_LENGTH);
_Helper->configure(_config_needed, _baudrate_changed, _baudrate, send_buffer, length, SEND_BUFFER_LENGTH);

/* the message needs to be sent at the old baudrate */
/* The config message is sent sent at the old baudrate */
if (length > 0) {
if (length != ::write(_serial_fd, _send_buffer, length)) {

if (length != ::write(_serial_fd, send_buffer, length)) {
debug("write config failed");
return;
}
}

/* Sometimes the baudrate needs to be changed, for instance UBX with factory settings are changed
* from 9600 to 38400
*/
if (_baudrate_changed) {
set_baudrate(_baudrate);
_baudrate_changed = false;
Expand All @@ -304,11 +302,10 @@ GPS::task_main()
log("starting");

/* open the serial port */
_serial_fd = ::open("/dev/ttyS3", O_RDWR);
_serial_fd = ::open("/dev/ttyS3", O_RDWR); //TODO make the device dynamic depending on startup parameters

uint8_t buf[256];
int baud_i = 0;
uint64_t time_before_configuration;
/* buffer to read from the serial port */
uint8_t buf[32];

if (_serial_fd < 0) {
log("failed to open serial port: %d", errno);
Expand All @@ -326,16 +323,16 @@ GPS::task_main()
/* lock against the ioctl handler */
lock();


unsigned baud_i = 0;
_baudrate = _baudrates_to_try[baud_i];
set_baudrate(_baudrate);

time_before_configuration = hrt_absolute_time();
uint64_t time_before_configuration = hrt_absolute_time();

uint64_t last_rate_measurement = hrt_absolute_time();
unsigned last_rate_count = 0;

/* loop handling received serial bytes */
/* loop handling received serial bytes and also configuring in between */
while (!_task_should_exit) {

if (_mode_changed) {
Expand Down Expand Up @@ -364,14 +361,17 @@ GPS::task_main()
_mode_changed = false;
}

/* If a configuration does not finish in the config timeout, change the baudrate */
if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) {
baud_i = (baud_i+1)%NUMBER_OF_BAUDRATES;
_baudrate = _baudrates_to_try[baud_i];
set_baudrate(_baudrate);
_Helper->reset();
time_before_configuration = hrt_absolute_time();
}


/* during configuration, the timeout should be small, so that we can send config messages in between parsing,
* but during normal operation, it should never timeout because updates should arrive with 5Hz */
int poll_timeout;
if (_config_needed) {
poll_timeout = 50;
Expand Down Expand Up @@ -409,11 +409,21 @@ GPS::task_main()

/* pass received bytes to the packet decoder */
int j;
int ret_parse = 0;
for (j = 0; j < count; j++) {
_messages_received += _Helper->parse(buf[j], &_report);
ret_parse += _Helper->parse(buf[j], &_report);

This comment has been minimized.

Sign in to view

Copy link
@LorenzMeier

LorenzMeier Feb 5, 2013

Member

If the result is once 1 and once -1, is the overall result defined?

This comment has been minimized.

Sign in to view

Copy link
@julianoes

julianoes Feb 5, 2013

Contributor

Yes here: https://github.com/PX4/Firmware/blob/gps/apps/drivers/gps/ubx.cpp#L253
}
if (_messages_received > 0) {
if (_config_needed == true) {

if (ret_parse < 0) {

This comment has been minimized.

Sign in to view

Copy link
@LorenzMeier

LorenzMeier Feb 5, 2013

Member

What is the combination of one successfully parsed and one failed packet if they're accumulated to ret_parse?

This comment has been minimized.

Sign in to view

Copy link
@julianoes

julianoes Feb 5, 2013

Contributor

Parse returns after each byte. So it's either:
error, so if something in the decoding went wrong: -1
still decoding, needs more bytes or does not have all messages that should arrive with 5Hz: 0
finished with all needed messages: 1

the only problem that I can think of is the additional delay on the position through other messages. So alternatively we could finish the report every time the position is updated. The others are just added to the report whenever available.

This comment has been minimized.

Sign in to view

Copy link
@LorenzMeier

LorenzMeier Feb 5, 2013

Member

See the comment above. The parser may return after each packet, but the result is accumulated: ret_parse +=. So if you have in one buffer one good and one corrupted packet you end up with nothing decoded = 0, which is not the same thing.

About the other argument: We clearly want to emit a changed message whenever the position changes and not at other time instants. After my changes on the last round this should be actually the case (and since it reports updates at 5 Hz, seems to work fine).

This comment has been minimized.

Sign in to view

Copy link
@julianoes

julianoes Feb 5, 2013

Contributor

Oh got it. You're right. I'll look into this, maybe with the parse scheme like in mavlink.
/* This means something went wrong in the parser, let's reconfigure */
if (!_config_needed) {
_config_needed = true;
debug("Lost GPS module");
}
config();
} else if (ret_parse > 0) {
/* Looks like we got a valid position update, stop configuring and publish it */
if (_config_needed) {
_config_needed = false;
debug("Found GPS module");
}
Expand All @@ -424,8 +434,6 @@ GPS::task_main()
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}

_messages_received = 0;
last_rate_count++;

/* measure update rate every 5 seconds */
Expand All @@ -435,10 +443,10 @@ GPS::task_main()
last_rate_count = 0;
}
}
/* else if ret_parse == 0: just keep parsing */
}
}
}
out:
debug("exiting");

::close(_serial_fd);
Expand Down Expand Up @@ -469,7 +477,6 @@ GPS::set_baudrate(unsigned baud)
warnx("ERROR: Unsupported baudrate: %d\n", baud);
return -EINVAL;
}

struct termios uart_config;
int termios_state;

Expand All @@ -482,24 +489,26 @@ GPS::set_baudrate(unsigned baud)
uart_config.c_cflag &= ~(CSTOPB | PARENB);

/* set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERROR setting config: %d (cfsetispeed, cfsetospeed)\n", termios_state);
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {

This comment has been minimized.

Sign in to view

Copy link
@LorenzMeier

LorenzMeier Feb 5, 2013

Member

ispeed / ospeed set the same thing, its not necessary to distinguish between them. Eats more flash space this way for text.

This comment has been minimized.

Sign in to view

Copy link
@julianoes

julianoes Feb 5, 2013

Contributor

well, with the former way the termios_state was not set and a warning was produced. If you can fix it differently, go ahead:)
warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
return -1;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
return -1;
}


if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
warnx("ERROR setting baudrate (tcsetattr)\n");
return -1;
}

/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
return 0;
}

void
GPS::cmd_reset()
{
_healthy = false;
_config_needed = true;
}

void
Expand All @@ -524,10 +533,12 @@ GPS::print_info()
warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK");
if (_report.timestamp != 0) {
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f));
(double)((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f));
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("update rate: %6.2f Hz", (double)_rate);
}

usleep(100000);

This comment has been minimized.

Sign in to view

Copy link
@LorenzMeier

LorenzMeier Feb 5, 2013

Member

Why sleeping here?

This comment has been minimized.

Sign in to view

Copy link
@julianoes

julianoes Feb 5, 2013

Contributor

Oh I added this while debugging and then forgot about it. Let me test it without sleeping.
}

/**
Expand Down Expand Up @@ -583,6 +594,9 @@ start()
errx(1, "driver start failed");
}

/**
* Stop the driver.
*/
void
stop()
{
Expand Down
5 changes: 3 additions & 2 deletions apps/drivers/gps/gps_helper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,9 @@
class GPS_Helper
{
public:
virtual void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned) = 0;
virtual int parse(uint8_t, struct vehicle_gps_position_s*) = 0;
virtual void reset() = 0;
virtual void configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) = 0;
virtual int parse(uint8_t b, struct vehicle_gps_position_s *gps_position) = 0;
};

#endif /* GPS_HELPER_H */
Loading

0 comments on commit 039d394

Please sign in to comment.