diff --git a/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp b/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp index af52587e56..54fa029563 100644 --- a/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp +++ b/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp @@ -31,7 +31,10 @@ static SITLRCInput sitlRCInput(&sitlState); static SITLRCOutput sitlRCOutput(&sitlState); static SITLAnalogIn sitlAnalogIn(&sitlState); +// use the Empty HAL for hardware we don't emulate static Empty::EmptyGPIO emptyGPIO; +static Empty::EmptyI2CDriver emptyI2C; +static Empty::EmptySPIDeviceManager emptySPI; static SITLUARTDriver sitlUart0Driver(0, &sitlState); static SITLUARTDriver sitlUart1Driver(1, &sitlState); @@ -42,8 +45,8 @@ HAL_AVR_SITL::HAL_AVR_SITL() : &sitlUart0Driver, /* uartA */ &sitlUart1Driver, /* uartB */ &sitlUart2Driver, /* uartC */ - NULL, /* i2c */ - NULL, /* spi */ + &emptyI2C, /* i2c */ + &emptySPI, /* spi */ &sitlAnalogIn, /* analogin */ &sitlEEPROMStorage, /* storage */ &consoleDriver, /* console */ diff --git a/libraries/AP_HAL_AVR_SITL/RCInput.cpp b/libraries/AP_HAL_AVR_SITL/RCInput.cpp index 6fad9b71bc..041b4f328f 100644 --- a/libraries/AP_HAL_AVR_SITL/RCInput.cpp +++ b/libraries/AP_HAL_AVR_SITL/RCInput.cpp @@ -13,10 +13,11 @@ void SITLRCInput::init(void* machtnichts) } uint8_t SITLRCInput::valid() { - return 0; + return _sitlState->pwm_valid; } uint16_t SITLRCInput::read(uint8_t ch) { + _sitlState->pwm_valid = false; return _override[ch]? _override[ch] : _sitlState->pwm_input[ch]; } @@ -24,7 +25,9 @@ uint8_t SITLRCInput::read(uint16_t* periods, uint8_t len) { for (uint8_t i=0; ipwm_input[i]; } - return len; + uint8_t v = _sitlState->pwm_valid; + _sitlState->pwm_valid = false; + return v; } bool SITLRCInput::set_overrides(int16_t *overrides, uint8_t len) { diff --git a/libraries/AP_HAL_AVR_SITL/RCInput.h b/libraries/AP_HAL_AVR_SITL/RCInput.h index 16ee291338..34a4b69330 100644 --- a/libraries/AP_HAL_AVR_SITL/RCInput.h +++ b/libraries/AP_HAL_AVR_SITL/RCInput.h @@ -19,8 +19,10 @@ public: bool set_overrides(int16_t *overrides, uint8_t len); bool set_override(uint8_t channel, int16_t override); void clear_overrides(); + private: SITL_State *_sitlState; + bool _valid; /* override state */ uint16_t _override[8]; diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp index 6dfb8405ec..91404f2093 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp @@ -42,6 +42,7 @@ int SITL_State::_sitl_fd; SITL *SITL_State::_sitl; uint16_t SITL_State::pwm_output[11]; uint16_t SITL_State::pwm_input[8]; +bool SITL_State::pwm_valid; // catch floating point exceptions void SITL_State::_sig_fpe(int signum) @@ -185,6 +186,8 @@ void SITL_State::_setup_fdm(void) void SITL_State::_timer_handler(int signum) { static uint32_t last_update_count; + static uint32_t last_pwm_input; + static bool in_timer; if (in_timer || ((SITLScheduler *)hal.scheduler)->interrupts_are_blocked()) { @@ -204,7 +207,7 @@ void SITL_State::_timer_handler(int signum) static uint16_t count = 0; static uint32_t last_report; - count++; + count++; if (millis() - last_report > 1000) { fprintf(stdout, "TH %u cps\n", count); count = 0; @@ -212,6 +215,12 @@ void SITL_State::_timer_handler(int signum) } #endif + // simulate RC input at 50Hz + if (hal.scheduler->millis() - last_pwm_input >= 20) { + last_pwm_input = hal.scheduler->millis(); + pwm_valid = true; + } + /* check for packet from flight sim */ _fdm_input(); @@ -287,7 +296,9 @@ void SITL_State::_fdm_input(void) return; } - _sitl->state = d.fg_pkt; + if (_sitl != NULL) { + _sitl->state = d.fg_pkt; + } _update_count++; count++; diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.h b/libraries/AP_HAL_AVR_SITL/SITL_State.h index 5338d877d0..655decf3c6 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.h +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.h @@ -37,6 +37,7 @@ public: ssize_t gps_read(int fd, void *buf, size_t count); static uint16_t pwm_output[11]; static uint16_t pwm_input[8]; + static bool pwm_valid; static void loop_hook(void); // simulated airspeed