mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 03:14:07 +08:00
8f3df7a97b
- extract motion checks performed on ground - move all non-timing check to controlOpticalFlowFusion. This simplifies and standardirzes the setOpticalFlowData function. Furthermore, in some cases (SITL, PX4Flow), the dt is forced to 0 when the quality is 0 (which is usual on ground) so the ekf needs to ignore those values on ground to initialize properly the flow fusion. - add filter convergence test - move check for dt time max in setOpticalFlowData - in the simulator, do not update dt as this is the sensor integration period.
40 lines
645 B
C++
40 lines
645 B
C++
#include "flow.h"
|
|
|
|
namespace sensor_simulator
|
|
{
|
|
namespace sensor
|
|
{
|
|
|
|
Flow::Flow(std::shared_ptr<Ekf> ekf):Sensor(ekf)
|
|
{
|
|
}
|
|
|
|
Flow::~Flow()
|
|
{
|
|
}
|
|
|
|
void Flow::send(uint64_t time)
|
|
{
|
|
_flow_data.time_us = time;
|
|
_ekf->setOpticalFlowData(_flow_data);
|
|
}
|
|
|
|
void Flow::setData(const flowSample& flow)
|
|
{
|
|
_flow_data = flow;
|
|
|
|
}
|
|
|
|
flowSample Flow::dataAtRest()
|
|
{
|
|
flowSample flow_at_rest;
|
|
flow_at_rest.dt = static_cast<float>(_update_period) * 1e-6f;
|
|
flow_at_rest.flow_xy_rad = Vector2f{0.0f, 0.0f};
|
|
flow_at_rest.gyro_xyz = Vector3f{0.0f, 0.0f, 0.0f};
|
|
flow_at_rest.quality = 255;
|
|
return flow_at_rest;
|
|
}
|
|
|
|
} // namespace sensor
|
|
} // namespace sensor_simulator
|