From 6c58c9953430fcadfe09ddd9629ae6cf285bb315 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 3 Apr 2020 11:47:06 +1100 Subject: [PATCH 1/4] AP_HAL_SITL: provide method to get amount of data still pending in outbound system queues --- libraries/AP_HAL_SITL/UARTDriver.cpp | 20 ++++++++++++++++++++ libraries/AP_HAL_SITL/UARTDriver.h | 2 ++ 2 files changed, 22 insertions(+) diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 7485c58c63fce..8af669e66ed5a 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -770,5 +770,25 @@ uint64_t UARTDriver::receive_time_constraint_us(uint16_t nbytes) return last_receive_us; } +ssize_t UARTDriver::get_system_outqueue_length() const +{ + if (!_connected) { + return 0; + } + +#if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD) + return 0; +#elif defined(__APPLE__) && defined(__MACH__) + return 0; +#else + int size; + if (ioctl(_fd, TIOCOUTQ, &size) == -1) { + // ::fprintf(stderr, "ioctl TIOCOUTQ failed: %m\n"); + return 0; + } + return size; +#endif +} + #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index cd5a4e4f72261..6c70b4080aee6 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -33,6 +33,8 @@ class HALSITL::UARTDriver : public AP_HAL::UARTDriver { return true; } + ssize_t get_system_outqueue_length() const; + void set_blocking_writes(bool blocking) override { _nonblocking_writes = !blocking; From e191982a49a71675840ce435ffa2832b47bc8869 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 3 Apr 2020 11:50:25 +1100 Subject: [PATCH 2/4] AP_HAL_SITL: do not let outbound queue length to grow too far This basically limits our loop rate to whatever is listening on uartA can handle in terms of mavlink traffic. --- libraries/AP_HAL_SITL/SITL_State.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 85448183c9e68..183642473b69e 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -212,6 +212,20 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) usleep(1000); } } + // check the outbound TCP queue size. If it is too long then + // MAVProxy/pymavlink take too long to process packets and it ends + // up seeing traffic well into our past and hits time-out + // conditions. + if (sitl_model->get_speedup() > 1) { + while (true) { + const int queue_length = ((HALSITL::UARTDriver*)hal.uartA)->get_system_outqueue_length(); + // ::fprintf(stderr, "queue_length=%d\n", (signed)queue_length); + if (queue_length < 1024) { + break; + } + usleep(1000); + } + } } #define streq(a, b) (!strcmp(a, b)) From 0088320855b9f5da87072f780172f8caa2fd2731 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 5 Apr 2020 19:19:43 +1000 Subject: [PATCH 3/4] autotest: sub: fix altitude-hold for being below target altitude --- Tools/autotest/ardusub.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 2d0191f46c98c..e6720cea2eb10 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -102,8 +102,14 @@ def test_alt_hold(self): self.mavproxy.send('mode ALT_HOLD\n') self.wait_mode('ALT_HOLD') - - self.set_rc(Joystick.Throttle, 1000) + msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5) + if msg is None: + raise NotAchievedException("Did not get GLOBAL_POSITION_INT") + pwm = 1000 + if msg.relative_alt/1000.0 < -5.5: + # need to g`o up, not down! + pwm = 2000 + self.set_rc(Joystick.Throttle, pwm) self.wait_altitude(altitude_min=-6, altitude_max=-5) self.set_rc(Joystick.Throttle, 1500) @@ -191,8 +197,7 @@ def dive_mission(self, filename): self.arm_vehicle() - self.mavproxy.send('mode auto\n') - self.wait_mode('AUTO') + self.change_mode('AUTO') self.wait_waypoint(1, 5, max_dist=5) @@ -214,8 +219,7 @@ def test_gripper_mission(self): self.mavproxy.send('mode loiter\n') self.wait_ready_to_arm() self.arm_vehicle() - self.mavproxy.send('mode auto\n') - self.wait_mode('AUTO') + self.change_mode('AUTO') self.mavproxy.expect("Gripper Grabbed") self.mavproxy.expect("Gripper Released") except Exception as e: From 7dcf7bb8f4423ec2b18c90404ee54911509a52ae Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 7 Apr 2020 13:56:22 +1000 Subject: [PATCH 4/4] SITL: provide method to retrieve desired speedup --- libraries/SITL/SIM_Aircraft.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 2e5f4248ad161..6f5ef664b5e6a 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -47,6 +47,7 @@ class Aircraft { set simulation speedup */ void set_speedup(float speedup); + float get_speedup() { return target_speedup; } /* set instance number