diff --git a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h index 1c63576e13e..d78e54ce57c 100644 --- a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h +++ b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h @@ -7,6 +7,7 @@ namespace PX4 { class PX4Scheduler; class PX4UARTDriver; class PX4EEPROMStorage; + class PX4RCInput; } #endif //__AP_HAL_PX4_NAMESPACE_H__ diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp index e8fe3de7814..67da6a29e86 100644 --- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp +++ b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp @@ -11,6 +11,7 @@ #include "Scheduler.h" #include "UARTDriver.h" #include "Storage.h" +#include "RCInput.h" #include #include @@ -23,13 +24,13 @@ static Empty::EmptyI2CDriver i2cDriver; static Empty::EmptySPIDeviceManager spiDeviceManager; static Empty::EmptyAnalogIn analogIn; static Empty::EmptyGPIO gpioDriver; -static Empty::EmptyRCInput rcinDriver; static Empty::EmptyRCOutput rcoutDriver; static Empty::EmptyUtil utilInstance; static PX4ConsoleDriver consoleDriver; static PX4Scheduler schedulerInstance; static PX4EEPROMStorage storageDriver; +static PX4RCInput rcinDriver; // only one real UART driver for now static PX4UARTDriver uartADriver("/dev/ttyS0"); diff --git a/libraries/AP_HAL_PX4/RCInput.cpp b/libraries/AP_HAL_PX4/RCInput.cpp new file mode 100644 index 00000000000..640e9a93b98 --- /dev/null +++ b/libraries/AP_HAL_PX4/RCInput.cpp @@ -0,0 +1,71 @@ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#include "RCInput.h" + +using namespace PX4; + +extern const AP_HAL::HAL& hal; + +PX4RCInput::PX4RCInput() +{} + +void PX4RCInput::init(void* machtnichts) +{ + for (uint8_t i=0; imillis() - _last_read > 20; +} + +uint16_t PX4RCInput::read(uint8_t ch) { + _last_read = hal.scheduler->millis(); + return _override[ch]; +} + +uint8_t PX4RCInput::read(uint16_t* periods, uint8_t len) { + if (len > PX4_NUM_RCINPUT_CHANNELS) { + len = PX4_NUM_RCINPUT_CHANNELS; + } + for (uint8_t i = 0; i < len; i++){ + periods[i] = read(i); + } + return len; +} + +bool PX4RCInput::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 PX4RCInput::set_override(uint8_t channel, int16_t override) { + if (override < 0) { + return false; /* -1: no change. */ + } + if (channel < PX4_NUM_RCINPUT_CHANNELS) { + _override[channel] = override; + if (override != 0) { + return true; + } + } + return false; +} + +void PX4RCInput::clear_overrides() +{ +#if 0 + for (uint8_t i = 0; i < PX4_NUM_RCINPUT_CHANNELS; i++) { + _override[i] = 0; + } +#endif +} + +#endif diff --git a/libraries/AP_HAL_PX4/RCInput.h b/libraries/AP_HAL_PX4/RCInput.h new file mode 100644 index 00000000000..1b7b997e418 --- /dev/null +++ b/libraries/AP_HAL_PX4/RCInput.h @@ -0,0 +1,28 @@ + +#ifndef __AP_HAL_PX4_RCINPUT_H__ +#define __AP_HAL_PX4_RCINPUT_H__ + +#include + +#define PX4_NUM_RCINPUT_CHANNELS 8 + +class PX4::PX4RCInput : public AP_HAL::RCInput { +public: + PX4RCInput(); + 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: + /* override state */ + uint16_t _override[PX4_NUM_RCINPUT_CHANNELS]; + bool _pwm_valid; + uint32_t _last_read; +}; + +#endif // __AP_HAL_PX4_RCINPUT_H__