mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 15:38:29 -04:00
SITL: implement SIM_TERRAIN
either use AP_Terrain or flat earth
This commit is contained in:
parent
9b65376961
commit
f13248e5f5
@ -669,4 +669,34 @@ void SITL_State::loop_hook(void)
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
return height above the ground in meters
|
||||||
|
*/
|
||||||
|
float SITL_State::height_agl(void)
|
||||||
|
{
|
||||||
|
static float home_alt = -1;
|
||||||
|
|
||||||
|
if (home_alt == -1 && _sitl->state.altitude > 0) {
|
||||||
|
// remember home altitude as first non-zero altitude
|
||||||
|
home_alt = _sitl->state.altitude;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_terrain &&
|
||||||
|
_sitl->terrain_enable) {
|
||||||
|
// get height above terrain from AP_Terrain. This assumes
|
||||||
|
// AP_Terrain is working
|
||||||
|
float terrain_height_amsl;
|
||||||
|
struct Location location;
|
||||||
|
location.lat = _sitl->state.latitude*1.0e7;
|
||||||
|
location.lng = _sitl->state.longitude*1.0e7;
|
||||||
|
|
||||||
|
if (_terrain->height_amsl(location, terrain_height_amsl)) {
|
||||||
|
return _sitl->state.altitude - terrain_height_amsl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// fall back to flat earth model
|
||||||
|
return _sitl->state.altitude - home_alt;
|
||||||
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
@ -63,6 +63,7 @@ private:
|
|||||||
|
|
||||||
// these methods are static as they are called
|
// these methods are static as they are called
|
||||||
// from the timer
|
// from the timer
|
||||||
|
static float height_agl(void);
|
||||||
static void _update_barometer(float height);
|
static void _update_barometer(float height);
|
||||||
static void _update_compass(float rollDeg, float pitchDeg, float yawDeg);
|
static void _update_compass(float rollDeg, float pitchDeg, float yawDeg);
|
||||||
static void _update_flow(void);
|
static void _update_flow(void);
|
||||||
@ -105,7 +106,7 @@ private:
|
|||||||
static void _simulator_output(void);
|
static void _simulator_output(void);
|
||||||
static void _apply_servo_filter(float deltat);
|
static void _apply_servo_filter(float deltat);
|
||||||
static uint16_t _airspeed_sensor(float airspeed);
|
static uint16_t _airspeed_sensor(float airspeed);
|
||||||
static uint16_t _ground_sonar(float altitude);
|
static uint16_t _ground_sonar();
|
||||||
static float _gyro_drift(void);
|
static float _gyro_drift(void);
|
||||||
static float _rand_float(void);
|
static float _rand_float(void);
|
||||||
static Vector3f _rand_vec3f(void);
|
static Vector3f _rand_vec3f(void);
|
||||||
|
@ -60,33 +60,31 @@ float SITL_State::_gyro_drift(void)
|
|||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t SITL_State::_ground_sonar(float altitude)
|
/*
|
||||||
|
emulate an analog rangefinder
|
||||||
|
*/
|
||||||
|
uint16_t SITL_State::_ground_sonar(void)
|
||||||
{
|
{
|
||||||
static float home_alt = -1;
|
float altitude = height_agl();
|
||||||
|
|
||||||
if (home_alt == -1 && altitude > 0)
|
float voltage = 5.0f;
|
||||||
home_alt = altitude;
|
if (fabsf(_sitl->state.rollDeg) < 90 &&
|
||||||
|
fabsf(_sitl->state.pitchDeg) < 90) {
|
||||||
|
// adjust for apparent altitude with roll
|
||||||
|
altitude /= cos(radians(_sitl->state.rollDeg)) * cos(radians(_sitl->state.pitchDeg));
|
||||||
|
|
||||||
altitude = altitude - home_alt;
|
altitude += _sitl->sonar_noise * _rand_float();
|
||||||
|
|
||||||
float voltage = 5.0f;
|
// Altitude in in m, scaler in meters/volt
|
||||||
if (fabsf(_sitl->state.rollDeg) < 90 &&
|
voltage = altitude / _sitl->sonar_scale;
|
||||||
fabsf(_sitl->state.pitchDeg) < 90) {
|
voltage = constrain_float(voltage, 0, 5.0f);
|
||||||
// adjust for apparent altitude with roll
|
|
||||||
altitude /= cos(radians(_sitl->state.rollDeg)) * cos(radians(_sitl->state.pitchDeg));
|
|
||||||
|
|
||||||
altitude += _sitl->sonar_noise * _rand_float();
|
if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f) {
|
||||||
|
voltage = 5.0f;
|
||||||
// Altitude in in m, scaler in meters/volt
|
|
||||||
voltage = altitude / _sitl->sonar_scale;
|
|
||||||
voltage = constrain_float(voltage, 0, 5.0f);
|
|
||||||
|
|
||||||
if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f) {
|
|
||||||
voltage = 5.0f;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
return 1023*(voltage / 5.0f);
|
return 1023*(voltage / 5.0f);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
@ -177,7 +175,7 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative
|
|||||||
_ins->set_gyro(1, Vector3f(p2, q2, r2) + _ins->get_gyro_offsets(1));
|
_ins->set_gyro(1, Vector3f(p2, q2, r2) + _ins->get_gyro_offsets(1));
|
||||||
|
|
||||||
|
|
||||||
sonar_pin_value = _ground_sonar(altitude);
|
sonar_pin_value = _ground_sonar();
|
||||||
airspeed_pin_value = _airspeed_sensor(airspeed + (_sitl->aspd_noise * _rand_float()));
|
airspeed_pin_value = _airspeed_sensor(airspeed + (_sitl->aspd_noise * _rand_float()));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -32,7 +32,6 @@ void SITL_State::_update_flow(void)
|
|||||||
Vector3f gyro;
|
Vector3f gyro;
|
||||||
|
|
||||||
if (!_optical_flow ||
|
if (!_optical_flow ||
|
||||||
!_terrain ||
|
|
||||||
!_sitl->flow_enable) {
|
!_sitl->flow_enable) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@ -48,20 +47,6 @@ void SITL_State::_update_flow(void)
|
|||||||
|
|
||||||
OpticalFlow::OpticalFlow_state state;
|
OpticalFlow::OpticalFlow_state state;
|
||||||
|
|
||||||
// get height above terrain from AP_Terrain. This assumes
|
|
||||||
// AP_Terrain is working
|
|
||||||
float terrain_height_amsl;
|
|
||||||
struct Location location;
|
|
||||||
location.lat = _sitl->state.latitude*1.0e7;
|
|
||||||
location.lng = _sitl->state.longitude*1.0e7;
|
|
||||||
|
|
||||||
if (!_terrain->height_amsl(location, terrain_height_amsl)) {
|
|
||||||
// no terrain height available
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
float height_agl = _sitl->state.altitude - terrain_height_amsl;
|
|
||||||
|
|
||||||
// NED velocity vector in m/s
|
// NED velocity vector in m/s
|
||||||
Vector3f velocity(_sitl->state.speedN,
|
Vector3f velocity(_sitl->state.speedN,
|
||||||
_sitl->state.speedE,
|
_sitl->state.speedE,
|
||||||
@ -80,7 +65,7 @@ void SITL_State::_update_flow(void)
|
|||||||
// estimate range to centre of image
|
// estimate range to centre of image
|
||||||
float range;
|
float range;
|
||||||
if (rotmat.c.z > 0.05f) {
|
if (rotmat.c.z > 0.05f) {
|
||||||
range = height_agl / rotmat.c.z;
|
range = height_agl() / rotmat.c.z;
|
||||||
} else {
|
} else {
|
||||||
range = 1e38f;
|
range = 1e38f;
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user