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_Logging.cpp
Views: 1798
1
#include <AP_Logger/AP_Logger_config.h>

2


3
#if HAL_LOGGING_ENABLED

4


5
#include "AP_AHRS.h"

6
#include <AP_Logger/AP_Logger.h>

7


8
#include <AC_AttitudeControl/AC_AttitudeControl.h>

9
#include <AC_AttitudeControl/AC_PosControl.h>

10


11


12
// Write an AHRS2 packet

13
void AP_AHRS::Write_AHRS2() const

14
{

15
    Vector3f euler;

16
    Location loc;

17
    Quaternion quat;

18
    if (!get_secondary_attitude(euler) || !get_secondary_position(loc) || !get_secondary_quaternion(quat)) {

19
        return;

20
    }

21
    const struct log_AHRS pkt{

22
        LOG_PACKET_HEADER_INIT(LOG_AHR2_MSG),

23
        time_us : AP_HAL::micros64(),

24
        roll  : (int16_t)(degrees(euler.x)*100),

25
        pitch : (int16_t)(degrees(euler.y)*100),

26
        yaw   : (uint16_t)(wrap_360_cd(degrees(euler.z)*100)),

27
        alt   : loc.alt*1.0e-2f,

28
        lat   : loc.lat,

29
        lng   : loc.lng,

30
        q1    : quat.q1,

31
        q2    : quat.q2,

32
        q3    : quat.q3,

33
        q4    : quat.q4,

34
    };

35
    AP::logger().WriteBlock(&pkt, sizeof(pkt));

36
}

37


38
// Write AOA and SSA

39
void AP_AHRS::Write_AOA_SSA(void) const

40
{

41
    const struct log_AOA_SSA aoa_ssa{

42
        LOG_PACKET_HEADER_INIT(LOG_AOA_SSA_MSG),

43
        time_us         : AP_HAL::micros64(),

44
        AOA             : getAOA(),

45
        SSA             : getSSA()

46
    };

47


48
    AP::logger().WriteBlock(&aoa_ssa, sizeof(aoa_ssa));

49
}

50


51
// Write an attitude packet, targets in degrees

52
void AP_AHRS::Write_Attitude(const Vector3f &targets) const

53
{

54
    const struct log_Attitude pkt{

55
        LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),

56
        time_us         : AP_HAL::micros64(),

57
        control_roll    : targets.x,

58
        roll            : degrees(roll),

59
        control_pitch   : targets.y,

60
        pitch           : degrees(pitch),

61
        control_yaw     : wrap_360(targets.z),

62
        yaw             : wrap_360(degrees(yaw)),

63
        active          : uint8_t(active_EKF_type()),

64
    };

65
    AP::logger().WriteBlock(&pkt, sizeof(pkt));

66
}

67


68
void AP_AHRS::Write_Origin(LogOriginType origin_type, const Location &loc) const

69
{

70
    const struct log_ORGN pkt{

71
        LOG_PACKET_HEADER_INIT(LOG_ORGN_MSG),

72
        time_us     : AP_HAL::micros64(),

73
        origin_type : (uint8_t)origin_type,

74
        latitude    : loc.lat,

75
        longitude   : loc.lng,

76
        altitude    : loc.alt

77
    };

78
    AP::logger().WriteBlock(&pkt, sizeof(pkt));

79
}

80


81
// Write a POS packet

82
void AP_AHRS::Write_POS() const

83
{

84
    Location loc;

85
    if (!get_location(loc)) {

86
        return;

87
    }

88
    float home, origin;

89
    AP::ahrs().get_relative_position_D_home(home);

90
    const struct log_POS pkt{

91
        LOG_PACKET_HEADER_INIT(LOG_POS_MSG),

92
        time_us        : AP_HAL::micros64(),

93
        lat            : loc.lat,

94
        lng            : loc.lng,

95
        alt            : loc.alt*1.0e-2f,

96
        rel_home_alt   : -home,

97
        rel_origin_alt : get_relative_position_D_origin(origin) ? -origin : AP::logger().quiet_nanf(),

98
    };

99
    AP::logger().WriteBlock(&pkt, sizeof(pkt));

100
}

101


102
// Write a packet for video stabilisation

103
void AP_AHRS::write_video_stabilisation() const

104
{

105
    Quaternion current_attitude;

106
    get_quat_body_to_ned(current_attitude);

107
    Vector3f accel = get_accel() - get_accel_bias();

108
    const struct log_Video_Stabilisation pkt {

109
        LOG_PACKET_HEADER_INIT(LOG_VIDEO_STABILISATION_MSG),

110
        time_us         : AP_HAL::micros64(),

111
        gyro_x          : state.gyro_estimate.x,

112
        gyro_y          : state.gyro_estimate.y,

113
        gyro_z          : state.gyro_estimate.z,

114
        accel_x         : accel.x,

115
        accel_y         : accel.y,

116
        accel_z         : accel.z,

117
        Q1              : current_attitude.q1,

118
        Q2              : current_attitude.q2,

119
        Q3              : current_attitude.q3,

120
        Q4              : current_attitude.q4,

121
    };

122
    AP::logger().WriteBlock(&pkt, sizeof(pkt));

123
}

124


125
// Write an attitude view packet, targets in degrees

126
void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const

127
{

128
    const struct log_Attitude pkt{

129
        LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),

130
        time_us         : AP_HAL::micros64(),

131
        control_roll    : targets.x,

132
        roll            : degrees(roll),

133
        control_pitch   : targets.y,

134
        pitch           : degrees(pitch),

135
        control_yaw     : wrap_360(targets.z),

136
        yaw             : wrap_360(degrees(yaw)),

137
        active          : uint8_t(AP::ahrs().active_EKF_type()),

138
    };

139
    AP::logger().WriteBlock(&pkt, sizeof(pkt));

140
}

141


142
#endif  // HAL_LOGGING_ENABLED

143


144