/* 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 . */ /* simple vicon simulator class XKFR */ #include "SIM_Vicon.h" #include #include #include extern const AP_HAL::HAL& hal; using namespace SITL; Vicon::Vicon() : SerialDevice::SerialDevice() { if (!valid_channel(mavlink_ch)) { AP_HAL::panic("Invalid mavlink channel"); } } void Vicon::maybe_send_heartbeat() { const uint32_t now = AP_HAL::millis(); if (now - last_heartbeat_ms < 100) { // we only provide a heartbeat every so often return; } last_heartbeat_ms = now; mavlink_message_t msg; mavlink_msg_heartbeat_pack(system_id, component_id, &msg, MAV_TYPE_GCS, MAV_AUTOPILOT_INVALID, 0, 0, 0); } // get unused index in msg_buf bool Vicon::get_free_msg_buf_index(uint8_t &index) { for (uint8_t i=0; i 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); if (::write(fd_my_end, (void*)&buf, buf_len) != buf_len) { hal.console->printf("Vicon: write failure\n"); } msg_buf[i].time_send_us = 0; } waiting_to_send = msg_buf[i].time_send_us != 0; } if (waiting_to_send) { // waiting for the last msg to go out return; } if (now_us - last_observation_usec < 20000) { // create observations at 20ms intervals (matches EKF max rate) return; } // failure simulation if (_sitl->vicon_fail.get() != 0) { return; } float roll; float pitch; float yaw; attitude.to_euler(roll, pitch, yaw); // calculate sensor offset in earth frame 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; // add earth frame sensor offset and glitch to position Vector3d pos_corrected = position + (pos_offset_ef + _sitl->vicon_glitch.get()).todouble(); // 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 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); Matrix3d vicon_yaw_rot; vicon_yaw_rot.from_euler(0, 0, -vicon_yaw_rad); pos_corrected = vicon_yaw_rot * pos_corrected; vel_corrected = vicon_yaw_rot.tofloat() * vel_corrected; } // add yaw error reported to vehicle yaw = wrap_PI(yaw + radians(_sitl->vicon_yaw_error.get())); // 25ms to 124ms delay before sending uint32_t delay_ms = 25 + unsigned(random()) % 100; 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)) { mavlink_msg_vision_position_estimate_pack_chan( system_id, component_id, mavlink_ch, &msg_buf[msg_buf_index].obs_msg, now_us + time_offset_us, pos_corrected.x, pos_corrected.y, pos_corrected.z, roll, pitch, yaw, NULL, 0); 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)) { mavlink_msg_vicon_position_estimate_pack_chan( system_id, component_id, mavlink_ch, &msg_buf[msg_buf_index].obs_msg, now_us + time_offset_us, pos_corrected.x, pos_corrected.y, pos_corrected.z, roll, pitch, yaw, NULL); 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)) { mavlink_msg_vision_speed_estimate_pack_chan( system_id, component_id, mavlink_ch, &msg_buf[msg_buf_index].obs_msg, now_us + time_offset_us, vel_corrected.x, vel_corrected.y, vel_corrected.z, NULL, 0); msg_buf[msg_buf_index].time_send_us = time_send_us; } // 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 float angle_delta[3] = {attitude_curr_prev.get_euler_roll(), attitude_curr_prev.get_euler_pitch(), attitude_curr_prev.get_euler_yaw()}; Matrix3f body_ned_m; attitude_curr.rotation_matrix(body_ned_m); Vector3f pos_delta = body_ned_m * (pos_corrected - _position_prev).tofloat(); float postion_delta[3] = {pos_delta.x, pos_delta.y, pos_delta.z}; // 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)) { mavlink_msg_vision_position_delta_pack_chan( system_id, component_id, mavlink_ch, &msg_buf[msg_buf_index].obs_msg, now_us + time_offset_us, time_delta, angle_delta, postion_delta, 0.0f); msg_buf[msg_buf_index].time_send_us = time_send_us; } // set previous position & attitude last_observation_usec = now_us; _position_prev = pos_corrected; _attitude_prev = attitude_curr; } /* update vicon sensor state */ void Vicon::update(const Location &loc, const Vector3d &position, const Vector3f &velocity, const Quaternion &attitude) { if (!init_sitl_pointer()) { return; } maybe_send_heartbeat(); update_vicon_position_estimate(loc, position, velocity, attitude); }