Added local position pub to att_pos_esitmator_ekf

2 files changed, 56 insertions, 24 deletions

diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 18fb6dcbc..ecca04dd7 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -41,6 +41,7 @@
 
 #include "KalmanNav.hpp"
 #include <systemlib/err.h>
+#include <geo/geo.h>
 
 // constants
 // Titterton pg. 52
@@ -67,15 +68,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	// attitude representations
 	C_nb(),
 	q(),
-	_accel_sub(-1),
-	_gyro_sub(-1),
-	_mag_sub(-1),
 	// subscriptions
 	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
 	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
 	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
 	// publications
 	_pos(&getPublications(), ORB_ID(vehicle_global_position)),
+	_localPos(&getPublications(), ORB_ID(vehicle_local_position)),
 	_att(&getPublications(), ORB_ID(vehicle_attitude)),
 	// timestamps
 	_pubTimeStamp(hrt_absolute_time()),
@@ -92,6 +91,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	phi(0), theta(0), psi(0),
 	vN(0), vE(0), vD(0),
 	lat(0), lon(0), alt(0),
+	lat0(0), lon0(0), alt0(0),
 	// parameters for ground station
 	_vGyro(this, "V_GYRO"),
 	_vAccel(this, "V_ACCEL"),
@@ -127,19 +127,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	lon = 0.0f;
 	alt = 0.0f;
 
-	// gyro, accel and mag subscriptions
-	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
-	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
-
-	struct accel_report accel;
-	orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel);
-
-	struct mag_report mag;
-	orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag);
-
 	// initialize rotation quaternion with a single raw sensor measurement
-	q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z);
+	_sensors.update();
+	q = init(
+		_sensors.accelerometer_m_s2[0],
+		_sensors.accelerometer_m_s2[1],
+		_sensors.accelerometer_m_s2[2],
+		_sensors.magnetometer_ga[0],
+		_sensors.magnetometer_ga[1],
+		_sensors.magnetometer_ga[2]);
 
 	// initialize dcm
 	C_nb = Dcm(q);
@@ -252,11 +248,21 @@ void KalmanNav::update()
 		setLatDegE7(_gps.lat);
 		setLonDegE7(_gps.lon);
 		setAltE3(_gps.alt);
+		// set reference position for
+		// local position
+		lat0 = lat;
+		lon0 = lon;
+		alt0 = alt;
+		// XXX map_projection has internal global
+		// states that multiple things could change, 
+		// should make map_projection take reference
+		// lat/lon and not have init
+		map_projection_init(lat0, lon0);
 		_positionInitialized = true;
 		warnx("initialized EKF state with GPS\n");
 		warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
 		       double(vN), double(vE), double(vD),
-		       lat, lon, alt);
+		       lat, lon, double(alt));
 	}
 
 	// prediction step
@@ -311,7 +317,7 @@ void KalmanNav::updatePublications()
 {
 	using namespace math;
 
-	// position publication
+	// global position publication
 	_pos.timestamp = _pubTimeStamp;
 	_pos.time_gps_usec = _gps.timestamp_position;
 	_pos.valid = true;
@@ -324,6 +330,31 @@ void KalmanNav::updatePublications()
 	_pos.vz = vD;
 	_pos.yaw = psi;
 
+	// local position publication
+	float x;
+	float y;
+	bool landed = alt < (alt0 + 0.1); // XXX improve?
+	map_projection_project(lat, lon, &x, &y); 
+	_localPos.timestamp = _pubTimeStamp;
+	_localPos.xy_valid = true;
+	_localPos.z_valid = true;
+	_localPos.v_xy_valid = true;
+	_localPos.v_z_valid = true;
+	_localPos.x = x;
+	_localPos.y = y;
+	_localPos.z = alt0 - alt;
+	_localPos.vx = vN;
+	_localPos.vy = vE;
+	_localPos.vz = vD;
+	_localPos.yaw = psi;
+	_localPos.xy_global = true;
+	_localPos.z_global = true;
+	_localPos.ref_timestamp = _pubTimeStamp;
+	_localPos.ref_lat = getLatDegE7();
+	_localPos.ref_lon = getLonDegE7();
+	_localPos.ref_alt = 0;
+	_localPos.landed = landed;
+
 	// attitude publication
 	_att.timestamp = _pubTimeStamp;
 	_att.roll = phi;
@@ -347,8 +378,10 @@ void KalmanNav::updatePublications()
 
 	// selectively update publications,
 	// do NOT call superblock do-all method
-	if (_positionInitialized)
+	if (_positionInitialized) {
 		_pos.update();
+		_localPos.update();
+	}
 
 	if (_attitudeInitialized)
 		_att.update();
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index 799fc2fb9..a69bde1a6 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -52,6 +52,7 @@
 
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/parameter_update.h>
@@ -142,12 +143,8 @@ protected:
 	control::UOrbSubscription<parameter_update_s> _param_update;    /**< parameter update sub. */
 	// publications
 	control::UOrbPublication<vehicle_global_position_s> _pos;       /**< position pub. */
+	control::UOrbPublication<vehicle_local_position_s> _localPos;   /**< local position pub. */
 	control::UOrbPublication<vehicle_attitude_s> _att;              /**< attitude pub. */
-
-	int			_accel_sub;		/**< Accelerometer subscription */
-	int			_gyro_sub;		/**< Gyroscope subscription */
-	int			_mag_sub;		/**< Magnetometer subscription */
-
 	// time stamps
 	uint64_t _pubTimeStamp;     /**< output data publication time stamp */
 	uint64_t _predictTimeStamp; /**< prediction time stamp */
@@ -164,8 +161,10 @@ protected:
 	float phi, theta, psi;                  /**< 3-2-1 euler angles */
 	float vN, vE, vD;                       /**< navigation velocity, m/s */
 	double lat, lon;                   	/**< lat, lon radians */
-	float alt;                   		/**< altitude, meters */
 	// parameters
+	float alt;                   		/**< altitude, meters */
+	double lat0, lon0;                   	/**< reference latitude and longitude */
+	float alt0;                   		/**<  refeerence altitude (ground height) */
 	control::BlockParamFloat _vGyro;      /**< gyro process noise */
 	control::BlockParamFloat _vAccel;     /**< accelerometer process noise  */
 	control::BlockParamFloat _rMag;       /**< magnetometer measurement noise  */