fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Changed parse interface, differentiation between config needed and position updated, working but might be solved more elegant

Browse Source
This commit is contained in:
Julian Oes 2013-02-05 23:16:32 -08:00
parent fbbeef7e29
commit a79ad17f09
5 changed files with 81 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
apps/drivers/drv_gps.h
View File

@ -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 {

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

@ -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.

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

@ -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
apps/drivers/gps/ubx.cpp
View File

@ -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();
/* we have received everything, this most probably means that the configuration is fine */
config_needed = false;
/* Reset the flags */
/* update on every position change, accept minor delay on other measurements */
_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

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

@ -341,8 +341,8 @@ public:
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:
/**