diff options
author | Julian Oes <joes@student.ethz.ch> | 2013-02-05 23:16:32 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2013-02-05 23:16:32 -0800 |
commit | a79ad17f09ac69bf2a19a4f617fa1df16bcaa6be (patch) | |
tree | 76a8624fb4b6487c39be356536b9bb71e271d78e /apps/drivers/gps/ubx.cpp | |
parent | fbbeef7e29cc6aa82e653916b7d5e8005948b815 (diff) | |
download | px4-firmware-a79ad17f09ac69bf2a19a4f617fa1df16bcaa6be.tar.gz px4-firmware-a79ad17f09ac69bf2a19a4f617fa1df16bcaa6be.tar.bz2 px4-firmware-a79ad17f09ac69bf2a19a4f617fa1df16bcaa6be.zip |
Changed parse interface, differentiation between config needed and position updated, working but might be solved more elegant
Diffstat (limited to 'apps/drivers/gps/ubx.cpp')
-rw-r--r-- | apps/drivers/gps/ubx.cpp | 65 |
1 files changed, 34 insertions, 31 deletions
diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index a6f181a73..440ec74c5 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 |