CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp
Views: 1798
1
#include <AP_HAL/AP_HAL.h>

2
#include "AC_PrecLand_SITL.h"

3


4
#if AC_PRECLAND_SITL_ENABLED

5


6
#include "AP_AHRS/AP_AHRS.h"

7
// init - perform initialisation of this backend

8
void AC_PrecLand_SITL::init()

9
{

10
    _sitl = AP::sitl();

11
}

12


13
// update - give chance to driver to get updates from sensor

14
void AC_PrecLand_SITL::update()

15
{

16
    _state.healthy = _sitl->precland_sim.healthy();

17


18
    if (_state.healthy && _sitl->precland_sim.last_update_ms() != _los_meas_time_ms) {

19
        const Vector3d position = _sitl->precland_sim.get_target_position();

20
        const Matrix3d body_to_ned = AP::ahrs().get_rotation_body_to_ned().todouble();

21
        _los_meas_body =  body_to_ned.mul_transpose(-position).tofloat();

22
        _distance_to_target = _sitl->precland_sim.option_enabled(SITL::SIM_Precland::Option::ENABLE_TARGET_DISTANCE) ? _los_meas_body.length() : 0.0f;

23
        _los_meas_body /= _los_meas_body.length();

24


25
        if (_frontend._orient != Rotation::ROTATION_PITCH_270) {

26
            // rotate body frame vector based on orientation

27
            // this is done to have homogeneity among backends

28
            // frontend rotates it back to get correct body frame vector

29
            _los_meas_body.rotate_inverse(_frontend._orient);

30
            _los_meas_body.rotate_inverse(ROTATION_PITCH_90);

31
        }

32


33
        _have_los_meas = true;

34
        _los_meas_time_ms = _sitl->precland_sim.last_update_ms();

35
    } else {

36
        _have_los_meas = false;

37
    }

38


39
    _have_los_meas = _have_los_meas && AP_HAL::millis() - _los_meas_time_ms <= 1000;

40
}

41


42
#endif  // AC_PRECLAND_SITL_ENABLED

43


44