FC v2 Collision prevention issue with voxl 2

Jetson Nano

I am using modalai fc v2 with voxl 2 , i am able to see the obstacle_distance message publishing in the mavlink console, but the drone is not slowing down or stopping.

the parameters are
mpc_pos_mode - acceleration based
cp_dist - 1 m
cp_no_go_data - enabled
cp_angle - 0

I checked with various builds and versions of PX4 from both PX4 github and Modalai github.

can anyone help me with which version or fork it works.

nsh> listener obstacle_distance

TOPIC: obstacle_distance
 obstacle_distance
    timestamp: 178940124 (0.026434 seconds ago)
    increment: 10.00000
    angle_offset: 0.00000
    distances: [236, 237, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001]
    min_distance: 0
    max_distance: 2000
    frame: 12
    sensor_type: 4

nsh> listener obstacle_distance 10

TOPIC: obstacle_distance
 obstacle_distance
    timestamp: 184239763 (0.014512 seconds ago)
    increment: 10.00000
    angle_offset: 0.00000
    distances: [236, 236, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001]
    min_distance: 0
    max_distance: 2000
    frame: 12
    sensor_type: 4

nsh> listener obstacle_distance 10

TOPIC: obstacle_distance
 obstacle_distance
    timestamp: 199589325 (0.028254 seconds ago)
    increment: 10.00000
    angle_offset: 0.00000
    distances: [236, 236, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001]
    min_distance: 0
    max_distance: 2000
    frame: 12
    sensor_type: 4

nsh> listener obstacle_distance 10

TOPIC: obstacle_distance
 obstacle_distance
    timestamp: 204539470 (0.012805 seconds ago)
    increment: 10.00000
    angle_offset: 0.00000
    distances: [445, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001]
    min_distance: 0
    max_distance: 2000
    frame: 12
    sensor_type: 4

nsh> listener obstacle_distance 10

TOPIC: obstacle_distance
 obstacle_distance
    timestamp: 206838947 (0.036977 seconds ago)
    increment: 10.00000
    angle_offset: 0.00000
    distances: [435, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001]
    min_distance: 0
    max_distance: 2000
    frame: 12
    sensor_type: 4

nsh> listener obstacle_distance 10

TOPIC: obstacle_distance
 obstacle_distance
    timestamp: 208539592 (0.000817 seconds ago)
    increment: 10.00000
    angle_offset: 0.00000
    distances: [436, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001]
    min_distance: 0
    max_distance: 2000
    frame: 12
    sensor_type: 4

nsh> listener obstacle_distance 10

TOPIC: obstacle_distance
 obstacle_distance
    timestamp: 210039039 (0.006330 seconds ago)
    increment: 10.00000
    angle_offset: 0.00000
    distances: [435, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001]
    min_distance: 0
    max_distance: 2000
    frame: 12
    sensor_type: 4

nsh> listener obstacle_distance 10

TOPIC: obstacle_distance
 obstacle_distance
    timestamp: 223538159 (0.007192 seconds ago)
    increment: 10.00000
    angle_offset: 0.00000
    distances: [114, 102, 98, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 129, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001]
    min_distance: 0
    max_distance: 2000
    frame: 12
    sensor_type: 4

nsh> listener obstacle_distance 10

TOPIC: obstacle_distance
 obstacle_distance
    timestamp: 225588237 (0.036362 seconds ago)
    increment: 10.00000
    angle_offset: 0.00000
    distances: [93, 90, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001]
    min_distance: 0
    max_distance: 2000
    frame: 12
    sensor_type: 4

nsh> listener obstacle_distance 10

TOPIC: obstacle_distance
 obstacle_distance
    timestamp: 227187560 (0.049067 seconds ago)
    increment: 10.00000
    angle_offset: 0.00000
    distances: [2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001]
    min_distance: 0
    max_distance: 2000
    frame: 12
    sensor_type: 4

nsh> listener obstacle_distance 10

TOPIC: obstacle_distance
 obstacle_distance
    timestamp: 228587517 (0.043813 seconds ago)
    increment: 10.00000
    angle_offset: 0.00000
    distances: [2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001, 2001]
    min_distance: 0
    max_distance: 2000
    frame: 12
    sensor_type: 4

nsh>

Jetson Nano

@Eric-Katzfey @Alex-Kushleyev

I have the ark flow MR attached to the FC. While I want to check the standard builds, the standard build do not have UAVCAN modules enabled. i have to enable them manually.

If anyone could share the built px4 file for the ModalAi FC v2 along with UAVCAN enabled and collision prevention working, It would mean alot.

Eric Katzfey

@Jetson-Nano Can you open the question about UAVCAN in a separate post? It has nothing to do with the initial question and we like each separate issue to have it's own posting / title. Thanks!

Jetson Nano

@Eric-Katzfey I will do that.

Sean Hickey

@Jetson-Nano

Looking through the PX4 codebase, the ARK Flow MR publishes to distance_sensor by default, not obstacle_distance. Have you made any changes to the codebase that would affect obstacle_distance or mounted any other sensors?

Also, how have you mounted the ark_flow_mr. Is it pointing downwards on the vehicle, or forward pointing or some other configuration?

Finally, have you tried setting the distance for collision prevention higher, and seeing how the drone reacts in that case?

Jetson Nano

@Sean-Hickey I am using the optical flow sensor for position hold, it is attached directly to the FC.

I am getting the obstacle_distance messages from the TOF sensor and I am able to see it.
I did check with 1 m cp_dist and it was not working.

Sean Hickey

@Jetson-Nano

What TOF sensor are you using for obstacle distance?

Just confirming, you are using the ARK Flow MR facing downwards just for optical flow and position hold, but not for collision prevention?

Jetson Nano

@Sean-Hickey PMD tof that comes with yellow board M0173 - https://docs.modalai.com/M0173/

Yes optical flow is facing down, it is used for position hold and not for collision prevention.

Sean Hickey

@Jetson-Nano

Can you see if you are using the MO169 or the MO178?

Jetson Nano

@Sean-Hickey I am using M0178

Sean Hickey

@Jetson-Nano

I was able to identify the issue.

mpc_pos_mode - acceleration based doesn't call Collision Prevention in any of the newest PX4 builds..

You need to use mpc_pos_mode - Simple Position Control (0) or mpc_pos_mode - smooth position control (3).

I was able to test this on a similar setup, and confirmed that collision prevention only worked with the mpc_pos_mode indicated above, and notably did not with mpc_pos_mode acceleration.

Jetson Nano

@Sean-Hickey that great, just making sure, did you use optical flow with FC?

Also could you let me know which branch you used, or the Px4 built file. i want to test it out in my setup.

Sean Hickey

@Jetson-Nano

I used VIO with FC, but the path from obstacle detection to Collision Prevention is unchanged between the 2 modes.

I used the mainline ModalAI PX4 SDK branch. I can share the built px4 deb build with you if that's helpful, but I would try changing the mpc_pos_mode parameter first on your current build to see if that fixes your issue.

Jetson Nano

@Sean-Hickey
I already checked with changing the MPC_POS_MODE, did not work.

Could you share the file, I would like to test it out.

Sean Hickey

@Jetson-Nano

https://drive.google.com/file/d/1FmaW4kXma6pxjQO86WugNJq9-5zZ75gU/view?usp=sharing

I attached the link above, I am using parameters:

MPC_POS_MODE: Simple position control
CP_DIST 1.5 m
CP_DELAY 0.4 s
CP_GUIDE_ANG : 0 deg
CP_GO_NO_DATA: Enabled

Sean Hickey

@Jetson-Nano

If this doesn't work, on the mavlink console (after testing the collision prevention) call:
listener obstacle_distance_fused 1
listener collision_constraints 1

and let me know the results.