Skip to content

Commit

Permalink
Changed parse interface, differentiation between config needed and po…
Browse files Browse the repository at this point in the history 
…sition updated, working but might be solved more elegant
  • Loading branch information
Julian Oes committed Feb 6, 2013
1 parent fbbeef7 commit a79ad17
Show file tree
Hide file tree
Showing 5 changed files with 81 additions and 69 deletions.
2 changes: 2 additions & 0 deletions apps/drivers/drv_gps.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,8 @@
#include "drv_sensor.h"
#include "drv_orb_dev.h"

#define GPS_DEFAULT_UART_PORT "/dev/ttyS3"

#define GPS_DEVICE_PATH "/dev/gps"

typedef enum {
Expand Down
73 changes: 40 additions & 33 deletions apps/drivers/gps/gps.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -278,7 +278,7 @@ 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(send_buffer, length, SEND_BUFFER_LENGTH, _baudrate_changed, _baudrate);

/* The config message is sent sent at the old baudrate */
if (length > 0) {
Expand Down Expand Up @@ -339,6 +339,9 @@ GPS::task_main()
uint64_t last_rate_measurement = hrt_absolute_time();
unsigned last_rate_count = 0;

bool pos_updated;
bool config_needed_res;

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

Expand Down Expand Up @@ -391,17 +394,17 @@ GPS::task_main()
lock();




/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
} else if (ret == 0) {
config();
/* no response from the GPS */
if (_config_needed == false) {
_config_needed = true;
warnx("lost GPS module");
warnx("GPS module timeout");
_Helper->reset();
}
config();
} else if (ret > 0) {
/* if we have new data from GPS, go handle it */
if (fds[0].revents & POLLIN) {
Expand All @@ -416,39 +419,43 @@ GPS::task_main()

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

if (pos_updated) {
/* 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);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
last_rate_count++;

/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > 5000000) {
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
last_rate_measurement = hrt_absolute_time();
last_rate_count = 0;
}

if (ret_parse < 0) {
/* This means something went wrong in the parser, let's reconfigure */
if (!_config_needed) {
_config_needed = true;
}
config();
} else if (ret_parse > 0) {
/* Looks like we got a valid position update, stop configuring and publish it */
if (_config_needed) {
if (config_needed_res == true && _config_needed == false) {
/* the parser told us that an error happened and reconfiguration is needed */
_config_needed = true;
warnx("GPS module lost");
_Helper->reset();
config();
} else if (config_needed_res == true && _config_needed == true) {
/* we are still configuring */
config();
} else if (config_needed_res == false && _config_needed == true) {
/* the parser is happy, stop configuring */
warnx("GPS module found");
_config_needed = false;
}

/* 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);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
last_rate_count++;

/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > 5000000) {
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
last_rate_measurement = hrt_absolute_time();
last_rate_count = 0;
}
}
/* else if ret_parse == 0: just keep parsing */
}
}
}
Expand Down Expand Up @@ -662,7 +669,7 @@ gps_main(int argc, char *argv[])
{

/* set to default */
char* device_name = "/dev/ttyS3";
char* device_name = GPS_DEFAULT_UART_PORT;

/*
* Start/load the driver.
Expand Down
6 changes: 3 additions & 3 deletions apps/drivers/gps/gps_helper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,9 +40,9 @@
class GPS_Helper
{
public:
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;
virtual void reset(void) = 0;
virtual void configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate) = 0;
virtual void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated) = 0;
};

#endif /* GPS_HELPER_H */
65 changes: 34 additions & 31 deletions apps/drivers/gps/ubx.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ UBX::reset()
}

void
UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length)
UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate)
{
/* make sure the buffer, where the message is written to, is long enough */
assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length);
Expand All @@ -84,9 +84,10 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate,
if (!_waiting_for_ack) {
_waiting_for_ack = true;
if (_config_state == UBX_CONFIG_STATE_CONFIGURED) {
config_needed = false;
length = 0;
_config_state = UBX_CONFIG_STATE_PRT; /* set the state for next time */
/* This should never happen, the parser should set the flag,
* if it should be reconfigured, reset() should be called first.
*/
warnx("ubx: already configured");
_waiting_for_ack = false;
return;
} else if (_config_state == UBX_CONFIG_STATE_PRT) {
Expand Down Expand Up @@ -246,12 +247,9 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate,
}
}

int
UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
void
UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated)
{
/* if no error happens and no new report is ready yet, ret will stay 0 */
int ret = 0;

switch (_decode_state) {
/* First, look for sync1 */
case UBX_DECODE_UNINIT:
Expand Down Expand Up @@ -382,7 +380,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
default: //should not happen because we set the class
warnx("UBX Error, we set a class that we don't know");
decodeInit();
ret = -1;
config_needed = true;
break;
}
break;
Expand Down Expand Up @@ -417,7 +415,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
gps_position->counter_pos_valid++;
gps_position->counter++;

/* Add timestamp to finish the report */
gps_position->timestamp = hrt_absolute_time();

_new_nav_posllh = true;
/* set flag to trigger publishing of new position */
pos_updated = true;

} else {
warnx("NAV_POSLLH: checksum invalid");
Expand All @@ -436,11 +439,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {

gps_position->fix_type = packet->gpsFix;

gps_position->counter++;
gps_position->s_variance = packet->sAcc;
gps_position->p_variance = packet->pAcc;

gps_position->counter++;
gps_position->timestamp = hrt_absolute_time();


_new_nav_sol = true;

} else {
Expand All @@ -463,6 +468,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
gps_position->epv = packet->vDOP;

gps_position->counter++;
gps_position->timestamp = hrt_absolute_time();

_new_nav_dop = true;

Expand Down Expand Up @@ -496,6 +502,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);

gps_position->counter++;
gps_position->timestamp = hrt_absolute_time();

_new_nav_timeutc = true;

Expand Down Expand Up @@ -587,6 +594,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)

gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones
gps_position->counter++;
gps_position->timestamp = hrt_absolute_time();

// as this message arrives only with 1Hz and is not essential, we don't take it into account for the report

Expand Down Expand Up @@ -614,6 +622,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f);

gps_position->counter++;
gps_position->timestamp = hrt_absolute_time();

_new_nav_velned = true;

Expand Down Expand Up @@ -647,8 +656,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
//
// // Reset state machine to decode next packet
// decodeInit();
// return ret;
//
// break;
// }

Expand Down Expand Up @@ -701,6 +708,8 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
case UBX_CONFIG_STATE_MSG_NAV_VELNED:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
_config_state = UBX_CONFIG_STATE_CONFIGURED;
/* set the flag to tell the driver that configuration was successful */
config_needed = false;
break;
// case UBX_CONFIG_STATE_MSG_RXM_SVSI:
// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
Expand All @@ -715,7 +724,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)

// Reset state machine to decode next packet
decodeInit();

break;
}

Expand All @@ -727,16 +735,14 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {

warnx("UBX: Received: Not Acknowledged");
ret = 1;

/* configuration obviously not successful */
config_needed = true;
} else {
warnx("ACK_NAK: checksum invalid\n");
}

// Reset state machine to decode next packet
decodeInit();
return ret;

break;
}

Expand All @@ -752,7 +758,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
} else {
warnx("buffer full, restarting");
decodeInit();
ret = -1;
config_needed = true;
}
break;
default:
Expand All @@ -762,21 +768,18 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
/* return 1 when position updates and the remaining packets updated at least once */
if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) {

/* Add timestamp to finish the report */
gps_position->timestamp = hrt_absolute_time();
/* Reset the flags */
/* we have received everything, this most probably means that the configuration is fine */
config_needed = false;

/* update on every position change, accept minor delay on other measurements */
/* Reset the flags */
_new_nav_posllh = false;
// _new_nav_timeutc = false;
// _new_nav_dop = false;
// _new_nav_sol = false;
// _new_nav_velned = false;
_new_nav_timeutc = false;
_new_nav_dop = false;
_new_nav_sol = false;
_new_nav_velned = false;

ret = 1;
}

return ret;
return;
}

void
Expand Down
4 changes: 2 additions & 2 deletions apps/drivers/gps/ubx.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -341,8 +341,8 @@ class UBX : public GPS_Helper
UBX();
~UBX();
void reset(void);
void configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length);
int parse(uint8_t, struct vehicle_gps_position_s*);
void configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate);
void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated);

private:
/**
Expand Down

2 comments on commit a79ad17

@julianoes
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So we have to flags now: config_needed and pos_updated.

I know this sounds complicated but I was looking for a way to reconfigure when not all messages arrive and still publish the position if it is available. If we only wait for a position update, we might forget about the other messages.

For MTK and NMEA this won't be that complicated.

@LorenzMeier
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think you're overwriting the timestamp now for position messages, which means the estimator has no idea from when the position measurement actually was (and experience suggests the filter has an internal more or less fixed delay, which combined would allow to properly fuse the GPS measurement). I don't think the timestamps should be updated for anything else than position, or if you do, you need to have per-measurement timestamps (e.g. a velocity estimate timestamp).

In addition, how likely is it that the GPS module decides halfway into the mission to stop sending one particular message? It still looks as if we should configure first, check for the configuration result (and throw away all other messages we're not interested in) and move through the config steps until the module is fully configured.

Not saying that the current approach is wrong or does not work, but it involves a couple of global states and state switches that are hard to read, understand and maintain.

Please sign in to comment.