AttPosEKF: Refactor and code cleanup

2026-06-12 11:26:31 +08:00 · 2015-02-11 17:57:33 +01:00
parent 8d8a66607a
commit 0cbfa65056
3 changed files with 526 additions and 342 deletions
@@ -4,9 +4,9 @@
 # att & pos estimator, att & pos control.
 #

-attitude_estimator_ekf start
-#ekf_att_pos_estimator start
-position_estimator_inav start
+#attitude_estimator_ekf start
+ekf_att_pos_estimator start
+#position_estimator_inav start

 if mc_att_control start
 then
@@ -81,6 +81,12 @@
 #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
 {
@@ -162,7 +168,7 @@ extern "C" __EXPORT int gps_main(int argc, char *argv[]);
 namespace
 {

-GPS	*g_dev;
+GPS	*g_dev = nullptr;

 }

@@ -271,6 +277,27 @@ 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()
 {
@@ -288,31 +315,62 @@ 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) {

-			_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;
+			//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;
+			}
+			//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;				
+			}

 			//no time and satellite information simulated

+
 			if (!(_pub_blocked)) {
 				if (_report_gps_pos_pub > 0) {
 					orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
@@ -364,6 +422,29 @@ 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);

@@ -478,25 +559,35 @@ GPS::cmd_reset()
 void
 GPS::print_info()
 {
-	switch (_mode) {
-	case GPS_DRIVER_MODE_UBX:
-		warnx("protocol: UBX");
-		break;
+	//GPS Mode
+	if(_fake_gps) {
+		warnx("protocol: SIMULATED");
+	}

-	case GPS_DRIVER_MODE_MTK:
-		warnx("protocol: MTK");
-		break;
+	else {
+		switch (_mode) {
+		case GPS_DRIVER_MODE_UBX:
+			warnx("protocol: UBX");
+			break;
+
+		case GPS_DRIVER_MODE_MTK:
+			warnx("protocol: MTK");
+			break;

 	case GPS_DRIVER_MODE_ASHTECH:
 		warnx("protocol: ASHTECH");
 		break;

-	default:
-		break;
+		default:
+			break;
+		}		
 	}

 	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
-	warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled");
+	warnx("sat info: %s, noise: %d, jamming detected: %s", 
+		(_p_report_sat_info != nullptr) ? "enabled" : "disabled", 
+		_report_gps_pos.noise_per_ms, 
+		_report_gps_pos.jamming_indicator == 255 ? "YES" : "NO");

 	if (_report_gps_pos.timestamp_position != 0) {
 		warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
@@ -520,7 +611,7 @@ GPS::print_info()
 namespace gps
 {

-GPS	*g_dev;
+GPS	*g_dev = nullptr;

 void	start(const char *uart_path, bool fake_gps, bool enable_sat_info);
 void	stop();
@@ -664,6 +755,14 @@ gps_main(int argc, char *argv[])
 		gps::start(device_name, fake_gps, enable_sat_info);
 	}

+	if (!strcmp(argv[1], "fake")) {
+		if(g_dev != nullptr) {
+			delete g_dev;
+			g_dev = nullptr;			
+		}
+		gps::start(GPS_DEFAULT_UART_PORT, true, false);
+	}
+
 	if (!strcmp(argv[1], "stop"))
 		gps::stop();

@@ -686,5 +785,5 @@ gps_main(int argc, char *argv[])
 		gps::info();

 out:
-	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]");
+	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'fake', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]");
 }