fix in flow yaw compensation

src/modules/position_estimator_inav/position_estimator_inav_main.cpp

@@ -621,20 +621,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 
 					/*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/
-					yaw_comp[0] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
-					yaw_comp[1] =  - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
+					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) */
 					float flow_ang[2];
 					//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 + gyro_offset_filtered[0] - yaw_comp[0];//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
-					flow_ang[1] = (flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1] - yaw_comp[1];//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
+					flow_ang[0] = (flow.pixel_flow_x_integral - flow.gyro_x_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[0];//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
+					flow_ang[1] = (flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1];//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
 					/* flow measurements vector */
 					
 					float flow_m[3];
-					flow_m[0] = -flow_ang[0] * flow_dist;
-					flow_m[1] = -flow_ang[1] * flow_dist;
+					flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0];
+					flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1];
 					flow_m[2] = z_est[1];
 					
 					/* velocity in NED */