Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 : #include "AP_InertialSensor_SITL.h"
3 : #include <AP_Logger/AP_Logger.h>
4 : #include <SITL/SITL.h>
5 : #include <AP_Filesystem/AP_Filesystem.h>
6 :
7 : #if AP_SIM_INS_ENABLED
8 :
9 : const extern AP_HAL::HAL& hal;
10 :
11 4177 : AP_InertialSensor_SITL::AP_InertialSensor_SITL(AP_InertialSensor &imu, const uint16_t sample_rates[]) :
12 : AP_InertialSensor_Backend(imu),
13 4177 : gyro_sample_hz(sample_rates[0]),
14 4177 : accel_sample_hz(sample_rates[1])
15 : {
16 4177 : }
17 :
18 : /*
19 : detect the sensor
20 : */
21 4177 : AP_InertialSensor_Backend *AP_InertialSensor_SITL::detect(AP_InertialSensor &_imu, const uint16_t sample_rates[])
22 : {
23 4177 : AP_InertialSensor_SITL *sensor = NEW_NOTHROW AP_InertialSensor_SITL(_imu, sample_rates);
24 4177 : if (sensor == nullptr) {
25 0 : return nullptr;
26 : }
27 4177 : if (!sensor->init_sensor()) {
28 0 : delete sensor;
29 0 : return nullptr;
30 : }
31 4177 : return sensor;
32 : }
33 :
34 4177 : bool AP_InertialSensor_SITL::init_sensor(void)
35 : {
36 4177 : sitl = AP::sitl();
37 4177 : if (sitl == nullptr) {
38 0 : return false;
39 : }
40 4177 : return true;
41 : }
42 :
43 : // calculate a noisy noise component
44 506668140 : static float calculate_noise(float noise, float noise_variation) {
45 506668140 : return noise * (1.0f + noise_variation * rand_float());
46 : }
47 :
48 2330818600 : float AP_InertialSensor_SITL::get_temperature(void)
49 : {
50 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
51 2330818600 : if (!is_zero(sitl->imu_temp_fixed)) {
52 : // user wants fixed temperature
53 544698 : return sitl->imu_temp_fixed;
54 : }
55 2330273902 : uint32_t now = AP_HAL::millis();
56 2330273902 : if (temp_start_ms == 0) {
57 4177 : temp_start_ms = now;
58 : }
59 : // follow a curve with given start, end and time constant
60 2330273902 : const float tsec = (AP_HAL::millis() - temp_start_ms) * 0.001f;
61 2330273902 : const float T0 = sitl->imu_temp_start;
62 2330273902 : const float T1 = sitl->imu_temp_end;
63 2330273902 : const float tconst = sitl->imu_temp_tconst;
64 2330273902 : return T1 - (T1 - T0) * expf(-tsec / tconst);
65 : #else
66 : return 20.0f;
67 : #endif
68 : }
69 :
70 : /*
71 : generate an accelerometer sample
72 : */
73 275666665 : void AP_InertialSensor_SITL::generate_accel()
74 : {
75 275666665 : Vector3f accel_accum;
76 275666665 : uint8_t nsamples = enable_fast_sampling(accel_instance) ? 4 : 1;
77 :
78 1004322527 : for (uint8_t j = 0; j < nsamples; j++) {
79 :
80 728655862 : Vector3f accel = Vector3f(sitl->state.xAccel,
81 728655862 : sitl->state.yAccel,
82 728655862 : sitl->state.zAccel);
83 :
84 : // SIM_BRD_TRIM: simulate a rigid board mounting offset by rotating
85 : // the sensor frame. Applied to both accel (here) and gyro so the two
86 : // stay consistent, as a real tilted mount would.
87 728655862 : const Vector3f &board_trim = sitl->board_trim.get();
88 728655862 : if (!board_trim.is_zero()) {
89 3571670 : Matrix3f trim_rotation;
90 3571670 : trim_rotation.from_euler(board_trim.x, board_trim.y, board_trim.z);
91 3571670 : accel = trim_rotation.transposed() * accel;
92 : }
93 :
94 : // add scaling
95 728655862 : Vector3f accel_scale = sitl->accel_scale[accel_instance].get();
96 : // note that we divide so the SIM_ACC values match the
97 : // INS_ACCSCAL values
98 728655862 : if (!is_zero(accel_scale.x)) {
99 393099 : accel.x /= accel_scale.x;
100 : }
101 728655862 : if (!is_zero(accel_scale.y)) {
102 392138 : accel.y /= accel_scale.y;
103 : }
104 728655862 : if (!is_zero(accel_scale.z)) {
105 392042 : accel.z /= accel_scale.z;
106 : }
107 :
108 : // apply bias
109 728655862 : const Vector3f &accel_bias = sitl->accel_bias[accel_instance].get();
110 728655862 : accel += accel_bias;
111 :
112 : // minimum noise levels are 2 bits, but averaged over many
113 : // samples, giving around 0.01 m/s/s
114 728655862 : float accel_noise = 0.01f;
115 728655862 : float noise_variation = 0.05f;
116 : // this smears the individual motor peaks somewhat emulating physical motors
117 728655862 : float freq_variation = 0.12f;
118 :
119 : // add in sensor noise
120 728655862 : accel += Vector3f{rand_float(), rand_float(), rand_float()} * accel_noise;
121 :
122 728655862 : bool motors_on = sitl->throttle > sitl->ins_noise_throttle_min;
123 :
124 : // on a real 180mm copter gyro noise varies between 0.8-4 m/s/s for throttle 0.2-0.8
125 : // giving a accel noise variation of 5.33 m/s/s over the full throttle range
126 728655862 : if (motors_on) {
127 : // add extra noise when the motors are on
128 425797318 : accel_noise = sitl->accel_noise[accel_instance];
129 : }
130 :
131 : // VIB_FREQ is a static vibration applied to each axis
132 728655862 : const Vector3f &vibe_freq = sitl->vibe_freq;
133 :
134 728655862 : if (!vibe_freq.is_zero() && motors_on) {
135 5470023 : accel.x += sinf(accel_time * 2 * M_PI * vibe_freq.x) * calculate_noise(accel_noise, noise_variation);
136 5470023 : accel.y += sinf(accel_time * 2 * M_PI * vibe_freq.y) * calculate_noise(accel_noise, noise_variation);
137 5470023 : accel.z += sinf(accel_time * 2 * M_PI * vibe_freq.z) * calculate_noise(accel_noise, noise_variation);
138 5470023 : accel_time += 1.0f / (accel_sample_hz * nsamples);
139 : }
140 :
141 : // VIB_MOT_MAX is a rpm-scaled vibration applied to each axis
142 728655862 : if (!is_zero(sitl->vibe_motor) && motors_on) {
143 9668776 : uint32_t mask = sitl->state.motor_mask;
144 : uint8_t mbit;
145 52854509 : while ((mbit = __builtin_ffs(mask)) != 0) {
146 43185733 : const uint8_t motor = mbit-1;
147 43185733 : mask &= ~(1U<<motor);
148 43185733 : uint32_t harmonics = uint32_t(sitl->vibe_motor_harmonics);
149 43185733 : const float base_freq = calculate_noise(sitl->state.rpm[motor] / 60.0f, freq_variation);
150 86371466 : while (harmonics != 0) {
151 43185733 : const uint8_t bit = __builtin_ffs(harmonics);
152 43185733 : harmonics &= ~(1U<<(bit-1U));
153 43185733 : const float phase = accel_motor_phase[motor] * float(bit);
154 43185733 : accel.x += sinf(phase) * calculate_noise(accel_noise * sitl->vibe_motor_scale, noise_variation);
155 43185733 : accel.y += sinf(phase) * calculate_noise(accel_noise * sitl->vibe_motor_scale, noise_variation);
156 43185733 : accel.z += sinf(phase) * calculate_noise(accel_noise * sitl->vibe_motor_scale, noise_variation);
157 : }
158 43185733 : const float phase_incr = base_freq * 2 * M_PI / (accel_sample_hz * nsamples);
159 43185733 : accel_motor_phase[motor] = wrap_PI(accel_motor_phase[motor] + phase_incr);
160 : }
161 : }
162 :
163 : // correct for the acceleration due to the IMU position offset and angular acceleration
164 : // correct for the centripetal acceleration
165 : // only apply corrections to first accelerometer
166 728655862 : Vector3f pos_offset = sitl->imu_pos_offset;
167 728655862 : if (!pos_offset.is_zero()) {
168 : // calculate sensed acceleration due to lever arm effect
169 : // Note: the % operator has been overloaded to provide a cross product
170 0 : Vector3f angular_accel = Vector3f(radians(sitl->state.angAccel.x), radians(sitl->state.angAccel.y), radians(sitl->state.angAccel.z));
171 0 : Vector3f lever_arm_accel = angular_accel % pos_offset;
172 :
173 : // calculate sensed acceleration due to centripetal acceleration
174 0 : Vector3f angular_rate = Vector3f(radians(sitl->state.rollRate), radians(sitl->state.pitchRate), radians(sitl->state.yawRate));
175 0 : Vector3f centripetal_accel = angular_rate % (angular_rate % pos_offset);
176 :
177 : // apply corrections
178 0 : accel += lever_arm_accel + centripetal_accel;
179 : }
180 :
181 728655862 : if (fabsf(sitl->accel_fail[accel_instance]) > 1.0e-6f) {
182 0 : accel.x = accel.y = accel.z = sitl->accel_fail[accel_instance];
183 : }
184 :
185 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
186 728655862 : const float T = get_temperature();
187 728655862 : sitl->imu_tcal[gyro_instance].sitl_apply_accel(T, accel);
188 : #endif
189 :
190 728655862 : _notify_new_accel_sensor_rate_sample(accel_instance, accel);
191 :
192 : #if AP_SIM_INS_FILE_ENABLED
193 728655862 : if (sitl->accel_file_rw == SITL::SIM::INSFileMode::INS_FILE_WRITE) {
194 0 : write_accel_to_file(accel);
195 : }
196 : #endif
197 728655862 : accel_accum += accel;
198 : }
199 :
200 275666665 : accel_accum /= nsamples;
201 :
202 275666665 : accel_accum.rotate(sitl->imu_orientation);
203 :
204 275666665 : _rotate_and_correct_accel(accel_instance, accel_accum);
205 275666665 : _notify_new_accel_raw_sample(accel_instance, accel_accum, AP_HAL::micros64());
206 :
207 275666665 : _publish_temperature(accel_instance, get_temperature());
208 275666665 : }
209 :
210 : /*
211 : generate a gyro sample
212 : */
213 269614695 : void AP_InertialSensor_SITL::generate_gyro()
214 : {
215 269614695 : Vector3f gyro_accum;
216 269614695 : uint8_t nsamples = enable_fast_sampling(gyro_instance) ? 8 : 1;
217 :
218 269614695 : const float _gyro_drift = gyro_drift();
219 1596110768 : for (uint8_t j = 0; j < nsamples; j++) {
220 1326496073 : float p = radians(sitl->state.rollRate) + _gyro_drift;
221 1326496073 : float q = radians(sitl->state.pitchRate) + _gyro_drift;
222 1326496073 : float r = radians(sitl->state.yawRate) + _gyro_drift;
223 :
224 : // minimum gyro noise is less than 1 bit
225 1326496073 : float gyro_noise = radians(0.04f);
226 1326496073 : constexpr float noise_variation = 0.05f;
227 : // this smears the individual motor peaks somewhat emulating physical motors
228 1326496073 : constexpr float freq_variation = 0.12f;
229 : // add in sensor noise
230 1326496073 : p += gyro_noise * rand_float();
231 1326496073 : q += gyro_noise * rand_float();
232 1326496073 : r += gyro_noise * rand_float();
233 :
234 1326496073 : bool motors_on = sitl->throttle > sitl->ins_noise_throttle_min;
235 : // on a real 180mm copter gyro noise varies between 0.2-0.4 rad/s for throttle 0.2-0.8
236 : // giving a gyro noise variation of 0.33 rad/s or 20deg/s over the full throttle range
237 1326496073 : if (motors_on) {
238 : // add extra noise when the motors are on
239 774730280 : gyro_noise = radians(sitl->gyro_noise[gyro_instance]) * sitl->throttle;
240 : }
241 :
242 : // VIB_FREQ is a static vibration applied to each axis
243 1326496073 : const Vector3f &vibe_freq = sitl->vibe_freq;
244 :
245 1326496073 : if (vibe_freq.is_zero() && is_zero(sitl->vibe_motor)) {
246 : // no rpm noise, so add in background noise if any
247 1301708601 : p += gyro_noise * rand_float();
248 1301708601 : q += gyro_noise * rand_float();
249 1301708601 : r += gyro_noise * rand_float();
250 : }
251 :
252 1326496073 : if (!vibe_freq.is_zero() && motors_on) {
253 8963001 : p += sinf(gyro_time * 2 * M_PI * vibe_freq.x) * calculate_noise(gyro_noise, noise_variation);
254 8963001 : q += sinf(gyro_time * 2 * M_PI * vibe_freq.y) * calculate_noise(gyro_noise, noise_variation);
255 8963001 : r += sinf(gyro_time * 2 * M_PI * vibe_freq.z) * calculate_noise(gyro_noise, noise_variation);
256 8963001 : gyro_time += 1.0f / (gyro_sample_hz * nsamples);
257 : }
258 :
259 : // VIB_MOT_MAX is a rpm-scaled vibration applied to each axis
260 1326496073 : if (!is_zero(sitl->vibe_motor) && motors_on) {
261 16106052 : uint32_t mask = sitl->state.motor_mask;
262 : uint8_t mbit;
263 88762586 : while ((mbit = __builtin_ffs(mask)) != 0) {
264 72656534 : const uint8_t motor = mbit-1;
265 72656534 : mask &= ~(1U<<motor);
266 72656534 : uint32_t harmonics = uint32_t(sitl->vibe_motor_harmonics);
267 72656534 : const float base_freq = calculate_noise(sitl->state.rpm[motor] / 60.0f, freq_variation);
268 145313068 : while (harmonics != 0) {
269 72656534 : const uint8_t bit = __builtin_ffs(harmonics);
270 72656534 : harmonics &= ~(1U<<(bit-1U));
271 72656534 : const float phase = gyro_motor_phase[motor] * float(bit);
272 72656534 : p += sinf(phase) * calculate_noise(gyro_noise * sitl->vibe_motor_scale, noise_variation);
273 72656534 : q += sinf(phase) * calculate_noise(gyro_noise * sitl->vibe_motor_scale, noise_variation);
274 72656534 : r += sinf(phase) * calculate_noise(gyro_noise * sitl->vibe_motor_scale, noise_variation);
275 : }
276 72656534 : const float phase_incr = base_freq * 2 * M_PI / (gyro_sample_hz * nsamples);
277 72656534 : gyro_motor_phase[motor] = wrap_PI(gyro_motor_phase[motor] + phase_incr);
278 : }
279 : }
280 :
281 1326496073 : Vector3f gyro {p, q, r};
282 :
283 : // SIM_BRD_TRIM: rigid board mounting offset, same rotation as accel:
284 1326496073 : const Vector3f &board_trim = sitl->board_trim.get();
285 1326496073 : if (!board_trim.is_zero()) {
286 6518658 : Matrix3f trim_rotation;
287 6518658 : trim_rotation.from_euler(board_trim.x, board_trim.y, board_trim.z);
288 6518658 : gyro = trim_rotation.transposed() * gyro;
289 : }
290 :
291 : #if HAL_INS_TEMPERATURE_CAL_ENABLE
292 1326496073 : sitl->imu_tcal[gyro_instance].sitl_apply_gyro(get_temperature(), gyro);
293 : #endif
294 :
295 : // add in gyro scaling
296 1326496073 : const Vector3f &scale = sitl->gyro_scale[gyro_instance];
297 1326496073 : gyro.x *= (1 + scale.x * 0.01f);
298 1326496073 : gyro.y *= (1 + scale.y * 0.01f);
299 1326496073 : gyro.z *= (1 + scale.z * 0.01f);
300 :
301 : // apply bias
302 1326496073 : const Vector3f &gyro_bias = sitl->gyro_bias[gyro_instance].get();
303 1326496073 : gyro += gyro_bias;
304 :
305 1326496073 : gyro_accum += gyro;
306 1326496073 : _notify_new_gyro_sensor_rate_sample(gyro_instance, gyro);
307 :
308 : #if AP_SIM_INS_FILE_ENABLED
309 1326496073 : if (sitl->gyro_file_rw == SITL::SIM::INSFileMode::INS_FILE_WRITE) {
310 3166744 : write_gyro_to_file(gyro);
311 : }
312 : #endif
313 : }
314 269614695 : gyro_accum /= nsamples;
315 :
316 269614695 : gyro_accum.rotate(sitl->imu_orientation);
317 :
318 269614695 : _rotate_and_correct_gyro(gyro_instance, gyro_accum);
319 269614695 : _notify_new_gyro_raw_sample(gyro_instance, gyro_accum, AP_HAL::micros64());
320 269614695 : }
321 :
322 367689203 : void AP_InertialSensor_SITL::timer_update(void)
323 : {
324 : // on some simulations (RealFlight) the aircraft sim decides when a new sample is available.
325 367689203 : if (sitl->state.flightaxis_imu_frame_num > 0) {
326 0 : update_from_frame();
327 0 : return;
328 : }
329 :
330 367689203 : uint64_t now = AP_HAL::micros64();
331 : #if 0
332 : // insert a 1s pause in IMU data. This triggers a pause in EK2
333 : // processing that leads to some interesting issues
334 : if (now > 5e6 && now < 6e6) {
335 : return;
336 : }
337 : #endif
338 367689203 : if (sitl == nullptr) {
339 0 : return;
340 : }
341 :
342 367689203 : if (now >= next_accel_sample) {
343 275666665 : if (((1U << accel_instance) & sitl->accel_fail_mask) == 0) {
344 : #if AP_SIM_INS_FILE_ENABLED
345 275666665 : if (sitl->accel_file_rw == SITL::SIM::INSFileMode::INS_FILE_READ
346 275666665 : || sitl->accel_file_rw == SITL::SIM::INSFileMode::INS_FILE_READ_STOP_ON_EOF) {
347 0 : read_accel_from_file();
348 : } else
349 : #endif
350 275666665 : generate_accel();
351 275666665 : if (next_accel_sample == 0) {
352 4177 : next_accel_sample = now + 1000000UL / accel_sample_hz;
353 : } else {
354 551334957 : while (now >= next_accel_sample) {
355 275672469 : next_accel_sample += 1000000UL / accel_sample_hz;
356 : }
357 : }
358 : }
359 : }
360 367689203 : if (now >= next_gyro_sample) {
361 269614695 : if (((1U << gyro_instance) & sitl->gyro_fail_mask) == 0) {
362 : #if AP_SIM_INS_FILE_ENABLED
363 269614695 : if (sitl->gyro_file_rw == SITL::SIM::INSFileMode::INS_FILE_READ
364 269614695 : || sitl->gyro_file_rw == SITL::SIM::INSFileMode::INS_FILE_READ_STOP_ON_EOF) {
365 0 : read_gyro_from_file();
366 : } else
367 : #endif
368 269614695 : generate_gyro();
369 :
370 269614695 : if (next_gyro_sample == 0) {
371 4177 : next_gyro_sample = now + 1000000UL / gyro_sample_hz;
372 : } else {
373 539229218 : while (now >= next_gyro_sample) {
374 269618700 : next_gyro_sample += 1000000UL / gyro_sample_hz;
375 : }
376 : }
377 : }
378 : }
379 : }
380 :
381 0 : void AP_InertialSensor_SITL::update_from_frame(void)
382 : {
383 0 : if (sitl == nullptr) {
384 0 : return;
385 : }
386 :
387 0 : if (flightaxis_imu_frame_num == sitl->state.flightaxis_imu_frame_num) {
388 0 : return;
389 : }
390 :
391 0 : flightaxis_imu_frame_num = sitl->state.flightaxis_imu_frame_num;
392 :
393 0 : if (((1U << accel_instance) & sitl->accel_fail_mask) == 0) {
394 0 : generate_accel();
395 : }
396 0 : if (((1U << gyro_instance) & sitl->gyro_fail_mask) == 0) {
397 0 : generate_gyro();
398 : }
399 : }
400 :
401 269614695 : float AP_InertialSensor_SITL::gyro_drift(void) const
402 : {
403 534638203 : if (is_zero(sitl->drift_speed) ||
404 265023508 : is_zero(sitl->drift_time)) {
405 4591196 : return 0;
406 : }
407 265023499 : double period = sitl->drift_time * 2;
408 265023499 : double minutes = fmod(AP_HAL::micros64() / 60.0e6, period);
409 265023499 : if (minutes < period/2) {
410 218397667 : return minutes * radians(sitl->drift_speed);
411 : }
412 46625832 : return (period - minutes) * radians(sitl->drift_speed);
413 : }
414 :
415 :
416 83728473 : bool AP_InertialSensor_SITL::update(void)
417 : {
418 83728473 : update_accel(accel_instance);
419 83728473 : update_gyro(gyro_instance);
420 83728473 : return true;
421 : }
422 :
423 : uint8_t AP_InertialSensor_SITL::bus_id = 0;
424 :
425 4177 : void AP_InertialSensor_SITL::start()
426 : {
427 4177 : if (!_imu.register_gyro(gyro_instance, gyro_sample_hz,
428 8354 : AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 1, DEVTYPE_SITL)) ||
429 4177 : !_imu.register_accel(accel_instance, accel_sample_hz,
430 : AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, bus_id, 2, DEVTYPE_SITL))) {
431 0 : return;
432 : }
433 4177 : bus_id++;
434 4177 : hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void));
435 :
436 : #if AP_SIM_INS_FILE_ENABLED
437 4177 : if (sitl->accel_file_rw == SITL::SIM::INSFileMode::INS_FILE_READ
438 4177 : || sitl->accel_file_rw == SITL::SIM::INSFileMode::INS_FILE_READ_STOP_ON_EOF) {
439 0 : hal.console->printf("Reading accel data from file for IMU[%u]\n", accel_instance);
440 : }
441 4177 : if (sitl->gyro_file_rw == SITL::SIM::INSFileMode::INS_FILE_READ
442 4177 : || sitl->gyro_file_rw == SITL::SIM::INSFileMode::INS_FILE_READ) {
443 0 : hal.console->printf("Reading gyro data from file for IMU[%u]\n", gyro_instance);
444 : }
445 : #endif
446 : }
447 :
448 : /*
449 : temporary method to use file as GPS data
450 : */
451 : #if AP_SIM_INS_FILE_ENABLED
452 0 : void AP_InertialSensor_SITL::read_gyro_from_file()
453 : {
454 0 : if (gyro_fd == -1) {
455 : char namebuf[32];
456 0 : snprintf(namebuf, sizeof(namebuf), "/tmp/gyro%d.dat", gyro_instance);
457 0 : gyro_fd = AP::FS().open(namebuf, O_RDONLY);
458 0 : if (gyro_fd == -1) {
459 0 : AP_HAL::panic("gyro data file %s not found", namebuf);
460 : }
461 : }
462 :
463 : float buf[8 * 3 * sizeof(float)];
464 :
465 0 : uint8_t nsamples = enable_fast_sampling(gyro_instance) ? 8 : 1;
466 0 : ssize_t ret = ::read(gyro_fd, buf, nsamples * 3 * sizeof(float));
467 0 : if (ret == (ssize_t)(nsamples * 3 * sizeof(float))) {
468 0 : read_gyro(buf, nsamples);
469 : }
470 :
471 0 : if (ret <= 0) {
472 0 : if (sitl->gyro_file_rw == SITL::SIM::INSFileMode::INS_FILE_READ_STOP_ON_EOF) {
473 : #if HAL_LOGGING_ENABLED
474 : //stop logging
475 0 : if (AP_Logger::get_singleton()) {
476 0 : AP::logger().StopLogging();
477 : }
478 : #endif
479 0 : exit(0);
480 : }
481 0 : lseek(gyro_fd, 0, SEEK_SET);
482 : }
483 0 : }
484 :
485 0 : void AP_InertialSensor_SITL::read_gyro(const float* buf, uint8_t nsamples)
486 : {
487 0 : Vector3f gyro_accum;
488 :
489 0 : for (uint8_t j = 0; j < nsamples; j++) {
490 0 : float p = buf[j*3];
491 0 : float q = buf[j*3+1];
492 0 : float r = buf[j*3+2];
493 :
494 0 : Vector3f gyro = Vector3f(p, q, r);
495 :
496 0 : _notify_new_gyro_sensor_rate_sample(gyro_instance, gyro);
497 :
498 0 : gyro_accum += gyro;
499 : }
500 0 : gyro_accum /= nsamples;
501 0 : _rotate_and_correct_gyro(gyro_instance, gyro_accum);
502 0 : _notify_new_gyro_raw_sample(gyro_instance, gyro_accum, AP_HAL::micros64());
503 0 : }
504 :
505 3166744 : void AP_InertialSensor_SITL::write_gyro_to_file(const Vector3f& gyro)
506 : {
507 3166744 : if (gyro_fd == -1) {
508 : char namebuf[32];
509 8 : snprintf(namebuf, sizeof(namebuf), "/tmp/gyro%d.dat", gyro_instance);
510 8 : gyro_fd = open(namebuf, O_WRONLY|O_TRUNC|O_CREAT, S_IRWXU|S_IRGRP|S_IROTH);
511 : }
512 :
513 3166744 : float buf[] { gyro.x, gyro.y, gyro.z };
514 :
515 3166744 : if (::write(gyro_fd, (void*)buf, sizeof(float) * 3) < 0) {
516 0 : ::printf("Could not write to file\n");
517 : }
518 3166744 : }
519 :
520 0 : void AP_InertialSensor_SITL::read_accel_from_file()
521 : {
522 0 : if (accel_fd == -1) {
523 : char namebuf[32];
524 0 : snprintf(namebuf, sizeof(namebuf), "/tmp/accel%d.dat", accel_instance);
525 0 : accel_fd = open(namebuf, O_RDONLY|O_CLOEXEC);
526 0 : if (accel_fd == -1) {
527 0 : AP_HAL::panic("accel data file %s not found", namebuf);
528 : }
529 : }
530 :
531 : float buf[4*3*sizeof(float)];
532 :
533 0 : uint8_t nsamples = enable_fast_sampling(accel_instance) ? 4 : 1;
534 0 : ssize_t ret = ::read(accel_fd, buf, nsamples * 3 * sizeof(float));
535 0 : if (ret == (ssize_t)(nsamples * 3 * sizeof(float))) {
536 0 : read_accel(buf, nsamples);
537 : }
538 :
539 0 : if (ret <= 0) {
540 0 : if (sitl->accel_file_rw == SITL::SIM::INSFileMode::INS_FILE_READ_STOP_ON_EOF) {
541 : #if HAL_LOGGING_ENABLED
542 : //stop logging
543 0 : if (AP_Logger::get_singleton()) {
544 0 : AP::logger().StopLogging();
545 : }
546 : #endif
547 0 : exit(0);
548 : }
549 0 : lseek(accel_fd, 0, SEEK_SET);
550 : }
551 0 : }
552 :
553 0 : void AP_InertialSensor_SITL::read_accel(const float* buf, uint8_t nsamples)
554 : {
555 0 : Vector3f accel_accum;
556 :
557 0 : for (uint8_t j = 0; j < nsamples; j++) {
558 0 : float p = buf[j*3];
559 0 : float q = buf[j*3+1];
560 0 : float r = buf[j*3+2];
561 :
562 0 : Vector3f accel = Vector3f(p, q, r);
563 :
564 0 : _notify_new_accel_sensor_rate_sample(accel_instance, accel);
565 :
566 0 : accel_accum += accel;
567 : }
568 :
569 0 : accel_accum /= nsamples;
570 0 : _rotate_and_correct_accel(accel_instance, accel_accum);
571 0 : _notify_new_accel_raw_sample(accel_instance, accel_accum, AP_HAL::micros64());
572 :
573 0 : _publish_temperature(accel_instance, get_temperature());
574 0 : }
575 :
576 0 : void AP_InertialSensor_SITL::write_accel_to_file(const Vector3f& accel)
577 : {
578 :
579 0 : if (accel_fd == -1) {
580 : char namebuf[32];
581 0 : snprintf(namebuf, sizeof(namebuf), "/tmp/accel%d.dat", accel_instance);
582 0 : accel_fd = open(namebuf, O_WRONLY|O_TRUNC|O_CREAT, S_IRWXU|S_IRGRP|S_IROTH);
583 : }
584 :
585 0 : float buf[] { accel.x, accel.y, accel.z };
586 :
587 0 : if (::write(accel_fd, (void*)buf, sizeof(float) * 3) < 0) {
588 0 : ::printf("Could not write to file\n");
589 : }
590 0 : }
591 :
592 : #endif // AP_SIM_INS_FILE_ENABLED
593 :
594 : #endif // AP_SIM_INS_ENABLED
|
