yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00: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
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;