// apply yaw angle to a vector void OpticalFlow_backend::_applyYaw(Vector2f &v) { float yawAngleRad = _yawAngleRad(); if (is_zero(yawAngleRad)) { return; } float cosYaw = cosf(yawAngleRad); float sinYaw = sinf(yawAngleRad); float x = v.x; float y = v.y; v.x = cosYaw * x - sinYaw * y; v.y = sinYaw * x + cosYaw * y; }
// 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 ); } }