AP_OpticalFlow: integrate calibrator

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

@@ -13,6 +13,7 @@
 #include "AP_OpticalFlow_MSP.h"
 #include "AP_OpticalFlow_UPFLOW.h"
 #include <AP_Logger/AP_Logger.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -182,6 +183,21 @@ void OpticalFlow::update(void)
 
     // only healthy if the data is less than 0.5s old
     _flags.healthy = (AP_HAL::millis() - _last_update_ms < 500);
+
+    // update calibrator and save resulting scaling
+    if (_calibrator != nullptr) {
+        if (_calibrator->update()) {
+            // apply new calibration values
+            const Vector2f new_scaling = _calibrator->get_scalars();
+            const float flow_scalerx_as_multiplier = (1.0 + (_flowScalerX / 1000.0)) * new_scaling.x;
+            const float flow_scalery_as_multiplier = (1.0 + (_flowScalerY / 1000.0)) * new_scaling.y;
+            _flowScalerX.set_and_save_ifchanged((flow_scalerx_as_multiplier - 1.0) * 1000.0);
+            _flowScalerY.set_and_save_ifchanged((flow_scalery_as_multiplier - 1.0) * 1000.0);
+            _flowScalerX.notify();
+            _flowScalerY.notify();
+            GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FlowCal: FLOW_FXSCALER=%d, FLOW_FYSCALER=%d", (int)_flowScalerX, (int)_flowScalerY);
+        }
+    }
 }
 
 void OpticalFlow::handle_msg(const mavlink_message_t &msg)
@@ -210,6 +226,29 @@ void OpticalFlow::handle_msp(const MSP::msp_opflow_data_message_t &pkt)
 }
 #endif //HAL_MSP_OPTICALFLOW_ENABLED
 
+// start calibration
+void OpticalFlow::start_calibration()
+{
+    if (_calibrator == nullptr) {
+        _calibrator = new AP_OpticalFlow_Calibrator();
+        if (_calibrator == nullptr) {
+            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "FlowCal: failed to start");
+            return;
+        }
+    }
+    if (_calibrator != nullptr) {
+        _calibrator->start();
+    }
+}
+
+// stop calibration
+void OpticalFlow::stop_calibration()
+{
+    if (_calibrator != nullptr) {
+        _calibrator->stop();
+    }
+}
+
 void OpticalFlow::update_state(const OpticalFlow_state &state)
 {
     _state = state;

libraries/AP_OpticalFlow/AP_OpticalFlow.h

@@ -34,7 +34,7 @@
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
-
+#include "AP_OpticalFlow_Calibrator.h"
 
 class OpticalFlow_backend;
 
@@ -110,6 +110,10 @@ public:
         return _pos_offset;
     }
 
+    // start or stop calibration
+    void start_calibration();
+    void stop_calibration();
+
     // parameter var info table
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -142,6 +146,9 @@ private:
     void Log_Write_Optflow();
     uint32_t _log_bit = -1;     // bitmask bit which indicates if we should log.  -1 means we always log
 
+    // calibrator
+    AP_OpticalFlow_Calibrator *_calibrator;
+
 };
 
 namespace AP {