dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

position_estimator_inav: Make optical flow data conversions consistent with uORB interface

This commit is contained in:
Paul Riseborough 2016-12-29 20:18:14 +11:00 committed by Lorenz Meier
parent 92f5211f55
commit eca2aeccf9
1 changed files with 12 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
View File

@ -649,7 +649,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
yaw_comp[0] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
yaw_comp[1] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
/* convert raw flow to angular flow (rad/s) */
/*
* Convert raw flow from the optical_flow uORB topic (rad) to angular flow (rad/s)
* Note that the optical_flow uORB topic defines positive delta angles as produced by RH rotations
* around the correspdonding body axes.
*/
float flow_ang[2];
/* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */
@ -661,25 +666,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float rate_threshold = 0.15f;
/* calculate the angular flow rate produced by a negative velocity along the X body axis */
if (fabsf(rates_setpoint.pitch) < rate_threshold) {
//warnx("[inav] test ohne comp");
flow_ang[0] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) *
flow_ang[0] = (-flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) *
params.flow_k;//for now the flow has to be scaled (to small)
} else {
//warnx("[inav] test mit comp");
//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f
flow_ang[0] = (-(flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f
+ gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small)
}
/* calculate the angular flow rate produced by a negative velocity along the Y body axis */
if (fabsf(rates_setpoint.roll) < rate_threshold) {
flow_ang[1] = (flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) *
flow_ang[1] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) *
params.flow_k;//for now the flow has to be scaled (to small)
} else {
//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f
flow_ang[1] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f
+ gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small)
}