GPS: Remove GPS disable debug code

1 files changed, 17 insertions, 100 deletions

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 1a190bf40..c880482a1 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -81,12 +81,6 @@
 #endif
 static const int ERROR = -1;
 
-//DEBUG BEGIN
- #include <uORB/topics/manual_control_setpoint.h>
-static int sp_man_sub = -1;
-static struct manual_control_setpoint_s sp_man;
-//DEBUG END
-
 /* class for dynamic allocation of satellite info data */
 class GPS_Sat_Info
 {
@@ -277,27 +271,6 @@ GPS::task_main_trampoline(void *arg)
 	g_dev->task_main();
 }
 
-static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer)
-{
-	bool newData = false;
-
-	// check if there is new data to grab
-	if (orb_check(handle, &newData) != OK) {
-		return false;
-	}
-
-	if (!newData) {
-		return false;
-	}
-
-	if (orb_copy(meta, handle, buffer) != OK) {
-		return false;
-	}
-
-	return true;
-}
-
-
 void
 GPS::task_main()
 {
@@ -315,61 +288,27 @@ GPS::task_main()
 	uint64_t last_rate_measurement = hrt_absolute_time();
 	unsigned last_rate_count = 0;
 
-	//DEBUG BEGIN
-	sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	memset(&sp_man, 0, sizeof(sp_man));
-	//DEBUG END
-
 	/* loop handling received serial bytes and also configuring in between */
 	while (!_task_should_exit) {
 
 		if (_fake_gps) {
-
-			//DEBUG BEGIN: Disable GPS using aux1
-			orb_update(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
-			if(isfinite(sp_man.aux1) && sp_man.aux1 >= 0.0f) {
-				_report_gps_pos.timestamp_position = hrt_absolute_time();
-				_report_gps_pos.timestamp_variance = hrt_absolute_time();
-				_report_gps_pos.timestamp_velocity = hrt_absolute_time();
-				_report_gps_pos.fix_type		= 0;
-				_report_gps_pos.satellites_used	= 0;
-
-				//Don't modify Lat/Lon/AMSL
-
-				_report_gps_pos.eph		      = (float)0xFFFF;
-				_report_gps_pos.epv		      = (float)0xFFFF;
-				_report_gps_pos.s_variance_m_s = (float)0xFFFF;
-
-				_report_gps_pos.vel_m_s		  = 0.0f;
-				_report_gps_pos.vel_n_m_s	  = 0.0f;
-				_report_gps_pos.vel_e_m_s	  = 0.0f;
-				_report_gps_pos.vel_d_m_s	  = 0.0f;
-				_report_gps_pos.vel_ned_valid  = false;
-
-				_report_gps_pos.cog_rad		  = 0.0f;
-				_report_gps_pos.c_variance_rad = (float)0xFFFF;
-			}
-			//DEBUG END	
-
-			else {
-				_report_gps_pos.timestamp_position = hrt_absolute_time();
-				_report_gps_pos.lat = (int32_t)47.378301e7f;
-				_report_gps_pos.lon = (int32_t)8.538777e7f;
-				_report_gps_pos.alt = (int32_t)1200e3f;
-				_report_gps_pos.timestamp_variance = hrt_absolute_time();
-				_report_gps_pos.s_variance_m_s = 10.0f;
-				_report_gps_pos.c_variance_rad = 0.1f;
-				_report_gps_pos.fix_type = 3;
-				_report_gps_pos.eph = 0.9f;
-				_report_gps_pos.epv = 1.8f;
-				_report_gps_pos.timestamp_velocity = hrt_absolute_time();
-				_report_gps_pos.vel_n_m_s = 0.0f;
-				_report_gps_pos.vel_e_m_s = 0.0f;
-				_report_gps_pos.vel_d_m_s = 0.0f;
-				_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
-				_report_gps_pos.cog_rad = 0.0f;
-				_report_gps_pos.vel_ned_valid = true;				
-			}
+			_report_gps_pos.timestamp_position = hrt_absolute_time();
+			_report_gps_pos.lat = (int32_t)47.378301e7f;
+			_report_gps_pos.lon = (int32_t)8.538777e7f;
+			_report_gps_pos.alt = (int32_t)1200e3f;
+			_report_gps_pos.timestamp_variance = hrt_absolute_time();
+			_report_gps_pos.s_variance_m_s = 10.0f;
+			_report_gps_pos.c_variance_rad = 0.1f;
+			_report_gps_pos.fix_type = 3;
+			_report_gps_pos.eph = 0.9f;
+			_report_gps_pos.epv = 1.8f;
+			_report_gps_pos.timestamp_velocity = hrt_absolute_time();
+			_report_gps_pos.vel_n_m_s = 0.0f;
+			_report_gps_pos.vel_e_m_s = 0.0f;
+			_report_gps_pos.vel_d_m_s = 0.0f;
+			_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
+			_report_gps_pos.cog_rad = 0.0f;
+			_report_gps_pos.vel_ned_valid = true;				
 
 			//no time and satellite information simulated
 
@@ -426,28 +365,6 @@ GPS::task_main()
 					if (!(_pub_blocked)) {
 						if (helper_ret & 1) {
 
-							//DEBUG BEGIN: Disable GPS using aux1
-							orb_update(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
-							if(isfinite(sp_man.aux1) && sp_man.aux1 >= 0.0f) {
-								_report_gps_pos.fix_type			= 0;
-								_report_gps_pos.satellites_used	= 0;
-
-								//Don't modify Lat/Lon/AMSL
-
-								_report_gps_pos.eph		      = (float)0xFFFF;
-								_report_gps_pos.epv		      = (float)0xFFFF;
-								_report_gps_pos.s_variance_m_s = (float)0xFFFF;
-
-								_report_gps_pos.vel_m_s		  = 0.0f;
-								_report_gps_pos.vel_n_m_s	  = 0.0f;
-								_report_gps_pos.vel_e_m_s	  = 0.0f;
-								_report_gps_pos.vel_d_m_s	  = 0.0f;
-								_report_gps_pos.vel_ned_valid  = false;
-
-								_report_gps_pos.cog_rad		  = 0.0f;
-								_report_gps_pos.c_variance_rad = (float)0xFFFF;
-							}
-
 							if (_report_gps_pos_pub > 0) {
 								orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);