diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 531a63a24f9d4..391edca98656f 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -1334,6 +1334,7 @@ def ThrottleFailsafe(self): self.reboot_sitl() def ThrottleFailsafeFence(self): + '''Fly fence survives throttle failsafe''' fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 4d51a6a9b9c65..0af681b0918a4 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -7822,6 +7822,12 @@ def assert_arm_failure(self, expected_statustext, timeout=5, ignore_prearm_failu if self.mav.motors_armed(): raise NotAchievedException("Armed when we shouldn't have") + def ready_to_arm(self): + '''returns true if the vehicle thinks it is ready to arm. Note that + currently this implementation is *significantly* different to + wait_ready_to_arm''' + return self.sensor_has_state(mavutil.mavlink.MAV_SYS_STATUS_PREARM_CHECK, True, True, True) + def wait_ready_to_arm(self, timeout=120, require_absolute=True, check_prearm_bit=True): # wait for EKF checks to pass self.progress("Waiting for ready to arm") @@ -7839,6 +7845,7 @@ def wait_ready_to_arm(self, timeout=120, require_absolute=True, check_prearm_bit self.waiting_to_arm_count += 1 def wait_heartbeat(self, drain_mav=True, quiet=False, *args, **x): + '''as opposed to mav.wait_heartbeat, raises an exception on timeout. Also, ignores heartbeats not from our target system''' if drain_mav: diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp index ab4a67dd83fe2..08511eb74d31b 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp @@ -306,6 +306,11 @@ void HAL_SITL::actually_reboot() AP_HAL::panic("PANIC: REBOOT FAILED: %s", strerror(errno)); } +uint16_t *HAL_SITL::get_pwm_input() const +{ + return _sitl_state->pwm_input; +} + static HAL_SITL hal_sitl_inst; const AP_HAL::HAL& AP_HAL::get_HAL() { diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.h b/libraries/AP_HAL_SITL/HAL_SITL_Class.h index 4ddc20b2cb666..9504fb2174942 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.h +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.h @@ -41,6 +41,8 @@ class HAL_SITL : public AP_HAL::HAL { bool run_in_maintenance_mode() const; #endif + uint16_t *get_pwm_input() const; + private: HALSITL::SITL_State *_sitl_state; @@ -53,6 +55,7 @@ class HAL_SITL : public AP_HAL::HAL { // set to true if simulation is to wipe storage as it is opened: bool wipe_storage; + }; #if HAL_NUM_CAN_IFACES diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 5bc6b410dd4d2..450c70e16912d 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -25,6 +25,8 @@ #include #include +#include + extern const AP_HAL::HAL& hal; using namespace HALSITL; @@ -242,6 +244,7 @@ void SITL_State::_check_rc_input(void) bool SITL_State::_read_rc_sitl_input() { + struct pwm_packet { uint16_t pwm[16]; } pwm_pkt; @@ -266,24 +269,19 @@ bool SITL_State::_read_rc_sitl_input() return size != -1; // we must continue to drain _sitl_rc } + // consider transforming the data in the packet into a different + // protocol here. + switch (size) { case -1: return false; case 8*2: case 16*2: { // a packet giving the receiver PWM inputs - for (uint8_t i=0; istate.rcin_chan_count) { - // we're using rc from simulator - continue; - } - uint16_t pwm = pwm_pkt.pwm[i]; - if (pwm != 0) { - pwm_input[i] = pwm; - } - } - return true; + const uint32_t baudrate = 0; + AP::RC().process_bytes((uint8_t*)pwm_pkt.pwm, size, baudrate); + + return false; } default: fprintf(stderr, "Malformed SITL RC input (%ld)", (long)size); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 7e53bda5e7979..a86c7411eaf1e 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -34,6 +34,7 @@ #include "AP_RCProtocol_FPort2.h" #include "AP_RCProtocol_DroneCAN.h" #include "AP_RCProtocol_GHST.h" +#include "AP_RCProtocol_SITL.h" #include #include @@ -88,6 +89,9 @@ void AP_RCProtocol::init() #if AP_RCPROTOCOL_GHST_ENABLED backend[AP_RCProtocol::GHST] = new AP_RCProtocol_GHST(*this); #endif +#if AP_RCPROTOCOL_SITL_ENABLED + backend[AP_RCProtocol::SITL] = new AP_RCProtocol_SITL(*this); +#endif } AP_RCProtocol::~AP_RCProtocol() @@ -199,6 +203,27 @@ void AP_RCProtocol::process_pulse_list(const uint32_t *widths, uint16_t n, bool } } +void AP_RCProtocol::process_bytes(uint8_t *bytes, uint16_t count, uint32_t baudrate) +{ + if (_detected_protocol != AP_RCProtocol::NONE && + !protocol_enabled(_detected_protocol)) { + _detected_protocol = AP_RCProtocol::NONE; + } + + uint32_t now = AP_HAL::millis(); + if (!should_search(now) && + _detected_protocol != AP_RCProtocol::NONE) { + // pass all the bytes to the backend directly: + backend[_detected_protocol]->process_bytes(bytes, count, baudrate); + return; + } + + // we're searching, so process bytes one-at-a-time: + for (uint8_t i=0; i +#include + +#include +extern const HAL_SITL& hal_sitl; + +void AP_RCProtocol_SITL::process_bytes(uint8_t *bytes, uint16_t count, uint32_t baudrate) +{ + const uint32_t now = AP_HAL::millis(); + if (now - last_byte_ms > 10) { + // could be a framing failure. Might also just be first byte + // in packet Discard. + search_buf_ofs = 0; + } + last_byte_ms = now; + + // if we get 32 bytes that's 16 values. 16 is 8 values. 1 byte is + // "the RCProtocol library is searching" + switch (count) { + case 32: + case 16: + add_input(count/2, (uint16_t*)bytes, false); + search_success_correct_size_start_ms = 0; + return; + case 1: + // constant shift of bytes down + if (search_buf_ofs >= ARRAY_SIZE(search_buf)) { + memmove(&search_buf[1], &search_buf[0], search_buf_ofs-1); + } + search_buf[search_buf_ofs++] = bytes[0]; + if (search_buf_ofs == 8 || search_buf_ofs == 16) { + search_success_correct_size_start_ms = now; + } else { + search_success_correct_size_start_ms = 0; + } + break; + default: + AP_HAL::panic("unexpected SITL byte count"); + } +} + +void AP_RCProtocol_SITL::update() +{ + const auto *sitl = AP::sitl(); + if (sitl->state.rcin_chan_count) { + const uint16_t *pwm_input = hal_sitl.get_pwm_input(); + add_input(sitl->state.rcin_chan_count, pwm_input, false); + return; + } + + // frame-gap protection: + const uint32_t now = AP_HAL::millis(); + if (now - search_success_correct_size_start_ms) { + add_input(search_buf_ofs/2, (uint16_t*)search_buf, false); + search_success_correct_size_start_ms = 0; + } +} + +#endif // AP_RCPROTOCOL_SITL_ENABLED + diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SITL.h b/libraries/AP_RCProtocol/AP_RCProtocol_SITL.h new file mode 100644 index 0000000000000..e50ef166a0c74 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SITL.h @@ -0,0 +1,30 @@ + +#pragma once + +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_SITL_ENABLED + +#include "AP_RCProtocol.h" + + +class AP_RCProtocol_SITL : public AP_RCProtocol_Backend { +public: + + using AP_RCProtocol_Backend::AP_RCProtocol_Backend; + + void process_bytes(uint8_t *bytes, uint16_t count, uint32_t baudrate) override; + void update() override; + +private: + + uint32_t last_byte_ms; + + // search support: + uint8_t search_buf[32]; + uint8_t search_buf_ofs; + uint32_t search_success_correct_size_start_ms; +}; + +#endif // AP_RCPROTOCOL_MAVLINK_RADIO_ENABLED + diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h index 7136a381e34ef..89ce8191105f3 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -69,3 +69,7 @@ #ifndef AP_RCPROTOCOL_GHST_ENABLED #define AP_RCPROTOCOL_GHST_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED #endif + +#ifndef AP_RCPROTOCOL_SITL_ENABLED +#define AP_RCPROTOCOL_SITL_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif