2018-03-07 21:25:44 -04:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
/*
simple vicon simulator class
XKFR
*/
# include "SIM_Vicon.h"
# include <stdio.h>
# include <unistd.h>
# include <fcntl.h>
2021-10-11 02:01:17 -03:00
extern const AP_HAL : : HAL & hal ;
2018-03-07 21:25:44 -04:00
using namespace SITL ;
2019-09-27 08:17:52 -03:00
Vicon : : Vicon ( ) :
SerialDevice : : SerialDevice ( )
2018-03-07 21:25:44 -04:00
{
}
void Vicon : : maybe_send_heartbeat ( )
{
const uint32_t now = AP_HAL : : millis ( ) ;
2024-03-06 22:38:01 -04:00
if ( now - last_heartbeat_ms < 500 ) {
2018-03-07 21:25:44 -04:00
// we only provide a heartbeat every so often
return ;
}
2024-03-06 22:38:01 -04:00
uint8_t msg_buf_index ;
if ( ! get_free_msg_buf_index ( msg_buf_index ) ) {
return ;
}
2018-03-07 21:25:44 -04:00
last_heartbeat_ms = now ;
2024-03-06 22:38:01 -04:00
const mavlink_heartbeat_t heartbeat {
type : MAV_TYPE_GCS ,
autopilot : MAV_AUTOPILOT_INVALID ,
} ;
mavlink_msg_heartbeat_encode_status (
system_id ,
component_id ,
& mav_status ,
& msg_buf [ msg_buf_index ] . obs_msg ,
& heartbeat
) ;
msg_buf [ msg_buf_index ] . time_send_us = AP_HAL : : millis ( ) ;
2018-03-07 21:25:44 -04:00
}
2020-05-14 23:14:00 -03:00
// get unused index in msg_buf
bool Vicon : : get_free_msg_buf_index ( uint8_t & index )
{
for ( uint8_t i = 0 ; i < ARRAY_SIZE ( msg_buf ) ; i + + ) {
if ( msg_buf [ i ] . time_send_us = = 0 ) {
index = i ;
return true ;
}
}
return false ;
}
2018-03-07 21:25:44 -04:00
void Vicon : : update_vicon_position_estimate ( const Location & loc ,
2021-06-20 23:59:02 -03:00
const Vector3d & position ,
2020-05-14 23:14:00 -03:00
const Vector3f & velocity ,
2018-03-07 21:25:44 -04:00
const Quaternion & attitude )
{
2018-05-14 00:53:32 -03:00
const uint64_t now_us = AP_HAL : : micros64 ( ) ;
2018-03-07 21:25:44 -04:00
2020-05-14 23:14:00 -03:00
// calculate a random time offset to the time sent in the message
// simulates a time difference between the remote computer and autopilot
2018-05-14 00:53:32 -03:00
if ( time_offset_us = = 0 ) {
time_offset_us = ( unsigned ( random ( ) ) % 7000 ) * 1000000ULL ;
2018-05-25 12:37:59 -03:00
printf ( " time_offset_us %llu \n " , ( long long unsigned ) time_offset_us ) ;
2018-03-07 21:25:44 -04:00
}
2020-05-14 23:14:00 -03:00
// send all messages in the buffer
bool waiting_to_send = false ;
for ( uint8_t i = 0 ; i < ARRAY_SIZE ( msg_buf ) ; i + + ) {
if ( ( msg_buf [ i ] . time_send_us > 0 ) & & ( now_us > = msg_buf [ i ] . time_send_us ) ) {
uint8_t buf [ 300 ] ;
uint16_t buf_len = mavlink_msg_to_send_buffer ( buf , & msg_buf [ i ] . obs_msg ) ;
2021-10-26 09:15:41 -03:00
if ( write_to_autopilot ( ( char * ) & buf , buf_len ) ! = buf_len ) {
2021-10-11 02:01:17 -03:00
hal . console - > printf ( " Vicon: write failure \n " ) ;
2020-05-14 23:14:00 -03:00
}
msg_buf [ i ] . time_send_us = 0 ;
2018-03-07 21:25:44 -04:00
}
2020-05-14 23:14:00 -03:00
waiting_to_send = msg_buf [ i ] . time_send_us ! = 0 ;
2018-05-14 00:53:32 -03:00
}
2020-05-14 23:14:00 -03:00
if ( waiting_to_send ) {
2018-05-14 00:53:32 -03:00
// waiting for the last msg to go out
return ;
}
2020-06-08 02:19:30 -03:00
if ( now_us - last_observation_usec < 20000 ) {
// create observations at 20ms intervals (matches EKF max rate)
2018-05-14 00:53:32 -03:00
return ;
2018-03-07 21:25:44 -04:00
}
2020-04-23 08:25:34 -03:00
// failure simulation
if ( _sitl - > vicon_fail . get ( ) ! = 0 ) {
return ;
}
2018-03-07 21:25:44 -04:00
float roll ;
float pitch ;
float yaw ;
2018-05-14 00:53:32 -03:00
attitude . to_euler ( roll , pitch , yaw ) ;
2018-03-07 21:25:44 -04:00
2020-04-23 08:25:34 -03:00
// calculate sensor offset in earth frame
2020-04-13 03:04:26 -03:00
const Vector3f & pos_offset = _sitl - > vicon_pos_offset . get ( ) ;
Matrix3f rot ;
rot . from_euler ( radians ( _sitl - > state . rollDeg ) , radians ( _sitl - > state . pitchDeg ) , radians ( _sitl - > state . yawDeg ) ) ;
Vector3f pos_offset_ef = rot * pos_offset ;
2020-04-23 08:25:34 -03:00
// add earth frame sensor offset and glitch to position
2021-06-20 23:59:02 -03:00
Vector3d pos_corrected = position + ( pos_offset_ef + _sitl - > vicon_glitch . get ( ) ) . todouble ( ) ;
2020-04-23 08:25:34 -03:00
2020-05-14 23:14:00 -03:00
// calculate a velocity offset due to the antenna position offset and body rotation rate
// note: % operator is overloaded for cross product
Vector3f gyro ( radians ( _sitl - > state . rollRate ) ,
radians ( _sitl - > state . pitchRate ) ,
radians ( _sitl - > state . yawRate ) ) ;
Vector3f vel_rel_offset_bf = gyro % pos_offset ;
// rotate the velocity offset into earth frame and add to the c.g. velocity
Vector3f vel_rel_offset_ef = rot * vel_rel_offset_bf ;
Vector3f vel_corrected = velocity + vel_rel_offset_ef + _sitl - > vicon_vel_glitch . get ( ) ;
// adjust yaw, position and velocity to account for vicon's yaw
2020-04-23 08:25:34 -03:00
const int16_t vicon_yaw_deg = _sitl - > vicon_yaw . get ( ) ;
if ( vicon_yaw_deg ! = 0 ) {
const float vicon_yaw_rad = radians ( vicon_yaw_deg ) ;
yaw = wrap_PI ( yaw - vicon_yaw_rad ) ;
2021-06-20 23:59:02 -03:00
Matrix3d vicon_yaw_rot ;
2020-04-23 08:25:34 -03:00
vicon_yaw_rot . from_euler ( 0 , 0 , - vicon_yaw_rad ) ;
pos_corrected = vicon_yaw_rot * pos_corrected ;
2021-06-20 23:59:02 -03:00
vel_corrected = vicon_yaw_rot . tofloat ( ) * vel_corrected ;
2020-04-23 08:25:34 -03:00
}
2020-04-13 03:04:26 -03:00
2020-05-13 01:09:09 -03:00
// add yaw error reported to vehicle
yaw = wrap_PI ( yaw + radians ( _sitl - > vicon_yaw_error . get ( ) ) ) ;
2020-05-26 08:52:51 -03:00
// 25ms to 124ms delay before sending
uint32_t delay_ms = 25 + unsigned ( random ( ) ) % 100 ;
2020-05-14 23:14:00 -03:00
uint64_t time_send_us = now_us + delay_ms * 1000UL ;
// send vision position estimate message
uint8_t msg_buf_index ;
if ( should_send ( ViconTypeMask : : VISION_POSITION_ESTIMATE ) & & get_free_msg_buf_index ( msg_buf_index ) ) {
2023-05-12 05:04:12 -03:00
const mavlink_vision_position_estimate_t vision_position_estimate {
2024-03-06 23:03:25 -04:00
usec : now_us + time_offset_us ,
x : float ( pos_corrected . x ) ,
y : float ( pos_corrected . y ) ,
z : float ( pos_corrected . z ) ,
roll : roll ,
pitch : pitch ,
yaw : yaw
2023-05-12 05:04:12 -03:00
} ;
2023-09-29 01:09:21 -03:00
mavlink_msg_vision_position_estimate_encode_status (
2023-05-12 05:04:12 -03:00
system_id ,
component_id ,
2023-09-29 01:09:21 -03:00
& mav_status ,
2023-05-12 05:04:12 -03:00
& msg_buf [ msg_buf_index ] . obs_msg ,
& vision_position_estimate
) ;
2020-05-14 23:14:00 -03:00
msg_buf [ msg_buf_index ] . time_send_us = time_send_us ;
}
// send older vicon position estimate message
if ( should_send ( ViconTypeMask : : VICON_POSITION_ESTIMATE ) & & get_free_msg_buf_index ( msg_buf_index ) ) {
2023-05-12 05:04:12 -03:00
const mavlink_vicon_position_estimate_t vicon_position_estimate {
2024-03-06 23:03:25 -04:00
usec : now_us + time_offset_us ,
x : float ( pos_corrected . x ) ,
y : float ( pos_corrected . y ) ,
z : float ( pos_corrected . z ) ,
roll : roll ,
pitch : pitch ,
yaw : yaw
2023-05-12 05:04:12 -03:00
} ;
2023-09-29 01:09:21 -03:00
mavlink_msg_vicon_position_estimate_encode_status (
2023-05-12 05:04:12 -03:00
system_id ,
component_id ,
2023-09-29 01:09:21 -03:00
& mav_status ,
2023-05-12 05:04:12 -03:00
& msg_buf [ msg_buf_index ] . obs_msg ,
& vicon_position_estimate ) ;
2020-05-14 23:14:00 -03:00
msg_buf [ msg_buf_index ] . time_send_us = time_send_us ;
}
// send vision speed estimate
if ( should_send ( ViconTypeMask : : VISION_SPEED_ESTIMATE ) & & get_free_msg_buf_index ( msg_buf_index ) ) {
2023-05-12 05:04:12 -03:00
const mavlink_vision_speed_estimate_t vicon_speed_estimate {
2024-03-06 22:56:57 -04:00
usec : now_us + time_offset_us ,
x : vel_corrected . x ,
y : vel_corrected . y ,
z : vel_corrected . z
2023-05-12 05:04:12 -03:00
} ;
2023-09-29 01:09:21 -03:00
mavlink_msg_vision_speed_estimate_encode_status (
2023-05-12 05:04:12 -03:00
system_id ,
component_id ,
2023-09-29 01:09:21 -03:00
& mav_status ,
2023-05-12 05:04:12 -03:00
& msg_buf [ msg_buf_index ] . obs_msg ,
& vicon_speed_estimate
) ;
2020-05-14 23:14:00 -03:00
msg_buf [ msg_buf_index ] . time_send_us = time_send_us ;
}
2021-02-09 04:45:41 -04:00
2021-12-17 23:18:33 -04:00
// send ODOMETRY message
if ( should_send ( ViconTypeMask : : ODOMETRY ) & & get_free_msg_buf_index ( msg_buf_index ) ) {
2022-11-24 19:22:21 -04:00
const Vector3f vel_corrected_frd = attitude . inverse ( ) * vel_corrected ;
2023-05-12 05:04:12 -03:00
const mavlink_odometry_t odometry {
2024-03-06 22:14:35 -04:00
time_usec : now_us + time_offset_us ,
x : float ( pos_corrected . x ) ,
y : float ( pos_corrected . y ) ,
z : float ( pos_corrected . z ) ,
q : { attitude [ 0 ] , attitude [ 1 ] , attitude [ 2 ] , attitude [ 3 ] } ,
vx : vel_corrected_frd . x ,
vy : vel_corrected_frd . y ,
vz : vel_corrected_frd . z ,
rollspeed : gyro . x ,
pitchspeed : gyro . y ,
yawspeed : gyro . z ,
pose_covariance : { } ,
velocity_covariance : { } ,
frame_id : MAV_FRAME_LOCAL_FRD ,
child_frame_id : MAV_FRAME_BODY_FRD ,
reset_counter : 0 ,
estimator_type : MAV_ESTIMATOR_TYPE_VIO ,
quality : 50 , // quality hardcoded to 50%
2023-05-12 05:04:12 -03:00
} ;
2023-09-29 01:09:21 -03:00
mavlink_msg_odometry_encode_status (
2023-05-12 05:04:12 -03:00
system_id ,
component_id ,
2023-09-29 01:09:21 -03:00
& mav_status ,
2023-05-12 05:04:12 -03:00
& msg_buf [ msg_buf_index ] . obs_msg ,
& odometry ) ;
2021-12-17 23:18:33 -04:00
msg_buf [ msg_buf_index ] . time_send_us = time_send_us ;
}
2021-02-09 04:45:41 -04:00
// determine time, position, and angular deltas
uint64_t time_delta = now_us - last_observation_usec ;
Quaternion attitude_curr ; // Rotation to current MAV_FRAME_BODY_FRD from MAV_FRAME_LOCAL_NED
attitude_curr . from_euler ( roll , pitch , yaw ) ; // Rotation to MAV_FRAME_LOCAL_NED from current MAV_FRAME_BODY_FRD
attitude_curr . invert ( ) ;
Quaternion attitude_curr_prev = attitude_curr * _attitude_prev . inverse ( ) ; // Get rotation to current MAV_FRAME_BODY_FRD from previous MAV_FRAME_BODY_FRD
Matrix3f body_ned_m ;
attitude_curr . rotation_matrix ( body_ned_m ) ;
2021-06-20 23:59:02 -03:00
Vector3f pos_delta = body_ned_m * ( pos_corrected - _position_prev ) . tofloat ( ) ;
2021-02-09 04:45:41 -04:00
// send vision position delta
// time_usec: (usec) Current time stamp
// time_delta_usec: (usec) Time since last reported camera frame
// angle_delta [3]: (radians) Roll, pitch, yaw angles that define rotation to current MAV_FRAME_BODY_FRD from previous MAV_FRAME_BODY_FRD
// 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
// confidence: Normalized confidence level [0, 100]
if ( should_send ( ViconTypeMask : : VISION_POSITION_DELTA ) & & get_free_msg_buf_index ( msg_buf_index ) ) {
2023-05-12 05:04:12 -03:00
const mavlink_vision_position_delta_t vision_position_delta {
2024-03-06 22:03:09 -04:00
time_usec : now_us + time_offset_us ,
time_delta_usec : time_delta ,
angle_delta : { attitude_curr_prev . get_euler_roll ( ) ,
attitude_curr_prev . get_euler_pitch ( ) ,
attitude_curr_prev . get_euler_yaw ( )
} ,
position_delta : { pos_delta . x , pos_delta . y , pos_delta . z } ,
confidence : 0
2023-05-12 05:04:12 -03:00
} ;
2023-09-29 01:09:21 -03:00
mavlink_msg_vision_position_delta_encode_status (
2021-02-09 04:45:41 -04:00
system_id ,
component_id ,
2023-09-29 01:09:21 -03:00
& mav_status ,
2021-02-09 04:45:41 -04:00
& msg_buf [ msg_buf_index ] . obs_msg ,
2023-05-12 05:04:12 -03:00
& vision_position_delta ) ;
2021-02-09 04:45:41 -04:00
msg_buf [ msg_buf_index ] . time_send_us = time_send_us ;
}
// set previous position & attitude
2021-02-16 02:20:37 -04:00
last_observation_usec = now_us ;
2021-02-09 04:45:41 -04:00
_position_prev = pos_corrected ;
_attitude_prev = attitude_curr ;
2018-03-07 21:25:44 -04:00
}
/*
update vicon sensor state
*/
2021-06-20 23:59:02 -03:00
void Vicon : : update ( const Location & loc , const Vector3d & position , const Vector3f & velocity , const Quaternion & attitude )
2018-03-07 21:25:44 -04:00
{
2018-03-25 23:54:05 -03:00
if ( ! init_sitl_pointer ( ) ) {
return ;
}
2018-03-07 21:25:44 -04:00
maybe_send_heartbeat ( ) ;
2020-05-14 23:14:00 -03:00
update_vicon_position_estimate ( loc , position , velocity , attitude ) ;
2018-03-07 21:25:44 -04:00
}