Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: vicon supports sending vision-speed-estimate 

SIM_VICON_TMASK controls which of the 3 supported messages are sent
SIM_VICON_VGLI_X/Y/Z allows introducing a velocity glitch
This commit is contained in:
Randy Mackay 2020-05-15 11:14:00 +09:00
parent 3a209d5d84
commit c8f6cb233b
4 changed files with 127 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

143
libraries/SITL/SIM_Vicon.cpp
View File

@ -26,9 +26,6 @@
using namespace SITL;
#define USE_VISION_POSITION_ESTIMATE 1 // 1 = send VISION_POSITION_ESTIMATE messages, 0 = send VICON_POSITION_ESTIMATE
Vicon::Vicon() :
SerialDevice::SerialDevice()
{
@ -58,27 +55,47 @@ void Vicon::maybe_send_heartbeat()
0);
}
// 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;
}
void Vicon::update_vicon_position_estimate(const Location &loc,
const Vector3f &position,
const Vector3f &velocity,
const Quaternion &attitude)
{
const uint64_t now_us = AP_HAL::micros64();
// calculate a random time offset to the time sent in the message
// simulates a time difference between the remote computer and autopilot
if (time_offset_us == 0) {
time_offset_us = (unsigned(random()) % 7000) * 1000000ULL;
printf("time_offset_us %llu\n", (long long unsigned)time_offset_us);
}
if (time_send_us && now_us >= time_send_us) {
uint8_t msgbuf[300];
uint16_t msgbuf_len = mavlink_msg_to_send_buffer(msgbuf, &obs_msg);
if (::write(fd_my_end, (void*)&msgbuf, msgbuf_len) != msgbuf_len) {
::fprintf(stderr, "Vicon: write failure\n");
// 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);
if (::write(fd_my_end, (void*)&buf, buf_len) != buf_len) {
::fprintf(stderr, "Vicon: write failure\n");
}
msg_buf[i].time_send_us = 0;
}
time_send_us = 0;
waiting_to_send = msg_buf[i].time_send_us != 0;
}
if (time_send_us != 0) {
if (waiting_to_send) {
// waiting for the last msg to go out
return;
}
@ -107,7 +124,18 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
// add earth frame sensor offset and glitch to position
Vector3f pos_corrected = position + pos_offset_ef + _sitl->vicon_glitch.get();
// adjust yaw and position to account for vicon's yaw
// 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);
@ -115,56 +143,79 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
Matrix3f 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 * vel_corrected;
}
// add yaw error reported to vehicle
yaw = wrap_PI(yaw + radians(_sitl->vicon_yaw_error.get()));
#if USE_VISION_POSITION_ESTIMATE
// use the more recent VISION_POSITION_ESTIMATE message
mavlink_msg_vision_position_estimate_pack_chan(
system_id,
component_id,
mavlink_ch,
&obs_msg,
now_us + time_offset_us,
pos_corrected.x,
pos_corrected.y,
pos_corrected.z,
roll,
pitch,
yaw,
NULL, 0);
#else
mavlink_msg_vicon_position_estimate_pack_chan(
system_id,
component_id,
mavlink_ch,
&obs_msg,
now_us + time_offset_us,
pos_corrected.x,
pos_corrected.y,
pos_corrected.z,
roll,
pitch,
yaw,
NULL);
#endif // USE_VISION_POSITION_ESTIMATE
// 25ms to 324ms delay before sending
uint32_t delay_ms = 25 + unsigned(random()) % 300;
time_send_us = now_us + delay_ms * 1000UL;
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;
}
}
/*
update vicon sensor state
*/
void Vicon::update(const Location &loc, const Vector3f &position, const Quaternion &attitude)
void Vicon::update(const Location &loc, const Vector3f &position, const Vector3f &velocity, const Quaternion &attitude)
{
if (!init_sitl_pointer()) {
return;
}
maybe_send_heartbeat();
update_vicon_position_estimate(loc, position, attitude);
update_vicon_position_estimate(loc, position, velocity, attitude);
}

27
libraries/SITL/SIM_Vicon.h
View File

@ -34,7 +34,7 @@ public:
Vicon();
// update state
void update(const Location &loc, const Vector3f &position, const Quaternion &attitude);
void update(const Location &loc, const Vector3f &position, const Vector3f &velocity, const Quaternion &attitude);
private:
@ -46,14 +46,31 @@ private:
// Beware: the mavlink rangefinder shares this channel.
const mavlink_channel_t mavlink_ch = (mavlink_channel_t)(MAVLINK_COMM_0+5);
uint64_t last_observation_usec;
uint64_t time_send_us;
uint64_t time_offset_us;
mavlink_message_t obs_msg;
uint64_t last_observation_usec; // time last observation was sent
uint64_t time_offset_us; // simulated timeoffset between external system and autopilot
// buffer of messages to send
struct {
uint64_t time_send_us; // system time this message should be sent or 0 if no message to send
mavlink_message_t obs_msg; // message to be sent
} msg_buf[3];
// SIM_VICON_TYPE parameter bit values
enum class ViconTypeMask : uint8_t {
VISION_POSITION_ESTIMATE = (1 << 0),
VISION_SPEED_ESTIMATE = (1 << 1),
VICON_POSITION_ESTIMATE = (1 << 2)
};
// return true if the given message type should be sent
bool should_send(ViconTypeMask type_mask) const { return (((uint8_t)type_mask & _sitl->vicon_type_mask.get()) > 0); }
// get unused index in msg_buf
bool get_free_msg_buf_index(uint8_t &index);
void update_vicon_position_estimate(const Location &loc,
const Vector3f &position,
const Vector3f &velocity,
const Quaternion &attitude);
void maybe_send_heartbeat();

6
libraries/SITL/SITL.cpp
View File

@ -251,6 +251,12 @@ const AP_Param::GroupInfo SITL::var_info3[] = {
// vicon yaw error in degrees (added to reported yaw sent to vehicle)
AP_GROUPINFO("VICON_YAWERR", 19, SITL, vicon_yaw_error, 0),
// vicon message type mask
AP_GROUPINFO("VICON_TMASK", 20, SITL, vicon_type_mask, 1),
// vicon velocity glitch in NED frame
AP_GROUPINFO("VICON_VGLI", 21, SITL, vicon_vel_glitch, 0),
AP_GROUPEND
};

2
libraries/SITL/SITL.h
View File

@ -367,6 +367,8 @@ public:
AP_Int8 vicon_fail; // trigger vicon failure
AP_Int16 vicon_yaw; // vicon local yaw in degrees
AP_Int16 vicon_yaw_error; // vicon yaw error in degrees (added to reported yaw sent to vehicle)
AP_Int8 vicon_type_mask; // vicon message type mask (bit0:vision position estimate, bit1:vision speed estimate, bit2:vicon position estimate)
AP_Vector3f vicon_vel_glitch; // velocity glitch in m/s in vicon's local frame
};
} // namespace SITL