void AP_OpticalFlow_Onboard::update()
{
AP_HAL::OpticalFlow::Data_Frame data_frame;
// read at maximum 10Hz
uint32_t now = AP_HAL::millis();
if (now - _last_read_ms < 100) {
return;
}
_last_read_ms = now;
if (!hal.opticalflow->read(data_frame)) {
return;
}
struct OpticalFlow::OpticalFlow_state state;
state.surface_quality = data_frame.quality;
if (data_frame.delta_time > 0) {
const Vector2f flowScaler = _flowScaler();
float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
// delta_time is in microseconds and flow is in milliradians
// per second, so multiply by 1000
state.flowRate.x = flowScaleFactorX * 1000.0f /
float(data_frame.delta_time) *
data_frame.pixel_flow_x_integral;
state.flowRate.y = flowScaleFactorY * 1000.0f /
float(data_frame.delta_time) *
data_frame.pixel_flow_y_integral;
// delta_time is in microseconds so multiply to get back to seconds
state.bodyRate.x = 1000000.0f / float(data_frame.delta_time) *
data_frame.gyro_x_integral;
state.bodyRate.y = 1000000.0f / float(data_frame.delta_time) *
data_frame.gyro_y_integral;
_applyYaw(state.flowRate);
} else {
state.flowRate.zero();
state.bodyRate.zero();
}
// copy results to front end
_update_frontend(state);
#if OPTICALFLOW_ONBOARD_DEBUG
hal.console->printf("FLOW_ONBOARD qual:%u FlowRateX:%4.2f Y:%4.2f"
"BodyRateX:%4.2f Y:%4.2f, delta_time = %u\n",
(unsigned)state.surface_quality,
(double)state.flowRate.x,
(double)state.flowRate.y,
(double)state.bodyRate.x,
(double)state.bodyRate.y,
data_frame.delta_time);
#endif
}
// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_Linux::update(void)
{
optical_flow_s report;
// return immediately if not initialised or more than half of last 40 reads have failed
if (!initialised || num_errors >= 20) {
return;
}
// throttle reads to no more than 10hz
uint32_t now = hal.scheduler->millis();
if (now - last_read_ms < 100) {
return;
}
last_read_ms = now;
// read the report from the sensor
if (!read(&report)) {
return;
}
// process
struct OpticalFlow::OpticalFlow_state state;
state.device_id = report.sensor_id;
state.surface_quality = report.quality;
if (report.integration_timespan > 0) {
const Vector2f flowScaler = _flowScaler();
float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
float integralToRate = 1e6f / float(report.integration_timespan);
state.flowRate.x = flowScaleFactorX * integralToRate * float(report.pixel_flow_x_integral); // rad/sec measured optically about the X sensor axis
state.flowRate.y = flowScaleFactorY * integralToRate * float(report.pixel_flow_y_integral); // rad/sec measured optically about the Y sensor axis
state.bodyRate.x = integralToRate * float(report.gyro_x_rate_integral); // rad/sec measured inertially about the X sensor axis
state.bodyRate.y = integralToRate * float(report.gyro_y_rate_integral); // rad/sec measured inertially about the Y sensor axis
} else {
state.flowRate.zero();
state.bodyRate.zero();
}
// copy results to front end
_update_frontend(state);
#if PX4FLOW_DEBUG
hal.console->printf_P(PSTR("PX4FLOW id:%u qual:%u FlowRateX:%4.2f Y:%4.2f BodyRateX:%4.2f y:%4.2f\n"),
(unsigned)state.device_id,
(unsigned)state.surface_quality,
(double)state.flowRate.x,
(double)state.flowRate.y,
(double)state.bodyRate.x,
(double)state.bodyRate.y);
#endif
}
// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_PX4::update(void)
{
// return immediately if not initialised
if (_fd == -1) {
return;
}
float sum = 0;
uint16_t count = 0;
struct optical_flow_s report;
while (::read(_fd, &report, sizeof(optical_flow_s)) == sizeof(optical_flow_s) &&
report.timestamp != _last_timestamp) {
struct OpticalFlow::OpticalFlow_state state;
state.device_id = report.sensor_id;
state.surface_quality = report.quality;
if (report.integration_timespan > 0) {
float yawAngleRad = _yawAngleRad();
float cosYaw = cosf(yawAngleRad);
float sinYaw = sinf(yawAngleRad);
const Vector2f flowScaler = _flowScaler();
float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
float integralToRate = 1e6f / float(report.integration_timespan);
// rotate sensor measurements from sensor to body frame through sensor yaw angle
state.flowRate.x = flowScaleFactorX * integralToRate * (cosYaw * float(report.pixel_flow_x_integral) - sinYaw * float(report.pixel_flow_y_integral)); // rad/sec measured optically about the X body axis
state.flowRate.y = flowScaleFactorY * integralToRate * (sinYaw * float(report.pixel_flow_x_integral) + cosYaw * float(report.pixel_flow_y_integral)); // rad/sec measured optically about the Y body axis
state.bodyRate.x = integralToRate * (cosYaw * float(report.gyro_x_rate_integral) - sinYaw * float(report.gyro_y_rate_integral)); // rad/sec measured inertially about the X body axis
state.bodyRate.y = integralToRate * (sinYaw * float(report.gyro_x_rate_integral) + cosYaw * float(report.gyro_y_rate_integral)); // rad/sec measured inertially about the Y body axis
} else {
state.flowRate.zero();
state.bodyRate.zero();
}
sum += report.ground_distance_m;
count++;
_last_timestamp = report.timestamp;
_update_frontend(state);
}
if (count != 0){
float dist = sum / count * 100.0f;
setDistance( dist );
}
}