forked from Archive/PX4-Autopilot
gps: fixed code style, more informative and clean messages
This commit is contained in:
Anton Babushkin
parent
cd8854e622
commit
8d6a47546a
|
|
@ -85,7 +85,7 @@ static const int ERROR = -1;
|
|||
class GPS : public device::CDev
|
||||
{
|
||||
public:
|
||||
GPS(const char* uart_path);
|
||||
GPS(const char *uart_path);
|
||||
virtual ~GPS();
|
||||
|
||||
virtual int init();
|
||||
|
|
@ -156,7 +156,7 @@ GPS *g_dev;
|
|||
}
|
||||
|
||||
|
||||
GPS::GPS(const char* uart_path) :
|
||||
GPS::GPS(const char *uart_path) :
|
||||
CDev("gps", GPS_DEVICE_PATH),
|
||||
_task_should_exit(false),
|
||||
_healthy(false),
|
||||
|
|
@ -192,6 +192,7 @@ GPS::~GPS()
|
|||
/* well, kill it anyway, though this will probably crash */
|
||||
if (_task != -1)
|
||||
task_delete(_task);
|
||||
|
||||
g_dev = nullptr;
|
||||
|
||||
}
|
||||
|
|
@ -273,16 +274,21 @@ GPS::task_main()
|
|||
case GPS_DRIVER_MODE_UBX:
|
||||
_Helper = new UBX(_serial_fd, &_report);
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_Helper = new MTK(_serial_fd, &_report);
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_NMEA:
|
||||
//_Helper = new NMEA(); //TODO: add NMEA
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
if (_Helper->configure(_baudrate) == 0) {
|
||||
unlock();
|
||||
|
||||
|
|
@ -294,6 +300,7 @@ GPS::task_main()
|
|||
/* 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);
|
||||
}
|
||||
|
|
@ -310,10 +317,30 @@ GPS::task_main()
|
|||
}
|
||||
|
||||
if (!_healthy) {
|
||||
warnx("module found");
|
||||
char *mode_str = "unknown";
|
||||
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
mode_str = "ubx";
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
mode_str = "mtk";
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_NMEA:
|
||||
mode_str = "nmea";
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
warnx("module found: %s", mode_str);
|
||||
_healthy = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (_healthy) {
|
||||
warnx("module lost");
|
||||
_healthy = false;
|
||||
|
|
@ -322,6 +349,7 @@ GPS::task_main()
|
|||
|
||||
lock();
|
||||
}
|
||||
|
||||
lock();
|
||||
|
||||
/* select next mode */
|
||||
|
|
@ -329,9 +357,11 @@ GPS::task_main()
|
|||
case GPS_DRIVER_MODE_UBX:
|
||||
_mode = GPS_DRIVER_MODE_MTK;
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_mode = GPS_DRIVER_MODE_UBX;
|
||||
break;
|
||||
|
||||
// case GPS_DRIVER_MODE_NMEA:
|
||||
// _mode = GPS_DRIVER_MODE_UBX;
|
||||
// break;
|
||||
|
|
@ -340,6 +370,7 @@ GPS::task_main()
|
|||
}
|
||||
|
||||
}
|
||||
|
||||
debug("exiting");
|
||||
|
||||
::close(_serial_fd);
|
||||
|
|
@ -364,16 +395,21 @@ GPS::print_info()
|
|||
case GPS_DRIVER_MODE_UBX:
|
||||
warnx("protocol: UBX");
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
warnx("protocol: MTK");
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_NMEA:
|
||||
warnx("protocol: NMEA");
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
|
||||
|
||||
if (_report.timestamp_position != 0) {
|
||||
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
|
||||
(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
|
||||
|
|
@ -428,6 +464,7 @@ start(const char *uart_path)
|
|||
errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
|
||||
goto fail;
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
fail:
|
||||
|
|
@ -503,7 +540,7 @@ gps_main(int argc, char *argv[])
|
|||
{
|
||||
|
||||
/* set to default */
|
||||
char* device_name = GPS_DEFAULT_UART_PORT;
|
||||
char *device_name = GPS_DEFAULT_UART_PORT;
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
|
|
@ -513,15 +550,18 @@ gps_main(int argc, char *argv[])
|
|||
if (argc > 3) {
|
||||
if (!strcmp(argv[2], "-d")) {
|
||||
device_name = argv[3];
|
||||
|
||||
} else {
|
||||
goto out;
|
||||
}
|
||||
}
|
||||
|
||||
gps::start(device_name);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop"))
|
||||
gps::stop();
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -93,7 +93,9 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
|
|||
warnx("ERROR: Unsupported baudrate: %d\n", baud);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
struct termios uart_config;
|
||||
|
||||
int termios_state;
|
||||
|
||||
/* fill the struct for the new configuration */
|
||||
|
|
@ -109,14 +111,17 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
|
|||
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(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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -48,9 +48,9 @@
|
|||
|
||||
|
||||
MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
|
||||
_fd(fd),
|
||||
_gps_position(gps_position),
|
||||
_mtk_revision(0)
|
||||
_fd(fd),
|
||||
_gps_position(gps_position),
|
||||
_mtk_revision(0)
|
||||
{
|
||||
decode_init();
|
||||
}
|
||||
|
|
@ -73,24 +73,28 @@ MTK::configure(unsigned &baudrate)
|
|||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
|
||||
|
|
@ -128,12 +132,15 @@ MTK::receive(unsigned timeout)
|
|||
handle_message(packet);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* in case we keep trying but only get crap from GPS */
|
||||
if (time_started + timeout*1000 < hrt_absolute_time() ) {
|
||||
if (time_started + timeout * 1000 < hrt_absolute_time()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
j++;
|
||||
}
|
||||
|
||||
/* everything is read */
|
||||
j = count = 0;
|
||||
}
|
||||
|
|
@ -181,6 +188,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
|
|||
if (b == MTK_SYNC1_V16) {
|
||||
_decode_state = MTK_DECODE_GOT_CK_A;
|
||||
_mtk_revision = 16;
|
||||
|
||||
} else if (b == MTK_SYNC1_V19) {
|
||||
_decode_state = MTK_DECODE_GOT_CK_A;
|
||||
_mtk_revision = 19;
|
||||
|
|
@ -201,7 +209,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
|
|||
add_byte_to_checksum(b);
|
||||
|
||||
// Fill packet buffer
|
||||
((uint8_t*)(&packet))[_rx_count] = b;
|
||||
((uint8_t *)(&packet))[_rx_count] = b;
|
||||
_rx_count++;
|
||||
|
||||
/* Packet size minus checksum, XXX ? */
|
||||
|
|
@ -209,14 +217,17 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
|
|||
/* Compare checksum */
|
||||
if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
|
||||
ret = 1;
|
||||
|
||||
} else {
|
||||
warnx("MTK Checksum invalid");
|
||||
ret = -1;
|
||||
}
|
||||
|
||||
// Reset state machine to decode next packet
|
||||
decode_init();
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -226,19 +237,22 @@ MTK::handle_message(gps_mtk_packet_t &packet)
|
|||
if (_mtk_revision == 16) {
|
||||
_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
|
||||
_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
|
||||
|
||||
} else if (_mtk_revision == 19) {
|
||||
_gps_position->lat = packet.latitude; // both degrees*1e7
|
||||
_gps_position->lon = packet.longitude; // both degrees*1e7
|
||||
|
||||
} else {
|
||||
warnx("mtk: unknown revision");
|
||||
_gps_position->lat = 0;
|
||||
_gps_position->lon = 0;
|
||||
}
|
||||
|
||||
_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
|
||||
_gps_position->fix_type = packet.fix_type;
|
||||
_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
|
||||
_gps_position->epv_m = 0.0; //unknown in mtk custom mode
|
||||
_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
|
||||
_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
|
||||
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
|
||||
_gps_position->satellites_visible = packet.satellites;
|
||||
|
||||
|
|
|
|||
|
|
@ -60,13 +60,14 @@
|
|||
#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK
|
||||
#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received
|
||||
#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls
|
||||
#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval
|
||||
|
||||
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
|
||||
_fd(fd),
|
||||
_gps_position(gps_position),
|
||||
_configured(false),
|
||||
_waiting_for_ack(false),
|
||||
_disable_cmd_counter(0)
|
||||
_disable_cmd_last(0)
|
||||
{
|
||||
decode_init();
|
||||
}
|
||||
|
|
@ -191,35 +192,35 @@ UBX::configure(unsigned &baudrate)
|
|||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
|
||||
|
||||
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
warnx("ubx: msg rate configuration failed: NAV POSLLH\n");
|
||||
warnx("ubx: msg rate configuration failed: NAV POSLLH");
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
|
||||
|
||||
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n");
|
||||
warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
|
||||
|
||||
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
warnx("ubx: msg rate configuration failed: NAV SOL\n");
|
||||
warnx("ubx: msg rate configuration failed: NAV SOL");
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
|
||||
|
||||
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
warnx("ubx: msg rate configuration failed: NAV VELNED\n");
|
||||
warnx("ubx: msg rate configuration failed: NAV VELNED");
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
|
||||
|
||||
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
warnx("ubx: msg rate configuration failed: NAV SVINFO\n");
|
||||
warnx("ubx: msg rate configuration failed: NAV SVINFO");
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
|
@ -271,11 +272,18 @@ UBX::receive(unsigned timeout)
|
|||
|
||||
if (ret < 0) {
|
||||
/* something went wrong when polling */
|
||||
warnx("ubx: poll error");
|
||||
return -1;
|
||||
|
||||
} else if (ret == 0) {
|
||||
/* return success after short delay after receiving a packet or timeout after long delay */
|
||||
return handled ? 1 : -1;
|
||||
if (handled) {
|
||||
return 1;
|
||||
|
||||
} else {
|
||||
warnx("ubx: timeout - no messages");
|
||||
return -1;
|
||||
}
|
||||
|
||||
} else if (ret > 0) {
|
||||
/* if we have new data from GPS, go handle it */
|
||||
|
|
@ -292,8 +300,6 @@ UBX::receive(unsigned timeout)
|
|||
/* pass received bytes to the packet decoder */
|
||||
for (int i = 0; i < count; i++) {
|
||||
if (parse_char(buf[i]) > 0) {
|
||||
/* return to configure during configuration or to the gps driver during normal work
|
||||
* if a packet has arrived */
|
||||
if (handle_message() > 0)
|
||||
handled = true;
|
||||
}
|
||||
|
|
@ -303,6 +309,7 @@ UBX::receive(unsigned timeout)
|
|||
|
||||
/* abort after timeout if no useful packets received */
|
||||
if (time_started + timeout * 1000 < hrt_absolute_time()) {
|
||||
warnx("ubx: timeout - no useful messages");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
|
@ -460,7 +467,7 @@ UBX::handle_message()
|
|||
timespec ts;
|
||||
ts.tv_sec = epoch;
|
||||
ts.tv_nsec = packet->time_nanoseconds;
|
||||
clock_settime(CLOCK_REALTIME,&ts);
|
||||
clock_settime(CLOCK_REALTIME, &ts);
|
||||
#endif
|
||||
|
||||
_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
|
||||
|
|
@ -564,10 +571,13 @@ UBX::handle_message()
|
|||
|
||||
if (ret == 0) {
|
||||
/* message not handled */
|
||||
warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id);
|
||||
warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
|
||||
|
||||
if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) {
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
|
||||
/* don't attempt for every message to disable, some might not be disabled */
|
||||
_disable_cmd_last = t;
|
||||
warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
|
||||
configure_message_rate(_message_class, _message_id, 0);
|
||||
}
|
||||
|
|
@ -640,7 +650,7 @@ UBX::add_checksum_to_message(uint8_t *message, const unsigned length)
|
|||
ck_b = ck_b + ck_a;
|
||||
}
|
||||
|
||||
/* The checksum is written to the last to bytes of a message */
|
||||
/* the checksum is written to the last to bytes of a message */
|
||||
message[length - 2] = ck_a;
|
||||
message[length - 1] = ck_b;
|
||||
}
|
||||
|
|
@ -669,17 +679,17 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
|
|||
{
|
||||
ssize_t ret = 0;
|
||||
|
||||
/* Calculate the checksum now */
|
||||
/* calculate the checksum now */
|
||||
add_checksum_to_message(packet, length);
|
||||
|
||||
const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
|
||||
|
||||
/* Start with the two sync bytes */
|
||||
/* start with the two sync bytes */
|
||||
ret += write(fd, sync_bytes, sizeof(sync_bytes));
|
||||
ret += write(fd, packet, length);
|
||||
|
||||
if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
|
||||
warnx("ubx: config write fail");
|
||||
warnx("ubx: configuration write fail");
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -696,7 +706,7 @@ UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
|
|||
add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
|
||||
add_checksum((uint8_t *)msg, size, ck_a, ck_b);
|
||||
|
||||
// Configure receive check
|
||||
/* configure ACK check */
|
||||
_message_class_needed = msg_class;
|
||||
_message_id_needed = msg_id;
|
||||
|
||||
|
|
|
|||
|
|
@ -347,7 +347,7 @@ private:
|
|||
/**
|
||||
* Add the two checksum bytes to an outgoing message
|
||||
*/
|
||||
void add_checksum_to_message(uint8_t* message, const unsigned length);
|
||||
void add_checksum_to_message(uint8_t *message, const unsigned length);
|
||||
|
||||
/**
|
||||
* Helper to send a config packet
|
||||
|
|
@ -358,7 +358,7 @@ private:
|
|||
|
||||
void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
|
||||
|
||||
void add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
|
||||
void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
|
||||
|
||||
int wait_for_ack(unsigned timeout);
|
||||
|
||||
|
|
@ -376,7 +376,7 @@ private:
|
|||
uint8_t _message_class;
|
||||
uint8_t _message_id;
|
||||
unsigned _payload_size;
|
||||
uint8_t _disable_cmd_counter;
|
||||
uint8_t _disable_cmd_last;
|
||||
};
|
||||
|
||||
#endif /* UBX_H_ */
|
||||
|
|
|
|||
Loading…
Reference in New Issue