fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 11:07:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

sih remove mag and use standalone sensor_mag_sim

Browse Source
This commit is contained in:
Daniel Agar
2022-08-23 16:13:58 -04:00
parent 99a20646e2
commit c5336abba2
38 changed files with 112 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/simulator_sih/CMakeLists.txt
-1
View File
@@ -44,7 +44,6 @@ px4_add_module(
mathlib
drivers_accelerometer
drivers_gyroscope
drivers_magnetometer
)
if(PX4_PLATFORM MATCHES "posix")
src/modules/simulation/simulator_sih/sih.cpp
-20
View File
@@ -67,7 +67,6 @@ void Sih::run()
{
_px4_accel.set_temperature(T1_C);
_px4_gyro.set_temperature(T1_C);
_px4_mag.set_temperature(T1_C);
parameters_updated();
init_variables();
@@ -218,15 +217,6 @@ void Sih::sensor_step()
reconstruct_sensors_signals(now);
// magnetometer published at 50 Hz
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));
}
// baro published at 20 Hz
if (now - _baro_time >= 50_ms
&& fabs(_baro_offset_m) < 10000) {
@@ -292,13 +282,8 @@ void Sih::parameters_updated()
// guards against too small determinants
_Im1 = 100.0f * inv(static_cast<typeof _I>(100.0f * _I));
_mu_I = Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get());
_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();
_distance_snsr_min = _sih_distance_snsr_min.get();
_distance_snsr_max = _sih_distance_snsr_max.get();
@@ -485,11 +470,6 @@ void Sih::reconstruct_sensors_signals(const hrt_abstime &time_now_us)
_px4_accel.update(time_now_us, acc(0), acc(1), acc(2));
_px4_gyro.update(time_now_us, gyro(0), gyro(1), gyro(2));
_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
_baro_p_mBar = CONSTANTS_STD_PRESSURE_MBAR * // reconstructed pressure in mBar
src/modules/simulation/simulator_sih/sih.hpp
+1 -12
View File
@@ -65,7 +65,6 @@
#include <drivers/drv_hrt.h> // to get the real time
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
@@ -122,7 +121,6 @@ private:
// simulated sensors
PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Magnetometer _px4_mag{197388}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
uORB::Publication<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::Publication<distance_sensor_s> _distance_snsr_pub{ORB_ID(distance_sensor)};
@@ -189,7 +187,6 @@ private:
hrt_abstime _baro_time{0};
hrt_abstime _gps_time{0};
hrt_abstime _airspeed_time{0};
hrt_abstime _mag_time{0};
hrt_abstime _dist_snsr_time{0};
bool _grounded{true};// whether the vehicle is on the ground
@@ -258,7 +255,6 @@ private:
// };
// sensors reconstruction
matrix::Vector3f _mag{};
matrix::Vector3f _gps_vel{};
double _gps_lat, _gps_lat_noiseless;
double _gps_lon, _gps_lon_noiseless;
@@ -272,10 +268,9 @@ private:
matrix::Vector3f _W_I; // weight of the vehicle in inertial frame [N]
matrix::Matrix3f _I; // vehicle inertia matrix
matrix::Matrix3f _Im1; // inverse of the inertia matrix
matrix::Vector3f _mu_I; // NED magnetic field in inertial frame [G]
int _gps_used;
float _baro_offset_m, _mag_offset_x, _mag_offset_y, _mag_offset_z;
float _baro_offset_m;
float _distance_snsr_min, _distance_snsr_max, _distance_snsr_override;
// parameters defined in sih_params.c
@@ -298,14 +293,8 @@ private:
(ParamInt<px4::params::SIH_LOC_LAT0>) _sih_lat0,
(ParamInt<px4::params::SIH_LOC_LON0>) _sih_lon0,
(ParamFloat<px4::params::SIH_LOC_H0>) _sih_h0,
(ParamFloat<px4::params::SIH_LOC_MU_X>) _sih_mu_x,
(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_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,
(ParamFloat<px4::params::SIH_DISTSNSR_MIN>) _sih_distance_snsr_min,
(ParamFloat<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max,
(ParamFloat<px4::params::SIH_DISTSNSR_OVR>) _sih_distance_snsr_override,
src/modules/simulation/simulator_sih/sih_params.c
-89
View File
@@ -284,66 +284,6 @@ PARAM_DEFINE_INT32(SIH_LOC_LON0, -737578370);
*/
PARAM_DEFINE_FLOAT(SIH_LOC_H0, 32.34f);
/**
* North magnetic field at the initial location
*
* This value represents the North magnetic field at the initial location.
*
* A magnetic field calculator can be found on the NOAA website
* Note, the values need to be converted from nano Tesla to Gauss
*
* LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
* to represent a physical ground location on Earth.
*
* @unit gauss
* @min -1.0
* @max 1.0
* @decimal 2
* @increment 0.001
* @group Simulation In Hardware
*/
PARAM_DEFINE_FLOAT(SIH_LOC_MU_X, 0.179f);
/**
* East magnetic field at the initial location
*
* This value represents the East magnetic field at the initial location.
*
* A magnetic field calculator can be found on the NOAA website
* Note, the values need to be converted from nano Tesla to Gauss
*
* LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
* to represent a physical ground location on Earth.
*
* @unit gauss
* @min -1.0
* @max 1.0
* @decimal 2
* @increment 0.001
* @group Simulation In Hardware
*/
PARAM_DEFINE_FLOAT(SIH_LOC_MU_Y, -0.045f);
/**
* Down magnetic field at the initial location
*
* This value represents the Down magnetic field at the initial location.
*
* A magnetic field calculator can be found on the NOAA website
* Note, the values need to be converted from nano Tesla to Gauss
*
* LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
* to represent a physical ground location on Earth.
*
* @unit gauss
* @min -1.0
* @max 1.0
* @decimal 2
* @increment 0.001
* @group Simulation In Hardware
*/
PARAM_DEFINE_FLOAT(SIH_LOC_MU_Z, 0.504f);
/**
* Number of GPS satellites used
*
@@ -363,35 +303,6 @@ PARAM_DEFINE_INT32(SIH_GPS_USED, 10);
*/
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);
/**
* distance sensor minimum range
*