From a79ad17f09ac69bf2a19a4f617fa1df16bcaa6be Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 5 Feb 2013 23:16:32 -0800 Subject: [PATCH] Changed parse interface, differentiation between config needed and position updated, working but might be solved more elegant --- apps/drivers/drv_gps.h | 2 + apps/drivers/gps/gps.cpp | 73 +++++++++++++++++++---------------- apps/drivers/gps/gps_helper.h | 6 +-- apps/drivers/gps/ubx.cpp | 65 ++++++++++++++++--------------- apps/drivers/gps/ubx.h | 4 +- 5 files changed, 81 insertions(+), 69 deletions(-) diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h index 1ae27ed2f9f1..dfde115eff7b 100644 --- a/apps/drivers/drv_gps.h +++ b/apps/drivers/drv_gps.h @@ -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 { diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 5905db6b82f8..28dc949d4bd5 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -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) { @@ -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) { @@ -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) { @@ -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 */ } } } @@ -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. diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index 8a6b0148b7b9..176b7eba81bb 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -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 */ diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index a6f181a73587..440ec74c58ef 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -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); @@ -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) { @@ -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: @@ -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; @@ -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"); @@ -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 { @@ -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; @@ -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; @@ -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 @@ -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; @@ -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; // } @@ -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) @@ -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; } @@ -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; } @@ -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: @@ -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 diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index 0cac10f0a310..d3c6c6d4cfe4 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -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: /**