Mission Planner SITL Rangefinder Not avoiding obstacles

Kevin_Tapia · July 21, 2023, 12:11pm

Hi, I’m using mission planner and SITL, I’m sending 3 Sonar distances using a python script through TCP = localhost:5762, I can see the sonars in the Proximity windows, but when I do either an Auto or Guided flight and I set the sonars from 5 meters to different distances down to 0 the copter keeps going without showing any error message or stopping.

The communication is made using:

mavutil.mavlink_connection(‘tcp:127.0.0.1:5762’, 1,mavlink2.MAV_COMP_ID_OBSTACLE_AVOIDANCE,dialect= ‘common’)

and the sonars:

    time_boot_ms = 0 
    min_distance = 20
    max_distance = 700 
    current_distance = distanceCm
    type = mavlink2.MAV_DISTANCE_SENSOR_ULTRASOUND
    id = 1
    orientation = orientationInt
    covariance = 255 # UINT8_MAX
    self.connection.mav.distance_sensor_send(time_boot_ms,min_distance, max_distance, current_distance,type,id, orientation, covariance)

Here some parameters I’m using:

I have tried PRX1_TYPE 2 and 4 too…

Thank you in advance.

rfv-370 · July 21, 2023, 1:55pm

Hi there,

Recently (2 weeks ago), I tried on dronex4 frame and on rover , real ones, not SITL, - both cubeblack, the X4 with 4.3.x and the Rover 4.2.x; and can’t get any Mavlink rangefinder to stop a vehicle either… like he has on SITL.

May be I am also doing something wrong as well, but indeed very strange that SITL and real drone implementation fail ?

I used simple sonar on rpizero serial connected to Pixhawk - onboard a Mavproxy relay mavlink messages to Pixhawk and the radar of the MissionPlanner can display the sonars very well and on the right orientation. (sonars on soft-I2C on rpiz).

For settings, as well I followed:
I followed : Simple Object Avoidance — Copter documentation.

And really I played a lot around the parameters… …but the drone or the rover just go forward to hit the obstacle… ! Albeit the MissionPlanner Radar shows the right distance… strange…very strange.

Have anyone tried recently a simple obstacle avoidance with MavLink rangefinders ?

@rmackay9 “long” ago there was a MavLink issue up to version 4.1 : Proximity Sensor - DISTANCE_SENSOR Mavlink Message - #7 by iamczar

This got corrected starting 4.2, if I remember, but may be some left over would impact the simple obstacle avoidance when using MavLink rangefinder?

Any idea from your experiments ?

rfv-370 · August 1, 2023, 10:46am

@rishabsingh3003 any idea about this issue with Mavlink and rangefinder being displayed don the proximity radar but not being taken into account ?

rishabsingh3003 · August 1, 2023, 10:55am

The AVOID_ parameters control Simple Avoidance. Simple avoidance only work with Loiter. If you want to use Auto mode, then you’ll have to use our Path Planner BendyRuler. Check OA_ params. If you want to use with Guided, then set the OA_ parameters and also set the 6th bit in GUID_OPTIONS param.
Upload a log if you need more help. I assume your proximity setup is correct since the MP viewer is displaying obstacles.

rfv-370 · August 1, 2023, 3:01pm

Many Thanks @rishabsingh3003 ! I believe that for the modes RTL, Auto and guided the idea is that bendy ruler must apply in order to complete the mission rather than being stuck and fail…
Will stabilize and/or AltHold modes be stopped by Simple avoidance ?

Kevin_Tapia · August 1, 2023, 3:55pm

Update:

In SITL: Using Loiter and AltHold Modes we need to send the command rc 3 1500, otherwise the drone will land.

Loiter and AltHold Modes:

Arm → Take-off → send rc 3 1500 → set Loiter mode/AltHold Modes

after that the drone will stop when it detects an object using the sonars

Multirotor in SITL won’t hold alt in loiter and other modes - ArduCopter / Copter Simulation - ArduPilot Discourse

In Auto Mode, Bendy Ruler working too.

Here Logs from the experiments Loiter, AltHold and Auto(Mission) and the Params used

Some Important Params:

AVOID_ACCEL_MAX,3
AVOID_ALT_MIN,0
AVOID_ANGLE_MAX,1000
AVOID_BACKUP_DZ,0.1
AVOID_BACKUP_SPD,0.75
AVOID_BEHAVE,1
AVOID_DIST_MAX,5
AVOID_ENABLE,2
AVOID_MARGIN,2
OA_BR_CONT_ANGLE,75
OA_BR_CONT_RATIO,1.5
OA_BR_LOOKAHEAD,2
OA_BR_TYPE,1
OA_DB_ALT_MIN,0.2
OA_DB_BEAM_WIDTH,5
OA_DB_DIST_MAX,5
OA_DB_EXPIRE,10
OA_DB_OUTPUT,2
OA_DB_QUEUE_SIZE,80
OA_DB_RADIUS_MIN,0.01
OA_DB_SIZE,100
OA_MARGIN_MAX,5
OA_OPTIONS,1
OA_TYPE,1
PRX_FILT,0.25
PRX_IGN_GND,0
PRX_LOG_RAW,0
PRX1_IGN_ANG1,0
PRX1_IGN_ANG2,0
PRX1_IGN_ANG3,0
PRX1_IGN_ANG4,0
PRX1_IGN_WID1,0
PRX1_IGN_WID2,0
PRX1_IGN_WID3,0
PRX1_IGN_WID4,0
PRX1_MAX,0.75
PRX1_MIN,0.2
PRX1_ORIENT,0
PRX1_TYPE,4
PRX1_YAW_CORR,0
PRX2_IGN_ANG1,0
PRX2_IGN_ANG2,0
PRX2_IGN_ANG3,0
PRX2_IGN_ANG4,0
PRX2_IGN_WID1,0
PRX2_IGN_WID2,0
PRX2_IGN_WID3,0
PRX2_IGN_WID4,0
PRX2_MAX,0
PRX2_MIN,0
PRX2_ORIENT,0
PRX2_TYPE,4
PRX2_YAW_CORR,0
PRX3_IGN_ANG1,0
PRX3_IGN_ANG2,0
PRX3_IGN_ANG3,0
PRX3_IGN_ANG4,0
PRX3_IGN_WID1,0
PRX3_IGN_WID2,0
PRX3_IGN_WID3,0
PRX3_IGN_WID4,0
PRX3_MAX,0
PRX3_MIN,0
PRX3_ORIENT,0
PRX3_TYPE,4
PRX3_YAW_CORR,0
RNGFND1_ADDR,0
RNGFND1_FUNCTION,0
RNGFND1_GNDCLEAR,10
RNGFND1_MAX_CM,700
RNGFND1_MIN_CM,20
RNGFND1_OFFSET,0
RNGFND1_ORIENT,0
RNGFND1_PIN,-1
RNGFND1_POS_X,0
RNGFND1_POS_Y,0
RNGFND1_POS_Z,0
RNGFND1_PWRRNG,0
RNGFND1_RMETRIC,1
RNGFND1_SCALING,1
RNGFND1_STOP_PIN,-1
RNGFND1_TYPE,10
RNGFND2_ADDR,0
RNGFND2_FUNCTION,0
RNGFND2_GNDCLEAR,10
RNGFND2_MAX_CM,700
RNGFND2_MIN_CM,20
RNGFND2_OFFSET,0
RNGFND2_ORIENT,2
RNGFND2_PIN,-1
RNGFND2_POS_X,0
RNGFND2_POS_Y,0
RNGFND2_POS_Z,0
RNGFND2_PWRRNG,0
RNGFND2_RMETRIC,1
RNGFND2_SCALING,1
RNGFND2_STOP_PIN,-1
RNGFND2_TYPE,10
RNGFND3_ADDR,0
RNGFND3_FUNCTION,0
RNGFND3_GNDCLEAR,10
RNGFND3_MAX_CM,700
RNGFND3_MIN_CM,20
RNGFND3_OFFSET,0
RNGFND3_ORIENT,6
RNGFND3_PIN,-1
RNGFND3_POS_X,0
RNGFND3_POS_Y,0
RNGFND3_POS_Z,0
RNGFND3_PWRRNG,0
RNGFND3_RMETRIC,1
RNGFND3_SCALING,1
RNGFND3_STOP_PIN,-1
RNGFND3_TYPE,10

params_SITL.param (24.1 KB)

Kevin_Tapia · August 4, 2023, 9:17am

Hi @rishabsingh3003 , I tried these parameters, but the Rover behaves strange, in real using Loiter Mode, the rover goes backwards and not responding to the RC, and in SITL the rover goes backwards again but after goes in circles (out of control):

Furia R23 + asimov v2.param (14.9 KB)

*with the previous parameters I sent in SITL the rover works fine.

Thank you.

rishabsingh3003 · August 4, 2023, 9:44am

Send me a log please

Kevin_Tapia · August 4, 2023, 9:45am

@rishabsingh3003 Here are the logs that we got with those parameters:

Kevin_Tapia · August 8, 2023, 2:11pm

@rishabsingh3003 Maybe you have any ideas we can try to check for missing values or tests to check what causes the rover might have?

The sonars look normal for me in the log analyzer.

Kevin_Tapia · August 8, 2023, 2:19pm

Hi @Nuthasith @bigboy061293, I saw that you have similar problems as well in the past ( Obstacle Avoidance,Obstacle as Red Circles not getting shown. - ArduCopter - ArduPilot Discourse ) any ideas here please?

rfv-370 · August 9, 2023, 6:16pm

@rishabsingh3003 @Kevin_Tapia , I wonder if we do not face a similar issue as the one faced here and reopened by Randy:

github.com/ArduPilot/ardupilot

Copter: using proximity sensor downward oriented for terrain following

opened 03:39PM - 05 Jul 18 UTC

roque-canales

Enhancement Copter

hello, I use mavlink prx messages for input to pixhawk distance from ground. … it's working perfectly in prx mode. But it no work as range finder…. How to set pixhawk for it use this same sensor (orientation 25) for terrain Following? I attempt to set rngfnd_type to mavlink but this no work….. I Don't find message type for "mavlink rangfinder" input to pixhawk. Please let me know how to do

any hint?

-which is weird, why would it work on sitl and not in the real life… ??? or are we all doing the same misatke?

bigboy061293 · October 22, 2023, 11:58am

The last time I work with Obstacle Avoidance in AP, I reallized it is not very reliable. I am not sure how it is recently.