AP_OpticalFlow: Adding support for OPTICAL_FLOW_RAD MAVLink message

Tools/scripts/build_options.py

@@ -425,6 +425,7 @@ def config_option(self):
     Feature('MAVLink', 'MAV_DEVICE_OP', 'AP_MAVLINK_MSG_DEVICE_OP_ENABLED', 'Enable DeviceOp MAVLink messages', 0, None),  # noqa
     Feature('MAVLink', 'MAV_SERVO_RELAY', 'AP_MAVLINK_SERVO_RELAY_ENABLED', 'Enable ServoRelay MAVLink messages', 0, 'SERVORELAY_EVENTS'),  # noqa
     Feature('MAVLink', 'MAV_MSG_SERIAL_CONTROL', 'AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED', 'Enable Serial Control MAVLink messages', 0, None),  # noqa
+    Feature('MAVLink', 'MAV_MSG_OPTICAL_FLOW_RAD', 'AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED', 'Enable Optical Flow Rad MAVLink message', 0, 'OPTICALFLOW'),  # noqa
     Feature('MAVLink', 'MAVLINK_MSG_MISSION_REQUEST', 'AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED', 'Enable MISSION_REQUEST MAVLink messages', 0, None),  # noqa
     Feature('MAVLink', 'MAVLINK_MSG_RC_CHANNELS_RAW', 'AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED', 'Enable RC_CHANNELS_RAW MAVLink messages', 0, None),  # noqa
     Feature('MAVLink', 'AP_MAVLINK_FTP_ENABLED', 'AP_MAVLINK_FTP_ENABLED', 'Enable MAVLink FTP protocol', 0, None),  # noqa

Tools/scripts/extract_features.py

@@ -258,6 +258,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
             ('AP_MAVLINK_MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES_ENABLED', 'GCS_MAVLINK::handle_command_request_autopilot_capabilities'),  # noqa
             ('AP_MAVLINK_MSG_RELAY_STATUS_ENABLED', 'GCS_MAVLINK::send_relay_status'),
             ('AP_MAVLINK_MSG_DEVICE_OP_ENABLED', 'GCS_MAVLINK::handle_device_op_write'),
+            ('AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED', 'GCS_MAVLINK::handle_optical_flow_rad'),
             ('AP_MAVLINK_SERVO_RELAY_ENABLED', 'GCS_MAVLINK::handle_servorelay_message'),
             ('AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED', 'GCS_MAVLINK::handle_serial_control'),
             ('AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED', r'GCS_MAVLINK::handle_mission_request\b'),

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

@@ -226,6 +226,21 @@ void AP_OpticalFlow::handle_msg(const mavlink_message_t &msg)
     }
 }
 
+#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+
+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_optical_flow_rad(msg);
+    }
+}
+#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

@@ -89,6 +89,11 @@ class AP_OpticalFlow
     // handle optical flow mavlink messages
     void handle_msg(const mavlink_message_t &msg);
 
+#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+    // handle optical flow rad mavlink message
+    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
     void handle_msp(const MSP::msp_opflow_data_message_t &pkt);

libraries/AP_OpticalFlow/AP_OpticalFlow_Backend.h

@@ -42,6 +42,11 @@ class OpticalFlow_backend
     // handle optical flow mavlink messages
     virtual void handle_msg(const mavlink_message_t &msg) {}
 
+#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+    // handle optical flow rad mavlink messages
+    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
     virtual void handle_msp(const MSP::msp_opflow_data_message_t &pkt) {}

libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp

@@ -35,19 +35,82 @@ void AP_OpticalFlow_MAV::update(void)
 {
     // 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++;
     }
 
-    // return without updating state if no readings
-    if (count == 0) {
-        return;
+    struct AP_OpticalFlow::OpticalFlow_state state{};
+    if (count != 0) {
+        // obtain data from OPTICAL_FLOW mavlink message
+        state = update_msg_optical_flow();
+
+        // reset local variables for OPTICAL_FLOW
+        flow_sum.zero();
+        quality_sum = 0;
+        count = 0;
+        gyro_sum.zero();
+        gyro_sum_count = 0;
+    }
+
+#if 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);
+
+}
+
+// handle OPTICAL_FLOW mavlink messages
+void AP_OpticalFlow_MAV::handle_msg(const mavlink_message_t &msg)
+{
+    mavlink_optical_flow_t packet;
+    mavlink_msg_optical_flow_decode(&msg, &packet);
 
-    struct AP_OpticalFlow::OpticalFlow_state state {};
+    // record time message was received
+    // ToDo: add jitter correction
+    latest_frame_us = AP_HAL::micros64();
+
+    // use flow_rate_x/y fields if non-zero values are ever provided
+    if (!flow_sum_is_rads && (!is_zero(packet.flow_rate_x) || !is_zero(packet.flow_rate_y))) {
+        flow_sum_is_rads = true;
+        flow_sum.zero();
+        quality_sum = 0;
+        count = 0;
+    }
+
+    // add sensor values to sum
+    if (flow_sum_is_rads) {
+        // higher precision flow_rate_x/y fields are used
+        flow_sum.x += packet.flow_rate_x;
+        flow_sum.y += packet.flow_rate_y;
+    } else {
+        // lower precision flow_x/y fields are used
+        flow_sum.x += packet.flow_x;
+        flow_sum.y += packet.flow_y;
+    }
+    quality_sum += packet.quality;
+    count++;
+
+    // take sensor id from message
+    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;
 
@@ -73,7 +136,7 @@ void AP_OpticalFlow_MAV::update(void)
             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 };   
+            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
@@ -84,51 +147,52 @@ void AP_OpticalFlow_MAV::update(void)
         state.bodyRate.zero();
     }
 
-    _update_frontend(state);
+    return state;
+}
+
+#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+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);
+
+    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;
 
-    // reset local buffers
-    flow_sum.zero();
-    quality_sum = 0;
-    count = 0;
+    count_rad++;
 
-    // reset gyro sum
-    gyro_sum.zero();
-    gyro_sum_count = 0;
 }
 
-// handle OPTICAL_FLOW mavlink messages
-void AP_OpticalFlow_MAV::handle_msg(const mavlink_message_t &msg)
-{
-    mavlink_optical_flow_t packet;
-    mavlink_msg_optical_flow_decode(&msg, &packet);
+AP_OpticalFlow::OpticalFlow_state AP_OpticalFlow_MAV::update_msg_optical_flow_rad() {
+    struct AP_OpticalFlow::OpticalFlow_state state;
+    const Vector2f flowScaler = _flowScaler();
 
-    // record time message was received
-    // ToDo: add jitter correction
-    latest_frame_us = AP_HAL::micros64();
+    float scale_factorx = 1.0f + 0.001f * flowScaler.x;
+    float scale_factory = 1.0f + 0.001f * flowScaler.y;
 
-    // use flow_rate_x/y fields if non-zero values are ever provided
-    if (!flow_sum_is_rads && (!is_zero(packet.flow_rate_x) || !is_zero(packet.flow_rate_y))) {
-        flow_sum_is_rads = true;
-        flow_sum.zero();
-        quality_sum = 0;
-        count = 0;
-    }
+    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;
 
-    // add sensor values to sum
-    if (flow_sum_is_rads) {
-        // higher precision flow_rate_x/y fields are used
-        flow_sum.x += packet.flow_rate_x;
-        flow_sum.y += packet.flow_rate_y;
-    } else {
-        // lower precision flow_x/y fields are used
-        flow_sum.x += packet.flow_x;
-        flow_sum.y += packet.flow_y;
-    }
-    quality_sum += packet.quality;
-    count++;
+    _applyYaw(state.flowRate);
+    _applyYaw(state.bodyRate);
+
+    return state;
 
-    // take sensor id from message
-    sensor_id = packet.sensor_id;
 }
 
+#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+
 #endif  // AP_OPTICALFLOW_MAV_ENABLED

libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.h

@@ -18,24 +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_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.h

@@ -717,6 +717,7 @@ class GCS_MAVLINK
 #endif  // AP_MAVLINKCAN_ENABLED
 
     void handle_optical_flow(const mavlink_message_t &msg);
+    void handle_optical_flow_rad(const mavlink_message_t &msg);
 
     void handle_manual_control(const mavlink_message_t &msg);
     void handle_radio_rc_channels(const mavlink_message_t &msg);

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -4193,6 +4193,16 @@ void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg)
 }
 #endif
 
+#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+void GCS_MAVLINK::handle_optical_flow_rad(const mavlink_message_t &msg)
+{
+    AP_OpticalFlow *optflow = AP::opticalflow();
+    if (optflow == nullptr) {
+        return;
+    }
+    optflow->handle_msg_optical_flow_rad(msg);
+}
+#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
 
 #if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED
 /*
@@ -4545,6 +4555,12 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg)
         break;
 #endif
 
+#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+    case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD:
+        handle_optical_flow_rad(msg);
+        break;
+#endif
+
     case MAVLINK_MSG_ID_DISTANCE_SENSOR:
         handle_distance_sensor(msg);
         break;

libraries/GCS_MAVLink/GCS_config.h

@@ -60,6 +60,10 @@
 #define AP_MAVLINK_MSG_WIND_ENABLED AP_AHRS_ENABLED
 #endif
 
+#ifndef AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+#define AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED (HAL_PROGRAM_SIZE_LIMIT_KB > 2048 && defined(AP_OPTICALFLOW_ENABLED) && AP_OPTICALFLOW_ENABLED)
+#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+
 // allow removal of developer-centric mavlink commands
 #ifndef AP_MAVLINK_FAILURE_CREATION_ENABLED
 #define AP_MAVLINK_FAILURE_CREATION_ENABLED 1

libraries/GCS_MAVLink/ap_message.h

@@ -114,6 +114,9 @@ enum ap_message : uint8_t {
     MSG_AVAILABLE_MODES_MONITOR        =  99,
 #if AP_MAVLINK_MSG_FLIGHT_INFORMATION_ENABLED
     MSG_FLIGHT_INFORMATION             = 100,
+#endif
+#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+    MSG_OPTICAL_FLOW_RAD               = 101,
 #endif
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };