RESOLVED: OBSTACLE_DISTANCE_3D message not working in pymavlink

ArduCopter Copter 4.1
1

I am trying to send a OBSTACLE_DISTANCE_3D message to Arducopter 4.1 beta5. The message is not being recognized (no errors) and the function immediately returns when the 3D message function is called. I compiled AC 4.1beta5 SITL and I believe I also installed the latest pymavlink. All other mavlink obstacle and distance messages like DISTANCE_SENSOR and OBSTACLE_DISTANCE work well in AC4.1beta5 SITL. It seems like just the new 3D message is completely ignored. @rishabsingh3003 I leveraged your coding - what am I missing?

from pymavlink import mavutil
.............
    # Prepare for Arducopter 4.1 3D Obstacle Avoidance
    def send_obstacle_distance_3D_message():
        global mavlink_obstacle_coordinates, min_depth_cm, max_depth_cm
        global last_obstacle_distance_sent_ms
        global current_time_us

        if (enable_3D_msg_obstacle_distance == True):
            if current_time_us == last_obstacle_distance_sent_ms:
                # no new frame
                progress("no new frame")
                return
            last_obstacle_distance_sent_ms = current_time_us
            for q in range(9):
                conn.mav.obstacle_distance_3d_send(
                current_time_us,    # us Timestamp (UNIX time or time since system boot)
                    0,
                    0,
                    65535,
                    float(mavlink_obstacle_coordinates[q][0]),
                    float(mavlink_obstacle_coordinates[q][1]),
                    float(mavlink_obstacle_coordinates[q][2]),
                    float(min_depth_cm/100),
                    float(max_depth_cm/100) #needs to be in meters
                )
            current_time_us = int(round(time.time() * 1000000))

This is a sample dataset for the 9 sector depth matrix I am feeding into the OBSTACLE_DISTANCE_3D message:

1625232381482134 0 0 65535 -1.5486565828323364 0.026432015001773834 0.6540128588676453 1.0 20.0
1625232381482134 0 0 65535 -1.5486565828323364 0.026432015001773834 0.6540128588676453 1.0 20.0
1625232381482134 0 0 65535 -1.5486565828323364 0.026432015001773834 0.6540128588676453 1.0 20.0
1625232381482134 0 0 65535 -1.5486565828323364 0.026432015001773834 0.6540128588676453 1.0 20.0
1625232381482134 0 0 65535 -1.5486565828323364 0.026432015001773834 0.6540128588676453 1.0 20.0
1625232381482134 0 0 65535 -1.5486565828323364 0.026432015001773834 0.6540128588676453 1.0 20.0
1625232381482134 0 0 65535 -1.5486565828323364 0.026432015001773834 0.6540128588676453 1.0 20.0
1625232381482134 0 0 65535 -1.5486565828323364 0.026432015001773834 0.6540128588676453 1.0 20.0
1625232381482134 0 0 65535 -1.5486565828323364 0.026432015001773834 0.6540128588676453 1.0 20.0
2

@mtbsteve this is completely my fault, I haven’t updated my repo where you got this code from. If you go see the branch: https://github.com/rishabsingh3003/Vision-Obstacle-Avoidance/tree/land_detection_final/Companion_Computer … it has a more up to date code

The last time I pushed on that branch, we didn’t have a check at the ardupilot backend for the “frame” of the Obstacle Vector being passed. The backend at the moment, only accepts “MAV_FRAME_BODY_FRD”, and it would be very dangerous if say, the user passed in an “FRU” vector, and we parsed it as FRD.
Please update your code to be something like this:

for q in range(9):
                conn.mav.obstacle_distance_3d_send(
                current_time_us,    # us Timestamp (UNIX time or time since system boot)
                    0,
                    mavutil.mavlink.MAV_FRAME_BODY_FRD,
                    65535,
                    float(mavlink_obstacle_coordinates[q][0]),
                    float(mavlink_obstacle_coordinates[q][1]),
                    float(mavlink_obstacle_coordinates[q][2]),
                    float(min_depth_cm/100),
                    float(max_depth_cm/100) #needs to be in meters
                )

It should now work properly. Sorry for the troubles

1 Like
3

@rishabsingh3003 thanks, but unfortunately that doesn’t solve my issue.
In my code, I call conn.mav.obstacle_distance_3d_send with the parameters above (now including MAV_FRAME_BODY_FRD), but for whatever reason the function is not executed and exits from the for: loop w/o an error message.
All other mavlink commands work just fine, including the former OBSTACLE_DISTANCE function.
It looks like the new OBSTACLE_DISTANCE_3D function is not available in the pymavlink installation I am on. I do have pymavlink 2.4.15 installed and a freshly compiled Arducopter 4.1 beta 5 forked from master.
Whart am I missing? Is there a newer pymavlink version required?

4

@mtbsteve can you share your script with me?

Maybe your version of pymavlink does not include the new message. Can you try this:

  1. Remove pymavlink: pip3 uninstall pymavlink
  2. On Arducopter 4.1 beta 5’s fork:
  3. CD to ardupilot/modules/mavlink/pymavlink and run python3 setup.py install --user

This should include all the latest messages including OBSTACLE_DISTANCE_3D

5

@rishabsingh3003 thanks. Yes thats what I did and its still not working.
Here is my code - if you can spot the issue it would be greatly appreciated :slight_smile:

6

@rishabsingh3003 I found the issue. My bad, I oversaw that the timestamp for the OBSTACLE_DISTANCE_3D message is in milliseconds and not as in OBSTACLE_DISTANCE in microseconds.