Now actually loading data into the filter when running it

1 files changed, 93 insertions, 13 deletions

diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index 90d02843d..b359348ed 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -85,7 +85,8 @@ __EXPORT uint32_t millis();
 
 static uint64_t last_run = 0;
 
-uint32_t millis() {
+uint32_t millis()
+{
 	return last_run / 1e3;
 }
 
@@ -139,6 +140,10 @@ private:
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
 	struct vehicle_gps_position_s			_gps;			/**< GPS position */
 
+	struct gyro_scale				_gyro_offsets;
+	struct accel_scale				_accel_offsets;
+	struct mag_scale				_mag_offsets;
+
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	bool						_initialized;
@@ -226,6 +231,31 @@ FixedwingEstimator::FixedwingEstimator() :
 
 	/* fetch initial parameter values */
 	parameters_update();
+
+	/* get offsets */
+
+	int fd, res;
+
+	fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+
+	if (fd > 0) {
+		res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets);
+		close(fd);
+	}
+
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd > 0) {
+		res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets);
+		close(fd);
+	}
+
+	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd > 0) {
+		res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets);
+		close(fd);
+	}
 }
 
 FixedwingEstimator::~FixedwingEstimator()
@@ -317,6 +347,9 @@ FixedwingEstimator::task_main()
 
 	parameters_update();
 
+	Vector3f lastAngRate;
+	Vector3f lastAccel;
+
 	/* wakeup source(s) */
 	struct pollfd fds[2];
 
@@ -359,13 +392,13 @@ FixedwingEstimator::task_main()
 		/* only run estimator if gyro updated */
 		if (fds[1].revents & POLLIN) {
 
-			
+
 
 			/* load local copies */
 			orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
 			orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
 
-			float IMUmsec = _gyro.timestamp / 1e6;
+			float IMUmsec = _gyro.timestamp / 1e3;
 
 			float deltaT = (_gyro.timestamp - last_run) / 1000000.0f;
 			last_run = _gyro.timestamp / 1e6 ;
@@ -378,6 +411,22 @@ FixedwingEstimator::task_main()
 
 			if (_initialized) {
 
+				/* fill in last data set */
+				dtIMU = dt;
+
+				angRate.x = _gyro.x;
+				angRate.y = _gyro.y;
+				angRate.z = _gyro.z;
+
+				accel.x = _accel.x;
+				accel.y = _accel.y;
+				accel.z = _accel.z;
+
+				dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
+				lastAngRate = angRate;
+				dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
+				lastAccel = accel;
+
 				/* predict states and covariances */
 
 				/* run the strapdown INS every sensor update */
@@ -407,16 +456,28 @@ FixedwingEstimator::task_main()
 
 			bool gps_updated;
 			orb_check(_gps_sub, &gps_updated);
+
 			if (gps_updated) {
 				orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
 
-				if (_gps.fix_type > 2 && !_initialized) {
+				if (_gps.fix_type > 2) {
+					/* fuse GPS updates */
+
+					//_gps.timestamp / 1e3;
+					GPSstatus = _gps.fix_type;
+					gpsCourse = _gps.cog_rad;
+					gpsGndSpd = sqrtf(_gps.vel_n_m_s * _gps.vel_n_m_s + _gps.vel_e_m_s * _gps.vel_e_m_s);
+					gpsVelD = _gps.vel_d_m_s;
+					gpsLat = math::radians(_gps.lat / (double)1e7);
+					gpsLon = math::radians(_gps.lon / (double)1e7);
+					gpsHgt = _gps.alt / 1e3f;
 
-					/* initialize */
-					InitialiseFilter();
+					newDataGps = true;
 
-				} else if (_gps.fix_type > 2) {
-					/* fuse GPS updates */
+					if (!_initialized) {
+						InitialiseFilter();
+						continue;
+					}
 
 					/* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */
 					calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD);
@@ -445,16 +506,32 @@ FixedwingEstimator::task_main()
 					fuseVelData = false;
 					fusePosData = false;
 					fuseHgtData = false;
+
+					newDataGps = true;
 				}
+
+			} else {
+				newDataGps = false;
 			}
 
 			bool mag_updated;
 
 			orb_check(_mag_sub, &mag_updated);
+
 			if (mag_updated) {
 				orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
 
 				if (_initialized) {
+
+					magData.x = _mag.x;
+					magBias.x = -_mag_offsets.x_offset;
+
+					magData.y = _mag.y;
+					magBias.y = -_mag_offsets.y_offset;
+
+					magData.z = _mag.z;
+					magBias.z = -_mag_offsets.z_offset;
+
 					fuseMagData = true;
 					RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms));
 					FuseMagnetometer();
@@ -466,11 +543,14 @@ FixedwingEstimator::task_main()
 
 			bool airspeed_updated;
 			orb_check(_airspeed_sub, &airspeed_updated);
+
 			if (airspeed_updated) {
 				orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
 
 				if (_initialized && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) {
 
+					VtasMeas = _airspeed.true_airspeed_m_s;
+
 					fuseVtasData = true;
 					RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
 					FuseAirspeed();
@@ -518,11 +598,11 @@ FixedwingEstimator::start()
 
 	/* start the task */
 	_estimator_task = task_spawn_cmd("fw_att_pos_estimator",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       12048,
-				       (main_t)&FixedwingEstimator::task_main_trampoline,
-				       nullptr);
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 5,
+					 12048,
+					 (main_t)&FixedwingEstimator::task_main_trampoline,
+					 nullptr);
 
 	if (_estimator_task < 0) {
 		warn("task start failed");