AP_OpticalFlow: handle stabilised sensors

Author: rmackay9

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

@@ -98,6 +98,13 @@ const AP_Param::GroupInfo AP_OpticalFlow::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO_FRAME("_HGT_OVR", 6,  AP_OpticalFlow, _height_override,   0.0f, AP_PARAM_FRAME_ROVER),
 
+    // @Param: _OPTIONS
+    // @DisplayName: Optical flow options
+    // @Description: Optical flow options. Bit 0 should be set if the sensor is stabilised (e.g. mounted on a gimbal)
+    // @Bitmask: 0:Roll/Pitch stabilised
+    // @User: Standard
+    AP_GROUPINFO("_OPTIONS", 7,  AP_OpticalFlow, _options,   0),
+
     AP_GROUPEND
 };
 

libraries/AP_OpticalFlow/AP_OpticalFlow.h

@@ -145,6 +145,7 @@ class AP_OpticalFlow
     AP_Vector3f _pos_offset;        // position offset of the flow sensor in the body frame
     AP_Int8  _address;              // address on the bus (allows selecting between 8 possible I2C addresses for px4flow)
     AP_Float  _height_override;              // height of the sensor above the ground. Only used in rover
+    AP_Int16 _options;              // options parameter
 
     // method called by backend to update frontend state:
     void update_state(const OpticalFlow_state &state);

libraries/AP_OpticalFlow/AP_OpticalFlow_Backend.h

@@ -65,7 +65,17 @@ class OpticalFlow_backend
 
     // get ADDR parameter value
     uint8_t get_address(void) const { return frontend._address; }
-    
+
+    // options parameter values
+    enum class Option : uint16_t {
+        Stabilised = (1 << 0U)      // sensor is stabilised (e.g. mounted on a gimbal)
+    };
+
+    // returns true if an option is enabled
+    bool option_is_enabled(Option option) const {
+        return ((uint8_t)frontend._options.get() & (uint16_t)option) != 0;
+    }
+
     // semaphore for access to shared frontend data
     HAL_Semaphore _sem;
 };

libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp

@@ -67,8 +67,14 @@ void AP_OpticalFlow_MAV::update(void)
         state.flowRate = { ((float)flow_sum.x / count) * flow_scale_factor_x * dt_used,
                            ((float)flow_sum.y / count) * flow_scale_factor_y * dt_used };
 
-        // copy average body rate to state structure
-        state.bodyRate = { gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count };
+        // copy body-rates from gyro or set to zero
+        if (option_is_enabled(Option::Stabilised)) {
+            // if the sensor is stabilised then body rates are always zero
+            state.bodyRate.zero();
+        } else {
+            // copy average body rate to state structure
+            state.bodyRate = { gyro_sum.x / gyro_sum_count, gyro_sum.y / gyro_sum_count };   
+        }
 
         // we only apply yaw to flowRate as body rate comes from AHRS
         _applyYaw(state.flowRate);