AP_OpticalFlow: Add parameter to compensate for flow sensor yaw angle

AP_OpticalFlow: update parameter name

libraries/AP_OpticalFlow/AP_OpticalFlow_PX4.cpp

@@ -69,14 +69,18 @@ void AP_OpticalFlow_PX4::update(void)
         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);
-            state.flowRate.x = flowScaleFactorX * integralToRate * float(report.pixel_flow_x_integral); // rad/sec measured optically about the X sensor axis
-            state.flowRate.y = flowScaleFactorY * integralToRate * float(report.pixel_flow_y_integral); // rad/sec measured optically about the Y sensor axis
-            state.bodyRate.x = integralToRate * float(report.gyro_x_rate_integral); // rad/sec measured inertially about the X sensor axis
-            state.bodyRate.y = integralToRate * float(report.gyro_y_rate_integral); // rad/sec measured inertially about the Y sensor axis
+            // 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();

libraries/AP_OpticalFlow/OpticalFlow.cpp

@@ -29,6 +29,14 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] PROGMEM = {
     // @User: Standard
     AP_GROUPINFO("_FYSCALER", 2,  OpticalFlow,    _flowScalerY,   0),
 
+    // @Param: ORIENT_YAW
+    // @DisplayName: Flow sensor yaw alignment
+    // @Description: Specifies the number of centi-degrees that the flow sensor is yawed relative to the vehicle. A sensor with its X-axis pointing to the right of the vehicle X axis has a positive yaw angle.
+    // @Range: -18000 +18000
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("_ORIENT_YAW", 3,  OpticalFlow,    _yawAngle_cd,   0),
+
     AP_GROUPEND
 };
 

libraries/AP_OpticalFlow/OpticalFlow.h

@@ -84,6 +84,7 @@ private:
     AP_Int8  _enabled;              // enabled/disabled flag
     AP_Int16 _flowScalerX;          // X axis flow scale factor correction - parts per thousand
     AP_Int16 _flowScalerY;          // Y axis flow scale factor correction - parts per thousand
+    AP_Int16 _yawAngle_cd;          // yaw angle of sensor X axis with respect to vehicle X axis - centi degrees
 
 
     // state filled in by backend

libraries/AP_OpticalFlow/OpticalFlow_backend.h

@@ -44,6 +44,9 @@ protected:
 
     // get the flow scaling parameters
     Vector2f _flowScaler(void) const { return Vector2f(frontend._flowScalerX, frontend._flowScalerY); }
+
+    // get the yaw angle in radians
+    float _yawAngleRad(void) const { return radians(float(frontend._yawAngle_cd) * 0.01f); }
 };
 
 #endif // __OpticalFlow_backend_H__