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 gps fix loss simulation from parameters

Browse Source 
A new parameter allows to change the SIH number of gps satellites used
If it is below 4, fix is lost
This commit is contained in:
Nicolas MARTIN
2021-02-02 14:19:55 +01:00
committed by Daniel Agar
parent a8a9832b59
commit 1df63cb6b1
3 changed files with 42 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -61,7 +61,7 @@ Sih::Sih() :
parameters_updated();
init_variables();
init_sensors();
gps_no_fix();
const hrt_abstime task_start = hrt_absolute_time();
_last_run = task_start;
@@ -182,6 +182,8 @@ void Sih::parameters_updated()
_Im1 = inv(_I);
_mu_I = Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get());
_gps_used = _sih_gps_used.get();
}
// initialization of the variables for the simulator
@@ -197,10 +199,10 @@ void Sih::init_variables()
_u[0] = _u[1] = _u[2] = _u[3] = 0.0f;
}
void Sih::init_sensors()
void Sih::gps_fix()
{
_sensor_gps.fix_type = 3; // 3D fix
_sensor_gps.satellites_used = 8;
_sensor_gps.satellites_used = _gps_used;
_sensor_gps.heading = NAN;
_sensor_gps.heading_offset = NAN;
_sensor_gps.s_variance_m_s = 0.5f;
@@ -211,6 +213,21 @@ void Sih::init_sensors()
_sensor_gps.vdop = 1.1f;
}
void Sih::gps_no_fix()
{
_sensor_gps.fix_type = 0; // 3D fix
_sensor_gps.satellites_used = _gps_used;
_sensor_gps.heading = NAN;
_sensor_gps.heading_offset = NAN;
_sensor_gps.s_variance_m_s = 100.f;
_sensor_gps.c_variance_rad = 100.f;
_sensor_gps.eph = 100.f;
_sensor_gps.epv = 100.f;
_sensor_gps.hdop = 100.f;
_sensor_gps.vdop = 100.f;
}
// read the motor signals outputted from the mixer
void Sih::read_motors()
{
@@ -316,6 +333,13 @@ void Sih::send_gps()
_sensor_gps.cog_rad = atan2(_gps_vel(1),
_gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
if (_gps_used >= 4) {
gps_fix();
} else {
gps_no_fix();
}
_sensor_gps_pub.publish(_sensor_gps);
}

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

@@ -118,7 +118,8 @@ private:
static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre
void init_variables();
void init_sensors();
void gps_fix();
void gps_no_fix();
void read_motors();
void generate_force_and_torques();
void equations_of_motion();
@@ -174,6 +175,8 @@ private:
matrix::Matrix3f _Im1; // inverse of the intertia matrix
matrix::Vector3f _mu_I; // NED magnetic field in inertial frame [G]
int _gps_used;
// parameters defined in sih_params.c
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _imu_gyro_ratemax,
@@ -196,6 +199,7 @@ private:
(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
(ParamFloat<px4::params::SIH_LOC_MU_Z>) _sih_mu_z,
(ParamInt<px4::params::SIH_GPS_USED>) _sih_gps_used
)
};

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

@@ -343,3 +343,12 @@ PARAM_DEFINE_FLOAT(SIH_LOC_MU_Y, -0.045f);
* @group Simulation In Hardware
*/
PARAM_DEFINE_FLOAT(SIH_LOC_MU_Z, 0.504f);
/**
* Number of GPS satellites used
*
* @min 0
* @max 50
* @group Simulation In Hardware
*/
PARAM_DEFINE_INT32(SIH_GPS_USED, 10);