Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: implement SIM_TERRAIN

Browse Source 
either use AP_Terrain or flat earth
This commit is contained in:
Andrew Tridgell 2015-01-03 21:47:54 +11:00
parent 9b65376961
commit f13248e5f5
4 changed files with 55 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_HAL_AVR_SITL/SITL_State.cpp
View File

@ -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

3
libraries/AP_HAL_AVR_SITL/SITL_State.h
View File

@ -63,6 +63,7 @@ private:
// these methods are static as they are called
// from the timer
static float height_agl(void);
static void _update_barometer(float height);
static void _update_compass(float rollDeg, float pitchDeg, float yawDeg);
static void _update_flow(void);
@ -105,7 +106,7 @@ private:
static void _simulator_output(void);
static void _apply_servo_filter(float deltat);
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 _rand_float(void);
static Vector3f _rand_vec3f(void);

40
libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
View File

@ -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)
home_alt = altitude;
float voltage = 5.0f;
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;
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 in in m, scaler in meters/volt
voltage = altitude / _sitl->sonar_scale;
voltage = constrain_float(voltage, 0, 5.0f);
altitude += _sitl->sonar_noise * _rand_float();
// 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;
}
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));
sonar_pin_value = _ground_sonar(altitude);
sonar_pin_value = _ground_sonar();
airspeed_pin_value = _airspeed_sensor(airspeed + (_sitl->aspd_noise * _rand_float()));
}

17
libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp
View File

@ -32,7 +32,6 @@ void SITL_State::_update_flow(void)
Vector3f gyro;
if (!_optical_flow ||
!_terrain ||
!_sitl->flow_enable) {
return;
}
@ -48,20 +47,6 @@ void SITL_State::_update_flow(void)
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
Vector3f velocity(_sitl->state.speedN,
_sitl->state.speedE,
@ -80,7 +65,7 @@ void SITL_State::_update_flow(void)
// estimate range to centre of image
float range;
if (rotmat.c.z > 0.05f) {
range = height_agl / rotmat.c.z;
range = height_agl() / rotmat.c.z;
} else {
range = 1e38f;
}