PX4-Autopilot/src/modules/ekf2/test/sensor_simulator/vio.cpp

#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)
{
	setVelocityCovariance(matrix::diag(velVar));
}

void Vio::setVelocityCovariance(const Matrix3f &velCov)
{
	_vio_data.velCov = velCov;
}

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.velCov = matrix::eye<float, 3>() * 0.1f;
	vio_data.angVar = 0.05f;
	vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
	return vio_data;
}

} // namespace sensor
} // namespace sensor_simulator