diff --git a/libraries/AP_HAL_AVR_SITL/AP_HAL_AVR_SITL_Namespace.h b/libraries/AP_HAL_AVR_SITL/AP_HAL_AVR_SITL_Namespace.h index 3a068b4a80..a70396185a 100644 --- a/libraries/AP_HAL_AVR_SITL/AP_HAL_AVR_SITL_Namespace.h +++ b/libraries/AP_HAL_AVR_SITL/AP_HAL_AVR_SITL_Namespace.h @@ -8,8 +8,10 @@ namespace AVR_SITL { class SITL_State; class SITLConsoleDriver; class SITLEEPROMStorage; - class SITLAnalogIn; - class ADCSource; + class SITLAnalogIn; + class SITLRCInput; + class SITLRCOutput; + class ADCSource; class RCInput; } 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 0477bd851b..6176ab97fd 100644 --- a/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp +++ b/libraries/AP_HAL_AVR_SITL/HAL_AVR_SITL_Class.cpp @@ -15,6 +15,7 @@ #include "Storage.h" #include "Console.h" #include "RCInput.h" +#include "RCOutput.h" #include "SITL_State.h" #include @@ -26,9 +27,9 @@ static SITLScheduler sitlScheduler; static SITLEEPROMStorage sitlEEPROMStorage; static SITLConsoleDriver consoleDriver; static SITL_State sitlState; +static SITLRCInput sitlRCInput(&sitlState); +static SITLRCOutput sitlRCOutput(&sitlState); -static Empty::EmptyRCInput emptyRCInput; -static Empty::EmptyRCOutput emptyRCOutput; static Empty::EmptyGPIO emptyGPIO; static Empty::EmptyAnalogIn emptyAnalogIn; @@ -46,11 +47,9 @@ HAL_AVR_SITL::HAL_AVR_SITL() : &emptyAnalogIn, /* analogin */ &sitlEEPROMStorage, /* storage */ &consoleDriver, /* console */ - &emptyGPIO, /* gpio */ - &emptyRCInput, /* rcinput */ - &emptyRCOutput, /* rcoutput */ - + &sitlRCInput, /* rcinput */ + &sitlRCOutput, /* rcoutput */ &sitlScheduler), /* scheduler */ _sitl_state(&sitlState) {} diff --git a/libraries/AP_HAL_AVR_SITL/RCInput.cpp b/libraries/AP_HAL_AVR_SITL/RCInput.cpp new file mode 100644 index 0000000000..b16303a375 --- /dev/null +++ b/libraries/AP_HAL_AVR_SITL/RCInput.cpp @@ -0,0 +1,52 @@ + +#include "RCInput.h" + +using namespace AVR_SITL; + +extern const AP_HAL::HAL& hal; + +void SITLRCInput::init(void* machtnichts) +{ + clear_overrides(); +} + +uint8_t SITLRCInput::valid() { + return 0; +} + +uint16_t SITLRCInput::read(uint8_t ch) { + return _override[ch]? _override[ch] : _sitlState->pwm_input[ch]; +} + +uint8_t SITLRCInput::read(uint16_t* periods, uint8_t len) { + for (uint8_t i=0; ipwm_input[i]; + } + return len; +} + +bool SITLRCInput::set_overrides(int16_t *overrides, uint8_t len) { + bool res = false; + for (uint8_t i = 0; i < len; i++) { + res |= set_override(i, overrides[i]); + } + return res; +} + +bool SITLRCInput::set_override(uint8_t channel, int16_t override) { + if (override < 0) return false; /* -1: no change. */ + if (channel < 8) { + _override[channel] = override; + if (override != 0) { + return true; + } + } + return false; +} + +void SITLRCInput::clear_overrides() +{ + for (uint8_t i = 0; i < 8; i++) { + _override[i] = 0; + } +} diff --git a/libraries/AP_HAL_AVR_SITL/RCInput.h b/libraries/AP_HAL_AVR_SITL/RCInput.h new file mode 100644 index 0000000000..b11b2ca1fb --- /dev/null +++ b/libraries/AP_HAL_AVR_SITL/RCInput.h @@ -0,0 +1,28 @@ + +#ifndef __AP_HAL_AVR_SITL_RCINPUT_H__ +#define __AP_HAL_AVR_SITL_RCINPUT_H__ + +#include + +class AVR_SITL::SITLRCInput : public AP_HAL::RCInput { +public: + SITLRCInput(SITL_State *sitlState) { + _sitlState = sitlState; + } + void init(void* machtnichts); + uint8_t valid(); + uint16_t read(uint8_t ch); + uint8_t read(uint16_t* periods, uint8_t len); + + 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; + + /* override state */ + uint16_t _override[8]; +}; + +#endif // __AP_HAL_AVR_SITL_RCINPUT_H__ + diff --git a/libraries/AP_HAL_AVR_SITL/RCOutput.cpp b/libraries/AP_HAL_AVR_SITL/RCOutput.cpp new file mode 100644 index 0000000000..6d7c215da2 --- /dev/null +++ b/libraries/AP_HAL_AVR_SITL/RCOutput.cpp @@ -0,0 +1,45 @@ + +#include "RCOutput.h" + +using namespace AVR_SITL; + +void SITLRCOutput::init(void* machtnichts) {} + +void SITLRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) { + _freq_hz = freq_hz; +} + +uint16_t SITLRCOutput::get_freq(uint8_t ch) { + return _freq_hz; +} + +void SITLRCOutput::enable_ch(uint8_t ch) +{} + +void SITLRCOutput::enable_mask(uint32_t chmask) +{} + +void SITLRCOutput::disable_ch(uint8_t ch) +{} + +void SITLRCOutput::disable_mask(uint32_t chmask) +{} + +void SITLRCOutput::write(uint8_t ch, uint16_t period_us) +{ + _sitlState->pwm_output[ch] = period_us; +} + +void SITLRCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len) +{ + memcpy(_sitlState->pwm_output+ch, period_us, len*sizeof(uint16_t)); +} + +uint16_t SITLRCOutput::read(uint8_t ch) { + return _sitlState->pwm_output[ch]; +} + +void SITLRCOutput::read(uint16_t* period_us, uint8_t len) +{ + memcpy(period_us, _sitlState->pwm_output, len*sizeof(uint16_t)); +} diff --git a/libraries/AP_HAL_AVR_SITL/RCOutput.h b/libraries/AP_HAL_AVR_SITL/RCOutput.h new file mode 100644 index 0000000000..6173bb8c35 --- /dev/null +++ b/libraries/AP_HAL_AVR_SITL/RCOutput.h @@ -0,0 +1,31 @@ + +#ifndef __AP_HAL_AVR_SITL_RCOUTPUT_H__ +#define __AP_HAL_AVR_SITL_RCOUTPUT_H__ + +#include + +class AVR_SITL::SITLRCOutput : public AP_HAL::RCOutput { +public: + SITLRCOutput(SITL_State *sitlState) { + _sitlState = sitlState; + _freq_hz = 50; + } + void init(void* machtnichts); + void set_freq(uint32_t chmask, uint16_t freq_hz); + uint16_t get_freq(uint8_t ch); + void enable_ch(uint8_t ch); + void enable_mask(uint32_t chmask); + void disable_ch(uint8_t ch); + void disable_mask(uint32_t chmask); + void write(uint8_t ch, uint16_t period_us); + void write(uint8_t ch, uint16_t* period_us, uint8_t len); + uint16_t read(uint8_t ch); + void read(uint16_t* period_us, uint8_t len); + +private: + SITL_State *_sitlState; + uint16_t _freq_hz; +}; + +#endif // __AP_HAL_AVR_SITL_RCOUTPUT_H__ + diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp index 98a038e3cb..4f5f76d88d 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp @@ -22,9 +22,6 @@ extern const AP_HAL::HAL& hal; -// emulate RC input -struct RC_ICR4 ICR4; - using namespace AVR_SITL; enum SITL_State::vehicle_type SITL_State::_vehicle; @@ -41,7 +38,8 @@ AP_Compass_HIL *SITL_State::_compass; int SITL_State::_sitl_fd; SITL *SITL_State::_sitl; -uint16_t SITL_State::_pwm_output[11]; +uint16_t SITL_State::pwm_output[11]; +uint16_t SITL_State::pwm_input[8]; // catch floating point exceptions void SITL_State::_sig_fpe(int signum) @@ -99,7 +97,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) _framerate = 50; } // set right default throttle for rover (allowing for reverse) - ICR4.set(2, 1500); + pwm_input[2] = 1500; } else { _vehicle = ArduPlane; if (_framerate == 0) { @@ -302,10 +300,9 @@ void SITL_State::_fdm_input(void) // a packet giving the receiver PWM inputs uint8_t i; for (i=0; i<8; i++) { - // setup the ICR4 register for the RC channel - // inputs + // setup the pwn input for the RC channel inputs if (d.pwm_pkt.pwm[i] != 0) { - ICR4.set(i, d.pwm_pkt.pwm[i]); + pwm_input[i] = d.pwm_pkt.pwm[i]; } } break; @@ -331,15 +328,15 @@ void SITL_State::_simulator_output(void) if (last_update == 0) { for (i=0; i<11; i++) { - _pwm_output[i] = 1000; + pwm_output[i] = 1000; } if (_vehicle == ArduPlane) { - _pwm_output[0] = _pwm_output[1] = _pwm_output[3] = 1500; - _pwm_output[7] = 1800; + pwm_output[0] = pwm_output[1] = pwm_output[3] = 1500; + pwm_output[7] = 1800; } if (_vehicle == APMrover2) { - _pwm_output[0] = _pwm_output[1] = _pwm_output[2] = _pwm_output[3] = 1500; - _pwm_output[7] = 1800; + pwm_output[0] = pwm_output[1] = pwm_output[2] = pwm_output[3] = 1500; + pwm_output[7] = 1800; } } @@ -354,10 +351,10 @@ void SITL_State::_simulator_output(void) last_update = hal.scheduler->millis(); for (i=0; i<11; i++) { - if (_pwm_output[i] == 0xFFFF) { + if (pwm_output[i] == 0xFFFF) { control.pwm[i] = 0; } else { - control.pwm[i] = _pwm_output[i]; + control.pwm[i] = pwm_output[i]; } } @@ -445,6 +442,10 @@ Vector3f SITL_State::_rand_vec3f(void) void SITL_State::init(int argc, char * const argv[]) { + pwm_input[0] = pwm_input[1] = pwm_input[3] = 1500; + pwm_input[4] = pwm_input[7] = 1800; + pwm_input[2] = pwm_input[5] = pwm_input[6] = 1000; + _scheduler = (SITLScheduler *)hal.scheduler; _parse_command_line(argc, argv); } diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.h b/libraries/AP_HAL_AVR_SITL/SITL_State.h index b49b06eba4..c20ec4e75b 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.h +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.h @@ -9,7 +9,6 @@ #include #include "AP_HAL_AVR_SITL_Namespace.h" #include "HAL_AVR_SITL_Class.h" -#include "sitl_rc.h" #include #include @@ -22,8 +21,6 @@ #include "../AP_Compass/AP_Compass.h" #include "../SITL/SITL.h" -extern struct RC_ICR4 ICR4; - class HAL_AVR_SITL; class AVR_SITL::SITL_State { @@ -38,6 +35,8 @@ public: int gps_pipe(void); ssize_t gps_read(int fd, void *buf, size_t count); + static uint16_t pwm_output[11]; + static uint16_t pwm_input[8]; private: void _parse_command_line(int argc, char * const argv[]); @@ -88,8 +87,6 @@ private: static SITL *_sitl; static const uint16_t _rcout_port = 5502; static const uint16_t _simin_port = 5501; - - static uint16_t _pwm_output[11]; }; #endif // CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL diff --git a/libraries/AP_HAL_AVR_SITL/sitl_rc.h b/libraries/AP_HAL_AVR_SITL/sitl_rc.h deleted file mode 100644 index efffc0c095..0000000000 --- a/libraries/AP_HAL_AVR_SITL/sitl_rc.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - RC input emulation - Code by Andrew Tridgell November 2011 - */ - -#ifndef _SITL_RC_H -#define _SITL_RC_H - -struct RC_ICR4 { - uint16_t channels[9]; // 9th channel is sync - uint32_t value; - uint8_t idx; - - RC_ICR4() { - // constructor - channels[0] = channels[1] = channels[3] = 1500; - channels[4] = channels[7] = 1800; - channels[2] = channels[5] = channels[6] = 1000; - channels[8] = 4500; // sync - idx = 0; - } - - /* - read from ICR4 fetches next channel - */ - operator int() { - value += channels[idx++]*2; - if (idx == 9) { - idx = 0; - } - value = value % 40000; - return (uint16_t)value; - } - - - /* - ignore rate assignment for now (needed for apm2 - emulation) - */ - RC_ICR4& operator=(uint16_t rate) { - return *this; - } - - /* - interface to set a channel value from SITL - */ - void set(uint8_t i, uint16_t v) { - channels[i] = v; - } -}; - -#endif