Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_OpticalFlow: correct names of variables in HereFlow OF driver 

my guess is that the heavy maths was moved out of the timer function
This commit is contained in:
Peter Barker 2023-07-12 21:08:10 +10:00 committed by Peter Barker
parent eb10b81964
commit d2400ad39b
2 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp
View File

@ -53,8 +53,8 @@ void AP_OpticalFlow_HereFlow::handle_measurement(AP_DroneCAN *ap_dronecan, const
if (_ap_dronecan == ap_dronecan && _node_id == transfer.source_node_id) { if (_ap_dronecan == ap_dronecan && _node_id == transfer.source_node_id) {
WITH_SEMAPHORE(_driver->_sem); WITH_SEMAPHORE(_driver->_sem);
_driver->new_data = true; _driver->new_data = true;
_driver->flowRate = Vector2f(msg.flow_integral[0], msg.flow_integral[1]); _driver->flow_integral = Vector2f(msg.flow_integral[0], msg.flow_integral[1]);
_driver->bodyRate = Vector2f(msg.rate_gyro_integral[0], msg.rate_gyro_integral[1]); _driver->rate_gyro_integral = Vector2f(msg.rate_gyro_integral[0], msg.rate_gyro_integral[1]);
_driver->integral_time = msg.integration_interval; _driver->integral_time = msg.integration_interval;
_driver->surface_quality = msg.quality; _driver->surface_quality = msg.quality;
} }
@ -79,9 +79,11 @@ void AP_OpticalFlow_HereFlow::_push_state(void)
float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y; float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
float integralToRate = 1.0f / integral_time; float integralToRate = 1.0f / integral_time;
//Convert to Raw Flow measurement to Flow Rate measurement //Convert to Raw Flow measurement to Flow Rate measurement
state.flowRate = Vector2f(flowRate.x * flowScaleFactorX, state.flowRate = Vector2f{
flowRate.y * flowScaleFactorY) * integralToRate; flow_integral.x * flowScaleFactorX,
state.bodyRate = bodyRate * integralToRate; flow_integral.y * flowScaleFactorY
} * integralToRate;
state.bodyRate = rate_gyro_integral * integralToRate;
state.surface_quality = surface_quality; state.surface_quality = surface_quality;
_applyYaw(state.flowRate); _applyYaw(state.flowRate);
_applyYaw(state.bodyRate); _applyYaw(state.bodyRate);

2
libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h
View File

@ -21,7 +21,7 @@ public:
private: private:
Vector2f flowRate, bodyRate; Vector2f flow_integral, rate_gyro_integral;
uint8_t surface_quality; uint8_t surface_quality;
float integral_time; float integral_time;
bool new_data; bool new_data;