From c6f43689e7f2a2dcfabffb9268f7db0de7b3434a Mon Sep 17 00:00:00 2001
From: Miguel Arroyo <miguel@arroyo.me>
Date: Tue, 6 Sep 2016 16:43:28 -0400
Subject: [PATCH] Adds Auto Mode Scanning

---
 posix-configs/rpi/px4.config |  2 +-
 src/drivers/gps/gps.cpp      | 32 ++++++++++++++++++++++++++++++--
 2 files changed, 31 insertions(+), 3 deletions(-)

diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config
index e5737cb72e..60a729e4f5 100644
--- a/posix-configs/rpi/px4.config
+++ b/posix-configs/rpi/px4.config
@@ -7,7 +7,7 @@ df_lsm9ds1_wrapper start -R 4
 #df_mpu9250_wrapper start -R 10
 #df_hmc5883_wrapper start
 df_ms5611_wrapper start
-gps start -d /dev/spidev0.0
+gps start -d /dev/spidev0.0 -i spi -p ubx
 sensors start
 commander start
 attitude_estimator_q start
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 1a3915afc0..56918ed350 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -124,8 +124,9 @@ private:
 	bool				_healthy;					///< flag to signal if the GPS is ok
 	bool				_baudrate_changed;				///< flag to signal that the baudrate with the GPS has changed
 	bool				_mode_changed;					///< flag that the GPS mode has changed
+	bool        _mode_auto;
 	gps_driver_mode_t		_mode;						///< current mode
-	gps_driver_interface_t  _interface;
+	gps_driver_interface_t  _interface;   ///< interface
 	GPSHelper			*_helper;					///< instance of GPS parser
 	GPS_Sat_Info			*_sat_info;					///< instance of GPS sat info data object
 	struct vehicle_gps_position_s	_report_gps_pos;				///< uORB topic for gps position
@@ -279,6 +280,10 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, gps_driver_interface_t interf
 		_p_report_sat_info = &_sat_info->_data;
 		memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
 	}
+
+	if (mode == GPS_DRIVER_MODE_NONE) {
+		_mode_auto = true;
+	}
 }
 
 GPS::~GPS()
@@ -719,6 +724,9 @@ GPS::task_main()
 			}
 
 			switch (_mode) {
+			case GPS_DRIVER_MODE_NONE:
+				_mode = GPS_DRIVER_MODE_UBX;
+
 			case GPS_DRIVER_MODE_UBX:
 				_helper = new GPSDriverUBX(helper_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
 				break;
@@ -825,6 +833,26 @@ GPS::task_main()
 					_rate_rtcm_injection = 0.0f;
 				}
 			}
+
+			if (_mode_auto) {
+				switch (_mode) {
+				case GPS_DRIVER_MODE_UBX:
+					_mode = GPS_DRIVER_MODE_MTK;
+					break;
+
+				case GPS_DRIVER_MODE_MTK:
+					_mode = GPS_DRIVER_MODE_ASHTECH;
+					break;
+
+				case GPS_DRIVER_MODE_ASHTECH:
+					_mode = GPS_DRIVER_MODE_UBX;
+					break;
+
+				default:
+					break;
+				}
+			}
+
 		}
 	}
 
@@ -1049,7 +1077,7 @@ gps_main(int argc, char *argv[])
 	bool fake_gps = false;
 	bool enable_sat_info = false;
 	gps_driver_interface_t interface = GPS_DRIVER_UART;
-	gps_driver_mode_t mode = GPS_DRIVER_MODE_UBX;
+	gps_driver_mode_t mode = GPS_DRIVER_MODE_NONE;
 
 	if (argc < 2) {
 		goto out;