Line data Source code
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 2461314 : void AP_AHRS::Write_AHRS2() const
14 : {
15 2461314 : const auto *estimates = secondary_estimates;
16 2461314 : if (estimates == nullptr) {
17 118441 : return;
18 : }
19 :
20 : // valid attitude required before logging (FIXME)
21 2446893 : if (!estimates->attitude_valid) {
22 104020 : return;
23 : }
24 :
25 2342873 : const Location &loc = estimates->location;
26 2342873 : const auto &quat = estimates->quaternion;
27 :
28 : float alt;
29 2342873 : if (!loc.initialised() || !loc.get_alt_m(Location::AltFrame::ABSOLUTE, alt)) {
30 100987 : alt = AP_Logger::quiet_nanf();
31 : }
32 2342873 : const struct log_AHRS pkt{
33 : LOG_PACKET_HEADER_INIT(LOG_AHR2_MSG),
34 2342873 : time_us : AP_HAL::micros64(),
35 4685746 : roll : (int16_t)(degrees(estimates->roll_rad)*100),
36 4685746 : pitch : (int16_t)(degrees(estimates->pitch_rad)*100),
37 2342873 : yaw : (uint16_t)(wrap_360_cd(degrees(estimates->yaw_rad)*100)),
38 : alt : alt,
39 2342873 : lat : loc.lat,
40 2342873 : lng : loc.lng,
41 2342873 : q1 : quat.q1,
42 2342873 : q2 : quat.q2,
43 2342873 : q3 : quat.q3,
44 2342873 : q4 : quat.q4,
45 2342873 : };
46 2342873 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
47 : }
48 :
49 : // Write AOA and SSA
50 524212 : void AP_AHRS::Write_AOA_SSA(void) const
51 : {
52 524212 : const struct log_AOA_SSA aoa_ssa{
53 : LOG_PACKET_HEADER_INIT(LOG_AOA_SSA_MSG),
54 524212 : time_us : AP_HAL::micros64(),
55 524212 : AOA : getAOA(),
56 524212 : SSA : getSSA()
57 524212 : };
58 :
59 524212 : AP::logger().WriteBlock(&aoa_ssa, sizeof(aoa_ssa));
60 524212 : }
61 :
62 : // Write an attitude packet, targets in degrees
63 2355800 : void AP_AHRS::Write_Attitude(const Vector3f &targets) const
64 : {
65 2355800 : const struct log_Attitude pkt{
66 : LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
67 2355800 : time_us : AP_HAL::micros64(),
68 2355800 : control_roll : targets.x,
69 2355800 : roll : degrees(roll),
70 2355800 : control_pitch : targets.y,
71 2355800 : pitch : degrees(pitch),
72 2355800 : control_yaw : wrap_360(targets.z),
73 2355800 : yaw : wrap_360(degrees(yaw)),
74 2355800 : active : uint8_t(active_EKF_type()),
75 4711600 : };
76 2355800 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
77 2355800 : }
78 :
79 17651 : void AP_AHRS::Write_Origin(LogOriginType origin_type, const Location &loc) const
80 : {
81 : int32_t alt_cm;
82 17651 : if (!loc.initialised() || !loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) {
83 0 : alt_cm = 0;
84 : }
85 17651 : const struct log_ORGN pkt{
86 : LOG_PACKET_HEADER_INIT(LOG_ORGN_MSG),
87 17651 : time_us : AP_HAL::micros64(),
88 : origin_type : (uint8_t)origin_type,
89 17651 : latitude : loc.lat,
90 17651 : longitude : loc.lng,
91 : altitude : alt_cm
92 17651 : };
93 17651 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
94 17651 : }
95 :
96 : // Write a POS packet
97 2461314 : void AP_AHRS::Write_POS() const
98 : {
99 2461314 : Location loc;
100 2461314 : if (!get_location(loc)) {
101 188600 : return;
102 : }
103 : float home;
104 2272714 : AP::ahrs().get_relative_position_D_home(home);
105 :
106 2272714 : const auto nanf = AP_Logger::quiet_nanf();
107 : float origin_pos_up;
108 2272714 : if (get_relative_position_D_origin_float(origin_pos_up)) {
109 2267835 : origin_pos_up *= -1; // down -> up
110 : } else {
111 4879 : origin_pos_up = nanf;
112 : }
113 : float alt;
114 2272714 : if (!loc.get_alt_m(Location::AltFrame::ABSOLUTE, alt)) {
115 0 : alt = nanf;
116 : }
117 2272714 : const struct log_POS pkt{
118 : LOG_PACKET_HEADER_INIT(LOG_POS_MSG),
119 2272714 : time_us : AP_HAL::micros64(),
120 2272714 : lat : loc.lat,
121 2272714 : lng : loc.lng,
122 : alt : alt,
123 2272714 : rel_home_alt : -home,
124 : rel_origin_alt : origin_pos_up,
125 2272714 : };
126 2272714 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
127 : }
128 :
129 : // Write a packet for video stabilisation
130 0 : void AP_AHRS::write_video_stabilisation() const
131 : {
132 0 : Quaternion current_attitude;
133 0 : get_quat_body_to_ned(current_attitude);
134 0 : Vector3f accel = get_accel() - get_accel_bias();
135 0 : const struct log_Video_Stabilisation pkt {
136 : LOG_PACKET_HEADER_INIT(LOG_VIDEO_STABILISATION_MSG),
137 0 : time_us : AP_HAL::micros64(),
138 0 : gyro_x : state.gyro_estimate.x,
139 0 : gyro_y : state.gyro_estimate.y,
140 0 : gyro_z : state.gyro_estimate.z,
141 0 : accel_x : accel.x,
142 0 : accel_y : accel.y,
143 0 : accel_z : accel.z,
144 0 : Q1 : current_attitude.q1,
145 0 : Q2 : current_attitude.q2,
146 0 : Q3 : current_attitude.q3,
147 0 : Q4 : current_attitude.q4,
148 0 : };
149 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
150 0 : }
151 :
152 : // Write an attitude view packet, targets in degrees
153 90173 : void AP_AHRS_View::Write_AttitudeView(const Vector3f &targets) const
154 : {
155 90173 : const struct log_Attitude pkt{
156 : LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
157 90173 : time_us : AP_HAL::micros64(),
158 90173 : control_roll : targets.x,
159 90173 : roll : degrees(roll),
160 90173 : control_pitch : targets.y,
161 90173 : pitch : degrees(pitch),
162 90173 : control_yaw : wrap_360(targets.z),
163 90173 : yaw : wrap_360(degrees(yaw)),
164 90173 : active : uint8_t(AP::ahrs().active_EKF_type()),
165 180346 : };
166 90173 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
167 90173 : }
168 :
169 : #endif // HAL_LOGGING_ENABLED
|
