AP_OpticalFlow: Adding support for OPTICAL_FLOW_RAD MAVLink message #31601

redn1nja · 2025-11-26T16:24:16Z

This PR adds support for OPTICAL_FLOW_RAD MAVLink message according to #31588 into the AP_OpticalFlow_MAV driver

peterbarker

CI must pass.

We also need an autotest for this - unfortunately we don't have a good example to swipe stuff from, but you would start with def OpticalFlow. Factor out the sanity checking function. Have a loop which sends in optical flow data corresponding to the vehicle's simulated velocity. Do the test at a speedup of 1. Let me know if this is a hill you won't be able to climb yet and I'll write something to get you going.

peterbarker · 2025-11-26T20:38:36Z

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

+        backend->handle_msg_rad(msg);
+    }
+}
+#endif


Suggested change

#endif

#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED

peterbarker · 2025-11-26T20:38:58Z

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp


+#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+
+void AP_OpticalFlow::handle_msg_rad(const mavlink_message_t &msg)


Suggested change

void AP_OpticalFlow::handle_msg_rad(const mavlink_message_t &msg)

void AP_OpticalFlow::handle_msg_optical_flow_rad(const mavlink_message_t &msg)

peterbarker · 2025-11-26T20:39:20Z

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

+        return;
+    }
+
+    if (backend != nullptr) {


Suggested change

if (backend != nullptr) {

if (backend == nullptr) {

return;

}

peterbarker · 2025-11-26T20:39:46Z

libraries/AP_OpticalFlow/AP_OpticalFlow.cpp

+
+    if (backend != nullptr) {
+        backend->handle_msg_rad(msg);
+    }


Suggested change

}

(and fix indenting)

peterbarker · 2025-11-26T20:40:13Z

libraries/AP_OpticalFlow/AP_OpticalFlow_Backend.h

+#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


Suggested change

#endif

#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED

peterbarker · 2025-11-26T20:44:43Z

libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp

+
+

Suggested change

peterbarker · 2025-11-26T20:46:50Z

libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp

+	// 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();


I don't think the comment is correct here.

mavlink is not guaranteed delivery; assume that you will lose 90% of your packets and see how well things work in that case.

peterbarker · 2025-11-26T20:48:54Z

libraries/AP_OpticalFlow/AP_OpticalFlow_MAV.cpp

+    // flow is already integrated so flow samples count will be equal to one
+    count = 1;
+    gyro_sum_count = 1;


We probably should aggregate received messages still - we're throwing away data otherwise.

However, this message is not dissimilar to the data handled on AP_OpticalFlow_HereFlow and it doesn't attempt to do that...

As I can observe from MAVLink message documentation, each message contains information that allows to obtain average flow, and for OPTICAL_FLOW it is used for aggregation and manual integration later.

AP_OpticalFlow_HereFlow has almost the same type of message with the only difference in timestamp format, so maybe similar approach can be used here as well.
Same fact applies to previous comment regarding guarantees of delivery: if most packets are lost but each packet is self-sufficient, can we update using only one packet?

I suppose the better approach here could be to separate the OPTICAL_FLOW state and OPTICAL_FLOW_RAD state inside the driver and decide which one is used for update based on some criteria (e.g. quality field from the messages). Then we can leave the state handling of OPTICAL_FLOW as it is now and implement state handling similar to AP_OpticalFlow_HereFlow for OPTICAL_FLOW_RAD

As I can observe from MAVLink message documentation, each message contains information that allows to obtain average flow, and for OPTICAL_FLOW it is used for aggregation and manual integration later.

AP_OpticalFlow_HereFlow has almost the same type of message with the only difference in timestamp format, so maybe similar approach can be used here as well. Same fact applies to previous comment regarding guarantees of delivery: if most packets are lost but each packet is self-sufficient, can we update using only one packet?

I suppose the better approach here could be to separate the OPTICAL_FLOW state and OPTICAL_FLOW_RAD state inside the driver and decide which one is used for update based on some criteria (e.g. quality field from the messages). Then we can leave the state handling of OPTICAL_FLOW as it is now and implement state handling similar to AP_OpticalFlow_HereFlow for OPTICAL_FLOW_RAD

Given how different any aggregation is likely to be, separate states probably does make sense.

So from this message you now have a bunch of messages which you need to aggregate so you can present a flow rate to the system. Each message gives you data for an interval. We need to get an aggregate for the entire measurement interval since last we presented data to the system. That would appear to be finding flow rates for every message we receive and then stretching each of those messages so that we cover the entire measurement interval. And then summing and dividing by the message interval to get an aggregate rate.

That sounds a little difficult. We don't really need to do better than what we do with our existing message, so maybe just take the average of all of the samples we've received and don't bother weighting them as I just described... some support is better than no support, we can just add comments saying we can do better.

peterbarker · 2025-11-26T20:51:18Z

libraries/GCS_MAVLink/ap_message.h

+#endif
+#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+    MSG_OPTICAL_FLOW_RAD               = 101,
 #endif


Suggested change

#endif

#endif // AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED

peterbarker · 2025-11-26T20:52:14Z

libraries/GCS_MAVLink/GCS_config.h

 #endif

+#ifndef AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED
+#define AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED 1


Suggested change

#define AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED 1

#define AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED (AP_OPTICALFLOW_ENABLED && HAL_PROGRAM_SIZE_LIMIT_KB > 2048)

…W_RAD and added support for handling both messages at a time

…AD_ENABLED by default

…FLOW_ENABLE

peterbarker requested changes Nov 26, 2025

View reviewed changes

redn1nja added 4 commits December 1, 2025 16:26

AP_OpticalFlow: Added support for OPTICAL_FLOW_RAD MAVLink message

9a1c8e1

GCS_MAVLink: Added support for OPTICAL_FLOW_RAD MAVLink message

1ec38a9

Tools: Added build parameters to use OPTICAL_FLOW_RAD

09730b4

AP_OpticalFlow: separated states between OPTICAL_FLOW and OPTICAL_FLO…

a8d8cd0

…W_RAD and added support for handling both messages at a time

redn1nja force-pushed the pr/optical_flow_rad_support branch from bafda30 to a8d8cd0 Compare December 1, 2025 14:28

redn1nja added 3 commits December 1, 2025 16:32

GCS_config: fixed condition on enabling AP_MAVLINK_MSG_OPTICAL_FLOW_R…

9a702f6

…AD_ENABLED by default

AP_OpticalFlow_MAV: replace ifdef with if

bd62fce

GSC_MAVLink: fixing build issues on board with not defined AP_OPTICAL…

fee9c58

…FLOW_ENABLE


		#if AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED

		void AP_OpticalFlow::handle_msg_rad(const mavlink_message_t &msg)

	void AP_OpticalFlow::handle_msg_rad(const mavlink_message_t &msg)
	void AP_OpticalFlow::handle_msg_optical_flow_rad(const mavlink_message_t &msg)

	#define AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED 1
	#define AP_MAVLINK_MSG_OPTICAL_FLOW_RAD_ENABLED (AP_OPTICALFLOW_ENABLED && HAL_PROGRAM_SIZE_LIMIT_KB > 2048)

AP_OpticalFlow: Adding support for OPTICAL_FLOW_RAD MAVLink message #31601

Are you sure you want to change the base?

AP_OpticalFlow: Adding support for OPTICAL_FLOW_RAD MAVLink message #31601

Conversation

redn1nja commented Nov 26, 2025

Uh oh!

peterbarker left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants