Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_OpticalFlow: fix for msp opflow sensor alignement

Browse Source
This commit is contained in:
yaapu 2021-01-04 21:58:37 +01:00 committed by Randy Mackay
parent c5ec0cfdee
commit 480c9f10df
1 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_OpticalFlow/AP_OpticalFlow_MSP.cpp
View File

@ -63,15 +63,18 @@ void AP_OpticalFlow_MSP::update(void)
const float flow_scale_factor_x = 1.0f + 0.001f * _flowScaler().x; const float flow_scale_factor_x = 1.0f + 0.001f * _flowScaler().x;
const float flow_scale_factor_y = 1.0f + 0.001f * _flowScaler().y; const float flow_scale_factor_y = 1.0f + 0.001f * _flowScaler().y;
// copy flow rates to state structure and invert flow vector (needed for MSP flow message) // copy flow rates to state structure
state.flowRate = { ((float)flow_sum.x / count) * flow_scale_factor_x * dt * -1, state.flowRate = { ((float)flow_sum.x / count) * flow_scale_factor_x * dt,
((float)flow_sum.y / count) * flow_scale_factor_y * dt * -1 }; ((float)flow_sum.y / count) * flow_scale_factor_y * dt};
// copy average body rate to state structure // copy average body rate to state structure
state.bodyRate = { gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count }; state.bodyRate = { gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count };
// invert flow vector to align it with default sensor orientation (required on matek 3901)
state.flowRate *= -1;
// we only apply yaw to flowRate as body rate comes from AHRS
_applyYaw(state.flowRate); _applyYaw(state.flowRate);
_applyYaw(state.bodyRate);
} else { } else {
// first frame received in some time so cannot calculate flow values // first frame received in some time so cannot calculate flow values
state.flowRate.zero(); state.flowRate.zero();
@ -105,4 +108,3 @@ void AP_OpticalFlow_MSP::handle_msp(const MSP::msp_opflow_data_message_t &pkt)
} }
#endif // HAL_MSP_OPTICALFLOW_ENABLED #endif // HAL_MSP_OPTICALFLOW_ENABLED