diff --git a/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM.h b/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM.h deleted file mode 100644 index 799cc6d723..0000000000 --- a/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM.h +++ /dev/null @@ -1,33 +0,0 @@ - -#ifndef __AP_HAL_SMACCM_H__ -#define __AP_HAL_SMACCM_H__ - -/* Your layer exports should depend on AP_HAL.h ONLY. */ -#include - -/** - * Umbrella header for AP_HAL_SMACCM 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 SMACCM namespace. - * The class implementing AP_HAL::HAL should be called HAL_SMACCM and exist - * in the global namespace. There should be a single const instance of the - * HAL_SMACCM class called AP_HAL_SMACCM, instantiated in the HAL_SMACCM_Class.cpp - * and exported as `extern const HAL_SMACCM AP_HAL_SMACCM;` in HAL_SMACCM_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_SMACCM. - * 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_SMACCM_Class.h" -#include "AP_HAL_SMACCM_Main.h" - -#endif //__AP_HAL_SMACCM_H__ diff --git a/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Main.cpp b/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Main.cpp deleted file mode 100644 index 2af5aa3bc3..0000000000 --- a/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Main.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */ -/* - * AP_HAL_SMACCM_Main.cpp --- AP_HAL_SMACCM main task implementation. - * - * Copyright (C) 2012, Galois, Inc. - * All Rights Reserved. - * - * This software is released under the "BSD3" license. Read the file - * "LICENSE" for more information. - * - * Written by James Bielman , 20 December 2012 - */ - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM - -#include -#include -#include -#include - -static xTaskHandle g_main_task; - -// These must be defined in the main ".pde" file. -extern const AP_HAL::HAL& hal; -extern void setup(); -extern void loop(); - -static void main_task(void *arg) -{ - hal.init(0, NULL); - setup(); - hal.scheduler->system_initialized(); - - for (;;) - loop(); -} - -void SMACCM::hal_main() -{ - xTaskCreate(main_task, (signed char *)"main", 1024, NULL, 0, &g_main_task); - vTaskStartScheduler(); - - for (;;) - ; -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_SMACCM diff --git a/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Main.h b/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Main.h deleted file mode 100644 index c3f15f2f10..0000000000 --- a/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Main.h +++ /dev/null @@ -1,22 +0,0 @@ - - -#ifndef __AP_HAL_SMACCM_MAIN_H__ -#define __AP_HAL_SMACCM_MAIN_H__ - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM - -namespace SMACCM -{ - void hal_main(); -} - -#define AP_HAL_MAIN() \ - extern "C" int main (void) \ - { \ - SMACCM::hal_main(); \ - return 0; \ - } - -#endif // HAL_BOARD_SMACCM */ - -#endif // __AP_HAL_SMACCM_MAIN_H__ diff --git a/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Namespace.h b/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Namespace.h deleted file mode 100644 index c342bf9d55..0000000000 --- a/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Namespace.h +++ /dev/null @@ -1,30 +0,0 @@ - -#ifndef __AP_HAL_SMACCM_NAMESPACE_H__ -#define __AP_HAL_SMACCM_NAMESPACE_H__ - -/* While not strictly required, names inside the SMACCM namespace are prefixed - * with SMACCM for clarity. (Some of our users aren't familiar with all of the - * C++ namespace rules.) - */ - -namespace SMACCM { - class SMACCMUARTDriver; - class SMACCMI2CDriver; - class SMACCMSPIDeviceManager; - class SMACCMSPIDeviceDriver; - class SMACCMAnalogSource; - class SMACCMAnalogIn; - class SMACCMStorage; - class SMACCMConsoleDriver; - class SMACCMGPIO; - class SMACCMDigitalSource; - class SMACCMRCInput; - class SMACCMRCOutput; - class SMACCMSemaphore; - class SMACCMScheduler; - class SMACCMUtil; - class SMACCMPrivateMember; -} - -#endif // __AP_HAL_SMACCM_NAMESPACE_H__ - diff --git a/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Private.h b/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Private.h deleted file mode 100644 index 23e8aca1f6..0000000000 --- a/libraries/AP_HAL_SMACCM/AP_HAL_SMACCM_Private.h +++ /dev/null @@ -1,24 +0,0 @@ - -#ifndef __AP_HAL_SMACCM_PRIVATE_H__ -#define __AP_HAL_SMACCM_PRIVATE_H__ - -/* Umbrella header for all private headers of the AP_HAL_SMACCM module. - * Only import this header from inside AP_HAL_SMACCM - */ - -#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" -#include "PrivateMember.h" - -#endif // __AP_HAL_SMACCM_PRIVATE_H__ - diff --git a/libraries/AP_HAL_SMACCM/AnalogIn.cpp b/libraries/AP_HAL_SMACCM/AnalogIn.cpp deleted file mode 100644 index bcab72dd2d..0000000000 --- a/libraries/AP_HAL_SMACCM/AnalogIn.cpp +++ /dev/null @@ -1,41 +0,0 @@ - -#include "AnalogIn.h" - -using namespace SMACCM; - -SMACCMAnalogSource::SMACCMAnalogSource(float v) : - _v(v) -{} - -float SMACCMAnalogSource::read_average() { - return _v; -} - -float SMACCMAnalogSource::voltage_average() { - // this assumes 5.0V scaling and 1024 range - return (5.0/1024.0) * read_average(); -} - -float SMACCMAnalogSource::voltage_latest() { - // this assumes 5.0V scaling and 1024 range - return (5.0/1024.0) * read_latest(); -} - -float SMACCMAnalogSource::read_latest() { - return _v; -} - -void SMACCMAnalogSource::set_pin(uint8_t p) -{} - - -SMACCMAnalogIn::SMACCMAnalogIn() -{} - -void SMACCMAnalogIn::init(void* machtnichts) -{} - -AP_HAL::AnalogSource* SMACCMAnalogIn::channel(int16_t n) { - return new SMACCMAnalogSource(1.11); -} - diff --git a/libraries/AP_HAL_SMACCM/AnalogIn.h b/libraries/AP_HAL_SMACCM/AnalogIn.h deleted file mode 100644 index 0bd79290dc..0000000000 --- a/libraries/AP_HAL_SMACCM/AnalogIn.h +++ /dev/null @@ -1,29 +0,0 @@ - -#ifndef __AP_HAL_SMACCM_ANALOGIN_H__ -#define __AP_HAL_SMACCM_ANALOGIN_H__ - -#include - -class SMACCM::SMACCMAnalogSource : public AP_HAL::AnalogSource { -public: - SMACCMAnalogSource(float v); - float read_average(); - float read_latest(); - void set_pin(uint8_t p); - float voltage_average(); - float voltage_latest(); - float voltage_average_ratiometric() { return voltage_average(); } - void set_stop_pin(uint8_t p) {} - void set_settle_time(uint16_t settle_time_ms) {} - -private: - float _v; -}; - -class SMACCM::SMACCMAnalogIn : public AP_HAL::AnalogIn { -public: - SMACCMAnalogIn(); - void init(void* implspecific); - AP_HAL::AnalogSource* channel(int16_t n); -}; -#endif // __AP_HAL_SMACCM_ANALOGIN_H__ diff --git a/libraries/AP_HAL_SMACCM/Console.cpp b/libraries/AP_HAL_SMACCM/Console.cpp deleted file mode 100644 index c7093d9c89..0000000000 --- a/libraries/AP_HAL_SMACCM/Console.cpp +++ /dev/null @@ -1,158 +0,0 @@ -/* - * Console.cpp --- AP_HAL_SMACCM console driver. - * - * Copyright (C) 2012, Galois, Inc. - * All Rights Reserved. - * - * This software is released under the "BSD3" license. Read the file - * "LICENSE" for more information. - */ - -#include -#include - -#include "Console.h" - -using namespace SMACCM; - -SMACCMConsoleDriver::SMACCMConsoleDriver(AP_HAL::BetterStream* delegate) : - _d(delegate), _user_backend(false) -{ -} - -void SMACCMConsoleDriver::init(void *args) -{ -} - -void SMACCMConsoleDriver::backend_open() -{ - _txbuf.allocate(128); - _rxbuf.allocate(16); - _user_backend = true; -} - -void SMACCMConsoleDriver::backend_close() -{ - _user_backend = false; -} - -size_t SMACCMConsoleDriver::backend_read(uint8_t *data, size_t len) -{ - for (size_t i = 0; i < len; i++) { - int16_t b = _txbuf.pop(); - if (b != -1) { - data[i] = (uint8_t) b; - } else { - return i; - } - } - - return len; -} - -size_t SMACCMConsoleDriver::backend_write(const uint8_t *data, size_t len) -{ - for (size_t i = 0; i < len; i++) { - bool valid = _rxbuf.push(data[i]); - if (!valid) { - return i; - } - } - - return len; - } - -int16_t SMACCMConsoleDriver::available() -{ - if (_user_backend) { - return _rxbuf.bytes_used(); - } else { - return _d->available(); - } -} - -int16_t SMACCMConsoleDriver::txspace() -{ - if (_user_backend) { - return _txbuf.bytes_free(); - } else { - return _d->txspace(); - } -} - -int16_t SMACCMConsoleDriver::read() -{ - if (_user_backend) { - return _rxbuf.pop(); - } else { - return _d->read(); - } -} - -size_t SMACCMConsoleDriver::write(uint8_t c) -{ - if (_user_backend) { - return (_txbuf.push(c) ? 1 : 0); - } else { - return _d->write(c); - } -} - -/** - * SMACCMConsoleDriver::Buffer implementation. - * A synchronous nonblocking ring buffer, based on the AVRUARTDriver::Buffer - */ - -bool SMACCMConsoleDriver::Buffer::allocate(uint16_t size) -{ - _head = 0; - _tail = 0; - uint8_t shift; - /* Hardcoded max size of 1024. sue me. */ - for ( shift = 1; - ( 1U << shift ) < 1024 && ( 1U << shift) < size; - shift++ - ) ; - uint16_t tmpmask = (1U << shift) - 1; - - if ( _bytes != NULL ) { - if ( _mask == tmpmask ) { - return true; - } - free(_bytes); - } - _mask = tmpmask; - _bytes = (uint8_t*) malloc(_mask+1); - return (_bytes != NULL); -} - -bool SMACCMConsoleDriver::Buffer::push(uint8_t b) -{ - uint16_t next = (_head + 1) & _mask; - if ( next == _tail ) { - return false; - } - _bytes[_head] = b; - _head = next; - return true; -} - -int16_t SMACCMConsoleDriver::Buffer::pop() -{ - if ( _tail == _head ) { - return -1; - } - uint8_t b = _bytes[_tail]; - _tail = ( _tail + 1 ) & _mask; - return (int16_t) b; -} - -uint16_t SMACCMConsoleDriver::Buffer::bytes_used() -{ - return ((_head - _tail) & _mask); -} - -uint16_t SMACCMConsoleDriver::Buffer::bytes_free() -{ - return ((_mask+1) - ((_head - _tail) & _mask)); -} diff --git a/libraries/AP_HAL_SMACCM/Console.h b/libraries/AP_HAL_SMACCM/Console.h deleted file mode 100644 index 7afd0b10ed..0000000000 --- a/libraries/AP_HAL_SMACCM/Console.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Console.h --- AP_HAL_SMACCM console driver. - * - * Copyright (C) 2012, Galois, Inc. - * All Rights Reserved. - * - * This software is released under the "BSD3" license. Read the file - * "LICENSE" for more information. - */ - -#ifndef __AP_HAL_SMACCM_CONSOLE_H__ -#define __AP_HAL_SMACCM_CONSOLE_H__ - -#include - -class SMACCM::SMACCMConsoleDriver : public AP_HAL::ConsoleDriver { -public: - SMACCMConsoleDriver(AP_HAL::BetterStream* delegate); - void init(void *arg); - 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; - - // Buffer implementation copied from the AVR HAL. - struct Buffer { - /* public methods:*/ - bool allocate(uint16_t size); - bool push(uint8_t b); - int16_t pop(); - - uint16_t bytes_free(); - uint16_t bytes_used(); - private: - uint16_t _head, _tail; /* Head and tail indicies */ - uint16_t _mask; /* Buffer size mask for index wrap */ - uint8_t *_bytes; /* Pointer to allocated buffer */ - }; - - Buffer _txbuf; - Buffer _rxbuf; - bool _user_backend; -}; - -#endif // __AP_HAL_SMACCM_CONSOLE_H__ diff --git a/libraries/AP_HAL_SMACCM/GPIO.cpp b/libraries/AP_HAL_SMACCM/GPIO.cpp deleted file mode 100644 index a452317017..0000000000 --- a/libraries/AP_HAL_SMACCM/GPIO.cpp +++ /dev/null @@ -1,76 +0,0 @@ - -#include "GPIO.h" - -using namespace SMACCM; - -SMACCMGPIO::SMACCMGPIO() -{ -} - -void SMACCMGPIO::init() -{ -} - -void SMACCMGPIO::pinMode(uint8_t pin, uint8_t output) -{ -} - -int8_t SMACCMGPIO::analogPinToDigitalPin(uint8_t pin) -{ - return -1; -} - -uint8_t SMACCMGPIO::read(uint8_t pin) -{ - return 0; -} - -void SMACCMGPIO::write(uint8_t pin, uint8_t value) -{ -} - -void SMACCMGPIO::toggle(uint8_t pin) -{ -} - -bool SMACCMGPIO::usb_connected(void) -{ - return false; -} - -/* Alternative interface: */ -AP_HAL::DigitalSource* SMACCMGPIO::channel(uint16_t n) -{ - return NULL; -} - -/* Interrupt interface: */ -bool SMACCMGPIO::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, - uint8_t mode) -{ - return true; -} - -SMACCMDigitalSource::SMACCMDigitalSource(uint8_t v) : - _v(v) -{ -} - -void SMACCMDigitalSource::mode(uint8_t output) -{ -} - -uint8_t SMACCMDigitalSource::read() -{ - return _v; -} - -void SMACCMDigitalSource::write(uint8_t value) -{ - _v = value; -} - -void SMACCMDigitalSource::toggle() -{ - _v = !_v; -} diff --git a/libraries/AP_HAL_SMACCM/GPIO.h b/libraries/AP_HAL_SMACCM/GPIO.h deleted file mode 100644 index 43eeaaae74..0000000000 --- a/libraries/AP_HAL_SMACCM/GPIO.h +++ /dev/null @@ -1,48 +0,0 @@ - -#ifndef __AP_HAL_SMACCM_GPIO_H__ -#define __AP_HAL_SMACCM_GPIO_H__ - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM -// XXX these are just copied, may not make sense - # define HAL_GPIO_A_LED_PIN 27 - # define HAL_GPIO_B_LED_PIN 26 - # define HAL_GPIO_C_LED_PIN 25 - # define HAL_GPIO_LED_ON LOW - # define HAL_GPIO_LED_OFF HIGH -#endif - -class SMACCM::SMACCMGPIO : public AP_HAL::GPIO { -public: - SMACCMGPIO(); - 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 SMACCM::SMACCMDigitalSource : public AP_HAL::DigitalSource { -public: - SMACCMDigitalSource(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_SMACCM_GPIO_H__ diff --git a/libraries/AP_HAL_SMACCM/HAL_SMACCM_Class.cpp b/libraries/AP_HAL_SMACCM/HAL_SMACCM_Class.cpp deleted file mode 100644 index 55f6401488..0000000000 --- a/libraries/AP_HAL_SMACCM/HAL_SMACCM_Class.cpp +++ /dev/null @@ -1,62 +0,0 @@ - -#include -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM - -#include "HAL_SMACCM_Class.h" -#include "AP_HAL_SMACCM_Private.h" - -using namespace SMACCM; - -// XXX make sure these are assigned correctly -static SMACCMUARTDriver uartADriver(usart1); -static SMACCMUARTDriver uartBDriver(usart6); -static SMACCMUARTDriver uartCDriver(NULL); - -static SMACCMI2CDriver i2cDriver; -static SMACCMSPIDeviceManager spiDeviceManager; -static SMACCMAnalogIn analogIn; -static SMACCMStorage storageDriver; -static SMACCMConsoleDriver consoleDriver(&uartADriver); -static SMACCMGPIO gpioDriver; -static SMACCMRCInput rcinDriver; -static SMACCMRCOutput rcoutDriver; -static SMACCMScheduler schedulerInstance; -static SMACCMUtil utilInstance; - -HAL_SMACCM::HAL_SMACCM() : - AP_HAL::HAL( - &uartADriver, - &uartBDriver, - &uartCDriver, - &i2cDriver, - &spiDeviceManager, - &analogIn, - &storageDriver, - &consoleDriver, - &gpioDriver, - &rcinDriver, - &rcoutDriver, - &schedulerInstance, - &utilInstance) -{ -} - -void HAL_SMACCM::init(int argc,char* const argv[]) const -{ - /* 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); - console->init(uartA); - i2c->begin(); - spi->init(NULL); - storage->init(NULL); - rcin->init(NULL); -} - -const HAL_SMACCM AP_HAL_SMACCM; - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_SMACCM diff --git a/libraries/AP_HAL_SMACCM/HAL_SMACCM_Class.h b/libraries/AP_HAL_SMACCM/HAL_SMACCM_Class.h deleted file mode 100644 index 438958d025..0000000000 --- a/libraries/AP_HAL_SMACCM/HAL_SMACCM_Class.h +++ /dev/null @@ -1,19 +0,0 @@ - -#ifndef __AP_HAL_SMACCM_CLASS_H__ -#define __AP_HAL_SMACCM_CLASS_H__ - -#include - -#include "AP_HAL_SMACCM_Namespace.h" -#include "PrivateMember.h" - -class HAL_SMACCM : public AP_HAL::HAL { -public: - HAL_SMACCM(); - void init(int argc, char * const * argv) const; -}; - -extern const HAL_SMACCM AP_HAL_SMACCM; - -#endif // __AP_HAL_SMACCM_CLASS_H__ - diff --git a/libraries/AP_HAL_SMACCM/I2CDriver.cpp b/libraries/AP_HAL_SMACCM/I2CDriver.cpp deleted file mode 100644 index 89c169abea..0000000000 --- a/libraries/AP_HAL_SMACCM/I2CDriver.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */ -/* - * I2CDriver.cpp --- AP_HAL_SMACCM I2C driver. - * - * Copyright (C) 2012, Galois, Inc. - * All Rights Reserved. - * - * This software is released under the "BSD3" license. Read the file - * "LICENSE" for more information. - * - * Written by James Bielman , 20 December 2012 - */ - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM - -#include -#include -#include - -#include "I2CDriver.h" - -using namespace SMACCM; - -// For now, we are assuming all devices are on a single bus. This -// will need to be refactored at some point to use a device manager -// like SPI if we need support for multiple busses. -#define I2C_BUS i2c2 -#define I2C_SDA pin_b11 -#define I2C_SCL pin_b10 - -void SMACCMI2CDriver::begin() -{ - i2c_init(I2C_BUS, I2C_SDA, I2C_SCL); - _semaphore.init(); -} - -// XXX hwf4 doesn't support de-initialization -void SMACCMI2CDriver::end() -{ -} - -// XXX hwf4 doesn't support non-blocking I2C -void SMACCMI2CDriver::setTimeout(uint16_t ms) -{ -} - -// XXX hwf4 always uses standard speed -void SMACCMI2CDriver::setHighSpeed(bool active) -{ -} - -AP_HAL::Semaphore* SMACCMI2CDriver::get_semaphore() -{ - return &_semaphore; -} - -uint8_t SMACCMI2CDriver::write(uint8_t addr, uint8_t len, uint8_t* data) -{ - return i2c_transfer(I2C_BUS, addr, data, len, NULL, 0) ? 0 : 1; -} - -uint8_t SMACCMI2CDriver::writeRegister(uint8_t addr, uint8_t reg, uint8_t val) -{ - return i2c_write_reg(I2C_BUS, addr, reg, val) ? 0 : 1; -} - -uint8_t SMACCMI2CDriver::writeRegisters(uint8_t addr, uint8_t reg, - uint8_t len, uint8_t* data) -{ - return i2c_write_regs(I2C_BUS, addr, reg, data, len) ? 0 : 1; -} - -uint8_t SMACCMI2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data) -{ - return i2c_transfer(I2C_BUS, addr, NULL, 0, data, len) ? 0 : 1; -} - -uint8_t SMACCMI2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data) -{ - return i2c_transfer(I2C_BUS, addr, ®, 1, data, 1) ? 0 : 1; -} - -uint8_t SMACCMI2CDriver::readRegisters(uint8_t addr, uint8_t reg, - uint8_t len, uint8_t* data) -{ - return i2c_transfer(I2C_BUS, addr, ®, 1, data, len) ? 0 : 1; -} - -uint8_t SMACCMI2CDriver::lockup_count() -{ - return 0; -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_SMACCM diff --git a/libraries/AP_HAL_SMACCM/I2CDriver.h b/libraries/AP_HAL_SMACCM/I2CDriver.h deleted file mode 100644 index a55fd8a916..0000000000 --- a/libraries/AP_HAL_SMACCM/I2CDriver.h +++ /dev/null @@ -1,52 +0,0 @@ -/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */ -/* - * I2CDriver.h --- AP_HAL_SMACCM I2C driver. - * - * Copyright (C) 2012, Galois, Inc. - * All Rights Reserved. - * - * This software is released under the "BSD3" license. Read the file - * "LICENSE" for more information. - * - * Written by James Bielman , 20 December 2012 - */ - -#ifndef __AP_HAL_SMACCM_I2CDRIVER_H__ -#define __AP_HAL_SMACCM_I2CDRIVER_H__ - -#include -#include "Semaphores.h" - -class SMACCM::SMACCMI2CDriver : public AP_HAL::I2CDriver { -public: - 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(); - -private: - SMACCMSemaphore _semaphore; -}; - -#endif // __AP_HAL_SMACCM_I2CDRIVER_H__ diff --git a/libraries/AP_HAL_SMACCM/LICENSE b/libraries/AP_HAL_SMACCM/LICENSE deleted file mode 100644 index 8a2bf59c58..0000000000 --- a/libraries/AP_HAL_SMACCM/LICENSE +++ /dev/null @@ -1,31 +0,0 @@ -You may use this license for the AP_HAL_SMACCM library: - -Copyright (C) 2012, Galois, Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are -met: - -1. Redistributions of source code must retain the above copyright -notice, this list of conditions and the following disclaimer. - -2. Redistributions in binary form must reproduce the above copyright -notice, this list of conditions and the following disclaimer in the -documentation and/or other materials provided with the distribution. - -3. Neither the name of the author nor the names of its contributors -may be used to endorse or promote products derived from this software -without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/libraries/AP_HAL_SMACCM/PrivateMember.cpp b/libraries/AP_HAL_SMACCM/PrivateMember.cpp deleted file mode 100644 index d0fcf67c18..0000000000 --- a/libraries/AP_HAL_SMACCM/PrivateMember.cpp +++ /dev/null @@ -1,11 +0,0 @@ - -#include "PrivateMember.h" - -using namespace SMACCM; - -SMACCMPrivateMember::SMACCMPrivateMember(uint16_t foo) : - _foo(foo) -{} - -void SMACCMPrivateMember::init() {} - diff --git a/libraries/AP_HAL_SMACCM/PrivateMember.h b/libraries/AP_HAL_SMACCM/PrivateMember.h deleted file mode 100644 index 869166c623..0000000000 --- a/libraries/AP_HAL_SMACCM/PrivateMember.h +++ /dev/null @@ -1,19 +0,0 @@ - -#ifndef __AP_HAL_SMACCM_PRIVATE_MEMBER_H__ -#define __AP_HAL_SMACCM_PRIVATE_MEMBER_H__ - -/* Just a stub as an example of how to implement a private member of an - * AP_HAL module */ - -#include "AP_HAL_SMACCM.h" - -class SMACCM::SMACCMPrivateMember { -public: - SMACCMPrivateMember(uint16_t foo); - void init(); -private: - uint16_t _foo; -}; - -#endif // __AP_HAL_SMACCM_PRIVATE_MEMBER_H__ - diff --git a/libraries/AP_HAL_SMACCM/RCInput.cpp b/libraries/AP_HAL_SMACCM/RCInput.cpp deleted file mode 100644 index 4179aa9016..0000000000 --- a/libraries/AP_HAL_SMACCM/RCInput.cpp +++ /dev/null @@ -1,107 +0,0 @@ - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM - -#include "RCInput.h" - -#include - -using namespace SMACCM; - -/* Constrain captured pulse to be between min and max pulsewidth. */ -static inline uint16_t constrain_pulse(uint16_t p) -{ - if (p > RC_INPUT_MAX_PULSEWIDTH) - return RC_INPUT_MAX_PULSEWIDTH; - if (p < RC_INPUT_MIN_PULSEWIDTH) - return RC_INPUT_MIN_PULSEWIDTH; - - return p; -} - -SMACCMRCInput::SMACCMRCInput() -{ -} - -void SMACCMRCInput::init(void *unused) -{ - clear_overrides(); -} - -uint8_t SMACCMRCInput::valid_channels() -{ - // If any of the overrides are positive, we have valid data. - for (int i = 0; i < SMACCM_RCINPUT_CHANNELS; ++i) - if (_override[i] > 0) - return true; - - return timer_is_ppm_valid(); -} - -// It looks like the APM2 driver clears the PPM sample after this -// function is called, so we do the same thing here for compatibility. -uint16_t SMACCMRCInput::read(uint8_t ch) -{ - uint16_t result = 0; - - if (_override[ch] != 0) { - result = _override[ch]; - } else { - timer_get_ppm_channel(ch, &result); - result = constrain_pulse(result); - } - - timer_clear_ppm(); - return constrain_pulse(result); -} - -// It looks like the APM2 driver clears the PPM sample after this -// function is called, so we do the same thing here for compatibility. -uint8_t SMACCMRCInput::read(uint16_t *periods, uint8_t len) -{ - uint8_t result; - result = timer_get_ppm(periods, len, NULL); - - for (int i = 0; i < result; ++i) { - periods[i] = constrain_pulse(periods[i]); - if (_override[i] != 0) - periods[i] = _override[i]; - } - - timer_clear_ppm(); - return result; -} - -bool SMACCMRCInput::set_overrides(int16_t *overrides, uint8_t len) -{ - bool result = false; - - for (int i = 0; i < len; ++i) - result |= set_override(i, overrides[i]); - - return result; -} - -bool SMACCMRCInput::set_override(uint8_t channel, int16_t override) -{ - if (override < 0) - return false; - - if (channel < SMACCM_RCINPUT_CHANNELS) { - _override[channel] = override; - if (override != 0) { - return true; - } - } - - return false; -} - -void SMACCMRCInput::clear_overrides() -{ - for (int i = 0; i < SMACCM_RCINPUT_CHANNELS; ++i) - _override[i] = 0; -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_SMACCM diff --git a/libraries/AP_HAL_SMACCM/RCInput.h b/libraries/AP_HAL_SMACCM/RCInput.h deleted file mode 100644 index 40637d1287..0000000000 --- a/libraries/AP_HAL_SMACCM/RCInput.h +++ /dev/null @@ -1,25 +0,0 @@ - -#ifndef __AP_HAL_SMACCM_RCINPUT_H__ -#define __AP_HAL_SMACCM_RCINPUT_H__ - -#include - -#define SMACCM_RCINPUT_CHANNELS 8 - -class SMACCM::SMACCMRCInput : public AP_HAL::RCInput { -public: - SMACCMRCInput(); - void init(void *unused); - 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(); - -private: - uint16_t _override[SMACCM_RCINPUT_CHANNELS]; -}; - -#endif // __AP_HAL_SMACCM_RCINPUT_H__ diff --git a/libraries/AP_HAL_SMACCM/RCOutput.cpp b/libraries/AP_HAL_SMACCM/RCOutput.cpp deleted file mode 100644 index 9716c68780..0000000000 --- a/libraries/AP_HAL_SMACCM/RCOutput.cpp +++ /dev/null @@ -1,38 +0,0 @@ - -#include "RCOutput.h" - -using namespace SMACCM; - -void SMACCMRCOutput::init(void* machtnichts) {} - -void SMACCMRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) {} - -uint16_t SMACCMRCOutput::get_freq(uint8_t ch) { - return 50; -} - -void SMACCMRCOutput::enable_ch(uint8_t ch) -{} - -void SMACCMRCOutput::enable_mask(uint32_t chmask) -{} - -void SMACCMRCOutput::disable_ch(uint8_t ch) -{} - -void SMACCMRCOutput::disable_mask(uint32_t chmask) -{} - -void SMACCMRCOutput::write(uint8_t ch, uint16_t period_us) -{} - -void SMACCMRCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len) -{} - -uint16_t SMACCMRCOutput::read(uint8_t ch) { - return 900; -} - -void SMACCMRCOutput::read(uint16_t* period_us, uint8_t len) -{} - diff --git a/libraries/AP_HAL_SMACCM/RCOutput.h b/libraries/AP_HAL_SMACCM/RCOutput.h deleted file mode 100644 index 33d4b6aedd..0000000000 --- a/libraries/AP_HAL_SMACCM/RCOutput.h +++ /dev/null @@ -1,21 +0,0 @@ - -#ifndef __AP_HAL_SMACCM_RCOUTPUT_H__ -#define __AP_HAL_SMACCM_RCOUTPUT_H__ - -#include - -class SMACCM::SMACCMRCOutput : 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_SMACCM_RCOUTPUT_H__ diff --git a/libraries/AP_HAL_SMACCM/SPIDriver.cpp b/libraries/AP_HAL_SMACCM/SPIDriver.cpp deleted file mode 100644 index 8352c30f5a..0000000000 --- a/libraries/AP_HAL_SMACCM/SPIDriver.cpp +++ /dev/null @@ -1,118 +0,0 @@ -/* - * SPIDriver.cpp --- AP_HAL_SMACCM SPI driver. - * - * Copyright (C) 2012, Galois, Inc. - * All Rights Reserved. - * - * This software is released under the "BSD3" license. Read the file - * "LICENSE" for more information. - */ - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM - -#include "SPIDriver.h" -#include -#include - -using namespace SMACCM; - -extern const AP_HAL::HAL& hal; - -////////////////////////////////////////////////////////////////////// -// SPI Device Driver - -SMACCMSPIDeviceDriver::SMACCMSPIDeviceDriver(spi_bus *bus, spi_device *device) - : _bus(bus), _device(device) -{ -} - -void SMACCMSPIDeviceDriver::init() -{ - _semaphore.init(); -} - -AP_HAL::Semaphore* SMACCMSPIDeviceDriver::get_semaphore() -{ - return &_semaphore; -} - -void SMACCMSPIDeviceDriver::transaction(const uint8_t *tx, uint8_t *rx, uint16_t len) -{ - if (spi_transfer(_bus, _device, 1000, tx, rx, len) < 0) { - hal.scheduler->panic("PANIC: SPI transaction timeout."); - } -} - -// XXX these methods are not implemented -void SMACCMSPIDeviceDriver::cs_assert() -{ -} - -void SMACCMSPIDeviceDriver::cs_release() -{ -} - -uint8_t SMACCMSPIDeviceDriver::transfer (uint8_t data) -{ - return 0; -} - -void SMACCMSPIDeviceDriver::transfer (const uint8_t *data, uint16_t len) -{ -} - -////////////////////////////////////////////////////////////////////// -// SPI Device Instances - -// SPIDevice_Dataflash (I2C device in PX4FMU) - -// SPIDevice_ADS7844 (not present in PX4FMU) - -// SPIDevice_MS5611 (I2C device in PX4FMU) - -// SPIDevice_MPU6000 (on SPI1) -static spi_device g_mpu6000_spi_dev = { - pin_b0, // chip_select - false, // chip_select_active - SPI_BAUD_DIV_128, // baud XXX check frequency - SPI_CLOCK_POLARITY_LOW, // clock_polarity - SPI_CLOCK_PHASE_1, // clock_phase - SPI_BIT_ORDER_MSB_FIRST // bit_order -}; - -static SMACCMSPIDeviceDriver g_mpu6000_dev(spi1, &g_mpu6000_spi_dev); - -// SPIDevice_ADNS3080_SPI0 (not present in PX4FMU) - -// SPIDevice_ADNS3080_SPI3 (not present in PX4FMU) - -////////////////////////////////////////////////////////////////////// -// SPI Device Manager - -SMACCMSPIDeviceManager::SMACCMSPIDeviceManager() -{ -} - -// Initialize all SPI busses and devices. -void SMACCMSPIDeviceManager::init(void *) -{ - spi_init(spi1); - - spi_device_init(&g_mpu6000_spi_dev); - g_mpu6000_dev.init(); -} - -AP_HAL::SPIDeviceDriver* SMACCMSPIDeviceManager::device(AP_HAL::SPIDevice dev) -{ - switch (dev) { - case AP_HAL::SPIDevice_MPU6000: - return &g_mpu6000_dev; - - default: - return NULL; - } -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_SMACCM diff --git a/libraries/AP_HAL_SMACCM/SPIDriver.h b/libraries/AP_HAL_SMACCM/SPIDriver.h deleted file mode 100644 index 8d5554346d..0000000000 --- a/libraries/AP_HAL_SMACCM/SPIDriver.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - * SPIDriver.h --- AP_HAL_SMACCM SPI driver. - * - * Copyright (C) 2012, Galois, Inc. - * All Rights Reserved. - * - * This software is released under the "BSD3" license. Read the file - * "LICENSE" for more information. - */ - -#ifndef __AP_HAL_SMACCM_SPIDRIVER_H__ -#define __AP_HAL_SMACCM_SPIDRIVER_H__ - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM - -#include -#include "Semaphores.h" - -#include - -class SMACCM::SMACCMSPIDeviceDriver : public AP_HAL::SPIDeviceDriver { -public: - SMACCMSPIDeviceDriver(spi_bus *bus, spi_device *device); - void init(); - AP_HAL::Semaphore* get_semaphore(); - void transaction(const uint8_t *tx, uint8_t *rx, uint16_t len); - - void cs_assert(); - void cs_release(); - uint8_t transfer (uint8_t data); - void transfer (const uint8_t *data, uint16_t len); -private: - SMACCMSemaphore _semaphore; - struct spi_bus *_bus; - struct spi_device *_device; -}; - -class SMACCM::SMACCMSPIDeviceManager : public AP_HAL::SPIDeviceManager { -public: - SMACCMSPIDeviceManager(); - void init(void *); - AP_HAL::SPIDeviceDriver* device(AP_HAL::SPIDevice); -}; - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_SMACCM -#endif // __AP_HAL_SMACCM_SPIDRIVER_H__ diff --git a/libraries/AP_HAL_SMACCM/Scheduler.cpp b/libraries/AP_HAL_SMACCM/Scheduler.cpp deleted file mode 100644 index ea95b922c9..0000000000 --- a/libraries/AP_HAL_SMACCM/Scheduler.cpp +++ /dev/null @@ -1,301 +0,0 @@ -/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */ -/* - * Scheduler.cpp --- AP_HAL_SMACCM scheduler. - * - * Copyright (C) 2012, Galois, Inc. - * All Rights Reserved. - * - * This software is released under the "BSD3" license. Read the file - * "LICENSE" for more information. - * - * Written by James Bielman , 20 December 2012 - */ - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM - -#include -#include - -#include -#include -#include - -#include "Scheduler.h" - -using namespace SMACCM; - -extern const AP_HAL::HAL& hal; - -/** Rate in milliseconds of timed process execution. (1kHz) */ -#define SCHEDULER_TICKS (1 / (portTICK_RATE_MS)) - -/** Stack size of the scheduler thread. */ -#define SCHEDULER_STACK_SIZE 1536 - -/** Priority of the scheduler timer process task. */ -#define SCHEDULER_PRIORITY (configMAX_PRIORITIES - 1) - -/** Rate in milliseconds of the delay callback task. */ -#define DELAY_CB_TICKS (1 / (portTICK_RATE_MS)) - -/** Stack size of the delay callback task. */ -#define DELAY_CB_STACK_SIZE 512 - -/** Priority of the delay callback task. */ -#define DELAY_CB_PRIORITY 0 - -/** - * Recursive mutex used to block "scheduler_task" during atomic - * sections. - */ -static xSemaphoreHandle g_atomic; - -/** High-priority thread managing timer procedures. */ -static void scheduler_task(void *arg) -{ - SMACCMScheduler *sched = (SMACCMScheduler *)arg; - portTickType last_wake_time; - portTickType now; - - vTaskSetApplicationTaskTag(NULL, (pdTASK_HOOK_CODE)3); - last_wake_time = xTaskGetTickCount(); - - for (;;) { - /* If "vTaskDelayUntil" would return immediately without blocking, - * call the failsafe callback to notify the client that we've - * missed our deadline, and reset the wakeup time to the current - * time. */ - now = xTaskGetTickCount(); - if (last_wake_time + SCHEDULER_TICKS <= now) { - sched->run_failsafe_cb(); - last_wake_time = now; - } else { - vTaskDelayUntil(&last_wake_time, SCHEDULER_TICKS); - - xSemaphoreTakeRecursive(g_atomic, portMAX_DELAY); - sched->run_callbacks(); - xSemaphoreGiveRecursive(g_atomic); - } - } -} - -/** Count of the number of threads in "delay". */ -static uint32_t g_delay_count; - -/** Binary semaphore given when a thread enters "delay". */ -static xSemaphoreHandle g_delay_event; - -/** - * Low-priority thread that calls the delay callback every 1ms. - * - * Whenever any thread enters a call to "delay", it unblocks this - * task. - * - * We use a count of the number of threads currently in "delay" - * because there could be more than one. - */ -static void delay_cb_task(void *arg) -{ - SMACCMScheduler *sched = (SMACCMScheduler *)arg; - portTickType last_wake_time; - portTickType now; - - vTaskSetApplicationTaskTag(NULL, (pdTASK_HOOK_CODE)4); - last_wake_time = xTaskGetTickCount(); - - for (;;) { - portENTER_CRITICAL(); - uint32_t delay_count = g_delay_count; - portEXIT_CRITICAL(); - - if (delay_count > 0) { - /* If some thread is in "delay", call the delay callback. */ - sched->run_delay_cb(); - - /* If "vTaskDelayUntil" would return immediately without - * blocking, that means we've missed our deadline. However, - * this thread is best-effort, so we'll just readjust our - * schedule accordingly and run 1ms from now. - * - * Without this, the thread runs many times to "catch up" if - * something takes too long in a higher priority thread. */ - now = xTaskGetTickCount(); - if (last_wake_time + DELAY_CB_TICKS <= now) { - last_wake_time = now; - } else { - vTaskDelayUntil(&last_wake_time, DELAY_CB_TICKS); - } - } else { - /* Wait for a thread to enter a delay. */ - xSemaphoreTake(g_delay_event, portMAX_DELAY); - last_wake_time = xTaskGetTickCount(); - } - } -} - -SMACCMScheduler::SMACCMScheduler() - : m_delay_cb(NULL), m_task(NULL), m_delay_cb_task(NULL), - m_failsafe_cb(NULL), m_num_procs(0), m_initializing(true) -{ -} - -void SMACCMScheduler::init(void *arg) -{ - timer_init(); - - g_atomic = xSemaphoreCreateRecursiveMutex(); - - vSemaphoreCreateBinary(g_delay_event); - xSemaphoreTake(g_delay_event, portMAX_DELAY); - - xTaskCreate(scheduler_task, (signed char *)"scheduler", - SCHEDULER_STACK_SIZE, this, SCHEDULER_PRIORITY, - &m_task); - - xTaskCreate(delay_cb_task, (signed char *)"delay_cb", - DELAY_CB_STACK_SIZE, this, DELAY_CB_PRIORITY, - &m_delay_cb_task); -} - -void SMACCMScheduler::delay(uint16_t ms) -{ - /* Wake up the delay callback thread. */ - portENTER_CRITICAL(); - ++g_delay_count; - portEXIT_CRITICAL(); - xSemaphoreGive(g_delay_event); - - timer_msleep(ms); - - /* Put the delay callback thread back to sleep. */ - portENTER_CRITICAL(); - --g_delay_count; - portEXIT_CRITICAL(); -} - -uint32_t SMACCMScheduler::millis() -{ - return (uint32_t)(timer_get_ticks() / 1000ULL); -} - -// XXX this is going to wrap every 1.1 hours -uint32_t SMACCMScheduler::micros() -{ - return (uint32_t)timer_get_ticks(); -} - -void SMACCMScheduler::delay_microseconds(uint16_t us) -{ - timer_usleep(us); -} - -void SMACCMScheduler::register_delay_callback(AP_HAL::Proc k, uint16_t) -{ - m_delay_cb = k; -} - -void SMACCMScheduler::register_timer_process(AP_HAL::TimedProc k) -{ - for (int i = 0; i < m_num_procs; ++i) { - if (m_procs[i] == k) - return; - } - - if (m_num_procs < SMACCM_SCHEDULER_MAX_TIMER_PROCS) { - portENTER_CRITICAL(); - m_procs[m_num_procs] = k; - ++m_num_procs; - portEXIT_CRITICAL(); - } -} - -void SMACCMScheduler::register_timer_failsafe(AP_HAL::TimedProc k, uint32_t) -{ - m_failsafe_cb = k; -} - -void SMACCMScheduler::suspend_timer_procs() -{ - xSemaphoreTakeRecursive(g_atomic, portMAX_DELAY); -} - -void SMACCMScheduler::resume_timer_procs() -{ - xSemaphoreGiveRecursive(g_atomic); -} - -void SMACCMScheduler::begin_atomic() -{ -} - -void SMACCMScheduler::end_atomic() -{ -} - -void SMACCMScheduler::panic(const prog_char_t *errormsg) -{ - hal.console->println_P(errormsg); - - // Try to grab "g_atomic" to suspend timer processes, but with a - // timeout in case a timer proc is locked up. - xSemaphoreTakeRecursive(g_atomic, 10); - - for(;;) - ; -} - -void SMACCMScheduler::reboot(bool hold_in_bootloader) -{ - for(;;) - ; -} - -void SMACCMScheduler::run_callbacks() -{ - uint32_t now = micros(); - - // Run timer processes if not suspended. - portENTER_CRITICAL(); - uint8_t num_procs = m_num_procs; - portEXIT_CRITICAL(); - - for (int i = 0; i < num_procs; ++i) { - if (m_procs[i] != NULL) { - m_procs[i](now); - } - } -} - -void SMACCMScheduler::run_failsafe_cb() -{ - if (m_failsafe_cb) - m_failsafe_cb(micros()); -} - -void SMACCMScheduler::run_delay_cb() -{ - if (m_delay_cb) - m_delay_cb(); -} - -/** Return true if in the context of a timer process. */ -bool SMACCMScheduler::in_timerprocess() -{ - return (xTaskGetCurrentTaskHandle() == m_task); -} - -/** Return true if the system is initializing. */ -bool SMACCMScheduler::system_initializing() -{ - return m_initializing; -} - -/** Set the system initializing flag to false. */ -void SMACCMScheduler::system_initialized() -{ - m_initializing = false; -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_SMACCM diff --git a/libraries/AP_HAL_SMACCM/Scheduler.h b/libraries/AP_HAL_SMACCM/Scheduler.h deleted file mode 100644 index 6530486c60..0000000000 --- a/libraries/AP_HAL_SMACCM/Scheduler.h +++ /dev/null @@ -1,130 +0,0 @@ -/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */ -/* - * Scheduler.h --- AP_HAL_SMACCM scheduler. - * - * Copyright (C) 2012, Galois, Inc. - * All Rights Reserved. - * - * This software is released under the "BSD3" license. Read the file - * "LICENSE" for more information. - * - * Written by James Bielman , 20 December 2012 - */ - -#ifndef __AP_HAL_SMACCM_SCHEDULER_H__ -#define __AP_HAL_SMACCM_SCHEDULER_H__ - -#include - -#define SMACCM_SCHEDULER_MAX_TIMER_PROCS 4 - -class SMACCM::SMACCMScheduler : public AP_HAL::Scheduler -{ -public: - SMACCMScheduler(); - - /** - * Initialize the scheduler. This initializes the HWF4 timer - * driver. - * - * @param arg reserved, pass NULL - */ - void init(void *arg); - - /** Delay for "ms" milliseconds. */ - void delay(uint16_t ms); - - /** Delay for "us" microseconds. */ - void delay_microseconds(uint16_t us); - - /** Return the time since init in milliseconds. */ - uint32_t millis(); - - /** Return the time since init in microseconds. */ - uint32_t micros(); - - /** - * Register a callback to run every millisecond during a call to - * "delay" as long as the delay remaining is at least "min_time_ms". - * - * The callback is executed in the thread that calls "delay". - */ - void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms); - - /** Register a callback to run every 1ms (1kHz). */ - void register_timer_process(AP_HAL::TimedProc); - - /** - * Register a callback to run if a timer process takes too long to - * execute. The second argument is ignored. - */ - void register_timer_failsafe(AP_HAL::TimedProc, uint32_t); - - /** - * Suspend execution of timed procedures. Calls to this function do - * not nest. - */ - void suspend_timer_procs(); - - /** - * Resume execution of timed procedures. Calls to this function do - * not nest. - */ - void resume_timer_procs(); - - /** - * Enter a critical section where timed processes will not be run. - * Calls to this function nest. - */ - void begin_atomic(); - - /** - * Leave a critical section, re-enabling timed processes. Calls to - * this function nest. - */ - void end_atomic(); - - /** Print an error message and halt execution. */ - void panic(const prog_char_t *errormsg); - - /** Reboot the firmware. Not implemented. */ - void reboot(bool hold_in_bootloader); - - /** - * Run timed and deferred processes. This should not be called from - * client code. - */ - void run_callbacks(); - - /** - * Run the failsafe callback. This should not be called from client - * code. - */ - void run_failsafe_cb(); - - /** - * Run the delay callback. This should not be called from client - * code. - */ - void run_delay_cb(); - - /** Return true if in the context of a timer process. */ - bool in_timerprocess(); - - /** Return true if the system is initializing. */ - bool system_initializing(); - - /** Set the system initializing flag to false. */ - void system_initialized(); - -private: - AP_HAL::Proc m_delay_cb; /* delay callback */ - void *m_task; /* opaque scheduler task handle */ - void *m_delay_cb_task; /* opaque delay cb task handle */ - AP_HAL::TimedProc m_procs[SMACCM_SCHEDULER_MAX_TIMER_PROCS]; - AP_HAL::TimedProc m_failsafe_cb; - uint8_t m_num_procs; /* number of entries in "m_procs" */ - bool m_initializing; /* true if initializing */ -}; - -#endif // __AP_HAL_SMACCM_SCHEDULER_H__ diff --git a/libraries/AP_HAL_SMACCM/Semaphores.cpp b/libraries/AP_HAL_SMACCM/Semaphores.cpp deleted file mode 100644 index 6d97b26cba..0000000000 --- a/libraries/AP_HAL_SMACCM/Semaphores.cpp +++ /dev/null @@ -1,57 +0,0 @@ - -#include "Semaphores.h" - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM - -#include - -using namespace SMACCM; - -extern const AP_HAL::HAL& hal; - -/** Return true if this thread already holds "sem". */ -static bool already_held(xSemaphoreHandle sem) -{ - xTaskHandle self = xTaskGetCurrentTaskHandle(); - return xSemaphoreGetMutexHolder(sem) == self; -} - -SMACCMSemaphore::SMACCMSemaphore() - : m_semaphore(NULL) -{ -} - -void SMACCMSemaphore::init() -{ - m_semaphore = xSemaphoreCreateMutex(); -} - -bool SMACCMSemaphore::take(uint32_t timeout_ms) -{ - portTickType delay; - - if (already_held(m_semaphore)) - hal.scheduler->panic("PANIC: Recursive semaphore take."); - - if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) - delay = portMAX_DELAY; - else - delay = timeout_ms / portTICK_RATE_MS; - - return xSemaphoreTake(m_semaphore, delay); -} - -bool SMACCMSemaphore::take_nonblocking() -{ - if (already_held(m_semaphore)) - hal.scheduler->panic("PANIC: Recursive semaphore take."); - - return xSemaphoreTake(m_semaphore, 0); -} - -bool SMACCMSemaphore::give() -{ - return xSemaphoreGive(m_semaphore); -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_SMACCM diff --git a/libraries/AP_HAL_SMACCM/Semaphores.h b/libraries/AP_HAL_SMACCM/Semaphores.h deleted file mode 100644 index d90ea31fde..0000000000 --- a/libraries/AP_HAL_SMACCM/Semaphores.h +++ /dev/null @@ -1,27 +0,0 @@ - -#ifndef __AP_HAL_SMACCM_SEMAPHORE_H__ -#define __AP_HAL_SMACCM_SEMAPHORE_H__ - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM - -#include -#include -#include - -class SMACCM::SMACCMSemaphore : public AP_HAL::Semaphore { -public: - SMACCMSemaphore(); - - void init(); - virtual bool take(uint32_t timeout_ms); - virtual bool take_nonblocking(); - virtual bool give(); - -private: - xSemaphoreHandle m_semaphore; -}; - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_SMACCM -#endif // __AP_HAL_SMACCM_SEMAPHORE_H__ diff --git a/libraries/AP_HAL_SMACCM/Storage.cpp b/libraries/AP_HAL_SMACCM/Storage.cpp deleted file mode 100644 index 515911e463..0000000000 --- a/libraries/AP_HAL_SMACCM/Storage.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */ -/* - * Storage.cpp --- AP_HAL_SMACCM storage driver. - * - * Copyright (C) 2012, Galois, Inc. - * All Rights Reserved. - * - * This software is released under the "BSD3" license. Read the file - * "LICENSE" for more information. - * - * Written by James Bielman , 20 December 2012 - */ - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM - -#include -#include - -#include "Storage.h" - -using namespace SMACCM; - -#define EEPROM_I2C_ADDR 0x50 - -// Note: These functions write multi-byte integers to the EEPROM in -// the native byte order, and so the format will depend on the -// endianness of the machine. - -SMACCMStorage::SMACCMStorage() -{ -} - -void SMACCMStorage::init(void*) -{ - eeprom_init(i2c2, EEPROM_I2C_ADDR); -} - -uint8_t SMACCMStorage::read_byte(uint16_t loc) -{ - uint8_t result = 0; - eeprom_read_byte(loc, &result); - return result; -} - -uint16_t SMACCMStorage::read_word(uint16_t loc) -{ - uint16_t result = 0; - eeprom_read(loc, (uint8_t*)&result, sizeof(result)); - return result; -} - -uint32_t SMACCMStorage::read_dword(uint16_t loc) -{ - uint32_t result = 0; - eeprom_read(loc, (uint8_t*)&result, sizeof(result)); - return result; -} - -void SMACCMStorage::read_block(void* dst, uint16_t src, size_t n) -{ - eeprom_read(src, (uint8_t*)dst, n); -} - -void SMACCMStorage::write_byte(uint16_t loc, uint8_t value) -{ - eeprom_write_byte(loc, value); -} - -void SMACCMStorage::write_word(uint16_t loc, uint16_t value) -{ - eeprom_write(loc, (uint8_t*)&value, sizeof(value)); -} - -void SMACCMStorage::write_dword(uint16_t loc, uint32_t value) -{ - eeprom_write(loc, (uint8_t*)&value, sizeof(value)); -} - -void SMACCMStorage::write_block(uint16_t loc, const void* src, size_t n) -{ - eeprom_write(loc, (const uint8_t *)src, n); -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_SMACCM diff --git a/libraries/AP_HAL_SMACCM/Storage.h b/libraries/AP_HAL_SMACCM/Storage.h deleted file mode 100644 index ac3946c494..0000000000 --- a/libraries/AP_HAL_SMACCM/Storage.h +++ /dev/null @@ -1,35 +0,0 @@ -/* -*- Mode: C++; indent-tabs-mode: nil; c-basic-offset: 2 -*- */ -/* - * Storage.h --- AP_HAL_SMACCM storage driver. - * - * Copyright (C) 2012, Galois, Inc. - * All Rights Reserved. - * - * This software is released under the "BSD3" license. Read the file - * "LICENSE" for more information. - * - * Written by James Bielman , 20 December 2012 - */ - -#ifndef __AP_HAL_SMACCM_STORAGE_H__ -#define __AP_HAL_SMACCM_STORAGE_H__ - -#include - -class SMACCM::SMACCMStorage : public AP_HAL::Storage -{ -public: - SMACCMStorage(); - 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_SMACCM_STORAGE_H__ diff --git a/libraries/AP_HAL_SMACCM/UARTDriver.cpp b/libraries/AP_HAL_SMACCM/UARTDriver.cpp deleted file mode 100644 index 6b2a88bd77..0000000000 --- a/libraries/AP_HAL_SMACCM/UARTDriver.cpp +++ /dev/null @@ -1,116 +0,0 @@ -/* - * UARTDriver.cpp --- AP_HAL_SMACCM UART driver. - * - * Copyright (C) 2012, Galois, Inc. - * All Rights Reserved. - * - * This software is released under the "BSD3" license. Read the file - * "LICENSE" for more information. - */ - -#include "UARTDriver.h" - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM - -#include // for vsnprintf - -using namespace SMACCM; - -// XXX the AVR driver enables itself in the constructor. This seems -// like a very bad idea, since it will run somewhere in the startup -// code before our clocks are all set up and such. -SMACCMUARTDriver::SMACCMUARTDriver(struct usart *dev) - : m_dev(dev), m_initialized(false), m_blocking(true) -{ -} - -void SMACCMUARTDriver::begin(uint32_t baud) -{ - if (m_dev != NULL) { - usart_init(m_dev, baud); - usart_enable(m_dev); - } - - m_initialized = true; -} - -// XXX buffer sizes ignored -void SMACCMUARTDriver::begin(uint32_t baud, uint16_t rxS, uint16_t txS) -{ - begin(baud); -} - -// XXX hwf4 doesn't support de-initializing a USART -void SMACCMUARTDriver::end() -{ -} - -// XXX hwf4 doesn't support flushing, could be tricky to get the -// synchronization right. Would we just force the TX/RX queues to -// empty? -void SMACCMUARTDriver::flush() -{ -} - -bool SMACCMUARTDriver::is_initialized() -{ - return m_initialized; -} - -void SMACCMUARTDriver::set_blocking_writes(bool blocking) -{ - m_blocking = blocking; -} - -bool SMACCMUARTDriver::tx_pending() -{ - if (m_dev != NULL) { - return usart_is_tx_pending(m_dev); - } - - return false; -} - -/* SMACCM implementations of Stream virtual methods */ -int16_t SMACCMUARTDriver::available() -{ - if (m_dev != NULL) - return (int16_t)usart_available(m_dev); - - return 0; -} - -int16_t SMACCMUARTDriver::txspace() -{ - if (m_dev != NULL) - return (int16_t)usart_txspace(m_dev); - - return 0; -} - -// It looks like this should always be a non-blocking read, so return -// -1 if there is nothing to receive immediately. -int16_t SMACCMUARTDriver::read() -{ - uint8_t c; - - if (m_dev == NULL) - return -1; - - if (usart_read_timeout(m_dev, 0, &c, 1) == 0) - return -1; - - return (int16_t)c; -} - -/* SMACCM implementations of Print virtual methods */ -size_t SMACCMUARTDriver::write(uint8_t c) -{ - if (m_dev == NULL) - return 1; - - portTickType delay = m_blocking ? portMAX_DELAY : 0; - return usart_write_timeout(m_dev, delay, &c, 1); -} - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_SMACCM diff --git a/libraries/AP_HAL_SMACCM/UARTDriver.h b/libraries/AP_HAL_SMACCM/UARTDriver.h deleted file mode 100644 index 2d1e05b7d1..0000000000 --- a/libraries/AP_HAL_SMACCM/UARTDriver.h +++ /dev/null @@ -1,41 +0,0 @@ - -#ifndef __AP_HAL_SMACCM_UARTDRIVER_H__ -#define __AP_HAL_SMACCM_UARTDRIVER_H__ - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM - -#include -#include - -class SMACCM::SMACCMUARTDriver : public AP_HAL::UARTDriver -{ -public: - SMACCMUARTDriver(struct usart *dev); - - /* SMACCM 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(); - - /* SMACCM implementations of Stream virtual methods */ - int16_t available(); - int16_t txspace(); - int16_t read(); - - /* SMACCM implementations of Print virtual methods */ - size_t write(uint8_t c); - -private: - struct usart *m_dev; - bool m_initialized; - bool m_blocking; -}; - -#endif // CONFIG_HAL_BOARD == HAL_BOARD_SMACCM -#endif // __AP_HAL_SMACCM_UARTDRIVER_H__ diff --git a/libraries/AP_HAL_SMACCM/Util.h b/libraries/AP_HAL_SMACCM/Util.h deleted file mode 100644 index d03b24341e..0000000000 --- a/libraries/AP_HAL_SMACCM/Util.h +++ /dev/null @@ -1,13 +0,0 @@ - -#ifndef __AP_HAL_SMACCM_UTIL_H__ -#define __AP_HAL_SMACCM_UTIL_H__ - -#include -#include "AP_HAL_SMACCM_Namespace.h" - -class SMACCM::SMACCMUtil : public AP_HAL::Util { -public: - bool run_debug_shell(AP_HAL::BetterStream *stream) { return false; } -}; - -#endif // __AP_HAL_SMACCM_UTIL_H__