Line data Source code
1 : #include "AP_InertialSensor.h"
2 :
3 : #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
4 : #include <GCS_MAVLink/GCS.h>
5 : #include <AP_Logger/AP_Logger.h>
6 :
7 : // Class level parameters
8 : const AP_Param::GroupInfo AP_InertialSensor::BatchSampler::var_info[] = {
9 : // @Param: BAT_CNT
10 : // @DisplayName: sample count per batch
11 : // @Description: Number of samples to take when logging streams of IMU sensor readings. Will be rounded down to a multiple of 32. This option takes effect on the next reboot.
12 : // @User: Advanced
13 : // @Increment: 32
14 : // @RebootRequired: True
15 : AP_GROUPINFO("BAT_CNT", 1, AP_InertialSensor::BatchSampler, _required_count, 1024),
16 :
17 : // @Param: BAT_MASK
18 : // @DisplayName: Sensor Bitmask
19 : // @Description: Bitmap of which IMUs to log batch data for. This option takes effect on the next reboot.
20 : // @User: Advanced
21 : // @Bitmask: 0:IMU1,1:IMU2,2:IMU3
22 : // @RebootRequired: True
23 : AP_GROUPINFO("BAT_MASK", 2, AP_InertialSensor::BatchSampler, _sensor_mask, DEFAULT_IMU_LOG_BAT_MASK),
24 :
25 : // @Param: BAT_OPT
26 : // @DisplayName: Batch Logging Options Mask
27 : // @Description: Options for the BatchSampler.
28 : // @Bitmask: 0:Sensor-Rate Logging (sample at full sensor rate seen by AP), 1: Sample post-filtering, 2: Sample pre- and post-filter
29 : // @User: Advanced
30 : AP_GROUPINFO("BAT_OPT", 3, AP_InertialSensor::BatchSampler, _batch_options_mask, 0),
31 :
32 : // @Param: BAT_LGIN
33 : // @DisplayName: logging interval
34 : // @Description: Interval between pushing samples to the AP_Logger log
35 : // @Units: ms
36 : // @Increment: 10
37 : AP_GROUPINFO("BAT_LGIN", 4, AP_InertialSensor::BatchSampler, push_interval_ms, 20),
38 :
39 : // @Param: BAT_LGCT
40 : // @DisplayName: logging count
41 : // @Description: Number of samples to push to count every @PREFIX@BAT_LGIN
42 : // @Increment: 1
43 : AP_GROUPINFO("BAT_LGCT", 5, AP_InertialSensor::BatchSampler, samples_per_msg, 32),
44 :
45 : AP_GROUPEND
46 : };
47 :
48 :
49 : extern const AP_HAL::HAL& hal;
50 2088 : void AP_InertialSensor::BatchSampler::init()
51 : {
52 2088 : if (_sensor_mask == 0) {
53 1767 : return;
54 : }
55 321 : if (_required_count <= 0) {
56 0 : return;
57 : }
58 :
59 321 : _required_count.set(_required_count - (_required_count % 32)); // round down to nearest multiple of 32
60 :
61 321 : _real_required_count = _required_count;
62 :
63 321 : const uint32_t total_allocation = 3*_real_required_count*sizeof(uint16_t);
64 321 : GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "INS: alloc %u bytes for ISB (free=%u)", (unsigned int)total_allocation, (unsigned int)hal.util->available_memory());
65 :
66 321 : data_x = (int16_t*)calloc(_real_required_count, sizeof(int16_t));
67 321 : data_y = (int16_t*)calloc(_real_required_count, sizeof(int16_t));
68 321 : data_z = (int16_t*)calloc(_real_required_count, sizeof(int16_t));
69 321 : if (data_x == nullptr || data_y == nullptr || data_z == nullptr) {
70 0 : free(data_x);
71 0 : free(data_y);
72 0 : free(data_z);
73 0 : data_x = nullptr;
74 0 : data_y = nullptr;
75 0 : data_z = nullptr;
76 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to allocate %u bytes for IMU batch sampling", (unsigned int)total_allocation);
77 0 : return;
78 : }
79 :
80 321 : rotate_to_next_sensor();
81 :
82 321 : initialised = true;
83 : }
84 :
85 36043796 : void AP_InertialSensor::BatchSampler::periodic()
86 : {
87 36043796 : if (_sensor_mask == 0) {
88 33953783 : return;
89 : }
90 : #if HAL_LOGGING_ENABLED
91 2090013 : push_data_to_log();
92 : #endif
93 : }
94 :
95 13128 : void AP_InertialSensor::BatchSampler::update_doing_sensor_rate_logging()
96 : {
97 13128 : if (has_option(BATCH_OPT_POST_FILTER)) {
98 950 : _doing_post_filter_logging = true;
99 : }
100 13128 : if (has_option(BATCH_OPT_PRE_POST_FILTER)) {
101 955 : _doing_pre_post_filter_logging = true;
102 : }
103 13128 : if (!has_option(BATCH_OPT_SENSOR_RATE)) {
104 13128 : _doing_sensor_rate_logging = false;
105 13128 : return;
106 : }
107 0 : const uint8_t bit = (1<<instance);
108 0 : switch (type) {
109 0 : case IMU_SENSOR_TYPE_GYRO:
110 0 : _doing_sensor_rate_logging = _imu._gyro_sensor_rate_sampling_enabled & bit;
111 0 : break;
112 0 : case IMU_SENSOR_TYPE_ACCEL:
113 0 : _doing_sensor_rate_logging = _imu._accel_sensor_rate_sampling_enabled & bit;
114 0 : break;
115 : }
116 : }
117 :
118 13128 : void AP_InertialSensor::BatchSampler::rotate_to_next_sensor()
119 : {
120 13128 : if (_sensor_mask == 0) {
121 : // should not have been called
122 0 : return;
123 : }
124 13128 : if ((1U<<instance) > (uint8_t)_sensor_mask) {
125 : // should only ever happen if user resets _sensor_mask
126 0 : instance = 0;
127 0 : post_filter = false;
128 : }
129 :
130 13128 : if (type == IMU_SENSOR_TYPE_ACCEL) {
131 : // we have logged accelerometers, now log gyros:
132 6643 : type = IMU_SENSOR_TYPE_GYRO;
133 6643 : multiplier = _imu._gyro_raw_sampling_multiplier[instance];
134 6643 : update_doing_sensor_rate_logging();
135 6643 : return;
136 : }
137 :
138 : // log for accel sensor:
139 6485 : type = IMU_SENSOR_TYPE_ACCEL;
140 :
141 : // move to next IMU backend:
142 :
143 : // we assume the number of gyros and accels is the same, taking
144 : // this minimum stops us doing bad things if that isn't true:
145 6485 : const uint8_t _count = MIN(_imu._accel_count, _imu._gyro_count);
146 :
147 : // find next backend instance to log:
148 6485 : bool haveinstance = false;
149 6485 : for (uint8_t i=instance+1; i<_count; i++) {
150 3333 : if (_sensor_mask & (1U<<i)) {
151 3333 : instance = i;
152 3333 : haveinstance = true;
153 3333 : break;
154 : }
155 : }
156 6485 : if (!haveinstance) {
157 3152 : for (uint8_t i=0; i<=instance; i++) {
158 3152 : if (_sensor_mask & (1U<<i)) {
159 3152 : instance = i;
160 3152 : haveinstance = true;
161 3152 : post_filter = !post_filter;
162 3152 : break;
163 : }
164 : }
165 : }
166 6485 : if (!haveinstance) {
167 : // should not happen!
168 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
169 0 : abort();
170 : #endif
171 0 : instance = 0;
172 0 : post_filter = false;
173 0 : return;
174 : }
175 :
176 6485 : multiplier = _imu._accel_raw_sampling_multiplier[instance];
177 6485 : update_doing_sensor_rate_logging();
178 : }
179 :
180 : #if HAL_LOGGING_ENABLED
181 2090013 : void AP_InertialSensor::BatchSampler::push_data_to_log()
182 : {
183 2090013 : if (!initialised) {
184 9662 : return;
185 : }
186 2080351 : if (_sensor_mask == 0) {
187 0 : return;
188 : }
189 2080351 : if (data_write_offset - data_read_offset < samples_per_msg) {
190 : // insuffucient data to pack a packet
191 1656062 : return;
192 : }
193 424289 : if (AP_HAL::millis() - last_sent_ms < (uint16_t)push_interval_ms) {
194 : // avoid flooding AP_Logger's buffer
195 532 : return;
196 : }
197 423757 : AP_Logger *logger = AP_Logger::get_singleton();
198 423757 : if (logger == nullptr) {
199 : // should not have been called
200 0 : return;
201 : }
202 :
203 : // possibly send isb header:
204 423757 : if (!isbh_sent && data_read_offset == 0) {
205 17217 : float sample_rate = 0; // avoid warning about uninitialised values
206 17217 : switch(type) {
207 10676 : case IMU_SENSOR_TYPE_GYRO:
208 10676 : sample_rate = _imu._gyro_raw_sample_rates[instance];
209 10676 : if (_doing_sensor_rate_logging) {
210 0 : sample_rate *= _imu._gyro_over_sampling[instance];
211 : }
212 10676 : break;
213 6541 : case IMU_SENSOR_TYPE_ACCEL:
214 6541 : sample_rate = _imu._accel_raw_sample_rates[instance];
215 6541 : if (_doing_sensor_rate_logging) {
216 0 : sample_rate *= _imu._accel_over_sampling[instance];
217 : }
218 6541 : break;
219 : }
220 17217 : if (!Write_ISBH(sample_rate)) {
221 : // buffer full?
222 4128 : return;
223 : }
224 13089 : isbh_sent = true;
225 : }
226 : // pack a nd send a data packet:
227 419629 : if (!Write_ISBD()) {
228 : // maybe later?!
229 5225 : return;
230 : }
231 :
232 414404 : data_read_offset += samples_per_msg;
233 414404 : last_sent_ms = AP_HAL::millis();
234 414404 : if (data_read_offset >= _real_required_count) {
235 : // that was the last one. Clean up:
236 12807 : data_read_offset = 0;
237 12807 : isb_seqnum++;
238 12807 : isbh_sent = false;
239 : // rotate to next instance:
240 12807 : rotate_to_next_sensor();
241 12807 : data_write_offset = 0; // unlocks writing process
242 : }
243 : }
244 :
245 545430663 : bool AP_InertialSensor::BatchSampler::should_log(uint8_t _instance, IMU_SENSOR_TYPE _type)
246 : {
247 545430663 : if (_sensor_mask == 0) {
248 486051312 : return false;
249 : }
250 59379347 : if (!initialised) {
251 696806 : return false;
252 : }
253 58682541 : if (_instance != instance) {
254 29507830 : return false;
255 : }
256 29174711 : if (_type != type) {
257 14588750 : return false;
258 : }
259 14585961 : if (data_write_offset >= _real_required_count) {
260 261041 : return false;
261 : }
262 14324920 : AP_Logger *logger = AP_Logger::get_singleton();
263 14324944 : if (logger == nullptr) {
264 0 : return false;
265 : }
266 : #define MASK_LOG_ANY 0xFFFF
267 14324944 : if (!logger->should_log(MASK_LOG_ANY)) {
268 997875 : return false;
269 : }
270 13327069 : return true;
271 : }
272 : #endif // HAL_LOGGING_ENABLED
273 :
274 545430688 : void AP_InertialSensor::BatchSampler::sample(uint8_t _instance, AP_InertialSensor::IMU_SENSOR_TYPE _type, uint64_t sample_us, const Vector3f &_sample)
275 : {
276 : #if HAL_LOGGING_ENABLED
277 545430688 : if (!should_log(_instance, _type)) {
278 532103633 : return;
279 : }
280 13327057 : if (data_write_offset == 0) {
281 13121 : measurement_started_us = sample_us;
282 : }
283 :
284 13327057 : data_x[data_write_offset] = multiplier*_sample.x;
285 13327057 : data_y[data_write_offset] = multiplier*_sample.y;
286 13327057 : data_z[data_write_offset] = multiplier*_sample.z;
287 :
288 13327057 : data_write_offset++; // may unblock the reading process
289 : #endif
290 : }
291 : #endif //#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
|
