From fe07079367015b2964e5e8f79d3cae30d1ddfec5 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 9 Jan 2016 09:11:31 +0100 Subject: [PATCH 1/4] added parameter interface to ekf2 --- src/modules/ekf2/ekf2_main.cpp | 66 +++++++++- src/modules/ekf2/ekf2_params.c | 226 +++++++++++++++++++++++++++++++++ 2 files changed, 288 insertions(+), 4 deletions(-) create mode 100644 src/modules/ekf2/ekf2_params.c diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 2bd5d25fa0..27c3e99110 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include @@ -71,6 +72,7 @@ #include #include #include +#include #include @@ -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, ¶ms->mag_delay_ms); + _baro_delay_ms = new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, ¶ms->baro_delay_ms); + _gps_delay_ms = new control::BlockParamFloat(this, "EKF2_GPS_DELAY", false, ¶ms->gps_delay_ms); + _airspeed_delay_ms = new control::BlockParamFloat(this, "EKF2_ASP_DELAY", false, ¶ms->airspeed_delay_ms); + _requiredEph = new control::BlockParamFloat(this, "EKF2_REQ_EPH", false, ¶ms->requiredEph); + _requiredEpv = new control::BlockParamFloat(this, "EKF2_REQ_EPV", false, ¶ms->requiredEpv); + + _gyro_noise = new control::BlockParamFloat(this, "EKF2_G_NOISE", false, ¶ms->gyro_noise); + _accel_noise = new control::BlockParamFloat(this, "EKF2_ACC_NOISE", false, ¶ms->accel_noise); + + _gyro_bias_p_noise = new control::BlockParamFloat(this, "EKF2_GB_NOISE", false, ¶ms->gyro_bias_p_noise); + _accel_bias_p_noise = new control::BlockParamFloat(this, "EKF2_ACCB_NOISE", false, ¶ms->accel_bias_p_noise); + _gyro_scale_p_noise = new control::BlockParamFloat(this, "EKF2_GS_NOISE", false, ¶ms->gyro_scale_p_noise); + _mag_p_noise = new control::BlockParamFloat(this, "EKF2_MAG_NOISE", false, ¶ms->mag_p_noise); + _wind_vel_p_noise = new control::BlockParamFloat(this, "EKF2_WIND_NOISE", false, ¶ms->wind_vel_p_noise); + + _gps_vel_noise = new control::BlockParamFloat(this, "EKF2_GPS_V_NOISE", false, ¶ms->gps_vel_noise); + _gps_pos_noise = new control::BlockParamFloat(this, "EKF2_GPS_P_NOISE", false, ¶ms->gps_pos_noise); + _baro_noise = new control::BlockParamFloat(this, "EKF2_BARO_NOISE", false, ¶ms->baro_noise); + + _mag_heading_noise = new control::BlockParamFloat(this, "EKF2_HEAD_NOISE", false, ¶ms->mag_heading_noise); + _mag_declination_deg = new control::BlockParamFloat(this, "EKF2_MAG_DECL", false, ¶ms->mag_declination_deg); + _heading_innov_gate = new control::BlockParamFloat(this, "EKF2_H_INOV_GATE", false, ¶ms->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; diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c new file mode 100644 index 0000000000..ea6643d329 --- /dev/null +++ b/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 + * + */ + +#include + +/** +* 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); From 8a9b27f8f3e290a62dede0f660650652e0261597 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 9 Jan 2016 10:49:18 +0100 Subject: [PATCH 2/4] ecl ekf: added parameter interface --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 2af5856361..a11709956f 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 2af5856361adf7ede444cc7f3fd6cf725dfa0213 +Subproject commit a11709956fd0d082d4fde4da0ad8c79117a38990 From 0510d2cb569b374e694114f6f019b23f677545ef Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 9 Jan 2016 10:51:09 +0100 Subject: [PATCH 3/4] fixed code style --- src/modules/ekf2/ekf2_main.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 27c3e99110..22fce7ec6f 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -352,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; @@ -366,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; @@ -374,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); } @@ -393,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); } @@ -411,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 @@ -449,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); } @@ -525,7 +532,7 @@ int ekf2_main(int argc, char *argv[]) if (!strcmp(argv[1], "print")) { if (ekf2::instance != nullptr) { - + return 0; } From 88b2c6c78da055d4238088119bf9ce338c752431 Mon Sep 17 00:00:00 2001 From: Roman Date: Sun, 10 Jan 2016 21:13:30 +0100 Subject: [PATCH 4/4] blockparam: added support for external parameter copy --- src/modules/controllib/block/BlockParam.cpp | 22 +++++++++++++++++---- src/modules/controllib/block/BlockParam.hpp | 3 ++- 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index b560d7999e..c3ed27a572 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -82,9 +82,10 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref template BlockParam::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 T BlockParam::get() { return _val; } template -void BlockParam::set(T val) { _val = val; } +void BlockParam::set(T val) +{ + _val = val; + + if (_extern_address != NULL) { + *_extern_address = val; + } +} template void BlockParam::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 diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index db035f9f91..693a8ec3b2 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/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 BlockParamFloat;