aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: correct on-ground correction

Browse Source 
Using the cached ground_height_difference may mean
you end up at the wrong height if you were near
the edge of an SRTM square and had high horizontal
velocity
This commit is contained in:
Peter Barker 2016-11-23 11:09:33 +11:00
parent 319cfa766b
commit e24f3f9419
2 changed files with 18 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/SITL/SIM_Aircraft.cpp
View File

@ -119,17 +119,25 @@ bool Aircraft::parse_home(const char *home_str, Location &loc, float &yaw_degree
}
/*
return true if we are on the ground
return difference in altitude between home position and current loc
*/
bool Aircraft::on_ground(const Vector3f &pos)
float Aircraft::ground_height_difference() const
{
float h1, h2;
if (sitl->terrain_enable && terrain &&
terrain->height_amsl(home, h1, false) &&
terrain->height_amsl(location, h2, false)) {
ground_height_difference = h2 - h1;
return h2 - h1;
}
return (-pos.z) + home.alt*0.01f <= ground_level + frame_height + ground_height_difference;
return 0.0f;
}
/*
return true if we are on the ground
*/
bool Aircraft::on_ground(const Vector3f &pos) const
{
return (-pos.z) + home.alt*0.01f <= ground_level + frame_height + ground_height_difference();
}
/*
@ -438,8 +446,8 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
// new velocity vector
velocity_ef += accel_earth * delta_time;
const bool was_on_ground = on_ground(position);
// new position vector
Vector3f old_position = position;
position += velocity_ef * delta_time;
// velocity relative to air mass, in earth frame
@ -456,11 +464,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
// constrain height to the ground
if (on_ground(position)) {
if (!on_ground(old_position) && AP_HAL::millis() - last_ground_contact_ms > 1000) {
if (!was_on_ground && AP_HAL::millis() - last_ground_contact_ms > 1000) {
printf("Hit ground at %f m/s\n", velocity_ef.z);
last_ground_contact_ms = AP_HAL::millis();
}
position.z = -(ground_level + frame_height - home.alt*0.01f + ground_height_difference);
position.z = -(ground_level + frame_height - home.alt*0.01f + ground_height_difference());
switch (ground_behavior) {
case GROUND_BEHAVIOR_NONE:

6
libraries/SITL/SIM_Aircraft.h
View File

@ -166,12 +166,12 @@ protected:
bool use_smoothing;
AP_Terrain *terrain;
float ground_height_difference;
float ground_height_difference() const;
const float FEET_TO_METERS = 0.3048f;
const float KNOTS_TO_METERS_PER_SECOND = 0.51444f;
bool on_ground(const Vector3f &pos);
bool on_ground(const Vector3f &pos) const;
/* update location from position */
void update_position(void);