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/AP_AHRS/AP_AHRS_External.cpp
Views: 1798
1
#include "AP_AHRS_External.h"

2


3
#if AP_AHRS_EXTERNAL_ENABLED

4


5
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>

6
#include <AP_AHRS/AP_AHRS.h>

7


8
// true if the AHRS has completed initialisation

9
bool AP_AHRS_External::initialised(void) const

10
{

11
    return AP::externalAHRS().initialised();

12
}

13


14
void AP_AHRS_External::update()

15
{

16
    AP::externalAHRS().update();

17
}

18


19
bool AP_AHRS_External::healthy() const {

20
    return AP::externalAHRS().healthy();

21
}

22


23
void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results)

24
{

25
    Quaternion quat;

26
    auto &extahrs = AP::externalAHRS();

27
    const AP_InertialSensor &_ins = AP::ins();

28
    if (!extahrs.get_quaternion(quat)) {

29
        return;

30
    }

31
    quat.rotation_matrix(results.dcm_matrix);

32
    results.dcm_matrix = results.dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body();

33
    results.dcm_matrix.to_euler(&results.roll_rad, &results.pitch_rad, &results.yaw_rad);

34


35
    results.gyro_drift.zero();

36
    if (!extahrs.get_gyro(results.gyro_estimate)) {

37
        results.gyro_estimate = _ins.get_gyro();

38
    }

39


40
    Vector3f accel;

41
    if (!extahrs.get_accel(accel)) {

42
        accel = _ins.get_accel();

43
    }

44


45
    const Vector3f accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel;

46
    results.accel_ef = accel_ef;

47


48
    results.location_valid = AP::externalAHRS().get_location(results.location);

49
}

50


51
bool AP_AHRS_External::get_quaternion(Quaternion &quat) const

52
{

53
    return AP::externalAHRS().get_quaternion(quat);

54
}

55


56
Vector2f AP_AHRS_External::groundspeed_vector()

57
{

58
    return AP::externalAHRS().get_groundspeed_vector();

59
}

60


61


62
bool AP_AHRS_External::get_relative_position_NED_origin(Vector3f &vec) const

63
{

64
    auto &extahrs = AP::externalAHRS();

65
    Location loc, orgn;

66
    if (extahrs.get_origin(orgn) &&

67
        extahrs.get_location(loc)) {

68
        const Vector2f diff2d = orgn.get_distance_NE(loc);

69
        vec = Vector3f(diff2d.x, diff2d.y,

70
                       -(loc.alt - orgn.alt)*0.01);

71
        return true;

72
    }

73
    return false;

74
}

75


76
bool AP_AHRS_External::get_relative_position_NE_origin(Vector2f &posNE) const

77
{

78
    auto &extahrs = AP::externalAHRS();

79


80
    Location loc, orgn;

81
    if (!extahrs.get_location(loc) ||

82
        !extahrs.get_origin(orgn)) {

83
        return false;

84
    }

85
    posNE = orgn.get_distance_NE(loc);

86
    return true;

87
}

88


89
bool AP_AHRS_External::get_relative_position_D_origin(float &posD) const

90
{

91
    auto &extahrs = AP::externalAHRS();

92


93
    Location orgn, loc;

94
    if (!extahrs.get_origin(orgn) ||

95
        !extahrs.get_location(loc)) {

96
        return false;

97
    }

98
    posD = -(loc.alt - orgn.alt)*0.01;

99
    return true;

100
}

101


102
bool AP_AHRS_External::get_velocity_NED(Vector3f &vec) const

103
{

104
    return AP::externalAHRS().get_velocity_NED(vec);

105
}

106


107
bool AP_AHRS_External::get_vert_pos_rate_D(float &velocity) const

108
{

109
    return AP::externalAHRS().get_speed_down(velocity);

110
}

111


112
bool AP_AHRS_External::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const

113
{

114
    return AP::externalAHRS().pre_arm_check(failure_msg, failure_msg_len);

115
}

116


117
bool AP_AHRS_External::get_filter_status(nav_filter_status &status) const

118
{

119
    AP::externalAHRS().get_filter_status(status);

120
    return true;

121
}

122


123
void AP_AHRS_External::send_ekf_status_report(GCS_MAVLINK &link) const

124
{

125
    AP::externalAHRS().send_status_report(link);

126
}

127


128
bool AP_AHRS_External::get_origin(Location &ret) const

129
{

130
    return AP::externalAHRS().get_origin(ret);

131
}

132


133
void AP_AHRS_External::get_control_limits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const

134
{

135
    // lower gains in VTOL controllers when flying on DCM

136
    ekfGndSpdLimit = 50.0;

137
    ekfNavVelGainScaler = 0.5;

138
}

139


140
#endif

141


142