mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 01:44:06 +08:00
532c9abd4a
* Fix comment * Ekf wrapper for testing Add utility function for accessing information in the ekf object * Add step function for Gps sensor * Add RangeFinder and Flow to simulated sensors * Add first fusion logic tests * Add units to function name * Use EXPECT_TRUE * Adding missing qualifiers * Improve EXPECT_ calls * Improve naming
46 lines
819 B
C++
46 lines
819 B
C++
#include "imu.h"
|
|
|
|
namespace sensor_simulator
|
|
{
|
|
namespace sensor
|
|
{
|
|
|
|
Imu::Imu(std::shared_ptr<Ekf> ekf):Sensor(ekf)
|
|
{
|
|
}
|
|
|
|
Imu::~Imu()
|
|
{
|
|
}
|
|
|
|
void Imu::send(uint32_t time)
|
|
{
|
|
imuSample imu_sample;
|
|
imu_sample.time_us = time;
|
|
imu_sample.delta_ang_dt = (time - _time_last_data_sent) * 1.e-6f;
|
|
imu_sample.delta_ang = _gyro_data * imu_sample.delta_ang_dt;
|
|
imu_sample.delta_vel_dt = (time - _time_last_data_sent) * 1.e-6f;
|
|
imu_sample.delta_vel = _accel_data * imu_sample.delta_vel_dt;
|
|
|
|
_ekf->setIMUData(imu_sample);
|
|
}
|
|
|
|
void Imu::setData(const Vector3f& accel, const Vector3f& gyro)
|
|
{
|
|
setAccelData(accel);
|
|
setGyroData(gyro);
|
|
}
|
|
|
|
void Imu::setAccelData(const Vector3f& accel)
|
|
{
|
|
_accel_data = accel;
|
|
}
|
|
|
|
void Imu::setGyroData(const Vector3f& gyro)
|
|
{
|
|
_gyro_data = gyro;
|
|
}
|
|
|
|
} // namespace sensor
|
|
} // namespace sensor_simulator
|