Display OBSTACLE_DISTANCE in QGC

[mrivi]

QGC issue mavlink/qgroundcontrol#7670

[LorenzMeier]

[LorenzMeier]

@dogmaphobic The message for this is:
https://mavlink.io/en/messages/common.html#OBSTACLE_DISTANCE

What I'm looking for is a baseline implementation that we can then tune interactively with the Autonomy team and Stefan.

[dogmaphobic]

What exactly is angle_offset?

This:

Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive values are offsets to the right.

Doesn't explain much.

Suppose I have 3 sensors. One pointing straight ahead, one 15° to the right and the other 15° to the left (345°). increment or increment_f would have a value of 15 and I would expect 3 values in distances to be used/valid. When I look at distances, where is "center"? I assume this is what angle_offset is telling me?

Also, the array length is even, making the left/right values asymmetric.

[mhkabir]

It's in the local frame, not the vehicle frame. We should probably make that more clear in the message definition.

[dogmaphobic]

Ok, how do I find "straight ahead" from this?

index 0 corresponding to local forward + angle_offset.

And how would the above, 3-sensor example be represented in the array?

[hamishwillee]

It's in the local frame, not the vehicle frame. We should probably make that more clear in the message definition.

@mhkabir Really? I would definitely interpret docs local forward and "in front of the sensor " as vehicle relative. Clearly quite a lot of docs improvements needed here.

This is how I interpret it according to what is written:

1. `increment_f=0 - original definition.

   • increment_f and angle offset are ignored.
   • distances is array of items with angle increment between each item.
   • Items are assisnged from left to right, where centre is assumed to be the front of the vehicle. So presumably the centre would map at around item 36 in the array.

2. increment_f>0

   • increment is ignored.
   • distances is array with elements starting at angle_offset, where the 0 is forward of the vehicle and increment_f being angle between the elements. So with small increment_f you might have a number of messages providing the full obstacle map, using a different angle_offset

Suppose I have 3 sensors. One pointing straight ahead, one 15° to the right and the other 15° to the left (345°). increment or increment_f would have a value of 15 and I would expect 3 values in distances to be used/valid. When I look at distances, where is "center"? I assume this is what angle_offset is telling me?

I don't know if I'm "right" but my understanding, but

Case 1

• increment = 15, increment_f=0, distance = array with values 35, 36, 37 set with obstacle distances. All other values set with max.
• Or you might "fill in" more values at different increments - but values still centred around 35 or 35.

Case 2

• increment = 0, increment_f=15, angle_offset=345, just fill in first three values.
• or send multiple messages, or whatever.

I guess we need to know how it has been implemented to progress.

[hamishwillee]

Just for completeness, since @dogmaphobic found my description useless. Note in both cases I assume that the frame is the vehicle heading = 0 degrees. NOT North = 0degrees.

Case 1 - old style message where the points are evenly spread relative to front of vehicle. Not very flexible.

Case 2. new message. Distance array is for a chunk of data at starting at some angle relative to the 0 degree position (front of vehicle) with fixed increment spacings. May take multiple messages to cover the whole range, and this allows different messages to have different spacing of sensors.

Caveat, I guess either all messages for sensor have to be sent in a burst so they are all relative to the same heading. Probably should add something to allow better sync.

[mrivi]

@dogmaphobic - The table sums up the possible states of the OA system. My idea is to show the user a single icon that captures both if the OA is enabled and healthy.

PX4 Mode	IsEnabled	SystemStatus	UI Status
Pos Control	CP_DIST > 0	OBSTACLE_DISTANCE received by QGC	Healthy
Pos Control	CP_DIST > 0	OBSTACLE_DISTANCE not received by QGC	Not Healthy
Pos Control	CP_DIST == -1	-	Not Healthy
Mission or Auto Land or Auto Takeoff or RTL	COM_OBS_AVOID == 1	HEARTBEAT, system_status == MAV_STATE_ACTIVE	Healthy
Mission or Auto Land or Auto Takeoff or RTL	COM_OBS_AVOID == 1	HEARTBEAT, system_status == MAV_STATE_FLIGHT_TERMINATION	Not Healthy
Mission or Auto Land or Auto Takeoff or RTL	COM_OBS_AVOID == 0	-	Not Healthy
Any other PX4 Mode	-	-	Not Healthy

We also would need a switch in QGC to enable OA that basically sets COM_OBS_AVOID to 1 and
CP_DIST to 5 to enable it and COM_OBS_AVOID to 0 and CP_DIST -1 to disabled it

[magicrub]

I think you guys are making this more complicated than it is. In the case of a forward facing camera with a 90deg wide view (45 left, 45 right) a message would look like:
angle_offset = (MiddleAngle-half) = 0 - (90/2) = -45
increment_f = (90/72) = 1.25

If the camera is facing directly backwards
angle_offset = (MiddleAngle-half) = 180 - (90/2) = 135

I found a Lidar simulator that likes to deliver it's 360 data in CCW order. I'd also like to note that the left-to-right increment is not necessarily a hard requirement. That's just the positive increment direction. On ArduPilot a negative increment means left (CCW).
So, this camera data would read in the distance values, index zero first on the right:
angle_offset = +45
increment_f = -1.25

[dogmaphobic]

In the case of a forward facing camera with a 90deg wide view

What if you have 4 of those pointing in all 4 directions? Will it send 4 messages, one for each or will it consolidate all into one message?

[dogmaphobic]

My current implementation assumes one single message. A subsequent message would override the previous one (the previous distance slots). There is no way to identify which sensor a message is coming from.

[magicrub]

What implementation? In the case of Ardupilot, you can receive multiple messages and they "add" internally. No need to identify them other than angle and increment

[dogmaphobic]

you can receive multiple messages and they "add" internally

Add to where? What GCS does this? How do you keep track of angle slots given that you have unpredictable (floating point) increment values that can be different from sensor to sensor? I guess I could keep an array and arbitrarily set its length to 360 and "fit" the distances within those slots. That would work. But I still don't know how to handle a case of a sensor going bad and stop sending data. I would not be able to detect that. Any distance set on its set of distances would get "stuck". Again, I could again add code to age these values out over time. Either way, none of this is documented.

[magicrub]

Please take a look at the implementation.
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Proximity/AP_Proximity_MAV.cpp#L86

[jkflying]

The Ardupilot and PX4 approaches are very different here.

In PX4, if there are onboard distance sensors that are configured for collision prevention, it will use those sensors and then publishes a single fused OBSTACLE_DISTANCE message. This will be fused with the data received from an external OBSTACLE_DISTANCE message. The idea is then that that the GCS could use this to display the data that the FCU is actually using to avoid, without having to attempt to replicate perfectly the internal fusion algorithm.

Conversely, PX4 does not support multiple sources sending OBSTACLE_DISTANCE messages. They need to be fused externally before sending them in. Unfortunately it would require some fairly major architecture changes to support this.

[jkflying]

@magicrub I discussed briefly with @dogmaphobic, it seems it would be quite a pain to replicate the multi-message fusion inside of the GCS.

Would it work on AP side to publish the internal data as a new OBSTACLE_DISTANCE message for the GCS to consume? It looks like it would be fairly easy, there are already the _angle and _distance arrays which just need to be put into a message and published.

[jkflying]

@magicrub it looks to me like the MAVLink message definition doesn't allow for negative increments.
Either you use the increment, which is uint8_t, or you use increment_f which is float but

"If greater than 0 then this value is used instead of the uint8_t increment field."

So either we need to update that description, or shouldn't support negative increments.

[magicrub]

@jkflying ahh yes, true. The negative is only supported on the float version. Description needs to be updated. It was something I just recently added though so I doubt PX4 supports it yet. Are you willing to update the docs or would you like me/someoneElse to?

[jkflying]

Could you update the docs? I'll get support into PX4 ASAP 👍

[magicrub]

PR: mavlink/mavlink#1211

[LorenzMeier]

Done by @mrivi

[junwoo091400]

Done in mavlink/qgroundcontrol#9720