Merge pull request #3485 from PX4/ekf2_blockparam

Ekf2 blockparam

src/modules/controllib/block/BlockParam.cpp

@@ -82,9 +82,10 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
 
 template <class T>
 BlockParam<T>::BlockParam(Block *block, const char *name,
-			  bool parent_prefix) :
+			  bool parent_prefix, T *extern_address) :
 	BlockParamBase(block, name, parent_prefix),
-	_val()
+	_val(),
+	_extern_address(extern_address)
 {
 	update();
 }
@@ -93,12 +94,25 @@ template <class T>
 T BlockParam<T>::get() { return _val; }
 
 template <class T>
-void BlockParam<T>::set(T val) { _val = val; }
+void BlockParam<T>::set(T val)
+{
+	_val = val;
+
+	if (_extern_address != NULL) {
+		*_extern_address = val;
+	}
+}
 
 template <class T>
 void BlockParam<T>::update()
 {
-	if (_handle != PARAM_INVALID) { param_get(_handle, &_val); }
+	if (_handle != PARAM_INVALID) {
+		param_get(_handle, &_val);
+
+		if (_extern_address != NULL) {
+			*_extern_address = _val;
+		}
+	}
 }
 
 template <class T>

src/modules/controllib/block/BlockParam.hpp

@@ -77,7 +77,7 @@ class BlockParam : public BlockParamBase
 {
 public:
 	BlockParam(Block *block, const char *name,
-		   bool parent_prefix = true);
+		   bool parent_prefix = true, T *extern_address = NULL);
 	T get();
 	void commit();
 	void set(T val);
@@ -85,6 +85,7 @@ public:
 	virtual ~BlockParam();
 protected:
 	T _val;
+	T *_extern_address;
 };
 
 typedef BlockParam<float> BlockParamFloat;

src/modules/ekf2/ekf2_main.cpp

@@ -63,6 +63,7 @@
 #include <mavlink/mavlink_log.h>
 #include <platforms/px4_defines.h>
 #include <drivers/drv_hrt.h>
+#include <controllib/uorb/blocks.hpp>
 
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -71,6 +72,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/parameter_update.h>
 
 #include <ecl/EKF/ekf.h>
 
@@ -86,7 +88,7 @@ Ekf2 *instance;
 }
 
 
-class Ekf2
+class Ekf2 : public control::SuperBlock
 {
 public:
 	/**
@@ -133,6 +135,31 @@ private:
 	math::LowPassFilter2p _lp_pitch_rate;
 	math::LowPassFilter2p _lp_yaw_rate;
 
+	control::BlockParamFloat *_mag_delay_ms;
+	control::BlockParamFloat *_baro_delay_ms;
+	control::BlockParamFloat *_gps_delay_ms;
+	control::BlockParamFloat *_airspeed_delay_ms;
+	control::BlockParamFloat 	*_requiredEph;
+	control::BlockParamFloat 	*_requiredEpv;
+
+	control::BlockParamFloat *_gyro_noise;
+	control::BlockParamFloat *_accel_noise;
+
+	// process noise
+	control::BlockParamFloat *_gyro_bias_p_noise;
+	control::BlockParamFloat *_accel_bias_p_noise;
+	control::BlockParamFloat *_gyro_scale_p_noise;
+	control::BlockParamFloat *_mag_p_noise;
+	control::BlockParamFloat *_wind_vel_p_noise;
+
+	control::BlockParamFloat *_gps_vel_noise;
+	control::BlockParamFloat *_gps_pos_noise;
+	control::BlockParamFloat *_baro_noise;
+
+	control::BlockParamFloat *_mag_heading_noise;	// measurement noise used for simple heading fusion
+	control::BlockParamFloat *_mag_declination_deg;	// magnetic declination in degrees
+	control::BlockParamFloat *_heading_innov_gate;	// innovation gate for heading innovation test
+
 	EstimatorBase *_ekf;
 
 
@@ -143,14 +170,40 @@ private:
 };
 
 Ekf2::Ekf2():
-_lp_roll_rate(250.0f, 30.0f),
-_lp_pitch_rate(250.0f, 30.0f),
-_lp_yaw_rate(250.0f, 20.0f)
+	SuperBlock(NULL, "EKF"),
+	_lp_roll_rate(250.0f, 30.0f),
+	_lp_pitch_rate(250.0f, 30.0f),
+	_lp_yaw_rate(250.0f, 20.0f)
 {
 	_ekf = new Ekf();
 	_att_pub = nullptr;
 	_lpos_pub = nullptr;
 	_control_state_pub = nullptr;
+
+	parameters *params = _ekf->getParamHandle();
+	_mag_delay_ms = new control::BlockParamFloat(this, "EKF2_MAG_DELAY", false, &params->mag_delay_ms);
+	_baro_delay_ms = new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, &params->baro_delay_ms);
+	_gps_delay_ms = new control::BlockParamFloat(this, "EKF2_GPS_DELAY", false, &params->gps_delay_ms);
+	_airspeed_delay_ms = new control::BlockParamFloat(this, "EKF2_ASP_DELAY", false, &params->airspeed_delay_ms);
+	_requiredEph = new control::BlockParamFloat(this, "EKF2_REQ_EPH", false, &params->requiredEph);
+	_requiredEpv = new control::BlockParamFloat(this, "EKF2_REQ_EPV", false, &params->requiredEpv);
+
+	_gyro_noise = new control::BlockParamFloat(this, "EKF2_G_NOISE", false, &params->gyro_noise);
+	_accel_noise = new control::BlockParamFloat(this, "EKF2_ACC_NOISE", false, &params->accel_noise);
+
+	_gyro_bias_p_noise = new control::BlockParamFloat(this, "EKF2_GB_NOISE", false, &params->gyro_bias_p_noise);
+	_accel_bias_p_noise = new control::BlockParamFloat(this, "EKF2_ACCB_NOISE", false, &params->accel_bias_p_noise);
+	_gyro_scale_p_noise = new control::BlockParamFloat(this, "EKF2_GS_NOISE", false, &params->gyro_scale_p_noise);
+	_mag_p_noise = new control::BlockParamFloat(this, "EKF2_MAG_NOISE", false, &params->mag_p_noise);
+	_wind_vel_p_noise = new control::BlockParamFloat(this, "EKF2_WIND_NOISE", false, &params->wind_vel_p_noise);
+
+	_gps_vel_noise = new control::BlockParamFloat(this, "EKF2_GPS_V_NOISE", false, &params->gps_vel_noise);
+	_gps_pos_noise = new control::BlockParamFloat(this, "EKF2_GPS_P_NOISE", false, &params->gps_pos_noise);
+	_baro_noise = new control::BlockParamFloat(this, "EKF2_BARO_NOISE", false, &params->baro_noise);
+
+	_mag_heading_noise = new control::BlockParamFloat(this, "EKF2_HEAD_NOISE", false, &params->mag_heading_noise);
+	_mag_declination_deg = new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, &params->mag_declination_deg);
+	_heading_innov_gate = new control::BlockParamFloat(this, "EKF2_H_INOV_GATE", false, &params->heading_innov_gate);
 }
 
 Ekf2::~Ekf2()
@@ -182,6 +235,9 @@ void Ekf2::task_main()
 	fds[0].fd = _sensors_sub;
 	fds[0].events = POLLIN;
 
+	// initialise parameter cache
+	updateParams();
+
 	while (!_task_should_exit) {
 		int ret = px4_poll(fds, 1, 1000);
 
@@ -195,6 +251,8 @@ void Ekf2::task_main()
 			continue;
 		}
 
+		updateParams();
+
 		bool gps_updated = false;
 		bool airspeed_updated = false;
 
@@ -294,11 +352,13 @@ void Ekf2::task_main()
 
 		// Position of local NED origin in GPS / WGS84 frame
 		lpos.ref_timestamp = _ekf->_last_gps_origin_time_us; // Time when reference position was set
- 		lpos.xy_global = _ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
- 		lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt)
+		lpos.xy_global =
+			_ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
+		lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt)
 		lpos.ref_lat = _ekf->_posRef.lat_rad * (double)180.0 * M_PI; // Reference point latitude in degrees
 		lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees
-		lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level		
+		lpos.ref_alt =
+			_ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level
 
 		// The rotation of the tangent plane vs. geographical north
 		lpos.yaw = 0.0f;
@@ -308,7 +368,7 @@ void Ekf2::task_main()
 		lpos.surface_bottom_timestamp	= 0; // Time when new bottom surface found
 		lpos.dist_bottom_valid = false; // true if distance to bottom surface is valid
 
- 		// TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres 
+		// TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
 		// TODO: Should use sqrt of filter position variances
 		lpos.eph = gps.eph;
 		lpos.epv = gps.epv;
@@ -316,6 +376,7 @@ void Ekf2::task_main()
 		// publish vehicle local position data
 		if (_lpos_pub == nullptr) {
 			_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);
+
 		} else {
 			orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
 		}
@@ -335,6 +396,7 @@ void Ekf2::task_main()
 		// publish control state data
 		if (_control_state_pub == nullptr) {
 			_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
+
 		} else {
 			orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
 		}
@@ -353,12 +415,14 @@ void Ekf2::task_main()
 		// publish vehicle attitude data
 		if (_att_pub == nullptr) {
 			_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
+
 		} else {
 			orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
 		}
 
 		// generate and publish global position data
 		struct vehicle_global_position_s global_pos;
+
 		if (_ekf->position_is_valid()) {
 			// TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator
 
@@ -391,6 +455,7 @@ void Ekf2::task_main()
 
 			if (_vehicle_global_position_pub == nullptr) {
 				_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+
 			} else {
 				orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
 			}
@@ -467,7 +532,7 @@ int ekf2_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "print")) {
 		if (ekf2::instance != nullptr) {
-			
+
 			return 0;
 		}
 

src/modules/ekf2/ekf2_params.c

@@ -0,0 +1,226 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file parameters.c
+ * Parameter definition for ekf2.
+ *
+ * @author Roman Bast <bapstroman@gmail.com>
+ *
+ */
+
+#include <systemlib/param/param.h>
+
+/**
+* Magnetometer measurement delay
+*
+* @group EKF2
+* @min 0
+* @max 300
+* @unit ms
+*/
+PARAM_DEFINE_FLOAT(EKF2_MAG_DELAY, 0);
+
+/**
+ * Barometer measurement delay
+ *
+ * @group EKF2
+ * @min 0
+ * @max 300
+ * @unit ms
+ */
+PARAM_DEFINE_FLOAT(EKF2_BARO_DELAY, 0);
+
+/**
+ * GPS measurement delay
+ *
+ * @group EKF2
+ * @min 0
+ * @max 300
+ * @unit ms
+ */
+PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 200);
+
+/**
+ * Airspeed measurement delay
+ *
+ * @group EKF2
+ * @min 0
+ * @max 300
+ * @unit ms
+ */
+PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 200);
+
+/**
+ * Required EPH to use GPS.
+ *
+ * @group EKF2
+ * @min 2
+ * @max 100
+ * @unit m
+ */
+PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 10);
+
+/**
+ * Required EPV to use GPS.
+ *
+ * @group EKF2
+ * @min 2
+ * @max 100
+ * @unit m
+ */
+PARAM_DEFINE_FLOAT(EKF2_REQ_EPV, 20);
+
+/**
+ * Gyro noise.
+ *
+ * @group EKF2
+ * @min 0.0001
+ * @max 0.05
+ */
+PARAM_DEFINE_FLOAT(EKF2_G_NOISE, 0.001f);
+
+/**
+ * Process noise for delta velocity prediction.
+ *
+ * @group EKF2
+ * @min 0.01
+ * @max 1
+ * @unit
+ */
+PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 0.1f);
+
+/**
+ * Process noise for delta angle bias prediction.
+ *
+ * @group EKF2
+ * @min 0
+ * @max 0.0001
+ * @unit
+ */
+PARAM_DEFINE_FLOAT(EKF2_GB_NOISE, 1e-5f);
+
+/**
+ * Process noise for delta velocity z bias prediction.
+ *
+ * @group EKF2
+ * @min 0.000001
+ * @max 0.01
+ * @unit
+ */
+PARAM_DEFINE_FLOAT(EKF2_ACCB_NOISE, 1e-3f);
+
+/**
+ * Process noise for delta angle scale prediction.
+ *
+ * @group EKF2
+ * @min 0.000001
+ * @max 0.01
+ * @unit
+ */
+PARAM_DEFINE_FLOAT(EKF2_GS_NOISE, 1e-4f);
+
+/**
+ * Process noise for earth magnetic field and bias prediction.
+ *
+ * @group EKF2
+ * @min 0.0001
+ * @max 0.1
+ * @unit
+ */
+PARAM_DEFINE_FLOAT(EKF2_MAG_NOISE, 1e-2f);
+
+/**
+ * Process noise for wind velocity prediction.
+ *
+ * @group EKF2
+ * @min 0.01
+ * @max 1
+ * @unit
+ */
+PARAM_DEFINE_FLOAT(EKF2_WIND_NOISE, 0.05f);
+
+/**
+ * Measurement noise for gps velocity.
+ *
+ * @group EKF2
+ * @min 0.001
+ * @max 0.5
+ * @unit
+ */
+PARAM_DEFINE_FLOAT(EKF2_GPS_V_NOISE, 0.05f);
+
+/**
+ * Measurement noise for gps position.
+ *
+ * @group EKF2
+ * @min 0.01
+ * @max 5
+ * @unit
+ */
+PARAM_DEFINE_FLOAT(EKF2_GPS_P_NOISE, 1.0f);
+
+/**
+ * Measurement noise for barometer.
+ *
+ * @group EKF2
+ * @min 0.001
+ * @max 1
+ * @unit
+ */
+PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 0.1f);
+
+/**
+ * Measurement noise for mag heading fusion.
+ *
+ * @group EKF2
+ * @min 0.0001
+ * @max 0.1
+ * @unit
+ */
+PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 3e-2f);
+
+/**
+ * Magnetic declination
+ *
+ * @group EKF2
+ * @unit degrees
+ */
+PARAM_DEFINE_FLOAT(EKF2_MAG_DECL, 0);
+
+/**
+ * Gate for maginetic heading fusion
+ *
+ * @group EKF2
+ */
+PARAM_DEFINE_FLOAT(EKF2_H_INOV_GATE, 0.5f);