Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : simple vicon simulator class
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_VICON_ENABLED
22 :
23 : #include "SIM_Vicon.h"
24 : #include <stdio.h>
25 : #include <unistd.h>
26 : #include <fcntl.h>
27 :
28 : extern const AP_HAL::HAL& hal;
29 :
30 : using namespace SITL;
31 :
32 : const AP_Param::GroupInfo SIM::ViconParms::var_info[] = {
33 : // @Param: POS_X
34 : // @DisplayName: SITL vicon position on vehicle in Forward direction
35 : // @Description: SITL vicon position on vehicle in Forward direction
36 : // @Units: m
37 : // @Range: 0 10
38 : // @User: Advanced
39 :
40 : // @Param: POS_Y
41 : // @DisplayName: SITL vicon position on vehicle in Right direction
42 : // @Description: SITL vicon position on vehicle in Right direction
43 : // @Units: m
44 : // @Range: 0 10
45 : // @User: Advanced
46 :
47 : // @Param: POS_Z
48 : // @DisplayName: SITL vicon position on vehicle in Down direction
49 : // @Description: SITL vicon position on vehicle in Down direction
50 : // @Units: m
51 : // @Range: 0 10
52 : // @User: Advanced
53 : AP_GROUPINFO("POS", 1, ViconParms, pos_offset, 0),
54 :
55 : // @Param: GLIT_X
56 : // @DisplayName: SITL vicon position glitch North
57 : // @Description: SITL vicon position glitch North
58 : // @Units: m
59 : // @User: Advanced
60 :
61 : // @Param: GLIT_Y
62 : // @DisplayName: SITL vicon position glitch East
63 : // @Description: SITL vicon position glitch East
64 : // @Units: m
65 : // @User: Advanced
66 :
67 : // @Param: GLIT_Z
68 : // @DisplayName: SITL vicon position glitch Down
69 : // @Description: SITL vicon position glitch Down
70 : // @Units: m
71 : // @User: Advanced
72 : AP_GROUPINFO("GLIT", 2, ViconParms, glitch, 0),
73 :
74 : // @Param: FAIL
75 : // @DisplayName: SITL vicon failure
76 : // @Description: SITL vicon failure
77 : // @Values: 0:Vicon Healthy, 1:Vicon Failed
78 : // @User: Advanced
79 : AP_GROUPINFO("FAIL", 3, ViconParms, fail, 0),
80 :
81 : // @Param: YAW
82 : // @DisplayName: SITL vicon yaw angle in earth frame
83 : // @Description: SITL vicon yaw angle in earth frame
84 : // @Units: deg
85 : // @Range: 0 360
86 : // @User: Advanced
87 : AP_GROUPINFO("YAW", 4, ViconParms, yaw, 0),
88 :
89 : // @Param: YAWERR
90 : // @DisplayName: SITL vicon yaw error
91 : // @Description: SITL vicon yaw added to reported yaw sent to vehicle
92 : // @Units: deg
93 : // @Range: -180 180
94 : // @User: Advanced
95 : AP_GROUPINFO("YAWERR", 5, ViconParms, yaw_error, 0),
96 :
97 : // @Param: TMASK
98 : // @DisplayName: SITL vicon type mask
99 : // @Description: SITL vicon messages sent
100 : // @Bitmask: 0:VISION_POSITION_ESTIMATE, 1:VISION_SPEED_ESTIMATE, 2:VICON_POSITION_ESTIMATE, 3:VISION_POSITION_DELTA, 4:ODOMETRY
101 : // @User: Advanced
102 : AP_GROUPINFO("TMASK", 6, ViconParms, type_mask, 3),
103 :
104 : // @Param: VGLI_X
105 : // @DisplayName: SITL vicon velocity glitch North
106 : // @Description: SITL vicon velocity glitch North
107 : // @Units: m/s
108 : // @User: Advanced
109 :
110 : // @Param: VGLI_Y
111 : // @DisplayName: SITL vicon velocity glitch East
112 : // @Description: SITL vicon velocity glitch East
113 : // @Units: m/s
114 : // @User: Advanced
115 :
116 : // @Param: VGLI_Z
117 : // @DisplayName: SITL vicon velocity glitch Down
118 : // @Description: SITL vicon velocity glitch Down
119 : // @Units: m/s
120 : // @User: Advanced
121 : AP_GROUPINFO("VGLI", 7, ViconParms, vel_glitch, 0),
122 :
123 : // @Param: P_SD
124 : // @DisplayName: SITL vicon position standard deviation for gaussian noise
125 : // @Description: SITL vicon position standard deviation for gaussian noise
126 : // @Units: m
127 : // @User: Advanced
128 : AP_GROUPINFO("P_SD", 8, ViconParms, pos_stddev, 0.0f),
129 :
130 : // @Param: V_SD
131 : // @DisplayName: SITL vicon velocity standard deviation for gaussian noise
132 : // @Description: SITL vicon velocity standard deviation for gaussian noise
133 : // @Units: m/s
134 : // @User: Advanced
135 : AP_GROUPINFO("V_SD", 9, ViconParms, vel_stddev, 0.0f),
136 :
137 : // @Param: RATE
138 : // @DisplayName: SITL vicon rate
139 : // @Description: SITL vicon rate
140 : // @Units: Hz
141 : // @User: Advanced
142 : AP_GROUPINFO("RATE", 10, ViconParms, rate_hz, 50),
143 :
144 : // @Param: QUAL
145 : // @DisplayName: SITL vicon odometry quality
146 : // @Description: SITL vicon odometry quality field sent in MAVLink ODOMETRY message (-1=failure, 0=unknown, 1-100=quality)
147 : // @Units: %
148 : // @Range: -1 100
149 : // @User: Advanced
150 : AP_GROUPINFO("QUAL", 11, ViconParms, quality, 50),
151 :
152 : AP_GROUPEND
153 : };
154 :
155 8 : Vicon::Vicon() :
156 8 : SerialDevice::SerialDevice()
157 : {
158 8 : }
159 :
160 886133 : void Vicon::maybe_send_heartbeat()
161 : {
162 886133 : const uint32_t now = AP_HAL::millis();
163 :
164 886133 : if (now - last_heartbeat_ms < 500) {
165 : // we only provide a heartbeat every so often
166 884698 : return;
167 : }
168 :
169 : uint8_t msg_buf_index;
170 23722 : if (!get_free_msg_buf_index(msg_buf_index)) {
171 22287 : return;
172 : }
173 :
174 1435 : last_heartbeat_ms = now;
175 :
176 1435 : const mavlink_heartbeat_t heartbeat{
177 : custom_mode: 0,
178 : type : MAV_TYPE_GCS,
179 : autopilot : MAV_AUTOPILOT_INVALID,
180 : base_mode: 0,
181 : system_status: 0,
182 : mavlink_version: 0,
183 : };
184 :
185 1435 : mavlink_msg_heartbeat_encode_status(
186 1435 : system_id,
187 1435 : component_id,
188 : &mav_status,
189 1435 : &msg_buf[msg_buf_index].obs_msg,
190 : &heartbeat
191 : );
192 1435 : msg_buf[msg_buf_index].time_send_us = AP_HAL::millis();
193 : }
194 :
195 : // get unused index in msg_buf
196 48025 : bool Vicon::get_free_msg_buf_index(uint8_t &index)
197 : {
198 152450 : for (uint8_t i=0; i<ARRAY_SIZE(msg_buf); i++) {
199 124553 : if (msg_buf[i].time_send_us == 0) {
200 20128 : index = i;
201 20128 : return true;
202 : }
203 : }
204 27897 : return false;
205 : }
206 :
207 886133 : void Vicon::update_vicon_position_estimate(const Location &loc,
208 : const Vector3d &position,
209 : const Vector3f &velocity,
210 : const Quaternion &attitude)
211 : {
212 886133 : const uint64_t now_us = AP_HAL::micros64();
213 :
214 : // calculate a random time offset to the time sent in the message
215 : // simulates a time difference between the remote computer and autopilot
216 886133 : if (time_offset_us == 0) {
217 8 : time_offset_us = (unsigned(random()) % 7000) * 1000000ULL;
218 8 : printf("time_offset_us %llu\n", (long long unsigned)time_offset_us);
219 : }
220 :
221 : // send all messages in the buffer
222 886133 : bool waiting_to_send = false;
223 3544532 : for (uint8_t i=0; i<ARRAY_SIZE(msg_buf); i++) {
224 2658399 : if ((msg_buf[i].time_send_us > 0) && (now_us >= msg_buf[i].time_send_us)) {
225 : uint8_t buf[300];
226 20113 : uint16_t buf_len = mavlink_msg_to_send_buffer(buf, &msg_buf[i].obs_msg);
227 :
228 20113 : if (write_to_autopilot((char*)&buf, buf_len) != buf_len) {
229 0 : hal.console->printf("Vicon: write failure\n");
230 : }
231 20113 : msg_buf[i].time_send_us = 0;
232 : }
233 2658399 : waiting_to_send = msg_buf[i].time_send_us != 0;
234 : }
235 886133 : if (waiting_to_send) {
236 : // waiting for the last msg to go out
237 870666 : return;
238 : }
239 :
240 232615 : if (_sitl->vicon.rate_hz == 0) {
241 0 : return;
242 : }
243 232615 : const uint64_t vicon_interval_us = 1000000UL / _sitl->vicon.rate_hz; // Interval in microseconds based on rate
244 232615 : if (now_us - last_observation_usec < vicon_interval_us) {
245 : // create observations at rate specified by vicon_rate_hz
246 : // by default runs at 50Hz
247 197718 : return;
248 : }
249 :
250 : // failure simulation
251 34897 : if (_sitl->vicon.fail.get() != 0) {
252 19430 : return;
253 : }
254 :
255 : float roll;
256 : float pitch;
257 : float yaw;
258 15467 : attitude.to_euler(roll, pitch, yaw);
259 :
260 : // calculate sensor offset in earth frame
261 15467 : const Vector3f& pos_offset = _sitl->vicon.pos_offset.get();
262 15467 : Matrix3f rot;
263 15467 : rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(_sitl->state.yawDeg));
264 15467 : Vector3f pos_offset_ef = rot * pos_offset;
265 :
266 : // add earth frame sensor offset and glitch to position
267 15467 : Vector3d pos_corrected = position + (pos_offset_ef + _sitl->vicon.glitch.get()).todouble();
268 : // add some gaussian noise to the position
269 15467 : pos_corrected += Vector3d(
270 15467 : Aircraft::rand_normal(0, _sitl->vicon.pos_stddev.get()),
271 15467 : Aircraft::rand_normal(0, _sitl->vicon.pos_stddev.get()),
272 15467 : Aircraft::rand_normal(0, _sitl->vicon.pos_stddev.get())
273 15467 : );
274 :
275 : // calculate a velocity offset due to the antenna position offset and body rotation rate
276 : // note: % operator is overloaded for cross product
277 15467 : Vector3f gyro(radians(_sitl->state.rollRate),
278 15467 : radians(_sitl->state.pitchRate),
279 15467 : radians(_sitl->state.yawRate));
280 15467 : Vector3f vel_rel_offset_bf = gyro % pos_offset;
281 :
282 : // rotate the velocity offset into earth frame and add to the c.g. velocity
283 15467 : Vector3f vel_rel_offset_ef = rot * vel_rel_offset_bf;
284 15467 : Vector3f vel_corrected = velocity + vel_rel_offset_ef + _sitl->vicon.vel_glitch.get();
285 :
286 : // adjust yaw, position and velocity to account for vicon's yaw
287 15467 : const int16_t vicon_yaw_deg = _sitl->vicon.yaw.get();
288 15467 : if (vicon_yaw_deg != 0) {
289 0 : const float vicon_yaw_rad = radians(vicon_yaw_deg);
290 0 : yaw = wrap_PI(yaw - vicon_yaw_rad);
291 0 : Matrix3d vicon_yaw_rot;
292 0 : vicon_yaw_rot.from_euler(0, 0, -vicon_yaw_rad);
293 0 : pos_corrected = vicon_yaw_rot * pos_corrected;
294 0 : vel_corrected = vicon_yaw_rot.tofloat() * vel_corrected;
295 : }
296 :
297 : // add some gaussian noise to the velocity
298 15467 : vel_corrected += Vector3f(
299 15467 : Aircraft::rand_normal(0, _sitl->vicon.vel_stddev.get()),
300 15467 : Aircraft::rand_normal(0, _sitl->vicon.vel_stddev.get()),
301 15467 : Aircraft::rand_normal(0, _sitl->vicon.vel_stddev.get())
302 15467 : );
303 :
304 : // add yaw error reported to vehicle
305 15467 : yaw = wrap_PI(yaw + radians(_sitl->vicon.yaw_error.get()));
306 :
307 : // 25ms to 124ms delay before sending
308 15467 : uint32_t delay_ms = 25 + unsigned(random()) % 100;
309 15467 : uint64_t time_send_us = now_us + delay_ms * 1000UL;
310 :
311 :
312 : float pose_cov[21];
313 15467 : memset(pose_cov, 0, sizeof(pose_cov));
314 : // Set variances (diagonal elements), assume no cross-correlation. TODO: figure out attitude variances
315 15467 : const float pos_variance = _sitl->vicon.pos_stddev*_sitl->vicon.pos_stddev;
316 15467 : pose_cov[0] = pos_variance; // x
317 15467 : pose_cov[6] = pos_variance; // y
318 15467 : pose_cov[11] = pos_variance; // z
319 :
320 : // send vision position estimate message
321 : uint8_t msg_buf_index;
322 15467 : if (should_send(ViconTypeMask::VISION_POSITION_ESTIMATE) && get_free_msg_buf_index(msg_buf_index)) {
323 10506 : mavlink_vision_position_estimate_t vision_position_estimate{
324 10506 : usec: now_us + time_offset_us,
325 10506 : x: float(pos_corrected.x),
326 10506 : y: float(pos_corrected.y),
327 10506 : z: float(pos_corrected.z),
328 : roll: roll,
329 : pitch: pitch,
330 : yaw: yaw
331 10506 : };
332 10506 : memcpy(vision_position_estimate.covariance, pose_cov, sizeof(pose_cov));
333 :
334 10506 : mavlink_msg_vision_position_estimate_encode_status(
335 10506 : system_id,
336 10506 : component_id,
337 : &mav_status,
338 10506 : &msg_buf[msg_buf_index].obs_msg,
339 : &vision_position_estimate
340 : );
341 10506 : msg_buf[msg_buf_index].time_send_us = time_send_us;
342 : }
343 :
344 : // send older vicon position estimate message
345 15467 : if (should_send(ViconTypeMask::VICON_POSITION_ESTIMATE) && get_free_msg_buf_index(msg_buf_index)) {
346 0 : const mavlink_vicon_position_estimate_t vicon_position_estimate{
347 0 : usec: now_us + time_offset_us,
348 0 : x: float(pos_corrected.x),
349 0 : y: float(pos_corrected.y),
350 0 : z: float(pos_corrected.z),
351 : roll: roll,
352 : pitch: pitch,
353 : yaw: yaw
354 0 : };
355 0 : mavlink_msg_vicon_position_estimate_encode_status(
356 0 : system_id,
357 0 : component_id,
358 : &mav_status,
359 0 : &msg_buf[msg_buf_index].obs_msg,
360 : &vicon_position_estimate);
361 0 : msg_buf[msg_buf_index].time_send_us = time_send_us;
362 : }
363 :
364 : // send vision speed estimate
365 15467 : if (should_send(ViconTypeMask::VISION_SPEED_ESTIMATE) && get_free_msg_buf_index(msg_buf_index)) {
366 : float cov[9];
367 4452 : memset(cov, 0, sizeof(cov));
368 : // Set variances (diagonal elements), assume no cross-correlation
369 4452 : const float vel_variance = _sitl->vicon.vel_stddev*_sitl->vicon.vel_stddev;
370 4452 : cov[0] = vel_variance; // x
371 4452 : cov[4] = vel_variance; // y
372 4452 : cov[8] = vel_variance; // z
373 4452 : mavlink_vision_speed_estimate_t vicon_speed_estimate{
374 4452 : usec: now_us + time_offset_us,
375 4452 : x: vel_corrected.x,
376 4452 : y: vel_corrected.y,
377 4452 : z: vel_corrected.z
378 4452 : };
379 4452 : memcpy(vicon_speed_estimate.covariance, cov, sizeof(cov));
380 :
381 4452 : mavlink_msg_vision_speed_estimate_encode_status(
382 4452 : system_id,
383 4452 : component_id,
384 : &mav_status,
385 4452 : &msg_buf[msg_buf_index].obs_msg,
386 : &vicon_speed_estimate
387 : );
388 4452 : msg_buf[msg_buf_index].time_send_us = time_send_us;
389 : }
390 :
391 :
392 : // send ODOMETRY message
393 15467 : if (should_send(ViconTypeMask::ODOMETRY) && get_free_msg_buf_index(msg_buf_index)) {
394 0 : const Vector3f vel_corrected_frd = attitude.inverse() * vel_corrected;
395 : float vel_cov[21];
396 0 : memset(vel_cov, 0, sizeof(vel_cov));
397 : // Set variances (diagonal elements), assume no cross-correlation. TODO: figure out angular velocity variances
398 0 : const float vel_variance = _sitl->vicon.vel_stddev*_sitl->vicon.vel_stddev;
399 0 : vel_cov[0] = vel_variance; // x
400 0 : vel_cov[6] = vel_variance; // y
401 0 : vel_cov[11] = vel_variance; // z
402 :
403 0 : mavlink_odometry_t odometry{
404 0 : time_usec: now_us + time_offset_us,
405 0 : x: float(pos_corrected.x),
406 0 : y: float(pos_corrected.y),
407 0 : z: float(pos_corrected.z),
408 0 : q: {attitude[0], attitude[1], attitude[2], attitude[3]},
409 0 : vx: vel_corrected_frd.x,
410 0 : vy: vel_corrected_frd.y,
411 0 : vz: vel_corrected_frd.z,
412 0 : rollspeed: gyro.x,
413 0 : pitchspeed: gyro.y,
414 0 : yawspeed: gyro.z,
415 : pose_covariance: {},
416 : velocity_covariance: {},
417 : frame_id: MAV_FRAME_LOCAL_FRD,
418 : child_frame_id: MAV_FRAME_BODY_FRD,
419 : reset_counter: 0,
420 : estimator_type: MAV_ESTIMATOR_TYPE_VIO,
421 0 : quality: constrain_int8(_sitl->vicon.quality.get(), -1, 100),
422 0 : };
423 0 : memcpy(odometry.pose_covariance, pose_cov, sizeof(pose_cov));
424 0 : memcpy(odometry.velocity_covariance, vel_cov, sizeof(vel_cov));
425 :
426 0 : mavlink_msg_odometry_encode_status(
427 0 : system_id,
428 0 : component_id,
429 : &mav_status,
430 0 : &msg_buf[msg_buf_index].obs_msg,
431 : &odometry);
432 0 : msg_buf[msg_buf_index].time_send_us = time_send_us;
433 : }
434 :
435 : // determine time, position, and angular deltas
436 15467 : uint64_t time_delta = now_us - last_observation_usec;
437 :
438 15467 : Quaternion attitude_curr; // Rotation to current MAV_FRAME_BODY_FRD from MAV_FRAME_LOCAL_NED
439 15467 : attitude_curr.from_euler(roll, pitch, yaw); // Rotation to MAV_FRAME_LOCAL_NED from current MAV_FRAME_BODY_FRD
440 15467 : attitude_curr.invert();
441 :
442 15467 : Quaternion attitude_curr_prev = attitude_curr * _attitude_prev.inverse(); // Get rotation to current MAV_FRAME_BODY_FRD from previous MAV_FRAME_BODY_FRD
443 :
444 15467 : Matrix3f body_ned_m;
445 15467 : attitude_curr.rotation_matrix(body_ned_m);
446 :
447 15467 : Vector3f pos_delta = body_ned_m * (pos_corrected - _position_prev).tofloat();
448 :
449 : // send vision position delta
450 : // time_usec: (usec) Current time stamp
451 : // time_delta_usec: (usec) Time since last reported camera frame
452 : // angle_delta [3]: (radians) Roll, pitch, yaw angles that define rotation to current MAV_FRAME_BODY_FRD from previous MAV_FRAME_BODY_FRD
453 : // delta_position [3]: (meters) Change in position: To current position from previous position rotated to current MAV_FRAME_BODY_FRD from MAV_FRAME_LOCAL_NED
454 : // confidence: Normalized confidence level [0, 100]
455 15467 : if (should_send(ViconTypeMask::VISION_POSITION_DELTA) && get_free_msg_buf_index(msg_buf_index)) {
456 3735 : const mavlink_vision_position_delta_t vision_position_delta{
457 3735 : time_usec: now_us + time_offset_us,
458 : time_delta_usec: time_delta,
459 3735 : angle_delta: { attitude_curr_prev.get_euler_roll(),
460 3735 : attitude_curr_prev.get_euler_pitch(),
461 3735 : attitude_curr_prev.get_euler_yaw()
462 : },
463 3735 : position_delta: {pos_delta.x, pos_delta.y, pos_delta.z},
464 : confidence: 0
465 3735 : };
466 3735 : mavlink_msg_vision_position_delta_encode_status(
467 3735 : system_id,
468 3735 : component_id,
469 : &mav_status,
470 3735 : &msg_buf[msg_buf_index].obs_msg,
471 : &vision_position_delta);
472 3735 : msg_buf[msg_buf_index].time_send_us = time_send_us;
473 : }
474 :
475 : // set previous position & attitude
476 15467 : last_observation_usec = now_us;
477 15467 : _position_prev = pos_corrected;
478 15467 : _attitude_prev = attitude_curr;
479 : }
480 :
481 : /*
482 : update vicon sensor state
483 : */
484 886133 : void Vicon::update(const Location &loc, const Vector3d &position, const Vector3f &velocity, const Quaternion &attitude)
485 : {
486 886133 : if (!init_sitl_pointer()) {
487 0 : return;
488 : }
489 :
490 886133 : maybe_send_heartbeat();
491 886133 : update_vicon_position_estimate(loc, position, velocity, attitude);
492 : }
493 :
494 : #endif // AP_SIM_VICON_ENABLED
|
