|
|
@ -19,6 +19,7 @@ void AC_PrecLand_SITL::update() |
|
|
|
const Vector3d position = _sitl->precland_sim.get_target_position(); |
|
|
|
const Vector3d position = _sitl->precland_sim.get_target_position(); |
|
|
|
const Matrix3d body_to_ned = AP::ahrs().get_rotation_body_to_ned().todouble(); |
|
|
|
const Matrix3d body_to_ned = AP::ahrs().get_rotation_body_to_ned().todouble(); |
|
|
|
_los_meas_body = body_to_ned.mul_transpose(-position).tofloat(); |
|
|
|
_los_meas_body = body_to_ned.mul_transpose(-position).tofloat(); |
|
|
|
|
|
|
|
_distance_to_target = _sitl->precland_sim.option_enabled(SITL::SIM_Precland::Option::ENABLE_TARGET_DISTANCE) ? _los_meas_body.length() : 0.0f; |
|
|
|
_los_meas_body /= _los_meas_body.length(); |
|
|
|
_los_meas_body /= _los_meas_body.length(); |
|
|
|
_have_los_meas = true; |
|
|
|
_have_los_meas = true; |
|
|
|
_los_meas_time_ms = _sitl->precland_sim.last_update_ms(); |
|
|
|
_los_meas_time_ms = _sitl->precland_sim.last_update_ms(); |
|
|
@ -33,7 +34,6 @@ bool AC_PrecLand_SITL::have_los_meas() { |
|
|
|
return AP_HAL::millis() - _los_meas_time_ms < 1000; |
|
|
|
return AP_HAL::millis() - _los_meas_time_ms < 1000; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// provides a unit vector towards the target in body frame
|
|
|
|
// provides a unit vector towards the target in body frame
|
|
|
|
// returns same as have_los_meas()
|
|
|
|
// returns same as have_los_meas()
|
|
|
|
bool AC_PrecLand_SITL::get_los_body(Vector3f& ret) { |
|
|
|
bool AC_PrecLand_SITL::get_los_body(Vector3f& ret) { |
|
|
|