added parameter interface to ekf2

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;
 

src/modules/ekf2/ekf2_params.c

+/****************************************************************************
+ *
+ *   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);