fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

added parameter interface to ekf2

Browse Source
This commit is contained in:
Roman 2016-01-09 09:11:31 +01:00
parent 4b659f1995
commit fe07079367
2 changed files with 288 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
src/modules/ekf2/ekf2_main.cpp
View File

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

226
src/modules/ekf2/ekf2_params.c Normal file
View File

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