Line data Source code
1 : #include <AP_Logger/AP_Logger_config.h>
2 :
3 : #if HAL_LOGGING_ENABLED
4 :
5 : #include "AP_InertialSensor.h"
6 : #include "AP_InertialSensor_Backend.h"
7 :
8 : #include <AP_AHRS/AP_AHRS.h>
9 : #include <AP_Logger/AP_Logger.h>
10 :
11 : // Write ACC data packet: raw accel data
12 0 : void AP_InertialSensor_Backend::Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const
13 : {
14 0 : const uint64_t now = AP_HAL::micros64();
15 0 : const struct log_ACC pkt {
16 : LOG_PACKET_HEADER_INIT(LOG_ACC_MSG),
17 : time_us : now,
18 : instance : instance,
19 0 : sample_us : sample_us?sample_us:now,
20 0 : AccX : accel.x,
21 0 : AccY : accel.y,
22 0 : AccZ : accel.z
23 0 : };
24 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
25 0 : }
26 :
27 : // Write GYR data packet: raw gyro data
28 0 : void AP_InertialSensor_Backend::Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro, bool use_sample_timestamp) const
29 : {
30 0 : const uint64_t now = use_sample_timestamp?sample_us:AP_HAL::micros64();
31 0 : const struct log_GYR pkt{
32 : LOG_PACKET_HEADER_INIT(LOG_GYR_MSG),
33 : time_us : now,
34 : instance : instance,
35 0 : sample_us : sample_us?sample_us:now,
36 0 : GyrX : gyro.x,
37 0 : GyrY : gyro.y,
38 0 : GyrZ : gyro.z
39 0 : };
40 0 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
41 0 : }
42 :
43 : // Write IMU data packet: raw accel/gyro data
44 20832976 : void AP_InertialSensor::Write_IMU_instance(const uint64_t time_us, const uint8_t imu_instance) const
45 : {
46 20832976 : const Vector3f &gyro = get_gyro(imu_instance);
47 20832976 : const Vector3f &accel = get_accel(imu_instance);
48 20832976 : const struct log_IMU pkt{
49 : LOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
50 : time_us : time_us,
51 : instance: imu_instance,
52 20832976 : gyro_x : gyro.x,
53 20832976 : gyro_y : gyro.y,
54 20832976 : gyro_z : gyro.z,
55 20832976 : accel_x : accel.x,
56 20832976 : accel_y : accel.y,
57 20832976 : accel_z : accel.z,
58 20832976 : gyro_error : _gyro_error_count[imu_instance],
59 20832976 : accel_error : _accel_error_count[imu_instance],
60 20832976 : temperature : get_temperature(imu_instance),
61 41665952 : gyro_health : (uint8_t)get_gyro_health(imu_instance),
62 41665952 : accel_health : (uint8_t)get_accel_health(imu_instance),
63 20832976 : gyro_rate : get_gyro_rate_hz(imu_instance),
64 20832976 : accel_rate : get_accel_rate_hz(imu_instance),
65 20832976 : };
66 20832976 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
67 20832976 : }
68 :
69 : // Write IMU data packet for all instances
70 10401783 : void AP_InertialSensor::Write_IMU() const
71 : {
72 10401783 : const uint64_t time_us = AP_HAL::micros64();
73 :
74 10401783 : uint8_t n = MAX(get_accel_count(), get_gyro_count());
75 31234759 : for (uint8_t i=0; i<n; i++) {
76 20832976 : Write_IMU_instance(time_us, i);
77 : }
78 10401783 : }
79 :
80 : // Write VIBE data packet for all instances
81 2211190 : void AP_InertialSensor::Write_Vibration() const
82 : {
83 2211190 : const uint64_t time_us = AP_HAL::micros64();
84 8844760 : for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
85 6633570 : if (!use_accel(i)) {
86 2196805 : continue;
87 : }
88 :
89 4436765 : const Vector3f vibration = get_vibration_levels(i);
90 4436765 : const struct log_Vibe pkt{
91 : LOG_PACKET_HEADER_INIT(LOG_VIBE_MSG),
92 : time_us : time_us,
93 : imu : i,
94 4436765 : vibe_x : vibration.x,
95 4436765 : vibe_y : vibration.y,
96 4436765 : vibe_z : vibration.z,
97 4436765 : clipping : get_accel_clip_count(i)
98 4436765 : };
99 4436765 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
100 : }
101 2211190 : }
102 :
103 : #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
104 : // Write information about a series of IMU readings to log:
105 17217 : bool AP_InertialSensor::BatchSampler::Write_ISBH(const float sample_rate_hz) const
106 : {
107 17217 : uint8_t instance_to_write = instance;
108 17217 : if (post_filter && (_doing_pre_post_filter_logging
109 5945 : || (_doing_post_filter_logging && _doing_sensor_rate_logging))) {
110 467 : instance_to_write += (type == IMU_SENSOR_TYPE_ACCEL ? _imu._accel_count : _imu._gyro_count);
111 : }
112 17217 : const struct log_ISBH pkt{
113 : LOG_PACKET_HEADER_INIT(LOG_ISBH_MSG),
114 17217 : time_us : AP_HAL::micros64(),
115 17217 : seqno : isb_seqnum,
116 17217 : sensor_type : (uint8_t)type,
117 : instance : instance_to_write,
118 17217 : multiplier : multiplier,
119 17217 : sample_count : (uint16_t)_real_required_count,
120 17217 : sample_us : measurement_started_us,
121 : sample_rate_hz : sample_rate_hz,
122 17217 : };
123 :
124 17217 : return AP::logger().WriteBlock_first_succeed(&pkt, sizeof(pkt));
125 : }
126 :
127 : // Write a series of IMU readings to log:
128 419629 : bool AP_InertialSensor::BatchSampler::Write_ISBD() const
129 : {
130 419629 : struct log_ISBD pkt = {
131 : LOG_PACKET_HEADER_INIT(LOG_ISBD_MSG),
132 419629 : time_us : AP_HAL::micros64(),
133 419629 : isb_seqno : isb_seqnum,
134 839258 : seqno : (uint16_t) (data_read_offset/samples_per_msg)
135 419629 : };
136 419629 : memcpy(pkt.x, &data_x[data_read_offset], sizeof(pkt.x));
137 419629 : memcpy(pkt.y, &data_y[data_read_offset], sizeof(pkt.y));
138 419629 : memcpy(pkt.z, &data_z[data_read_offset], sizeof(pkt.z));
139 :
140 419629 : return AP::logger().WriteBlock_first_succeed(&pkt, sizeof(pkt));
141 : }
142 : #endif
143 :
144 : #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
145 : // @LoggerMessage: FTN
146 : // @Description: Filter Tuning Message - per motor
147 : // @Field: TimeUS: microseconds since system startup
148 : // @Field: I: instance
149 : // @Field: NDn: number of active harmonic notches
150 : // @Field: NF1: desired harmonic notch centre frequency for motor 1
151 : // @Field: NF2: desired harmonic notch centre frequency for motor 2
152 : // @Field: NF3: desired harmonic notch centre frequency for motor 3
153 : // @Field: NF4: desired harmonic notch centre frequency for motor 4
154 : // @Field: NF5: desired harmonic notch centre frequency for motor 5
155 : // @Field: NF6: desired harmonic notch centre frequency for motor 6
156 : // @Field: NF7: desired harmonic notch centre frequency for motor 7
157 : // @Field: NF8: desired harmonic notch centre frequency for motor 8
158 : // @Field: NF9: desired harmonic notch centre frequency for motor 9
159 : // @Field: NF10: desired harmonic notch centre frequency for motor 10
160 : // @Field: NF11: desired harmonic notch centre frequency for motor 11
161 : // @Field: NF12: desired harmonic notch centre frequency for motor 12
162 :
163 : // @LoggerMessage: FTNS
164 : // @Description: Filter Tuning Message
165 : // @Field: TimeUS: microseconds since system startup
166 : // @Field: I: instance
167 : // @Field: NF: desired harmonic notch centre frequency
168 :
169 2091374 : void AP_InertialSensor::write_notch_log_messages() const
170 : {
171 2091374 : const uint64_t now_us = AP_HAL::micros64();
172 8365496 : for (auto ¬ch : harmonic_notches) {
173 6274122 : const uint8_t i = ¬ch - &harmonic_notches[0];
174 6274122 : if (!notch.params.enabled()) {
175 6250260 : continue;
176 : }
177 23862 : const float* notches = notch.calculated_notch_freq_hz;
178 23862 : if (notch.num_calculated_notch_frequencies > 1) {
179 : // log per motor center frequencies
180 9732 : AP::logger().WriteStreaming(
181 : "FTN", "TimeUS,I,NDn,NF1,NF2,NF3,NF4,NF5,NF6,NF7,NF8,NF9,NF10,NF11,NF12", "s#-zzzzzzzzzzzz", "F--------------", "QBBffffffffffff",
182 : now_us,
183 : i,
184 9732 : notch.num_calculated_notch_frequencies,
185 9732 : notches[0], notches[1], notches[2], notches[3],
186 9732 : notches[4], notches[5], notches[6], notches[7],
187 9732 : notches[8], notches[9], notches[10], notches[11]);
188 : } else {
189 : // log single center frequency
190 14130 : AP::logger().WriteStreaming(
191 : "FTNS", "TimeUS,I,NF", "s#z", "F--", "QBf",
192 : now_us,
193 : i,
194 14130 : notches[0]);
195 : }
196 :
197 : // ask the HarmonicNotchFilter object for primary gyro to
198 : // log the actual notch centers
199 23862 : notch.filter[_primary].log_notch_centers(i, now_us);
200 : }
201 2091374 : }
202 : #endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
203 :
204 : #endif // HAL_LOGGING_ENABLED
|
