yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

SIH: add mag offset from parameters

Browse Source 
An absolute value superior to 10000 block mag publication
This commit is contained in:
Nicolas MARTIN
2021-02-22 11:28:39 +01:00
committed by Daniel Agar
parent 8f54dc402d
commit 50ec809fdf
3 changed files with 44 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/sih/sih.cpp
View File

@@ -125,9 +125,11 @@ void Sih::Run()
_px4_gyro.update(_now, _gyro(0), _gyro(1), _gyro(2));
// magnetometer published at 50 Hz
if (_now - _mag_time >= 20_ms) {
if (_now - _mag_time >= 20_ms
&& fabs(_mag_offset_x) < 10000
&& fabs(_mag_offset_y) < 10000
&& fabs(_mag_offset_z) < 10000) {
_mag_time = _now;
_px4_mag.update(_now, _mag(0), _mag(1), _mag(2));
}
@@ -186,6 +188,9 @@ void Sih::parameters_updated()
_gps_used = _sih_gps_used.get();
_baro_offset_m = _sih_baro_offset.get();
_mag_offset_x = _sih_mag_offset_x.get();
_mag_offset_y = _sih_mag_offset_y.get();
_mag_offset_z = _sih_mag_offset_z.get();
}
// initialization of the variables for the simulator
@@ -301,6 +306,9 @@ void Sih::reconstruct_sensors_signals()
_acc = _C_IB.transpose() * (_v_I_dot - Vector3f(0.0f, 0.0f, CONSTANTS_ONE_G)) + noiseGauss3f(0.5f, 1.7f, 1.4f);
_gyro = _w_B + noiseGauss3f(0.14f, 0.07f, 0.03f);
_mag = _C_IB.transpose() * _mu_I + noiseGauss3f(0.02f, 0.02f, 0.03f);
_mag(0) += _mag_offset_x;
_mag(1) += _mag_offset_y;
_mag(2) += _mag_offset_z;
// barometer
float altitude = (_H0 - _p_I(2)) + _baro_offset_m + generate_wgn() * 0.14f; // altitude with noise

7
src/modules/sih/sih.hpp
View File

@@ -176,7 +176,7 @@ private:
matrix::Vector3f _mu_I; // NED magnetic field in inertial frame [G]
int _gps_used;
float _baro_offset_m;
float _baro_offset_m, _mag_offset_x, _mag_offset_y, _mag_offset_z;
// parameters defined in sih_params.c
DEFINE_PARAMETERS(
@@ -202,6 +202,9 @@ private:
(ParamFloat<px4::params::SIH_LOC_MU_Y>) _sih_mu_y,
(ParamFloat<px4::params::SIH_LOC_MU_Z>) _sih_mu_z,
(ParamInt<px4::params::SIH_GPS_USED>) _sih_gps_used,
(ParamFloat<px4::params::SIH_BARO_OFFSET>) _sih_baro_offset
(ParamFloat<px4::params::SIH_BARO_OFFSET>) _sih_baro_offset,
(ParamFloat<px4::params::SIH_MAG_OFFSET_X>) _sih_mag_offset_x,
(ParamFloat<px4::params::SIH_MAG_OFFSET_Y>) _sih_mag_offset_y,
(ParamFloat<px4::params::SIH_MAG_OFFSET_Z>) _sih_mag_offset_z
)
};

29
src/modules/sih/sih_params.c
View File

@@ -362,3 +362,32 @@ PARAM_DEFINE_INT32(SIH_GPS_USED, 10);
* @group Simulation In Hardware
*/
PARAM_DEFINE_FLOAT(SIH_BARO_OFFSET, 0.0f);
/**
* magnetometer X offset in Gauss
*
* Absolute value superior to 10000 will disable magnetometer
*
* @unit gauss
* @group Simulation In Hardware
*/
PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_X, 0.0f);
/**
* magnetometer Y offset in Gauss
*
* Absolute value superior to 10000 will disable magnetometer
*
* @unit gauss
* @group Simulation In Hardware
*/
PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_Y, 0.0f);
/**
* magnetometer Z offset in Gauss
*
* Absolute value superior to 10000 will disable magnetometer
*
* @unit gauss
* @group Simulation In Hardware
*/
PARAM_DEFINE_FLOAT(SIH_MAG_OFFSET_Z, 0.0f);