|
|
|
@ -103,8 +103,6 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = {
@@ -103,8 +103,6 @@ const AP_Param::GroupInfo SIM_Precland::var_info[] = {
|
|
|
|
|
|
|
|
|
|
void SIM_Precland::update(const Location &loc, const Vector3f &position) |
|
|
|
|
{ |
|
|
|
|
const uint32_t now = AP_HAL::millis(); |
|
|
|
|
|
|
|
|
|
if (!_enable) { |
|
|
|
|
_healthy = false; |
|
|
|
|
return; |
|
|
|
@ -114,11 +112,6 @@ void SIM_Precland::update(const Location &loc, const Vector3f &position)
@@ -114,11 +112,6 @@ void SIM_Precland::update(const Location &loc, const Vector3f &position)
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (now - _last_update_ms < 1000.0f * (1.0f / _rate)) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
_last_update_ms = now; |
|
|
|
|
|
|
|
|
|
const float distance_z = -position.z; |
|
|
|
|
const float origin_height_m = _origin_height * 0.01f; |
|
|
|
|
if (distance_z > _alt_limit + origin_height_m) { |
|
|
|
@ -136,6 +129,11 @@ void SIM_Precland::update(const Location &loc, const Vector3f &position)
@@ -136,6 +129,11 @@ void SIM_Precland::update(const Location &loc, const Vector3f &position)
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
center = center * 0.01f; // cm to m
|
|
|
|
|
const uint32_t now = AP_HAL::millis(); |
|
|
|
|
if (now - _last_update_ms < 1000.0f * (1.0f / _rate)) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
_last_update_ms = now; |
|
|
|
|
|
|
|
|
|
switch (_type) { |
|
|
|
|
case PRECLAND_TYPE_CONE: { |
|
|
|
|