You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
57 lines
1.4 KiB
57 lines
1.4 KiB
#include <AP_HAL/AP_HAL.h> |
|
#include "AC_PrecLand_SITL.h" |
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
|
|
#include <stdio.h> |
|
|
|
// init - perform initialisation of this backend |
|
void AC_PrecLand_SITL::init() |
|
{ |
|
} |
|
|
|
// update - give chance to driver to get updates from sensor |
|
void AC_PrecLand_SITL::update() |
|
{ |
|
const uint32_t now = AP_HAL::millis(); |
|
if (_los_meas_time_ms + 10 > now) { // 100Hz update |
|
return; |
|
} |
|
|
|
// get new sensor data; we always point home |
|
Vector3f home; |
|
if (! AP::ahrs().get_relative_position_NED_home(home)) { |
|
_state.healthy = false; |
|
return; |
|
} |
|
if (home.length() > 10.0f) { // we can see the target out to 10 metres |
|
return; |
|
} |
|
_state.healthy = true; |
|
|
|
const Matrix3f &body_to_ned = AP::ahrs().get_rotation_body_to_ned(); |
|
|
|
_los_meas_body = body_to_ned.mul_transpose(-home); |
|
_los_meas_body /= _los_meas_body.length(); |
|
_los_meas_time_ms = now; |
|
} |
|
|
|
bool AC_PrecLand_SITL::have_los_meas() { |
|
return AP_HAL::millis() - _los_meas_time_ms < 1000; |
|
} |
|
|
|
|
|
// provides a unit vector towards the target in body frame |
|
// returns same as have_los_meas() |
|
bool AC_PrecLand_SITL::get_los_body(Vector3f& ret) { |
|
if (AP_HAL::millis() - _los_meas_time_ms > 1000) { |
|
// no measurement for a full second; no vector available |
|
return false; |
|
} |
|
ret = _los_meas_body; |
|
return true; |
|
} |
|
|
|
#endif
|
|
|