mirror of https://github.com/ArduPilot/ardupilot
58 lines
1.4 KiB
C++
58 lines
1.4 KiB
C++
#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
|