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);