Another rewrite: most of the polling, reading and writing is now inside the GPS classes

apps/drivers/drv_gps.h

@@ -49,7 +49,8 @@
 #define GPS_DEVICE_PATH	"/dev/gps"
 
 typedef enum {
-	GPS_DRIVER_MODE_UBX = 0,
+	GPS_DRIVER_MODE_NONE = 0,
+	GPS_DRIVER_MODE_UBX,
 	GPS_DRIVER_MODE_MTK,
 	GPS_DRIVER_MODE_NMEA,
 } gps_driver_mode_t;

apps/drivers/gps/gps.cpp

@@ -36,10 +36,7 @@
  * Driver for the GPS on a serial port
  */
 
-#include <nuttx/config.h>
+#include <nuttx/clock.h>
-
-#include <drivers/device/i2c.h>
-
 #include <sys/types.h>
 #include <stdint.h>
 #include <stdio.h>
@@ -54,30 +51,24 @@
 #include <math.h>
 #include <unistd.h>
 #include <fcntl.h>
-
+#include <nuttx/config.h>
 #include <nuttx/arch.h>
-#include <nuttx/clock.h>
-
 #include <arch/board/board.h>
-
 #include <drivers/drv_hrt.h>
-
+#include <drivers/device/i2c.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/scheduling_priorities.h>
 #include <systemlib/err.h>
-
 #include <drivers/drv_gps.h>
-
+#include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
-//#include "ubx.h"
+#include "ubx.h"
 #include "mtk.h"
 
-#define SEND_BUFFER_LENGTH 100
-#define TIMEOUT 1000000 //1s
 
-#define NUMBER_OF_TRIES 5
+#define TIMEOUT_5HZ 400
-#define CONFIG_TIMEOUT 2000000
+#define RATE_MEASUREMENT_PERIOD 5000000
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -170,7 +161,7 @@ GPS::GPS(const char* uart_path) :
 	_task_should_exit(false),
 	_healthy(false),
 	_mode_changed(false),
-	_mode(GPS_DRIVER_MODE_MTK),
+	_mode(GPS_DRIVER_MODE_UBX),
 	_Helper(nullptr),
 	_report_pub(-1),
 	_rate(0.0f)
@@ -279,12 +270,11 @@ GPS::task_main()
 		}
 
 		switch (_mode) {
-//			case GPS_DRIVER_MODE_UBX:
+			case GPS_DRIVER_MODE_UBX:
-//				_Helper = new UBX();
+				_Helper = new UBX(_serial_fd, &_report);
-//				break;
+				break;
 			case GPS_DRIVER_MODE_MTK:
-				printf("try new mtk\n");
+				_Helper = new MTK(_serial_fd, &_report);
-				_Helper = new MTK();
 				break;
 			case GPS_DRIVER_MODE_NMEA:
 				//_Helper = new NMEA(); //TODO: add NMEA
@@ -292,11 +282,11 @@ GPS::task_main()
 			default:
 				break;
 		}
-		// XXX unlock/lock makes sense?
 		unlock();
-		if (_Helper->configure(_serial_fd, _baudrate) == 0) {
+		if (_Helper->configure(_baudrate) == 0) {
-
+			unlock();
-			while (_Helper->receive(_serial_fd, _report) > 0 && !_task_should_exit) {
+			while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
+//				lock();
 				/* 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);
@@ -307,7 +297,7 @@ GPS::task_main()
 				last_rate_count++;
 
 				/* measure update rate every 5 seconds */
-				if (hrt_absolute_time() - last_rate_measurement > 5000000) {
+				if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
 					_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
 					last_rate_measurement = hrt_absolute_time();
 					last_rate_count = 0;
@@ -323,8 +313,26 @@ GPS::task_main()
 				_healthy = false;
 				_rate = 0.0f;
 			}
+
+			lock();
 		}
 		lock();
+
+		/* select next mode */
+		switch (_mode) {
+			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;
+			default:
+				break;
+		}
+
 	}
 	debug("exiting");
 

apps/drivers/gps/gps_helper.cpp

@@ -57,6 +57,8 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 
 	case 115200: speed = B115200; break;
 
+	warnx("try baudrate: %d\n", speed);
+
 	default:
 		warnx("ERROR: Unsupported baudrate: %d\n", baud);
 		return -EINVAL;

apps/drivers/gps/gps_helper.h

@@ -44,8 +44,8 @@
 class GPS_Helper
 {
 public:
-	virtual int				configure(const int &fd, unsigned &baud) = 0;
+	virtual int				configure(unsigned &baud) = 0;
-	virtual int 			receive(const int &fd, struct vehicle_gps_position_s &gps_position) = 0;
+	virtual int 			receive(unsigned timeout) = 0;
 	int 					set_baudrate(const int &fd, unsigned baud);
 };
 

apps/drivers/gps/mtk.cpp

@@ -47,7 +47,9 @@
 #include "mtk.h"
 
 
-MTK::MTK() :
+MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
+_fd(fd),
+_gps_position(gps_position),
 _mtk_revision(0)
 {
 	decode_init();
@@ -58,40 +60,40 @@ MTK::~MTK()
 }
 
 int
-MTK::configure(const int &fd, unsigned &baudrate)
+MTK::configure(unsigned &baudrate)
 {
 	/* set baudrate first */
-	if (GPS_Helper::set_baudrate(fd, MTK_BAUDRATE) != 0)
+	if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0)
 		return -1;
 
 	baudrate = MTK_BAUDRATE;
 
 	/* Write config messages, don't wait for an answer */
-	if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
+	if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
 	usleep(10000);
 
-	if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
+	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))) {
+	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))) {
+	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))) {
+	if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
@@ -100,16 +102,19 @@ MTK::configure(const int &fd, unsigned &baudrate)
 }
 
 int
-MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
+MTK::receive(unsigned timeout)
 {
 	/* poll descriptor */
 	pollfd fds[1];
-	fds[0].fd = fd;
+	fds[0].fd = _fd;
 	fds[0].events = POLLIN;
 
 	uint8_t buf[32];
 	gps_mtk_packet_t packet;
 
+	/* timeout additional to poll */
+	uint64_t time_started = hrt_absolute_time();
+
 	int j = 0;
 	ssize_t count = 0;
 
@@ -120,9 +125,13 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
 			/* pass received bytes to the packet decoder */
 			while (j < count) {
 				if (parse_char(buf[j], packet) > 0) {
-					handle_message(packet, gps_position);
+					handle_message(packet);
 					return 1;
 				}
+				/* in case we keep trying but only get crap from GPS */
+				if (time_started + timeout*1000 < hrt_absolute_time() ) {
+					return -1;
+				}
 				j++;
 			}
 			/* everything is read */
@@ -130,7 +139,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
 		}
 
 		/* then poll for new data */
-		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), MTK_TIMEOUT_5HZ);
+		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
 
 		if (ret < 0) {
 			/* something went wrong when polling */
@@ -148,7 +157,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
 				 * won't block even on a blocking device.  If more bytes are
 				 * available, we'll go back to poll() again...
 				 */
-				count = ::read(fd, buf, sizeof(buf));
+				count = ::read(_fd, buf, sizeof(buf));
 			}
 		}
 	}
@@ -212,26 +221,26 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 }
 
 void
-MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position)
+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->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
-		gps_position.lon = packet.longitude * 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->lat = packet.latitude; // both degrees*1e7
-		gps_position.lon = packet.longitude; // both degrees*1e7
+		_gps_position->lon = packet.longitude; // both degrees*1e7
 	} else {
 		warnx("mtk: unknown revision");
-		gps_position.lat = 0;
+		_gps_position->lat = 0;
-		gps_position.lon = 0;
+		_gps_position->lon = 0;
 	}
-	gps_position.alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
+	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
-	gps_position.fix_type = packet.fix_type;
+	_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->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->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->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
-	gps_position.satellites_visible = packet.satellites;
+	_gps_position->satellites_visible = packet.satellites;
 
 	/* convert time and date information to unix timestamp */
 	struct tm timeinfo; //TODO: test this conversion
@@ -250,9 +259,9 @@ MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps
 	timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
 	time_t epoch = mktime(&timeinfo);
 
-	gps_position.time_gps_usec = epoch * 1e6; //TODO: test this
+	_gps_position->time_gps_usec = epoch * 1e6; //TODO: test this
-	gps_position.time_gps_usec += timeinfo_conversion_temp * 1e3;
+	_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
-	gps_position.timestamp_position = gps_position.timestamp_time = hrt_absolute_time();
+	_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
 
 	return;
 }

apps/drivers/gps/mtk.h

@@ -85,10 +85,10 @@ typedef struct {
 class MTK : public GPS_Helper
 {
 public:
-	MTK();
+	MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
 	~MTK();
-	int					receive(const int &fd, struct vehicle_gps_position_s &gps_position);
+	int					receive(unsigned timeout);
-	int					configure(const int &fd, unsigned &baudrate);
+	int					configure(unsigned &baudrate);
 
 private:
 	/**
@@ -99,7 +99,7 @@ private:
 	/**
 	 * Handle the package once it has arrived
 	 */
-	void				handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position);
+	void				handle_message(gps_mtk_packet_t &packet);
 
 	/**
 	 * Reset the parse state machine for a fresh start
@@ -111,6 +111,8 @@ private:
 	 */
 	void				add_byte_to_checksum(uint8_t);
 
+	int					_fd;
+	struct vehicle_gps_position_s *_gps_position;
 	mtk_decode_state_t	_decode_state;
 	uint8_t				_mtk_revision;
 	uint8_t				_rx_buffer[MTK_RECV_BUFFER_SIZE];

apps/drivers/gps/ubx.h

@@ -86,6 +86,8 @@
 #define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
 #define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
 
+#define UBX_MAX_PAYLOAD_LENGTH 500
+
 // ************
 /** the structures of the binary packets */
 #pragma pack(push, 1)
@@ -337,35 +339,49 @@ typedef enum {
 class UBX : public GPS_Helper
 {
 public:
-	UBX();
+	UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
 	~UBX();
-	void				reset(void);
+	int					receive(unsigned timeout);
-	void				configure(const int &fd, bool &baudrate_changed, unsigned &baudrate);
+	int					configure(unsigned &baudrate);
-	void 				parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated);
 
 private:
+
+	/**
+	 * Parse the binary MTK packet
+	 */
+	int					parse_char(uint8_t b);
+
+	/**
+	 * Handle the package once it has arrived
+	 */
+	int					handle_message(void);
+
 	/**
 	 * Reset the parse state machine for a fresh start
 	 */
-	void				decodeInit(void);
+	void				decode_init(void);
 
 	/**
 	 * While parsing add every byte (except the sync bytes) to the checksum
 	 */
-	void				addByteToChecksum(uint8_t);
+	void				add_byte_to_checksum(uint8_t);
 
 	/**
 	 * Add the two checksum bytes to an outgoing message
 	 */
-	void				addChecksumToMessage(uint8_t* message, const unsigned length);
+	void				add_checksum_to_message(uint8_t* message, const unsigned length);
 
 	/**
 	 * Helper to send a config packet
 	 */
-	void				sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length);
+	void				send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
 
+	int 				_fd;
+	struct vehicle_gps_position_s *_gps_position;
 	ubx_config_state_t	_config_state;
 	bool 				_waiting_for_ack;
+	uint8_t				_clsID_needed;
+	uint8_t				_msgID_needed;
 	ubx_decode_state_t	_decode_state;
 	uint8_t				_rx_buffer[RECV_BUFFER_SIZE];
 	unsigned			_rx_count;