Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Tools: Add RealFlight Autotest #28293

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions Tools/autotest/autotest.py
Original file line number Diff line number Diff line change
Expand Up @@ -525,6 +525,8 @@ def run_step(step):
}
if opts.speedup is not None:
fly_opts["speedup"] = opts.speedup
if opts.realflight_address is not None:
fly_opts["realflight_address"] = opts.realflight_address

# handle "test.Copter" etc:
if step in tester_class_map:
Expand Down Expand Up @@ -1017,6 +1019,9 @@ def format_epilog(self, formatter):
group_sim.add_option("", "--replay",
action='store_true',
help="enable replay logging for tests")
group_sim.add_option("--realflight-address",
default=None,
help="IP address of RealFlight simulator")
parser.add_option_group(group_sim)

group_completion = optparse.OptionGroup(parser, "Completion helpers")
Expand Down
2 changes: 2 additions & 0 deletions Tools/autotest/default_params/realflight-autotest-extra.parm
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
SIM_RFL_OPTS,9 # Reset vehicle on start | Don't print frame rate stats
RC_PROTOCOLS,0x40000 # Only accept UDP RC input
98 changes: 98 additions & 0 deletions Tools/autotest/default_params/realflight-titan-cobra.parm
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
# Titan Cobra RealFlight ArduPilot 4.6
AIRSPEED_CRUISE,12.4
AIRSPEED_MAX,18
AIRSPEED_MIN,11
ARMING_RUDDER,2
ARSPD_OFFSET,2013
ARSPD_SKIP_CAL,1
ARSPD_USE,1
BATT_ARM_VOLT,14.7
BATT_CAPACITY,10800
BATT_CRT_VOLT,14
BATT_LOW_VOLT,14.4
EK3_GSF_RUN_MASK,0 # causes floating point exceptions
FLIGHT_OPTIONS,1026
FLTMODE_CH,8
FLTMODE1,17
FLTMODE2,17
FLTMODE3,19
FLTMODE4,19
FLTMODE5,5
FLTMODE6,5
INS_GYRO_FILTER,36
NAVL1_PERIOD,9
PTCH_LIM_MIN_DEG,-14
PTCH_RATE_D,0.00903
PTCH_RATE_FF,1.042
PTCH_RATE_FLTD,18
PTCH_RATE_FLTT,2.122
PTCH_RATE_I,1.042
PTCH_RATE_P,0.806
PTCH_TRIM_DEG,4.2
PTCH2SRV_RMAX_DN,75
PTCH2SRV_RMAX_UP,75
PTCH2SRV_TCONST,0.75
Q_A_ACCEL_P_MAX,53000
Q_A_ACCEL_R_MAX,49000
Q_A_ACCEL_Y_MAX,4500
Q_A_ANG_PIT_P,6
Q_A_ANG_RLL_P,6
Q_A_ANG_YAW_P,1.8
Q_A_RAT_PIT_D,0.00527
Q_A_RAT_PIT_FLTD,18
Q_A_RAT_PIT_FLTT,18
Q_A_RAT_PIT_I,0.328
Q_A_RAT_PIT_P,0.328
Q_A_RAT_RLL_D,0.00707
Q_A_RAT_RLL_FLTD,18
Q_A_RAT_RLL_FLTT,18
Q_A_RAT_RLL_I,0.385
Q_A_RAT_RLL_P,0.385
Q_A_RAT_YAW_FLTE,5
Q_A_RAT_YAW_FLTT,18
Q_A_RAT_YAW_I,0.2
Q_A_RAT_YAW_P,2
Q_A_THR_MIX_MAN,0.5
Q_ACRO_PIT_RATE,100
Q_ACRO_RLL_RATE,100
Q_ACRO_YAW_RATE,50
Q_ASSIST_SPEED,9
Q_ENABLE,1
Q_M_BAT_VOLT_MAX,16.8
Q_M_BAT_VOLT_MIN,13.2
Q_M_THST_EXPO,0.67
Q_M_THST_HOVER,0.322
Q_OPTIONS,16384
Q_P_ACCZ_I,0.656
Q_P_ACCZ_P,0.328
Q_P_VELXY_P,4
Q_PILOT_SPD_DN,1.5
Q_PLT_Y_RATE,50
Q_TRANS_DECEL,0.8
Q_VFWD_ALT,5
Q_VFWD_GAIN,0.1
Q_WVANE_ANG_MIN,0.5
Q_WVANE_GAIN,4
Q_WVANE_HGT_MIN,2
RC3_DZ,80
RLL_RATE_D,0.00625
RLL_RATE_FF,0.387
RLL_RATE_FLTD,18
RLL_RATE_FLTT,3.183
RLL_RATE_I,0.251
RLL_RATE_P,0.251
RLL2SRV_RMAX,75
SERVO2_FUNCTION,80
SERVO2_TRIM,1600
SERVO4_FUNCTION,79
SERVO4_TRIM,1600
STAB_PITCH_DOWN,2.5
TECS_CLMB_MAX,5.6
TECS_PITCH_MAX,0
TECS_RLL2THR,7
TECS_SINK_MAX,3
TECS_SINK_MIN,1.5
THROTTLE_NUDGE,0
TRIM_THROTTLE,47
WP_LOITER_RAD,70
WP_RADIUS,70
6 changes: 6 additions & 0 deletions Tools/autotest/pysim/vehicleinfo.py
Original file line number Diff line number Diff line change
Expand Up @@ -376,6 +376,12 @@ def __init__(self):
"waf_target": "bin/arduplane",
"default_params_filename": "default_params/stratoblimp.parm",
},
"realflight-titan-cobra": {
"model": "flightaxis",
"waf_target": "bin/arduplane",
"default_params_filename": "default_params/realflight-titan-cobra.parm",
"external": True,
},
},
},
"Rover": {
Expand Down
62 changes: 62 additions & 0 deletions Tools/autotest/quadplane.py
Original file line number Diff line number Diff line change
Expand Up @@ -2161,6 +2161,64 @@ def WindEstimateConsistency(self):
self.progress("Wind estimates correlated")
break

def setup_RealFlight_vehicle(self, model, home):
'''
Restart the SITL for RealFlight. RealFlight must already be running and
the correct airport and model must be loaded.
'''
defaults_filepath = self.model_defaults_filepath(model)
extra_params = 'default_params/realflight-autotest-extra.parm'
defaults_filepath.append(os.path.join(testdir, extra_params))
self.customise_SITL_commandline(
[
f"--home={home}",
],
model=f"flightaxis:{self.realflight_address}",
defaults_filepath=defaults_filepath
)

def RealFlightHover(self, model, home):
'''
Perform a simple hover test in RealFlight. Useful for generating logs
for comparative analysis.
'''
if not self.realflight_address:
self.progress("Specify an IP address with --realflight-address or REALFLIGHT_IPADDR to run this test")
return

# Log fullrate attitude for PID Review Tool
self.set_parameters({
"LOG_BITMASK": 0x10FFFF,
})
self.setup_RealFlight_vehicle(model, home)

self.wait_ready_to_arm()
self.change_mode("QLOITER")
self.arm_vehicle()
self.set_rc(3, 2000)
self.wait_altitude(8, 12, relative=True)
self.set_rc(3, 1500)
stick_deflections = [
(2000, 1500, "Roll right"),
(1500, 1500, "Center"),
(1000, 1500, "Roll left"),
(1500, 1500, "Center"),
(1500, 2000, "Pitch forward"),
(1500, 1500, "Center"),
(1500, 1000, "Pitch back"),
(1500, 1500, "Center"),
]
n_iterations = 10
for i in range(n_iterations):
print(f"Control input cycle: {i+1}/{n_iterations}")
for roll, pitch, msg in stick_deflections:
self.progress(f"{msg}")
self.set_rc(1, roll)
self.set_rc(2, pitch)
self.delay_sim_time(0.5)
self.change_mode("QLAND")
self.wait_disarmed(timeout=120)

def tests(self):
'''return list of all tests'''

Expand Down Expand Up @@ -2213,5 +2271,9 @@ def tests(self):
self.RTL_AUTOLAND_1, # as in fly-home then go to landing sequence
self.RTL_AUTOLAND_1_FROM_GUIDED, # as in fly-home then go to landing sequence
self.AHRSFlyForwardFlag,
Test(self.RealFlightHover, speedup=1, kwargs={
'model': 'realflight-titan-cobra',
'home': 'EliField'
}),
])
return ret
2 changes: 2 additions & 0 deletions Tools/autotest/vehicle_test_suite.py
Original file line number Diff line number Diff line change
Expand Up @@ -1906,6 +1906,7 @@ def __init__(self,
dronecan_tests=False,
generate_junit=False,
enable_fgview=False,
realflight_address=None,
build_opts={}):

self.start_time = time.time()
Expand Down Expand Up @@ -1973,6 +1974,7 @@ def __init__(self,
self.in_drain_mav = False
self.tlog = None
self.enable_fgview = enable_fgview
self.realflight_address = realflight_address or os.getenv("REALFLIGHT_IPADDR")

self.rc_thread = None
self.rc_thread_should_quit = False
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_RCProtocol/AP_RCProtocol_UDP.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -64,8 +64,8 @@ bool AP_RCProtocol_UDP::init()
void AP_RCProtocol_UDP::update()
{
#if AP_RCPROTOCOL_FDM_ENABLED
// yield to the FDM backend if it is getting data
if (fdm_backend->active()) {
// yield to the FDM backend if it is active and getting data
if (protocol_enabled(AP_RCProtocol::FDM) && fdm_backend->active()) {
return;
}
#endif
Expand Down
6 changes: 5 additions & 1 deletion libraries/SITL/SIM_FlightAxis.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -134,9 +134,13 @@ FlightAxis::FlightAxis(const char *frame_str) :
}

const char *colon = strchr(frame_str, ':');
const char *env = getenv("REALFLIGHT_IPADDR");
if (colon) {
controller_ip = colon+1;
}
else if (env) {
controller_ip = env;
}
for (uint8_t i=0; i<ARRAY_SIZE(sim_defaults); i++) {
AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);
if (sim_defaults[i].save) {
Expand Down Expand Up @@ -399,7 +403,7 @@ void FlightAxis::exchange_data(const struct sitl_input &input)
reply = soap_request_end(0);
if (reply == nullptr) {
sock_error_count++;
if (sock_error_count >= 10000 && timestamp_sec() - last_recv_sec > 1) {
if (sock_error_count >= 10000 && timestamp_sec() - last_recv_sec > 3*average_frame_time_s) {
printf("socket timeout\n");
delete sock;
sock = nullptr;
Expand Down
Loading