diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux.h b/libraries/AP_HAL_Linux/AP_HAL_Linux.h new file mode 100644 index 0000000000..5e0647d080 --- /dev/null +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux.h @@ -0,0 +1,34 @@ + +#ifndef __AP_HAL_LINUX_H__ +#define __AP_HAL_LINUX_H__ + +/* Your layer exports should depend on AP_HAL.h ONLY. */ +#include + +/** + * Umbrella header for AP_HAL_Linux module. + * The module header exports singleton instances which must conform the + * AP_HAL::HAL interface. It may only expose implementation details (class + * names, headers) via the Linux namespace. + * The class implementing AP_HAL::HAL should be called HAL_Linux and exist + * in the global namespace. There should be a single const instance of the + * HAL_Linux class called AP_HAL_Linux, instantiated in the HAL_Linux_Class.cpp + * and exported as `extern const HAL_Linux AP_HAL_Linux;` in HAL_Linux_Class.h + * + * All declaration and compilation should be guarded by CONFIG_HAL_BOARD macros. + * In this case, we're using CONFIG_HAL_BOARD == HAL_BOARD_LINUX. + * When creating a new HAL, declare a new HAL_BOARD_ in AP_HAL/AP_HAL_Boards.h + * + * The module should also export an appropriate AP_HAL_MAIN() macro iff the + * appropriate CONFIG_HAL_BOARD value is set. + * The AP_HAL_MAIN macro expands to a main function (either an `int main (void)` + * or `int main (int argc, const char * argv[]), depending on platform) of an + * ArduPilot application, whose entry points are the c++ functions + * `void setup()` and `void loop()`, ala Arduino. + */ + +#include "HAL_Linux_Class.h" +#include "AP_HAL_Linux_Main.h" + +#endif //__AP_HAL_LINUX_H__ + diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Main.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Main.h new file mode 100644 index 0000000000..45013809d8 --- /dev/null +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Main.h @@ -0,0 +1,18 @@ + + +#ifndef __AP_HAL_LINUX_MAIN_H__ +#define __AP_HAL_LINUX_MAIN_H__ + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#define AP_HAL_MAIN() extern "C" {\ +int main (int argc, char * const argv[]) { \ + hal.init(argc, argv); \ + setup();\ + hal.scheduler->system_initialized(); \ + for(;;) loop();\ + return 0;\ + }\ + } +#endif // HAL_BOARD_LINUX + +#endif // __AP_HAL_LINUX_MAIN_H__ diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h new file mode 100644 index 0000000000..a773680531 --- /dev/null +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Namespace.h @@ -0,0 +1,29 @@ + +#ifndef __AP_HAL_LINUX_NAMESPACE_H__ +#define __AP_HAL_LINUX_NAMESPACE_H__ + +/* While not strictly required, names inside the Linux namespace are prefixed + * with Linux for clarity. (Some of our users aren't familiar with all of the + * C++ namespace rules.) + */ + +namespace Linux { + class LinuxUARTDriver; + class LinuxI2CDriver; + class LinuxSPIDeviceManager; + class LinuxSPIDeviceDriver; + class LinuxAnalogSource; + class LinuxAnalogIn; + class LinuxStorage; + class LinuxConsoleDriver; + class LinuxGPIO; + class LinuxDigitalSource; + class LinuxRCInput; + class LinuxRCOutput; + class LinuxSemaphore; + class LinuxScheduler; + class LinuxUtil; +} + +#endif // __AP_HAL_LINUX_NAMESPACE_H__ + diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h new file mode 100644 index 0000000000..7adcdf5937 --- /dev/null +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h @@ -0,0 +1,23 @@ + +#ifndef __AP_HAL_LINUX_PRIVATE_H__ +#define __AP_HAL_LINUX_PRIVATE_H__ + +/* Umbrella header for all private headers of the AP_HAL_Linux module. + * Only import this header from inside AP_HAL_Linux + */ + +#include "UARTDriver.h" +#include "I2CDriver.h" +#include "SPIDriver.h" +#include "AnalogIn.h" +#include "Storage.h" +#include "Console.h" +#include "GPIO.h" +#include "RCInput.h" +#include "RCOutput.h" +#include "Semaphores.h" +#include "Scheduler.h" +#include "Util.h" + +#endif // __AP_HAL_LINUX_PRIVATE_H__ + diff --git a/libraries/AP_HAL_Linux/AnalogIn.cpp b/libraries/AP_HAL_Linux/AnalogIn.cpp new file mode 100644 index 0000000000..ff9fdcdc28 --- /dev/null +++ b/libraries/AP_HAL_Linux/AnalogIn.cpp @@ -0,0 +1,44 @@ +#include "AnalogIn.h" + +using namespace Linux; + +LinuxAnalogSource::LinuxAnalogSource(float v) : + _v(v) +{} + +float LinuxAnalogSource::read_average() { + return _v; +} + +float LinuxAnalogSource::voltage_average() { + return 5.0 * _v / 1024.0; +} + +float LinuxAnalogSource::voltage_latest() { + return 5.0 * _v / 1024.0; +} + +float LinuxAnalogSource::read_latest() { + return _v; +} + +void LinuxAnalogSource::set_pin(uint8_t p) +{} + +void LinuxAnalogSource::set_stop_pin(uint8_t p) +{} + +void LinuxAnalogSource::set_settle_time(uint16_t settle_time_ms) +{} + +LinuxAnalogIn::LinuxAnalogIn() +{} + +void LinuxAnalogIn::init(void* machtnichts) +{} + +AP_HAL::AnalogSource* LinuxAnalogIn::channel(int16_t n) { + return new LinuxAnalogSource(1.11); +} + + diff --git a/libraries/AP_HAL_Linux/AnalogIn.h b/libraries/AP_HAL_Linux/AnalogIn.h new file mode 100644 index 0000000000..6645417676 --- /dev/null +++ b/libraries/AP_HAL_Linux/AnalogIn.h @@ -0,0 +1,28 @@ + +#ifndef __AP_HAL_LINUX_ANALOGIN_H__ +#define __AP_HAL_LINUX_ANALOGIN_H__ + +#include + +class Linux::LinuxAnalogSource : public AP_HAL::AnalogSource { +public: + LinuxAnalogSource(float v); + float read_average(); + float read_latest(); + void set_pin(uint8_t p); + void set_stop_pin(uint8_t p); + void set_settle_time(uint16_t settle_time_ms); + float voltage_average(); + float voltage_latest(); + float voltage_average_ratiometric() { return voltage_average(); } +private: + float _v; +}; + +class Linux::LinuxAnalogIn : public AP_HAL::AnalogIn { +public: + LinuxAnalogIn(); + void init(void* implspecific); + AP_HAL::AnalogSource* channel(int16_t n); +}; +#endif // __AP_HAL_LINUX_ANALOGIN_H__ diff --git a/libraries/AP_HAL_Linux/Console.cpp b/libraries/AP_HAL_Linux/Console.cpp new file mode 100644 index 0000000000..1b7c32c41c --- /dev/null +++ b/libraries/AP_HAL_Linux/Console.cpp @@ -0,0 +1,42 @@ +#include +#include "Console.h" + +using namespace Linux; + +LinuxConsoleDriver::LinuxConsoleDriver(AP_HAL::BetterStream* delegate) : + _d(delegate) +{} + +void LinuxConsoleDriver::init(void* machtnichts) +{} + +void LinuxConsoleDriver::backend_open() +{} + +void LinuxConsoleDriver::backend_close() +{} + +size_t LinuxConsoleDriver::backend_read(uint8_t *data, size_t len) { + return 0; +} + +size_t LinuxConsoleDriver::backend_write(const uint8_t *data, size_t len) { + return 0; +} + +int16_t LinuxConsoleDriver::available() { + return _d->available(); +} + +int16_t LinuxConsoleDriver::txspace() { + return _d->txspace(); +} + +int16_t LinuxConsoleDriver::read() { + return _d->read(); +} + +size_t LinuxConsoleDriver::write(uint8_t c) { + return _d->write(c); +} + diff --git a/libraries/AP_HAL_Linux/Console.h b/libraries/AP_HAL_Linux/Console.h new file mode 100644 index 0000000000..d4e1ef4822 --- /dev/null +++ b/libraries/AP_HAL_Linux/Console.h @@ -0,0 +1,25 @@ + +#ifndef __AP_HAL_LINUX_CONSOLE_H__ +#define __AP_HAL_LINUX_CONSOLE_H__ + +#include + +class Linux::LinuxConsoleDriver : public AP_HAL::ConsoleDriver { +public: + LinuxConsoleDriver(AP_HAL::BetterStream* delegate); + void init(void* machtnichts); + void backend_open(); + void backend_close(); + size_t backend_read(uint8_t *data, size_t len); + size_t backend_write(const uint8_t *data, size_t len); + + int16_t available(); + int16_t txspace(); + int16_t read(); + + size_t write(uint8_t c); +private: + AP_HAL::BetterStream *_d; +}; + +#endif // __AP_HAL_LINUX_CONSOLE_H__ diff --git a/libraries/AP_HAL_Linux/GPIO.cpp b/libraries/AP_HAL_Linux/GPIO.cpp new file mode 100644 index 0000000000..5025c31c7c --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO.cpp @@ -0,0 +1,64 @@ + +#include "GPIO.h" + +using namespace Linux; + +LinuxGPIO::LinuxGPIO() +{} + +void LinuxGPIO::init() +{} + +void LinuxGPIO::pinMode(uint8_t pin, uint8_t output) +{} + +int8_t LinuxGPIO::analogPinToDigitalPin(uint8_t pin) +{ + return -1; +} + + +uint8_t LinuxGPIO::read(uint8_t pin) { + return 0; +} + +void LinuxGPIO::write(uint8_t pin, uint8_t value) +{} + +void LinuxGPIO::toggle(uint8_t pin) +{} + +/* Alternative interface: */ +AP_HAL::DigitalSource* LinuxGPIO::channel(uint16_t n) { + return new LinuxDigitalSource(0); +} + +/* Interrupt interface: */ +bool LinuxGPIO::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, + uint8_t mode) { + return true; +} + +bool LinuxGPIO::usb_connected(void) +{ + return false; +} + +LinuxDigitalSource::LinuxDigitalSource(uint8_t v) : + _v(v) +{} + +void LinuxDigitalSource::mode(uint8_t output) +{} + +uint8_t LinuxDigitalSource::read() { + return _v; +} + +void LinuxDigitalSource::write(uint8_t value) { + _v = value; +} + +void LinuxDigitalSource::toggle() { + _v = !_v; +} diff --git a/libraries/AP_HAL_Linux/GPIO.h b/libraries/AP_HAL_Linux/GPIO.h new file mode 100644 index 0000000000..82023d8c50 --- /dev/null +++ b/libraries/AP_HAL_Linux/GPIO.h @@ -0,0 +1,39 @@ + +#ifndef __AP_HAL_LINUX_GPIO_H__ +#define __AP_HAL_LINUX_GPIO_H__ + +#include + +class Linux::LinuxGPIO : public AP_HAL::GPIO { +public: + LinuxGPIO(); + void init(); + void pinMode(uint8_t pin, uint8_t output); + int8_t analogPinToDigitalPin(uint8_t pin); + uint8_t read(uint8_t pin); + void write(uint8_t pin, uint8_t value); + void toggle(uint8_t pin); + + /* Alternative interface: */ + AP_HAL::DigitalSource* channel(uint16_t n); + + /* Interrupt interface: */ + bool attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, + uint8_t mode); + + /* return true if USB cable is connected */ + bool usb_connected(void); +}; + +class Linux::LinuxDigitalSource : public AP_HAL::DigitalSource { +public: + LinuxDigitalSource(uint8_t v); + void mode(uint8_t output); + uint8_t read(); + void write(uint8_t value); + void toggle(); +private: + uint8_t _v; +}; + +#endif // __AP_HAL_LINUX_GPIO_H__ diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp new file mode 100644 index 0000000000..0e4200bab6 --- /dev/null +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -0,0 +1,83 @@ +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + +#include "HAL_Linux_Class.h" +#include "AP_HAL_Linux_Private.h" + +#include +#include + +#include +#include +#include +#include + +using namespace Linux; + +// only using 2 serial ports on Linux for now +static LinuxUARTDriver uartADriver; +static LinuxUARTDriver uartBDriver; +static Empty::EmptyUARTDriver uartCDriver; + +static LinuxSemaphore i2cSemaphore; +static LinuxI2CDriver i2cDriver(&i2cSemaphore); +static Empty::EmptySPIDeviceManager spiDeviceManager; +static LinuxAnalogIn analogIn; +static LinuxStorage storageDriver; +static LinuxConsoleDriver consoleDriver(&uartADriver); +static LinuxGPIO gpioDriver; +static LinuxRCInput rcinDriver; +static LinuxRCOutput rcoutDriver; +static LinuxScheduler schedulerInstance; +static LinuxUtil utilInstance; + +HAL_Linux::HAL_Linux() : + AP_HAL::HAL( + &uartADriver, + &uartBDriver, + &uartCDriver, + &i2cDriver, + &spiDeviceManager, + &analogIn, + &storageDriver, + &consoleDriver, + &gpioDriver, + &rcinDriver, + &rcoutDriver, + &schedulerInstance, + &utilInstance) +{} + +void HAL_Linux::init(int argc,char* const argv[]) const +{ + int opt; + /* + parse command line options + */ + while ((opt = getopt(argc, argv, "A:B:h")) != -1) { + switch (opt) { + case 'A': + uartADriver.set_device_path(optarg); + break; + case 'B': + uartBDriver.set_device_path(optarg); + break; + case 'h': + printf("Usage: -A uartAPath -B uartAPath\n"); + exit(0); + default: + printf("Unknown option '%c'\n", (char)opt); + exit(1); + } + } + + /* initialize all drivers and private members here. + * up to the programmer to do this in the correct order. + * Scheduler should likely come first. */ + scheduler->init(NULL); + uartA->begin(115200); +} + +const HAL_Linux AP_HAL_Linux; + +#endif diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.h b/libraries/AP_HAL_Linux/HAL_Linux_Class.h new file mode 100644 index 0000000000..12a4474124 --- /dev/null +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.h @@ -0,0 +1,18 @@ + +#ifndef __AP_HAL_LINUX_CLASS_H__ +#define __AP_HAL_LINUX_CLASS_H__ + +#include + +#include "AP_HAL_Linux_Namespace.h" + +class HAL_Linux : public AP_HAL::HAL { +public: + HAL_Linux(); + void init(int argc, char * const * argv) const; +}; + +extern const HAL_Linux AP_HAL_Linux; + +#endif // __AP_HAL_LINUX_CLASS_H__ + diff --git a/libraries/AP_HAL_Linux/I2CDriver.cpp b/libraries/AP_HAL_Linux/I2CDriver.cpp new file mode 100644 index 0000000000..f2822a1c7b --- /dev/null +++ b/libraries/AP_HAL_Linux/I2CDriver.cpp @@ -0,0 +1,28 @@ + +#include +#include "I2CDriver.h" + +using namespace Linux; + +void LinuxI2CDriver::begin() {} +void LinuxI2CDriver::end() {} +void LinuxI2CDriver::setTimeout(uint16_t ms) {} +void LinuxI2CDriver::setHighSpeed(bool active) {} + +uint8_t LinuxI2CDriver::write(uint8_t addr, uint8_t len, uint8_t* data) +{return 0;} +uint8_t LinuxI2CDriver::writeRegister(uint8_t addr, uint8_t reg, uint8_t val) +{return 0;} +uint8_t LinuxI2CDriver::writeRegisters(uint8_t addr, uint8_t reg, + uint8_t len, uint8_t* data) +{return 0;} + +uint8_t LinuxI2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data) +{return 0;} +uint8_t LinuxI2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data) +{return 0;} +uint8_t LinuxI2CDriver::readRegisters(uint8_t addr, uint8_t reg, + uint8_t len, uint8_t* data) +{return 0;} + +uint8_t LinuxI2CDriver::lockup_count() {return 0;} diff --git a/libraries/AP_HAL_Linux/I2CDriver.h b/libraries/AP_HAL_Linux/I2CDriver.h new file mode 100644 index 0000000000..2d13668afe --- /dev/null +++ b/libraries/AP_HAL_Linux/I2CDriver.h @@ -0,0 +1,41 @@ + +#ifndef __AP_HAL_LINUX_I2CDRIVER_H__ +#define __AP_HAL_LINUX_I2CDRIVER_H__ + +#include + +class Linux::LinuxI2CDriver : public AP_HAL::I2CDriver { +public: + LinuxI2CDriver(AP_HAL::Semaphore* semaphore) : _semaphore(semaphore) {} + void begin(); + void end(); + void setTimeout(uint16_t ms); + void setHighSpeed(bool active); + + /* write: for i2c devices which do not obey register conventions */ + uint8_t write(uint8_t addr, uint8_t len, uint8_t* data); + /* writeRegister: write a single 8-bit value to a register */ + uint8_t writeRegister(uint8_t addr, uint8_t reg, uint8_t val); + /* writeRegisters: write bytes to contigious registers */ + uint8_t writeRegisters(uint8_t addr, uint8_t reg, + uint8_t len, uint8_t* data); + + /* read: for i2c devices which do not obey register conventions */ + uint8_t read(uint8_t addr, uint8_t len, uint8_t* data); + /* readRegister: read from a device register - writes the register, + * then reads back an 8-bit value. */ + uint8_t readRegister(uint8_t addr, uint8_t reg, uint8_t* data); + /* readRegister: read contigious device registers - writes the first + * register, then reads back multiple bytes */ + uint8_t readRegisters(uint8_t addr, uint8_t reg, + uint8_t len, uint8_t* data); + + uint8_t lockup_count(); + + AP_HAL::Semaphore* get_semaphore() { return _semaphore; } + +private: + AP_HAL::Semaphore* _semaphore; +}; + +#endif // __AP_HAL_LINUX_I2CDRIVER_H__ diff --git a/libraries/AP_HAL_Linux/RCInput.cpp b/libraries/AP_HAL_Linux/RCInput.cpp new file mode 100644 index 0000000000..3603a27d59 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCInput.cpp @@ -0,0 +1,38 @@ + +#include "RCInput.h" + +using namespace Linux; +LinuxRCInput::LinuxRCInput() +{} + +void LinuxRCInput::init(void* machtnichts) +{} + +uint8_t LinuxRCInput::valid_channels() { + return 0; +} + +uint16_t LinuxRCInput::read(uint8_t ch) { + if (ch == 2) return 900; /* throttle should be low, for safety */ + else return 1500; +} + +uint8_t LinuxRCInput::read(uint16_t* periods, uint8_t len) { + for (uint8_t i = 0; i < len; i++){ + if (i == 2) periods[i] = 900; + else periods[i] = 1500; + } + return len; +} + +bool LinuxRCInput::set_overrides(int16_t *overrides, uint8_t len) { + return true; +} + +bool LinuxRCInput::set_override(uint8_t channel, int16_t override) { + return true; +} + +void LinuxRCInput::clear_overrides() +{} + diff --git a/libraries/AP_HAL_Linux/RCInput.h b/libraries/AP_HAL_Linux/RCInput.h new file mode 100644 index 0000000000..2b25f6e4e6 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCInput.h @@ -0,0 +1,20 @@ + +#ifndef __AP_HAL_LINUX_RCINPUT_H__ +#define __AP_HAL_LINUX_RCINPUT_H__ + +#include + +class Linux::LinuxRCInput : public AP_HAL::RCInput { +public: + LinuxRCInput(); + void init(void* machtnichts); + uint8_t valid_channels(); + 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(); +}; + +#endif // __AP_HAL_LINUX_RCINPUT_H__ diff --git a/libraries/AP_HAL_Linux/RCOutput.cpp b/libraries/AP_HAL_Linux/RCOutput.cpp new file mode 100644 index 0000000000..0bda3d17e9 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCOutput.cpp @@ -0,0 +1,38 @@ + +#include "RCOutput.h" + +using namespace Linux; + +void LinuxRCOutput::init(void* machtnichts) {} + +void LinuxRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) {} + +uint16_t LinuxRCOutput::get_freq(uint8_t ch) { + return 50; +} + +void LinuxRCOutput::enable_ch(uint8_t ch) +{} + +void LinuxRCOutput::enable_mask(uint32_t chmask) +{} + +void LinuxRCOutput::disable_ch(uint8_t ch) +{} + +void LinuxRCOutput::disable_mask(uint32_t chmask) +{} + +void LinuxRCOutput::write(uint8_t ch, uint16_t period_us) +{} + +void LinuxRCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len) +{} + +uint16_t LinuxRCOutput::read(uint8_t ch) { + return 900; +} + +void LinuxRCOutput::read(uint16_t* period_us, uint8_t len) +{} + diff --git a/libraries/AP_HAL_Linux/RCOutput.h b/libraries/AP_HAL_Linux/RCOutput.h new file mode 100644 index 0000000000..84e1524826 --- /dev/null +++ b/libraries/AP_HAL_Linux/RCOutput.h @@ -0,0 +1,21 @@ + +#ifndef __AP_HAL_LINUX_RCOUTPUT_H__ +#define __AP_HAL_LINUX_RCOUTPUT_H__ + +#include + +class Linux::LinuxRCOutput : public AP_HAL::RCOutput { + 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); +}; + +#endif // __AP_HAL_LINUX_RCOUTPUT_H__ diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp new file mode 100644 index 0000000000..e7e953a00d --- /dev/null +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -0,0 +1,90 @@ + +#include "Scheduler.h" +#include +#include + +using namespace Linux; + +extern const AP_HAL::HAL& hal; + +LinuxScheduler::LinuxScheduler() +{} + +void LinuxScheduler::init(void* machtnichts) +{ + gettimeofday(&_sketch_start_time, NULL); +} + +void LinuxScheduler::delay(uint16_t ms) +{ + usleep(ms * 1000); +} + +uint32_t LinuxScheduler::millis() +{ + struct timeval tp; + gettimeofday(&tp,NULL); + return 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - + (_sketch_start_time.tv_sec + + (_sketch_start_time.tv_usec*1.0e-6))); +} + +uint32_t LinuxScheduler::micros() +{ + struct timeval tp; + gettimeofday(&tp,NULL); + return 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - + (_sketch_start_time.tv_sec + + (_sketch_start_time.tv_usec*1.0e-6))); +} + +void LinuxScheduler::delay_microseconds(uint16_t us) +{ + usleep(us); +} + +void LinuxScheduler::register_delay_callback(AP_HAL::Proc k, + uint16_t min_time_ms) +{} + +void LinuxScheduler::register_timer_process(AP_HAL::TimedProc k) +{} + +void LinuxScheduler::register_io_process(AP_HAL::TimedProc k) +{} + +void LinuxScheduler::register_timer_failsafe(AP_HAL::TimedProc, + uint32_t period_us) +{} + +void LinuxScheduler::suspend_timer_procs() +{} + +void LinuxScheduler::resume_timer_procs() +{} + +bool LinuxScheduler::in_timerprocess() { + return false; +} + +void LinuxScheduler::begin_atomic() +{} + +void LinuxScheduler::end_atomic() +{} + +bool LinuxScheduler::system_initializing() { + return false; +} + +void LinuxScheduler::system_initialized() +{} + +void LinuxScheduler::panic(const prog_char_t *errormsg) { + hal.console->println_P(errormsg); + for(;;); +} + +void LinuxScheduler::reboot(bool hold_in_bootloader) { + for(;;); +} diff --git a/libraries/AP_HAL_Linux/Scheduler.h b/libraries/AP_HAL_Linux/Scheduler.h new file mode 100644 index 0000000000..b9eda348b2 --- /dev/null +++ b/libraries/AP_HAL_Linux/Scheduler.h @@ -0,0 +1,42 @@ + +#ifndef __AP_HAL_LINUX_SCHEDULER_H__ +#define __AP_HAL_LINUX_SCHEDULER_H__ + +#include +#include + +class Linux::LinuxScheduler : public AP_HAL::Scheduler { +public: + LinuxScheduler(); + void init(void* machtnichts); + void delay(uint16_t ms); + uint32_t millis(); + uint32_t micros(); + void delay_microseconds(uint16_t us); + void register_delay_callback(AP_HAL::Proc, + uint16_t min_time_ms); + + void register_timer_process(AP_HAL::TimedProc); + void register_io_process(AP_HAL::TimedProc); + void suspend_timer_procs(); + void resume_timer_procs(); + + bool in_timerprocess(); + + void register_timer_failsafe(AP_HAL::TimedProc, + uint32_t period_us); + + void begin_atomic(); + void end_atomic(); + + bool system_initializing(); + void system_initialized(); + + void panic(const prog_char_t *errormsg); + void reboot(bool hold_in_bootloader); + +private: + struct timeval _sketch_start_time; +}; + +#endif // __AP_HAL_LINUX_SCHEDULER_H__ diff --git a/libraries/AP_HAL_Linux/Semaphores.cpp b/libraries/AP_HAL_Linux/Semaphores.cpp new file mode 100644 index 0000000000..9d2abb8fd1 --- /dev/null +++ b/libraries/AP_HAL_Linux/Semaphores.cpp @@ -0,0 +1,27 @@ + +#include "Semaphores.h" + +using namespace Linux; + +bool LinuxSemaphore::give() { + if (_taken) { + _taken = false; + return true; + } else { + return false; + } +} + +bool LinuxSemaphore::take(uint32_t timeout_ms) { + return take_nonblocking(); +} + +bool LinuxSemaphore::take_nonblocking() { + /* No syncronisation primitives to garuntee this is correct */ + if (!_taken) { + _taken = true; + return true; + } else { + return false; + } +} diff --git a/libraries/AP_HAL_Linux/Semaphores.h b/libraries/AP_HAL_Linux/Semaphores.h new file mode 100644 index 0000000000..bf1c3a2e88 --- /dev/null +++ b/libraries/AP_HAL_Linux/Semaphores.h @@ -0,0 +1,17 @@ + +#ifndef __AP_HAL_LINUX_SEMAPHORE_H__ +#define __AP_HAL_LINUX_SEMAPHORE_H__ + +#include + +class Linux::LinuxSemaphore : public AP_HAL::Semaphore { +public: + LinuxSemaphore() : _taken(false) {} + bool give(); + bool take(uint32_t timeout_ms); + bool take_nonblocking(); +private: + bool _taken; +}; + +#endif // __AP_HAL_LINUX_SEMAPHORE_H__ diff --git a/libraries/AP_HAL_Linux/Storage.cpp b/libraries/AP_HAL_Linux/Storage.cpp new file mode 100644 index 0000000000..1c4d06eb97 --- /dev/null +++ b/libraries/AP_HAL_Linux/Storage.cpp @@ -0,0 +1,40 @@ + +#include +#include "Storage.h" + +using namespace Linux; + +LinuxStorage::LinuxStorage() +{} + +void LinuxStorage::init(void*) +{} + +uint8_t LinuxStorage::read_byte(uint16_t loc){ + return 0; +} + +uint16_t LinuxStorage::read_word(uint16_t loc){ + return 0; +} + +uint32_t LinuxStorage::read_dword(uint16_t loc){ + return 0; +} + +void LinuxStorage::read_block(void* dst, uint16_t src, size_t n) { + memset(dst, 0, n); +} + +void LinuxStorage::write_byte(uint16_t loc, uint8_t value) +{} + +void LinuxStorage::write_word(uint16_t loc, uint16_t value) +{} + +void LinuxStorage::write_dword(uint16_t loc, uint32_t value) +{} + +void LinuxStorage::write_block(uint16_t loc, const void* src, size_t n) +{} + diff --git a/libraries/AP_HAL_Linux/Storage.h b/libraries/AP_HAL_Linux/Storage.h new file mode 100644 index 0000000000..90c2c16547 --- /dev/null +++ b/libraries/AP_HAL_Linux/Storage.h @@ -0,0 +1,22 @@ + +#ifndef __AP_HAL_LINUX_STORAGE_H__ +#define __AP_HAL_LINUX_STORAGE_H__ + +#include + +class Linux::LinuxStorage : public AP_HAL::Storage { +public: + LinuxStorage(); + void init(void *); + uint8_t read_byte(uint16_t loc); + uint16_t read_word(uint16_t loc); + uint32_t read_dword(uint16_t loc); + void read_block(void *dst, uint16_t src, size_t n); + + void write_byte(uint16_t loc, uint8_t value); + void write_word(uint16_t loc, uint16_t value); + void write_dword(uint16_t loc, uint32_t value); + void write_block(uint16_t dst, const void* src, size_t n); +}; + +#endif // __AP_HAL_LINUX_STORAGE_H__ diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp new file mode 100644 index 0000000000..8bb79d3d28 --- /dev/null +++ b/libraries/AP_HAL_Linux/UARTDriver.cpp @@ -0,0 +1,174 @@ +#include "UARTDriver.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Linux; + +LinuxUARTDriver::LinuxUARTDriver() : + device_path(NULL), + _fd(-1) +{ +} + +/* + set the tty device to use for this UART + */ +void LinuxUARTDriver::set_device_path(const char *path) +{ + device_path = path; +} + +/* + open the tty + */ +void LinuxUARTDriver::begin(uint32_t b) +{ + if (device_path == NULL) { + return; + } + + if (_fd == -1) { + _fd = open(device_path, O_RDWR); + if (_fd == -1) { + ::printf("UARTDriver: Failed to open %s - %s\n", + device_path, + strerror(errno)); + return; + } + } + + /* if baudrate has been specified, then set the baudrate */ + if (b != 0) { + struct termios t; + tcgetattr(_fd, &t); + cfsetspeed(&t, b); + + // disable LF -> CR/LF + t.c_oflag &= ~ONLCR; + tcsetattr(_fd, TCSANOW, &t); + } +} + +void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) +{ + // ignore buffer sizes for now + begin(b); +} + +/* + shutdown a UART + */ +void LinuxUARTDriver::end() +{ + if (_fd != -1) { + close(_fd); + _fd = -1; + } +} + + +void LinuxUARTDriver::flush() +{ + // we are not doing any buffering, so flush is a no-op +} + + +/* + return true if the UART is initialised + */ +bool LinuxUARTDriver::is_initialized() +{ + return (_fd != -1); +} + + +/* + enable or disable blocking writes + */ +void LinuxUARTDriver::set_blocking_writes(bool blocking) +{ + unsigned v; + if (_fd == -1) { + return; + } + + v = fcntl(_fd, F_GETFL, 0); + + if (blocking) { + v &= ~O_NONBLOCK; + } else { + v |= O_NONBLOCK; + } + + fcntl(_fd, F_SETFL, v); +} + +/* + do we have any bytes pending transmission? + */ +bool LinuxUARTDriver::tx_pending() +{ + // no buffering, so always false + return false; +} + + +/* + return the number of bytes available to be read + */ +int16_t LinuxUARTDriver::available() +{ + int nread; + if (_fd == -1) { + return 0; + } + + nread = 0; + if (ioctl(_fd, FIONREAD, (unsigned long)&nread) == 0) { + return nread; + } + // ioctl failed?? + return 0; +} + +/* + how many bytes are available in the output buffer? + */ +int16_t LinuxUARTDriver::txspace() +{ + // for now lie and say we always have 128, we will need a ring + // buffer later and a IO thread + return 128; +} + +int16_t LinuxUARTDriver::read() +{ + char c; + if (_fd == -1) { + return -1; + } + if (::read(_fd, &c, 1) == 1) { + return (int16_t)c; + } + return -1; +} + +/* Linux implementations of Print virtual methods */ +size_t LinuxUARTDriver::write(uint8_t c) +{ + if (_fd == -1) { + return 0; + } + if (::write(_fd, &c, 1) == 1) { + return 1; + } + return 0; +} diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h new file mode 100644 index 0000000000..271adba223 --- /dev/null +++ b/libraries/AP_HAL_Linux/UARTDriver.h @@ -0,0 +1,34 @@ + +#ifndef __AP_HAL_LINUX_UARTDRIVER_H__ +#define __AP_HAL_LINUX_UARTDRIVER_H__ + +#include + +class Linux::LinuxUARTDriver : public AP_HAL::UARTDriver { +public: + LinuxUARTDriver(); + /* Linux implementations of UARTDriver virtual methods */ + void begin(uint32_t b); + void begin(uint32_t b, uint16_t rxS, uint16_t txS); + void end(); + void flush(); + bool is_initialized(); + void set_blocking_writes(bool blocking); + bool tx_pending(); + + /* Linux implementations of Stream virtual methods */ + int16_t available(); + int16_t txspace(); + int16_t read(); + + /* Linux implementations of Print virtual methods */ + size_t write(uint8_t c); + + void set_device_path(const char *path); + +private: + const char *device_path; + int _fd; +}; + +#endif // __AP_HAL_LINUX_UARTDRIVER_H__ diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h new file mode 100644 index 0000000000..7c97bb2918 --- /dev/null +++ b/libraries/AP_HAL_Linux/Util.h @@ -0,0 +1,13 @@ + +#ifndef __AP_HAL_LINUX_UTIL_H__ +#define __AP_HAL_LINUX_UTIL_H__ + +#include +#include "AP_HAL_Linux_Namespace.h" + +class Linux::LinuxUtil : public AP_HAL::Util { +public: + bool run_debug_shell(AP_HAL::BetterStream *stream) { return false; } +}; + +#endif // __AP_HAL_LINUX_UTIL_H__