mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-30 00:50:34 +08:00
Daniel Agar
dfdfbbfa9c
- replace float32[21] URT covariances with smaller dedicated position/velocity/orientation variances (the crossterms are unused, awkward, and relatively costly) - these are easier to casually inspect and more representative of what's actually being used currently and reduces the size of vehicle_odometry_s quite a bit - ekf2: add new helper to get roll/pitch/yaw covariances - mavlink: receiver ODOMETRY handle more frame types for both pose (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU) and velocity (MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_ENU, MAV_FRAME_LOCAL_FRD, MAV_FRAME_LOCAL_FLU, MAV_FRAME_BODY_FRD) - mavlink: delete unused ATT_POS_MOCAP stream (this is just a passthrough) Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
82 lines
1.4 KiB
C++
82 lines
1.4 KiB
C++
#include "vio.h"
|
|
|
|
namespace sensor_simulator
|
|
{
|
|
namespace sensor
|
|
{
|
|
|
|
Vio::Vio(std::shared_ptr<Ekf> ekf): Sensor(ekf)
|
|
{
|
|
}
|
|
|
|
Vio::~Vio()
|
|
{
|
|
}
|
|
|
|
void Vio::send(uint64_t time)
|
|
{
|
|
_vio_data.time_us = time;
|
|
_ekf->setExtVisionData(_vio_data);
|
|
}
|
|
|
|
void Vio::setData(const extVisionSample &vio_data)
|
|
{
|
|
_vio_data = vio_data;
|
|
}
|
|
|
|
void Vio::setVelocityVariance(const Vector3f &velVar)
|
|
{
|
|
_vio_data.velVar = velVar;
|
|
}
|
|
|
|
void Vio::setPositionVariance(const Vector3f &posVar)
|
|
{
|
|
_vio_data.posVar = posVar;
|
|
}
|
|
|
|
void Vio::setAngularVariance(float angVar)
|
|
{
|
|
_vio_data.angVar = angVar;
|
|
}
|
|
|
|
void Vio::setVelocity(const Vector3f &vel)
|
|
{
|
|
_vio_data.vel = vel;
|
|
}
|
|
|
|
void Vio::setPosition(const Vector3f &pos)
|
|
{
|
|
_vio_data.pos = pos;
|
|
}
|
|
|
|
void Vio::setOrientation(const Quatf &quat)
|
|
{
|
|
_vio_data.quat = quat;
|
|
}
|
|
|
|
void Vio::setVelocityFrameToBody()
|
|
{
|
|
_vio_data.vel_frame = VelocityFrame::BODY_FRAME_FRD;
|
|
}
|
|
|
|
void Vio::setVelocityFrameToLocal()
|
|
{
|
|
_vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
|
|
}
|
|
|
|
extVisionSample Vio::dataAtRest()
|
|
{
|
|
extVisionSample vio_data;
|
|
vio_data.pos = Vector3f{0.0f, 0.0f, 0.0f};;
|
|
vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};;
|
|
vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f};
|
|
vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
|
|
vio_data.velVar = Vector3f{0.1f, 0.1f, 0.1f};
|
|
vio_data.angVar = 0.05f;
|
|
vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
|
|
return vio_data;
|
|
}
|
|
|
|
} // namespace sensor
|
|
} // namespace sensor_simulator
|