AP_OpticalFlow: separated states between OPTICAL_FLOW and OPTICAL_FLOW_RAD and added support for handling both messages at a time

Author: redn1nja

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

@@ -228,18 +228,18 @@ void AP_OpticalFlow::handle_msg(const mavlink_message_t &msg)
 
 #if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
 
-void AP_OpticalFlow::handle_msg_rad(const mavlink_message_t &msg)
+void AP_OpticalFlow::handle_msg_optical_flow_rad(const mavlink_message_t &msg)
 {
     // exit immediately if not enabled
     if (!enabled()){
         return;
     }
 
     if (backend != nullptr) {
-        backend->handle_msg_rad(msg);
+        backend->handle_msg_optical_flow_rad(msg);
     }
 }
-#endif
+#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
 
 #if HAL_MSP_OPTICALFLOW_ENABLED
 void AP_OpticalFlow::handle_msp(const MSP::msp_opflow_data_message_t &pkt)

libraries/AP_OpticalFlow/AP_OpticalFlow.h

@@ -91,8 +91,8 @@ class AP_OpticalFlow
 
 #if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
     // handle optical flow rad mavlink message
-    void handle_msg_rad(const mavlink_message_t &msg);
-#endif
+    void handle_msg_optical_flow_rad(const mavlink_message_t &msg);
+#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
 
 #if HAL_MSP_OPTICALFLOW_ENABLED
     // handle optical flow msp messages

libraries/AP_OpticalFlow/AP_OpticalFlow_Backend.h

@@ -44,8 +44,8 @@ class OpticalFlow_backend
 
 #if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
     // handle optical flow rad mavlink messages
-    virtual void handle_msg_rad(const mavlink_message_t &msg) {}
-#endif
+    virtual void handle_msg_optical_flow_rad(const mavlink_message_t &msg) {}
+#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
 
 #if HAL_MSP_OPTICALFLOW_ENABLED
     // handle optical flow msp messages

libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp

@@ -33,70 +33,44 @@ AP_OpticalFlow_MAV *AP_OpticalFlow_MAV::detect(AP_OpticalFlow &_frontend)
 // read latest values from sensor and fill in x,y and totals.
 void AP_OpticalFlow_MAV::update(void)
 {
-    //gyroscope integration is not required for OPTICAL_FLOW_RAD
-#if !AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
     // record gyro values as long as they are being used
     // the sanity check of dt below ensures old gyro values are not used
-    if (gyro_sum_count < 1000) {
+    if (gyro_sum_count < 1000 ) {
         const Vector3f& gyro = AP::ahrs().get_gyro();
         gyro_sum.x += gyro.x;
         gyro_sum.y += gyro.y;
         gyro_sum_count++;
     }
-#endif
 
-    // return without updating state if no readings
-    if (count == 0) {
-        return;
-    }
-
-    struct AP_OpticalFlow::OpticalFlow_state state {};
-
-    state.surface_quality = quality_sum / count;
-
-    // calculate dt
-    const float dt = (latest_frame_us - prev_frame_us) * 1.0e-6;
-    prev_frame_us = latest_frame_us;
-
-    // sanity check dt
-    if (is_positive(dt) && (dt < OPTFLOW_MAV_TIMEOUT_SEC)) {
-        // calculate flow values
-        const float flow_scale_factor_x = 1.0f + 0.001f * _flowScaler().x;
-        const float flow_scale_factor_y = 1.0f + 0.001f * _flowScaler().y;
+    struct AP_OpticalFlow::OpticalFlow_state state{};
+    if (count != 0) {
+        // obtain data from OPTICAL_FLOW mavlink message
+        state = update_msg_optical_flow();
 
-        // scale and copy flow rates to state structure
-        // if using flow_rates these are in rad/s (as opposed to pixels) and do not need to be multiplied by dt
-        const float dt_used = flow_sum_is_rads ? 1.0f : dt;
-        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 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 };   
-        }
+        // reset local variables for OPTICAL_FLOW
+        flow_sum.zero();
+        quality_sum = 0;
+        count = 0;
+        gyro_sum.zero();
+        gyro_sum_count = 0;
+    }
 
-        // we only apply yaw to flowRate as body rate comes from AHRS
-        _applyYaw(state.flowRate);
-    } else {
-        // first frame received in some time so cannot calculate flow values
-        state.flowRate.zero();
-        state.bodyRate.zero();
+#ifdef AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+    if (count_rad != 0 && (quality_rad_sum / count_rad) > state.surface_quality) {
+        // state quality is better from OPTICAL_FLOW_RAD, so we can take it instead
+        state = update_msg_optical_flow_rad();
+
+        // reset local variables for OPTICAL_FLOW_RAD
+        flow_rad_integral_sum.zero();
+        rate_gyro_rad_integral_sum.zero();
+        count_rad = 0;
+        quality_rad_sum = 0;
+        integration_time_sum_rad_us = 0;
     }
+#endif //AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
 
     _update_frontend(state);
 
-    // reset local buffers
-    flow_sum.zero();
-    quality_sum = 0;
-    count = 0;
-
-    // reset gyro sum
-    gyro_sum.zero();
-    gyro_sum_count = 0;
 }
 
 // handle OPTICAL_FLOW mavlink messages
@@ -134,33 +108,91 @@ void AP_OpticalFlow_MAV::handle_msg(const mavlink_message_t &msg)
     sensor_id = packet.sensor_id;
 }
 
+AP_OpticalFlow::OpticalFlow_state AP_OpticalFlow_MAV::update_msg_optical_flow()
+{
+    struct AP_OpticalFlow::OpticalFlow_state state;
+
+    state.surface_quality = quality_sum / count;
+
+    // calculate dt
+    const float dt = (latest_frame_us - prev_frame_us) * 1.0e-6;
+    prev_frame_us = latest_frame_us;
+
+    // sanity check dt
+    if (is_positive(dt) && (dt < OPTFLOW_MAV_TIMEOUT_SEC)) {
+        // calculate flow values
+        const float flow_scale_factor_x = 1.0f + 0.001f * _flowScaler().x;
+        const float flow_scale_factor_y = 1.0f + 0.001f * _flowScaler().y;
+
+        // scale and copy flow rates to state structure
+        // if using flow_rates these are in rad/s (as opposed to pixels) and do not need to be multiplied by dt
+        const float dt_used = flow_sum_is_rads ? 1.0f : dt;
+        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 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);
+    } else {
+        // first frame received in some time so cannot calculate flow values
+        state.flowRate.zero();
+        state.bodyRate.zero();
+    }
+
+    return state;
+}
+
 #if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
-void AP_OpticalFlow_MAV::handle_msg_rad(const mavlink_message_t &msg)
+void AP_OpticalFlow_MAV::handle_msg_optical_flow_rad(const mavlink_message_t &msg)
 {
     mavlink_optical_flow_rad_t packet;
     mavlink_msg_optical_flow_rad_decode(&msg, &packet);
 
-	// record time message was received
-    // TODO: dt between frames which is calculated in update state function
-    // is already stored in OPTICAL_FLOW_RAD message and can be taken from there
-    // instead of calculating it manually
-    latest_frame_us = AP_HAL::micros64();
+    flow_rad_integral_sum.x += packet.integrated_x;
+    flow_rad_integral_sum.y += packet.integrated_y;
+
+    rate_gyro_rad_integral_sum.x += packet.integrated_xgyro;
+    rate_gyro_rad_integral_sum.y += packet.integrated_ygyro;
+
+    quality_rad_sum += packet.quality;
+    sensor_id_rad = packet.sensor_id;
+
+    integration_time_sum_rad_us += packet.integration_time_us;
 
-    flow_sum_is_rads = true;
+    count_rad++;
 
-	// OPTICAL_FLOW_RAD contains already calculated integrated values
-	// so it is not required to integrate them in msg handling
-    quality_sum = packet.quality;
-	flow_sum = Vector2f(packet.integrated_x, packet.integrated_y);
-	gyro_sum = Vector2f(packet.integrated_xgyro, packet.integrated_ygyro);
-	sensor_id = packet.sensor_id;
+}
+
+AP_OpticalFlow::OpticalFlow_state AP_OpticalFlow_MAV::update_msg_optical_flow_rad() {
+    struct AP_OpticalFlow::OpticalFlow_state state;
+    const Vector2f flowScaler = _flowScaler();
+
+    float scale_factorx = 1.0f + 0.001f * flowScaler.x;
+    float scale_factory = 1.0f + 0.001f * flowScaler.y;
 
-    // flow is already integrated so flow samples count will be equal to one
-    count = 1;
-    gyro_sum_count = 1;
+    float integral_to_rate = 1.0f / (integration_time_sum_rad_us * 1e-6) / count_rad;
+    state.flowRate = Vector2f{
+        flow_rad_integral_sum.x * scale_factorx,
+        flow_rad_integral_sum.y * scale_factory,
+        } / count_rad * integral_to_rate;
+    state.bodyRate = rate_gyro_rad_integral_sum / count_rad * integral_to_rate;
+    state.surface_quality = quality_rad_sum / count_rad;
 
+    _applyYaw(state.flowRate);
+    _applyYaw(state.bodyRate);
+
+    return state;
 
 }
-#endif
+
+#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
 
 #endif  // AP_OPTICALFLOW_MAV_ENABLED

libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.h

@@ -18,28 +18,47 @@ class AP_OpticalFlow_MAV : public OpticalFlow_backend
 
     // read latest values from sensor and fill in x,y and totals.
     void update(void) override;
+    AP_OpticalFlow::OpticalFlow_state update_msg_optical_flow();
 
     // get update from mavlink
     void handle_msg(const mavlink_message_t &msg) override;
 
 #if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
-    void handle_msg_rad(const mavlink_message_t &msg) override;
-#endif
+    void handle_msg_optical_flow_rad(const mavlink_message_t &msg) override;
+    AP_OpticalFlow::OpticalFlow_state update_msg_optical_flow_rad();
+#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
 
     // detect if the sensor is available
     static AP_OpticalFlow_MAV *detect(AP_OpticalFlow &_frontend);
 
 private:
 
+    // OPTICAL_FLOW state variables
     uint64_t prev_frame_us;             // system time of last message when update was last called
     uint64_t latest_frame_us;           // system time of most recent messages processed
     Vector2f flow_sum;                  // sum of sensor's flow_x and flow_y values since last call to update
     bool flow_sum_is_rads;              // true if flow_sum holds values in rad/s, if false flow_sum holds dpi
     uint16_t quality_sum;               // sum of sensor's quality values since last call to update
     uint16_t count;                     // number of sensor readings since last call to update
-    uint8_t sensor_id;                  // sensor_id received in latest mavlink message
+    uint8_t sensor_id;                  // sensor_id received in latest OPTICAL_FLOW message
     Vector2f gyro_sum;                  // sum of gyro sensor values since last frame from flow sensor
     uint16_t gyro_sum_count;            // number of gyro sensor values in sum
+
+
+
+#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+
+    // OPTICAL_FLOW_RAD state variables
+    Vector2f flow_rad_integral_sum;       // sum of all integrated_x and integrated_y values since last call to update
+    Vector2f rate_gyro_rad_integral_sum;  // sum of all integrated_xgyro/integrated_ygyro values since last update
+    uint32_t integration_time_sum_rad_us; // sum of all integratation_time_us values used to obtain average flow
+    uint16_t quality_rad_sum;             // sum of sensor quality values since last call to update
+    uint16_t count_rad;                   // number of sensor readings since last call to update
+    uint8_t sensor_id_rad;                // sensor_id received in last OPTICAL_FLOW_RAD message
+
+
+
+#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
 };
 
 #endif  // AP_OPTICALFLOW_MAV_ENABLED

libraries/AP_OpticalFlow/AP_OpticalFlow_config.h

@@ -51,3 +51,4 @@
 #ifndef AP_OPTICALFLOW_UPFLOW_ENABLED
 #define AP_OPTICALFLOW_UPFLOW_ENABLED AP_OPTICALFLOW_BACKEND_DEFAULT_ENABLED
 #endif
+

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -4200,7 +4200,7 @@ void GCS_MAVLINK::handle_optical_flow_rad(const mavlink_message_t &msg)
     if (optflow == nullptr) {
         return;
     }
-    optflow->handle_msg_rad(msg);
+    optflow->handle_msg_optical_flow_rad(msg);
 }
 #endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
 

libraries/GCS_MAVLink/GCS_config.h

@@ -61,8 +61,8 @@
 #endif
 
 #ifndef AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
-#define AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED 1
-#endif
+#define AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED (AP_OPTICALFLOW_ENABLED && HAL_PROGRAM_SIZE_LIMIT_KB >= 2048)
+#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
 
 // allow removal of developer-centric mavlink commands
 #ifndef AP_MAVLINK_FAILURE_CREATION_ENABLED