Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-11 10:23:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Tools: allow for more than 327m range rangefinders

Browse Source
This commit is contained in:
Peter Barker 2024-10-11 15:13:11 +11:00 committed by Andrew Tridgell
parent 6a2582f217
commit 18f95ba41b
16 changed files with 216 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/Frame_params/Parrot_Disco/Parrot_Disco.param
View File

@ -43,8 +43,8 @@ RLL_RATE_P 0.040758
RLL2SRV_RMAX 75.000000
RLL2SRV_TCONST 0.450000
RNGFND_LANDING 1
RNGFND_MAX_CM 700
RNGFND_MIN_CM 20
RNGFND_MAX 7.00
RNGFND_MIN 0.20
RNGFND_PIN -1
RNGFND_SCALING 1
RNGFND_TYPE 9

4
Tools/Frame_params/deset-mapping-boat.param
View File

@ -25,8 +25,8 @@ MOT_SLEWRATE,25
MOT_VEC_ANGLEMAX,30
NAVL1_DAMPING,0.8
NAVL1_PERIOD,16
RNGFND1_MAX_CM,20000
RNGFND1_MIN_CM,0
RNGFND1_MAX,200.00
RNGFND1_MIN,0
RNGFND1_TYPE,17
SERIAL2_BAUD,19
SERIAL2_PROTOCOL,39

4
Tools/Frame_params/eLAB_EX700_AC34.param
View File

@ -39,8 +39,8 @@ MOT_THST_HOVER,0.25
MOT_THST_EXPO,0.7
MOT_BAT_VOLT_MAX,25.2
MOT_BAT_VOLT_MIN,21.0
RNGFND_MAX_CM,5000
RNGFND_MIN_CM,5
RNGFND_MAX,50.00
RNGFND_MIN,0.05
RNGFND_SCALING,1
RNGFND_TYPE,8
PILOT_THR_BHV,1

188
Tools/autotest/arducopter.py
View File

@ -2870,7 +2870,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.set_analog_rangefinder_parameters()
# set use-height to 20m (the parameter is a percentage of max range)
self.set_parameters({
'EK3_RNG_USE_HGT': 200000 / self.get_parameter('RNGFND1_MAX_CM'),
'EK3_RNG_USE_HGT': (20 / self.get_parameter('RNGFND1_MAX')) * 100,
})
self.reboot_sitl()
@ -3044,7 +3044,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
"COMPASS_USE3" : 0,
# use DroneCAN rangefinder
"RNGFND1_TYPE" : 24,
"RNGFND1_MAX_CM" : 11000,
"RNGFND1_MAX" : 110.00,
# use DroneCAN battery monitoring, and enforce with a arming voltage
"BATT_MONITOR" : 8,
"BATT_ARM_VOLT" : 12.0,
@ -4026,7 +4026,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.set_analog_rangefinder_parameters()
self.set_parameters({
"RNGFND1_MAX_CM": 1500
"RNGFND1_MAX": 15.00
})
# configure EKF to use rangefinder for altitude at low altitudes
@ -4757,7 +4757,9 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
while True:
if self.get_sim_time_cached() - tstart > timeout:
raise NotAchievedException("Did not reach destination")
if self.distance_to_local_position((x, y, -z_up)) < 1:
dist = self.distance_to_local_position((x, y, -z_up))
if dist < 1:
self.progress(f"Reach distance ({dist})")
break
def test_guided_local_position_target(self, x, y, z_up):
@ -8522,7 +8524,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.set_parameter("SERIAL5_PROTOCOL", 1)
self.set_parameter("RNGFND1_TYPE", 10)
self.reboot_sitl()
self.set_parameter("RNGFND1_MAX_CM", 32767)
self.set_parameter("RNGFND1_MAX", 327.67)
self.progress("Should be unhealthy while we don't send messages")
self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION, True, True, False)
@ -8787,10 +8789,10 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.set_parameters({
"RNGFND1_TYPE": 2, # maxbotix
"RNGFND1_ADDR": 0x70,
"RNGFND1_MIN_CM": 150,
"RNGFND1_MIN": 1.50,
"RNGFND2_TYPE": 2, # maxbotix
"RNGFND2_ADDR": 0x71,
"RNGFND2_MIN_CM": 250,
"RNGFND2_MIN": 2.50,
})
self.reboot_sitl()
self.do_timesync_roundtrip()
@ -8932,6 +8934,176 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.do_RTL()
def RangeFinderDriversLongRange(self):
'''test rangefinder above 327m'''
# FIXME: when we get a driver+simulator for a rangefinder with
# >327m range change this to use that driver
self.set_parameters({
"SERIAL4_PROTOCOL": 9,
"RNGFND1_TYPE": 19, # BenewakeTF02
"WPNAV_SPEED_UP": 1000, # cm/s
})
self.customise_SITL_commandline([
"--serial4=sim:benewake_tf02",
])
text_good = "GOOD"
text_out_of_range_high = "OUT_OF_RANGE_HIGH"
rf_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION
alt = 3
self.takeoff(alt, mode='GUIDED')
self.assert_parameter_value("RNGFND1_MAX", 7.00)
self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True)
m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True)
if abs(m.current_distance * 0.01 - alt) > 1:
raise NotAchievedException(f"Expected {alt}m range")
self.assert_parameter_value("RNGFND1_MAX", m.max_distance * 0.01, epsilon=0.00001)
self.assert_parameter_value("RNGFND1_MIN", m.min_distance * 0.01, epsilon=0.00001)
self.send_statustext(text_good)
alt = 10
self.fly_guided_move_local(0, 0, alt)
self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True)
m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True)
if abs(m.current_distance * 0.01 - alt) > 1:
raise NotAchievedException(f"Expected {alt}m range")
self.assert_parameter_value("RNGFND1_MAX", m.max_distance * 0.01, epsilon=0.00001)
self.assert_parameter_value("RNGFND1_MIN", m.min_distance * 0.01, epsilon=0.00001)
self.send_statustext(text_out_of_range_high)
self.progress("Moving the goalposts")
self.set_parameter("RNGFND1_MAX", 20)
self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True)
m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True)
if abs(m.current_distance * 0.01 - alt) > 1:
raise NotAchievedException(f"Expected {alt}m range")
self.assert_parameter_value("RNGFND1_MAX", m.max_distance * 0.01, epsilon=0.00001)
self.assert_parameter_value("RNGFND1_MIN", m.min_distance * 0.01, epsilon=0.00001)
self.send_statustext(text_good)
self.delay_sim_time(2)
dfreader = self.dfreader_for_current_onboard_log()
self.do_RTL()
self.progress("Checking in/out of range markers in log")
required_range = None
count = 0
while True:
m = dfreader.recv_match(type=["MSG", "RFND"])
if m is None:
break
m_type = m.get_type()
if m_type == "MSG":
for t in [text_good, text_out_of_range_high]:
if t in m.Message:
required_range = t
continue
if m_type == "RFND":
if required_range is None:
continue
if required_range == text_good:
required_state = 4
elif required_range == text_out_of_range_high:
required_state = 3
else:
raise ValueError(f"Unexpected text {required_range}")
if m.Stat != required_state:
raise NotAchievedException(f"Not in expected state want={required_state} got={m.Stat} dist={m.Dist}")
self.progress(f"In expected state {required_range}")
required_range = None
count += 1
if count < 3:
raise NotAchievedException("Did not see range markers")
def RangeFinderSITLLongRange(self):
'''test rangefinder above 327m'''
# FIXME: when we get a driver+simulator for a rangefinder with
# >327m range change this to use that driver
self.set_parameters({
"RNGFND1_TYPE": 100, # SITL
"WPNAV_SPEED_UP": 1000, # cm/s
"RNGFND1_MAX": 7000, # metres
})
self.reboot_sitl()
text_good = "GOOD"
text_high = "HIGH"
rf_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_LASER_POSITION
alt = 3
self.takeoff(alt, mode='GUIDED')
self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True)
m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True)
got = m.current_distance * 0.01
if abs(got - alt) > 1:
raise NotAchievedException(f"Expected {alt}m range {got=}")
self.send_statustext(text_good)
alt = 635
self.fly_guided_move_local(0, 0, alt)
self.assert_sensor_state(rf_bit, present=True, enabled=True, healthy=True)
m = self.assert_receive_message('DISTANCE_SENSOR', verbose=True)
if abs(m.current_distance * 0.01 - alt) > 1:
raise NotAchievedException(f"Expected {alt}m range")
self.send_statustext(text_high)
dfreader = self.dfreader_for_current_onboard_log()
# fast descent!
def hook(msg, m):
if m.get_type() != 'HEARTBEAT':
return
# tell vehicle to only pay attention to the attitude:
bitmask = 0
bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE
bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE
bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE
bitmask |= mavutil.mavlink.ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE
self.mav.mav.set_attitude_target_send(
0, # time_boot_ms
1, # target sysid
1, # target compid
0, # bitmask of things to ignore
mavextra.euler_to_quat([
math.radians(-180),
math.radians(0),
math.radians(0)]), # att
0, # roll rate (rad/s)
0, # pitch rate (rad/s)
0, # yaw rate (rad/s)
1 # thrust, 0 to 1, translated to a climb/descent rate
)
self.install_message_hook_context(hook)
self.wait_altitude(0, 30, timeout=200, relative=True)
self.do_RTL()
self.progress("Checking in/out of range markers in log")
good = False
while True:
m = dfreader.recv_match(type=["MSG", "RFND"])
if m is None:
break
m_type = m.get_type()
if m_type == "RFND":
if m.Dist < 600:
continue
good = True
break
if not good:
raise NotAchievedException("Did not see good alt")
def ShipTakeoff(self):
'''Fly Simulated Ship Takeoff'''
# test ship takeoff
@ -10876,6 +11048,8 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
self.ModeFollow,
self.RangeFinderDrivers,
self.RangeFinderDriversMaxAlt,
self.RangeFinderDriversLongRange,
self.RangeFinderSITLLongRange,
self.MaxBotixI2CXL,
self.MAVProximity,
self.ParameterValidation,

15
Tools/autotest/ardusub.py
View File

@ -16,7 +16,6 @@ from pymavlink import mavutil
import vehicle_test_suite
from vehicle_test_suite import NotAchievedException
from vehicle_test_suite import AutoTestTimeoutException
from vehicle_test_suite import PreconditionFailedException
if sys.version_info[0] < 3:
ConnectionResetError = AutoTestTimeoutException
@ -196,8 +195,8 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
"SCR_ENABLE": 1,
"RNGFND1_TYPE": 36,
"RNGFND1_ORIENT": 25,
"RNGFND1_MIN_CM": 10,
"RNGFND1_MAX_CM": 5000,
"RNGFND1_MIN": 0.10,
"RNGFND1_MAX": 50.00,
})
self.install_example_script_context("rangefinder_quality_test.lua")
@ -244,8 +243,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
def Surftrak(self):
"""Test SURFTRAK mode"""
if self.get_parameter('RNGFND1_MAX_CM') != 3000.0:
raise PreconditionFailedException("RNGFND1_MAX_CM is not %g" % 3000.0)
self.assert_parameter_value('RNGFND1_MAX', 30)
# Something closer to Bar30 noise
self.context_push()
@ -292,8 +290,8 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
"SCR_ENABLE": 1,
"RNGFND1_TYPE": 36,
"RNGFND1_ORIENT": 25,
"RNGFND1_MIN_CM": 10,
"RNGFND1_MAX_CM": 3000,
"RNGFND1_MIN": 0.10,
"RNGFND1_MAX": 30.00,
"SCR_USER1": 2, # Configuration bundle
"SCR_USER2": sea_floor_depth, # Depth in meters
"SCR_USER3": 101, # Output log records
@ -788,8 +786,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
def TerrainMission(self):
"""Mission using surface tracking"""
if self.get_parameter('RNGFND1_MAX_CM') != 3000.0:
raise PreconditionFailedException("RNGFND1_MAX_CM is not %g" % 3000.0)
self.assert_parameter_value('RNGFND1_MAX', 30)
filename = "terrain_mission.txt"
self.progress("Executing mission %s" % filename)

4
Tools/autotest/default_params/copter-optflow.parm
View File

@ -5,8 +5,8 @@ EK3_SRC1_VELXY 5
EK3_SRC1_VELZ 0
FLOW_TYPE 10
RNGFND1_TYPE 1
RNGFND1_MIN_CM 0
RNGFND1_MAX_CM 4000
RNGFND1_MIN 0
RNGFND1_MAX 40.00
RNGFND1_PIN 0
RNGFND1_SCALING 12.12
SIM_FLOW_ENABLE 1

4
Tools/autotest/default_params/copter-rangefinder.parm
View File

@ -1,5 +1,5 @@
RNGFND1_TYPE 1
RNGFND1_MIN_CM 0
RNGFND1_MAX_CM 4000
RNGFND1_MIN 0
RNGFND1_MAX 40.00
RNGFND1_PIN 0
RNGFND1_SCALING 12.12

2
Tools/autotest/default_params/gazebo-iris.parm
View File

@ -10,4 +10,4 @@ SIM_SONAR_SCALE 10
RNGFND1_TYPE 1
RNGFND1_SCALING 10
RNGFND1_PIN 0
RNGFND1_MAX_CM 5000
RNGFND1_MAX 50.00

2
Tools/autotest/default_params/periph.parm
View File

@ -5,7 +5,7 @@ COMPASS_ENABLE 1
BARO_ENABLE 1
ARSPD_TYPE 100
RNGFND1_TYPE 100
RNGFND1_MAX_CM 12000
RNGFND1_MAX 120.00
BATT_MONITOR 16
BATT_I2C_BUS 2

2
Tools/autotest/default_params/quad-can.parm
View File

@ -4,5 +4,5 @@ SIM_CAN_SRV_MSK 0xFf
SIM_VIB_MOT_MAX 270
GPS1_TYPE 9
RNGFND1_TYPE 24
RNGFND1_MAX_CM 11000
RNGFND1_MAX 110.00
BATT_MONITOR 8

2
Tools/autotest/default_params/quadplane-can.parm
View File

@ -8,6 +8,6 @@ SIM_VIB_MOT_MAX 270
GPS1_TYPE 9
ARSPD_TYPE 8
RNGFND1_TYPE 24
RNGFND1_MAX_CM 11000
RNGFND1_MAX 110.00
RNGFND_LANDING 1
BATT_MONITOR 8

2
Tools/autotest/default_params/sub.parm
View File

@ -75,7 +75,7 @@ PSC_POSXY_P 2.5
PSC_VELXY_D 0.8
PSC_VELXY_I 0.5
PSC_VELXY_P 5.0
RNGFND1_MAX_CM 3000
RNGFND1_MAX 30.00
RNGFND1_PIN 0
RNGFND1_SCALING 12.12
RNGFND1_TYPE 1

12
Tools/autotest/default_params/vee-gull_005.param
View File

@ -465,10 +465,10 @@ RLL2SRV_RMAX,0
RLL2SRV_TCONST,0.5
RNGFND1_ADDR,0
RNGFND1_FUNCTION,0
RNGFND1_GNDCLEAR,10
RNGFND1_GNDCLR,0.01
RNGFND1_LANDING,0
RNGFND1_MAX_CM,700
RNGFND1_MIN_CM,20
RNGFND1_MAX,7.00
RNGFND1_MIN,0.20
RNGFND1_OFFSET,0
RNGFND1_ORIENT,25
RNGFND1_PIN,-1
@ -483,9 +483,9 @@ RNGFND1_STOP_PIN,-1
RNGFND1_TYPE,0
RNGFND2_ADDR,0
RNGFND2_FUNCTION,0
RNGFND2_GNDCLEAR,10
RNGFND2_MAX_CM,700
RNGFND2_MIN_CM,20
RNGFND2_GNDCLR,0.01
RNGFND2_MAX,7.00
RNGFND2_MIN,0.20
RNGFND2_OFFSET,0
RNGFND2_ORIENT,25
RNGFND2_PIN,-1

2
Tools/autotest/models/Callisto.param
View File

@ -55,7 +55,7 @@ PSC_ANGLE_MAX,45
PSC_JERK_Z,10
PSC_POSZ_P,0.5
PSC_VELZ_P,2.5
RNGFND1_MAX_CM,10000
RNGFND1_MAX,100.00
RNGFND1_PIN,0
RNGFND1_SCALING,12.1212
RNGFND1_TYPE,1

2
Tools/autotest/quadplane.py
View File

@ -1524,7 +1524,7 @@ class AutoTestQuadPlane(vehicle_test_suite.TestSuite):
"RNGFND1_TYPE": 100,
"RNGFND1_PIN" : 0,
"RNGFND1_SCALING" : 12.2,
"RNGFND1_MAX_CM" : 5000,
"RNGFND1_MAX" : 50.00,
"RNGFND_LANDING" : 1,
})

6
Tools/autotest/vehicle_test_suite.py
View File

@ -5823,8 +5823,8 @@ class TestSuite(ABC):
def analog_rangefinder_parameters(self):
return {
"RNGFND1_TYPE": 1,
"RNGFND1_MIN_CM": 0,
"RNGFND1_MAX_CM": 4000,
"RNGFND1_MIN": 0,
"RNGFND1_MAX": 40.00,
"RNGFND1_SCALING": 12.12,
"RNGFND1_PIN": 0,
}
@ -7209,6 +7209,8 @@ class TestSuite(ABC):
raise NotAchievedException("above max height (%f > %f)" %
(m.distance, dist_max))
self.progress(f"Rangefinder distance {m.distance} is between {dist_min} and {dist_max}")
def assert_distance_sensor_quality(self, quality):
m = self.assert_receive_message('DISTANCE_SENSOR')