From 8b6bab7b17d1b04bdf7a34abaf33d89447c142c2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 5 Jan 2018 17:19:51 +1100 Subject: [PATCH] HAL_Chibios: added ChibiOS HAL this is based on initial work by Sid, reset here for easier merging --- libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS.h | 21 + .../AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h | 22 + .../AP_HAL_ChibiOS/AP_HAL_ChibiOS_Private.h | 16 + libraries/AP_HAL_ChibiOS/AnalogIn.cpp | 289 ++ libraries/AP_HAL_ChibiOS/AnalogIn.h | 75 + libraries/AP_HAL_ChibiOS/Device.cpp | 144 + libraries/AP_HAL_ChibiOS/Device.h | 51 + libraries/AP_HAL_ChibiOS/GPIO.cpp | 197 ++ libraries/AP_HAL_ChibiOS/GPIO.h | 70 + .../AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp | 210 ++ libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h | 39 + libraries/AP_HAL_ChibiOS/I2CDevice.cpp | 195 ++ libraries/AP_HAL_ChibiOS/I2CDevice.h | 107 + libraries/AP_HAL_ChibiOS/RCInput.cpp | 208 ++ libraries/AP_HAL_ChibiOS/RCInput.h | 68 + libraries/AP_HAL_ChibiOS/RCOutput.cpp | 325 +++ libraries/AP_HAL_ChibiOS/RCOutput.h | 84 + libraries/AP_HAL_ChibiOS/SPIDevice.cpp | 343 +++ libraries/AP_HAL_ChibiOS/SPIDevice.h | 139 + libraries/AP_HAL_ChibiOS/Scheduler.cpp | 349 +++ libraries/AP_HAL_ChibiOS/Scheduler.h | 108 + libraries/AP_HAL_ChibiOS/Semaphores.cpp | 57 + libraries/AP_HAL_ChibiOS/Semaphores.h | 35 + libraries/AP_HAL_ChibiOS/Storage.cpp | 201 ++ libraries/AP_HAL_ChibiOS/Storage.h | 73 + libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 568 ++++ libraries/AP_HAL_ChibiOS/UARTDriver.h | 100 + libraries/AP_HAL_ChibiOS/Util.cpp | 107 + libraries/AP_HAL_ChibiOS/Util.h | 49 + .../hwdef/common/chibios_common.mk | 331 +++ libraries/AP_HAL_ChibiOS/hwdef/common/flash.c | 324 +++ libraries/AP_HAL_ChibiOS/hwdef/common/flash.h | 30 + libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c | 67 + libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h | 11 + .../AP_HAL_ChibiOS/hwdef/common/malloc.c | 54 + libraries/AP_HAL_ChibiOS/hwdef/common/posix.c | 2521 +++++++++++++++++ libraries/AP_HAL_ChibiOS/hwdef/common/posix.h | 388 +++ libraries/AP_HAL_ChibiOS/hwdef/common/ppm.c | 101 + libraries/AP_HAL_ChibiOS/hwdef/common/ppm.h | 28 + libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c | 313 ++ libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h | 39 + libraries/AP_HAL_ChibiOS/hwdef/common/stubs.c | 222 ++ .../AP_HAL_ChibiOS/hwdef/common/usbcfg.c | 393 +++ .../AP_HAL_ChibiOS/hwdef/common/usbcfg.h | 36 + libraries/AP_HAL_ChibiOS/hwdef/fmuv3/board.c | 180 ++ libraries/AP_HAL_ChibiOS/hwdef/fmuv3/board.h | 47 + libraries/AP_HAL_ChibiOS/hwdef/fmuv3/chconf.h | 519 ++++ .../hwdef/fmuv3/chibios_board.mk | 228 ++ libraries/AP_HAL_ChibiOS/hwdef/fmuv3/ffconf.h | 272 ++ .../AP_HAL_ChibiOS/hwdef/fmuv3/halconf.h | 401 +++ .../AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat | 114 + .../AP_HAL_ChibiOS/hwdef/fmuv3/ldscript.ld | 393 +++ .../AP_HAL_ChibiOS/hwdef/fmuv3/mcuconf.h | 306 ++ .../hwdef/scripts/STM32F405xx.py | 964 +++++++ .../hwdef/scripts/STM32F412Rx.py | 672 +++++ .../hwdef/scripts/STM32F427xx.py | 739 +++++ .../AP_HAL_ChibiOS/hwdef/scripts/af_parse.py | 69 + .../hwdef/scripts/chibios_hwdef.py | 387 +++ .../AP_HAL_ChibiOS/hwdef/scripts/dma_parse.py | 61 + .../hwdef/scripts/dma_resolver.py | 184 ++ .../hwdef/skyviper-f412/board.c | 154 + .../hwdef/skyviper-f412/board.h | 60 + .../hwdef/skyviper-f412/chconf.h | 533 ++++ .../hwdef/skyviper-f412/chibios_board.mk | 225 ++ .../hwdef/skyviper-f412/halconf.h | 402 +++ .../hwdef/skyviper-f412/hwdef.dat | 67 + .../hwdef/skyviper-f412/ldscript.ld | 86 + .../hwdef/skyviper-f412/mcuconf.h | 253 ++ libraries/AP_HAL_ChibiOS/shared_dma.cpp | 117 + libraries/AP_HAL_ChibiOS/shared_dma.h | 71 + libraries/AP_HAL_ChibiOS/system.cpp | 167 ++ 71 files changed, 16779 insertions(+) create mode 100644 libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS.h create mode 100644 libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h create mode 100644 libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Private.h create mode 100644 libraries/AP_HAL_ChibiOS/AnalogIn.cpp create mode 100644 libraries/AP_HAL_ChibiOS/AnalogIn.h create mode 100644 libraries/AP_HAL_ChibiOS/Device.cpp create mode 100644 libraries/AP_HAL_ChibiOS/Device.h create mode 100644 libraries/AP_HAL_ChibiOS/GPIO.cpp create mode 100644 libraries/AP_HAL_ChibiOS/GPIO.h create mode 100644 libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp create mode 100644 libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h create mode 100644 libraries/AP_HAL_ChibiOS/I2CDevice.cpp create mode 100644 libraries/AP_HAL_ChibiOS/I2CDevice.h create mode 100644 libraries/AP_HAL_ChibiOS/RCInput.cpp create mode 100644 libraries/AP_HAL_ChibiOS/RCInput.h create mode 100644 libraries/AP_HAL_ChibiOS/RCOutput.cpp create mode 100644 libraries/AP_HAL_ChibiOS/RCOutput.h create mode 100644 libraries/AP_HAL_ChibiOS/SPIDevice.cpp create mode 100644 libraries/AP_HAL_ChibiOS/SPIDevice.h create mode 100644 libraries/AP_HAL_ChibiOS/Scheduler.cpp create mode 100644 libraries/AP_HAL_ChibiOS/Scheduler.h create mode 100644 libraries/AP_HAL_ChibiOS/Semaphores.cpp create mode 100644 libraries/AP_HAL_ChibiOS/Semaphores.h create mode 100644 libraries/AP_HAL_ChibiOS/Storage.cpp create mode 100644 libraries/AP_HAL_ChibiOS/Storage.h create mode 100644 libraries/AP_HAL_ChibiOS/UARTDriver.cpp create mode 100644 libraries/AP_HAL_ChibiOS/UARTDriver.h create mode 100644 libraries/AP_HAL_ChibiOS/Util.cpp create mode 100644 libraries/AP_HAL_ChibiOS/Util.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/chibios_common.mk create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/flash.c create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/flash.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/posix.c create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/posix.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/ppm.c create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/ppm.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/stubs.c create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/fmuv3/board.c create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/fmuv3/board.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/fmuv3/chconf.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/fmuv3/chibios_board.mk create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/fmuv3/ffconf.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/fmuv3/halconf.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/fmuv3/ldscript.ld create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/fmuv3/mcuconf.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F405xx.py create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F412Rx.py create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F427xx.py create mode 100755 libraries/AP_HAL_ChibiOS/hwdef/scripts/af_parse.py create mode 100755 libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py create mode 100755 libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_parse.py create mode 100755 libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/board.c create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/board.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/chconf.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/chibios_board.mk create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/halconf.h create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/hwdef.dat create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/ldscript.ld create mode 100644 libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/mcuconf.h create mode 100644 libraries/AP_HAL_ChibiOS/shared_dma.cpp create mode 100644 libraries/AP_HAL_ChibiOS/shared_dma.h create mode 100644 libraries/AP_HAL_ChibiOS/system.cpp diff --git a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS.h b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS.h new file mode 100644 index 0000000000..58b8b3a321 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS.h @@ -0,0 +1,21 @@ +#pragma once + +/* Your layer exports should depend on AP_HAL.h ONLY. */ +#include + +/** + * Umbrella header for AP_HAL_Empty 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 Empty namespace. + * The class implementing AP_HAL::HAL should be called HAL_Empty and exist + * in the global namespace. There should be a single const instance of the + * HAL_Empty class called AP_HAL_Empty, instantiated in the HAL_Empty_Class.cpp + * and exported as `extern const HAL_Empty AP_HAL_Empty;` in HAL_Empty_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_EMPTY. + * When creating a new HAL, declare a new HAL_BOARD_ in AP_HAL/AP_HAL_Boards.h + */ + +#include "HAL_ChibiOS_Class.h" diff --git a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h new file mode 100644 index 0000000000..a391cbd77c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h @@ -0,0 +1,22 @@ +#pragma once + +namespace ChibiOS { + class ChibiAnalogIn; + class ChibiAnalogSource; + class ChibiDigitalSource; + class ChibiGPIO; + class ChibiI2CDevice; + class I2CDeviceManager; + class OpticalFlow; + class PrivateMember; + class ChibiRCInput; + class ChibiRCOutput; + class ChibiScheduler; + class Semaphore; + class SPIDevice; + class SPIDeviceDriver; + class SPIDeviceManager; + class ChibiStorage; + class ChibiUARTDriver; + class ChibiUtil; +} diff --git a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Private.h b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Private.h new file mode 100644 index 0000000000..b9a770f5d0 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Private.h @@ -0,0 +1,16 @@ +#pragma once + +/* Umbrella header for all private headers of the AP_HAL_ChibiOS module. + * Only import this header from inside AP_HAL_ChibiOS + */ + +#include "AnalogIn.h" +#include "GPIO.h" +#include "Scheduler.h" +#include "Util.h" +#include "UARTDriver.h" +#include "SPIDevice.h" +#include "Storage.h" +#include "RCInput.h" +#include "RCOutput.h" +#include "I2CDevice.h" diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp new file mode 100644 index 0000000000..3a4d0c992e --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp @@ -0,0 +1,289 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include + +#include "AnalogIn.h" + +#if HAL_WITH_IO_MCU +#include +extern AP_IOMCU iomcu; +#endif + +#define ANLOGIN_DEBUGGING 0 + +// base voltage scaling for 12 bit 3.3V ADC +#define VOLTAGE_SCALING (3.3f/4096.0f) + +#if ANLOGIN_DEBUGGING + # define Debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) +#else + # define Debug(fmt, args ...) +#endif + +extern const AP_HAL::HAL& hal; + +#define ADC_GRP1_NUM_CHANNELS 3 +#define ADC_GRP1_BUF_DEPTH 8 +static adcsample_t samples[ADC_GRP1_NUM_CHANNELS * ADC_GRP1_BUF_DEPTH] = {0}; +static float avg_samples[ADC_GRP1_NUM_CHANNELS]; + +// special pin numbers +#define ANALOG_VCC_5V_PIN 4 + +/* + scaling table between ADC count and actual input voltage, to account + for voltage dividers on the board. + */ +static const struct { + uint8_t channel; + float scaling; +} pin_scaling[] = { +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 + { ANALOG_VCC_5V_PIN, 0.007734 }, // VCC 5V rail sense +#else + { ANALOG_VCC_5V_PIN, 6.6f/4096 }, // VCC 5V rail sense +#endif + { 2, 3.3f/4096 }, // 3DR Brick voltage, usually 10.1:1 + // scaled from battery voltage + { 3, 3.3f/4096 }, // 3DR Brick current, usually 17:1 scaled + // for APM_PER_VOLT +}; + +using namespace ChibiOS; + +ChibiAnalogSource::ChibiAnalogSource(int16_t pin, float initial_value) : + _pin(pin), + _value(initial_value), + _value_ratiometric(initial_value), + _latest_value(initial_value), + _sum_count(0), + _sum_value(0), + _sum_ratiometric(0) +{ +/*#ifdef PX4_ANALOG_VCC_5V_PIN + if (_pin == ANALOG_INPUT_BOARD_VCC) { + _pin = PX4_ANALOG_VCC_5V_PIN; + } +#endif +*/ +} + + +float ChibiAnalogSource::read_average() +{ + if (_sum_count == 0) { + return _value; + } + _value = _sum_value / _sum_count; + _value_ratiometric = _sum_ratiometric / _sum_count; + _sum_value = 0; + _sum_ratiometric = 0; + _sum_count = 0; + return _value; +} + +float ChibiAnalogSource::read_latest() +{ + return _latest_value; +} + +/* + return scaling from ADC count to Volts + */ +float ChibiAnalogSource::_pin_scaler(void) +{ + float scaling = VOLTAGE_SCALING; + uint8_t num_scalings = ARRAY_SIZE(pin_scaling) - 1; + for (uint8_t i=0; i_pin) { + // add a value + c->_add_value(buf_adc[i], _board_voltage); + } + } + } + +#if HAL_WITH_IO_MCU + // now handle special inputs from IOMCU + _servorail_voltage = iomcu.get_vservo(); +#endif +} + +AP_HAL::AnalogSource* ChibiAnalogIn::channel(int16_t pin) +{ + for (uint8_t j=0; jprintf("Out of analog channels\n"); + return nullptr; +} + diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.h b/libraries/AP_HAL_ChibiOS/AnalogIn.h new file mode 100644 index 0000000000..1fe7d62268 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.h @@ -0,0 +1,75 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ + +#pragma once + +#include "AP_HAL_ChibiOS.h" + +#define ANALOG_MAX_CHANNELS 16 + + +class ChibiOS::ChibiAnalogSource : public AP_HAL::AnalogSource { +public: + friend class ChibiOS::ChibiAnalogIn; + ChibiAnalogSource(int16_t pin, float initial_value); + float read_average(); + float read_latest(); + void set_pin(uint8_t p); + float voltage_average(); + float voltage_latest(); + float voltage_average_ratiometric(); + void set_stop_pin(uint8_t p) {} + void set_settle_time(uint16_t settle_time_ms) {} + +private: + // what value it has + int16_t _pin; + float _value; + float _value_ratiometric; + float _latest_value; + uint8_t _sum_count; + float _sum_value; + float _sum_ratiometric; + void _add_value(float v, float vcc5V); + float _pin_scaler(); +}; + +class ChibiOS::ChibiAnalogIn : public AP_HAL::AnalogIn { +public: + ChibiAnalogIn(); + void init() override; + AP_HAL::AnalogSource* channel(int16_t pin) override; + void _timer_tick(void); + float board_voltage(void) override { return _board_voltage; } + float servorail_voltage(void) override { return _servorail_voltage; } + uint16_t power_status_flags(void) override { return _power_flags; } + static void adccallback(ADCDriver *adcp, adcsample_t *buffer, size_t n); +private: + void read_adc(uint32_t *val); + int _battery_handle; + int _servorail_handle; + int _system_power_handle; + uint64_t _battery_timestamp; + uint64_t _servorail_timestamp; + ChibiOS::ChibiAnalogSource* _channels[ANALOG_MAX_CHANNELS]; + + uint32_t _last_run; + float _board_voltage; + float _servorail_voltage; + uint16_t _power_flags; + ADCConversionGroup adcgrpcfg; +}; diff --git a/libraries/AP_HAL_ChibiOS/Device.cpp b/libraries/AP_HAL_ChibiOS/Device.cpp new file mode 100644 index 0000000000..9df61e8fab --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/Device.cpp @@ -0,0 +1,144 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "Device.h" + +#include +#include +#include +#include "Scheduler.h" +#include "Semaphores.h" + + +namespace ChibiOS { + +static const AP_HAL::HAL &hal = AP_HAL::get_HAL(); + +/* + per-bus callback thread +*/ +void DeviceBus::bus_thread(void *arg) +{ + struct DeviceBus *binfo = (struct DeviceBus *)arg; + + while (true) { + uint64_t now = AP_HAL::micros64(); + DeviceBus::callback_info *callback; + + // find a callback to run + for (callback = binfo->callbacks; callback; callback = callback->next) { + if (now >= callback->next_usec) { + while (now >= callback->next_usec) { + callback->next_usec += callback->period_usec; + } + // call it with semaphore held + if (binfo->semaphore.take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + callback->cb(); + binfo->semaphore.give(); + } + } + } + + // work out when next loop is needed + uint64_t next_needed = 0; + now = AP_HAL::micros64(); + + for (callback = binfo->callbacks; callback; callback = callback->next) { + if (next_needed == 0 || + callback->next_usec < next_needed) { + next_needed = callback->next_usec; + if (next_needed < now) { + next_needed = now; + } + } + } + + // delay for at most 50ms, to handle newly added callbacks + uint32_t delay = 50000; + if (next_needed >= now && next_needed - now < delay) { + delay = next_needed - now; + } + // don't delay for less than 400usec, so one thread doesn't + // completely dominate the CPU + if (delay < 400) { + delay = 400; + } + hal.scheduler->delay_microseconds(delay); + } + return; +} + +AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb, AP_HAL::Device *_hal_device) +{ + if (!thread_started) { + thread_started = true; + + hal_device = _hal_device; + // setup a name for the thread + char name[] = "XXX:X"; + switch (hal_device->bus_type()) { + case AP_HAL::Device::BUS_TYPE_I2C: + snprintf(name, sizeof(name), "I2C:%u", + hal_device->bus_num()); + break; + + case AP_HAL::Device::BUS_TYPE_SPI: + snprintf(name, sizeof(name), "SPI:%u", + hal_device->bus_num()); + break; + default: + break; + } + + thread_ctx = chThdCreateFromHeap(NULL, + THD_WORKING_AREA_SIZE(1024), + name, + thread_priority, /* Initial priority. */ + DeviceBus::bus_thread, /* Thread function. */ + this); /* Thread parameter. */ + } + DeviceBus::callback_info *callback = new DeviceBus::callback_info; + if (callback == nullptr) { + return nullptr; + } + callback->cb = cb; + callback->period_usec = period_usec; + callback->next_usec = AP_HAL::micros64() + period_usec; + + // add to linked list of callbacks on thread + callback->next = callbacks; + callbacks = callback; + + return callback; +} + +/* + * Adjust the timer for the next call: it needs to be called from the bus + * thread, otherwise it will race with it + */ +bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + if (chThdGetSelfX() != thread_ctx) { + return false; + } + + DeviceBus::callback_info *callback = static_cast(h); + + callback->period_usec = period_usec; + callback->next_usec = AP_HAL::micros64() + period_usec; + + return true; +} + +} diff --git a/libraries/AP_HAL_ChibiOS/Device.h b/libraries/AP_HAL_ChibiOS/Device.h new file mode 100644 index 0000000000..b5882143ea --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/Device.h @@ -0,0 +1,51 @@ +/* + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published + * by the Free Software Foundation, either version 3 of the License, + * or (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once + +#include +#include +#include "Semaphores.h" +#include "Scheduler.h" +#include "shared_dma.h" + +namespace ChibiOS { + +class DeviceBus { +public: + DeviceBus(uint8_t _thread_priority = APM_I2C_PRIORITY) : + thread_priority(_thread_priority) {} + + struct DeviceBus *next; + Semaphore semaphore; + Shared_DMA *dma_handle; + + AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb, AP_HAL::Device *hal_device); + bool adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec); + static void bus_thread(void *arg); + +private: + struct callback_info { + struct callback_info *next; + AP_HAL::Device::PeriodicCb cb; + uint32_t period_usec; + uint64_t next_usec; + } *callbacks; + uint8_t thread_priority; + thread_t* thread_ctx; + bool thread_started; + AP_HAL::Device *hal_device; +}; + +} diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp new file mode 100644 index 0000000000..436ff56ded --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp @@ -0,0 +1,197 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include "GPIO.h" +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + +using namespace ChibiOS; + +static uint32_t _gpio_tab[] = { +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 + PAL_LINE(GPIOB, 7U), + PAL_LINE(GPIOB, 6U) +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_FMUV3 + LINE_LED1, + PAL_LINE(GPIOB, 0U) +#endif +}; + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 +static const uint8_t num_leds = 3; +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_FMUV3 +static const uint8_t num_leds = 1; +#endif + +static void ext_interrupt_cb(EXTDriver *extp, expchannel_t channel); + +static AP_HAL::Proc ext_irq[22]; // ext int irq list +static EXTConfig extcfg = { + { + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL}, + {EXT_CH_MODE_DISABLED, NULL} + } +}; + +static const uint32_t irq_port_list[] = { + EXT_MODE_GPIOD, //Chan 0 + EXT_MODE_GPIOD, //Chan 1 + EXT_MODE_GPIOD, //Chan 2 + EXT_MODE_GPIOD, //Chan 3 + EXT_MODE_GPIOD, //Chan 4 + EXT_MODE_GPIOD, //Chan 5 + EXT_MODE_GPIOD, //Chan 6 + EXT_MODE_GPIOD, //Chan 7 + EXT_MODE_GPIOD, //Chan 8 + EXT_MODE_GPIOD, //Chan 9 + EXT_MODE_GPIOD, //Chan 10 + EXT_MODE_GPIOD, //Chan 11 + EXT_MODE_GPIOD, //Chan 12 + EXT_MODE_GPIOD, //Chan 13 + EXT_MODE_GPIOD, //Chan 14 + EXT_MODE_GPIOD //Chan 15 +}; + +ChibiGPIO::ChibiGPIO() +{} + +void ChibiGPIO::init() +{ + //palClearLine(_gpio_tab[0]); + extStart(&EXTD1, &extcfg); +} + +void ChibiGPIO::pinMode(uint8_t pin, uint8_t output) +{ + if(pin >= num_leds) { + return; + } + palSetLineMode(_gpio_tab[pin], output); +} + +int8_t ChibiGPIO::analogPinToDigitalPin(uint8_t pin) +{ + return -1; +} + + +uint8_t ChibiGPIO::read(uint8_t pin) { + if(pin >= num_leds) { + return 0; + } + return palReadLine(_gpio_tab[pin]); +} + +void ChibiGPIO::write(uint8_t pin, uint8_t value) +{ + if(pin >= num_leds) { + return; + } + if (value == PAL_LOW) { + palClearLine(_gpio_tab[pin]); + } else { + palSetLine(_gpio_tab[pin]); + } +} + +void ChibiGPIO::toggle(uint8_t pin) +{ + if(pin >= num_leds) { + return; + } + palToggleLine(_gpio_tab[pin]); +} + +/* Alternative interface: */ +AP_HAL::DigitalSource* ChibiGPIO::channel(uint16_t n) { + return new ChibiDigitalSource(0); +} + +/* Interrupt interface: */ +bool ChibiGPIO::attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc p, uint8_t mode) { + extStop(&EXTD1); + switch(mode) { + case HAL_GPIO_INTERRUPT_LOW: + extcfg.channels[interrupt_num].mode = EXT_CH_MODE_LOW_LEVEL; + break; + case HAL_GPIO_INTERRUPT_FALLING: + extcfg.channels[interrupt_num].mode = EXT_CH_MODE_FALLING_EDGE; + break; + case HAL_GPIO_INTERRUPT_RISING: + extcfg.channels[interrupt_num].mode = EXT_CH_MODE_RISING_EDGE; + break; + case HAL_GPIO_INTERRUPT_BOTH: + extcfg.channels[interrupt_num].mode = EXT_CH_MODE_BOTH_EDGES; + break; + default: return false; + } + extcfg.channels[interrupt_num].mode |= EXT_CH_MODE_AUTOSTART | irq_port_list[interrupt_num]; + ext_irq[interrupt_num] = p; + extcfg.channels[interrupt_num].cb = ext_interrupt_cb; + extStart(&EXTD1, &extcfg); + return true; +} + +bool ChibiGPIO::usb_connected(void) +{ + return _usb_connected; +} + +ChibiDigitalSource::ChibiDigitalSource(uint8_t v) : + _v(v) +{} + +void ChibiDigitalSource::mode(uint8_t output) +{} + +uint8_t ChibiDigitalSource::read() { + return _v; +} + +void ChibiDigitalSource::write(uint8_t value) { + _v = value; +} + +void ChibiDigitalSource::toggle() { + _v = !_v; +} + +void ext_interrupt_cb(EXTDriver *extp, expchannel_t channel) { + if (ext_irq[channel] != nullptr) { + ext_irq[channel](); + } +} +#endif //HAL_BOARD_ChibiOS diff --git a/libraries/AP_HAL_ChibiOS/GPIO.h b/libraries/AP_HAL_ChibiOS/GPIO.h new file mode 100644 index 0000000000..9a11b21312 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/GPIO.h @@ -0,0 +1,70 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#pragma once + +#include "AP_HAL_ChibiOS.h" + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + # define HAL_GPIO_A_LED_PIN 0 + # define HAL_GPIO_B_LED_PIN 0 + # define HAL_GPIO_C_LED_PIN 0 + # define HAL_GPIO_LED_ON LOW + # define HAL_GPIO_LED_OFF HIGH +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 + # define HAL_GPIO_CYRF_RESET 1 + # define HAL_GPIO_CYRF_IRQ 2 +#else + # define HAL_GPIO_CYRF_RESET 1 + # define HAL_GPIO_CYRF_IRQ 15 +#endif +#endif + +class ChibiOS::ChibiGPIO : public AP_HAL::GPIO { +public: + ChibiGPIO(); + 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) override; + + void set_usb_connected() { _usb_connected = true; } +private: + bool _usb_connected = false; +}; + +class ChibiOS::ChibiDigitalSource : public AP_HAL::DigitalSource { +public: + ChibiDigitalSource(uint8_t v); + void mode(uint8_t output); + uint8_t read(); + void write(uint8_t value); + void toggle(); +private: + uint8_t _v; +}; diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp new file mode 100644 index 0000000000..61ed0b162b --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -0,0 +1,210 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + +#include + +#include "HAL_ChibiOS_Class.h" +#include +#include +#include "shared_dma.h" + +static ChibiOS::ChibiUARTDriver uartADriver(0); +static ChibiOS::ChibiUARTDriver uartBDriver(1); +static ChibiOS::ChibiUARTDriver uartCDriver(2); +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 +static Empty::UARTDriver uartDDriver; +static Empty::UARTDriver uartEDriver; +#else +static ChibiOS::ChibiUARTDriver uartDDriver(3); +static ChibiOS::ChibiUARTDriver uartEDriver(4); +#endif +static ChibiOS::I2CDeviceManager i2cDeviceManager; +static ChibiOS::SPIDeviceManager spiDeviceManager; +static ChibiOS::ChibiAnalogIn analogIn; +static ChibiOS::ChibiStorage storageDriver; +static ChibiOS::ChibiGPIO gpioDriver; +static ChibiOS::ChibiRCInput rcinDriver; +static ChibiOS::ChibiRCOutput rcoutDriver; +static ChibiOS::ChibiScheduler schedulerInstance; +static ChibiOS::ChibiUtil utilInstance; +static Empty::OpticalFlow opticalFlowDriver; +#ifdef USE_POSIX +static FATFS SDC_FS; // FATFS object +#endif + +#if HAL_WITH_IO_MCU +#include +ChibiOS::ChibiUARTDriver uart_io(5); +AP_IOMCU iomcu(uart_io); +#endif + +HAL_ChibiOS::HAL_ChibiOS() : + AP_HAL::HAL( + &uartADriver, + &uartBDriver, + &uartCDriver, + &uartDDriver, + &uartEDriver, + nullptr, /* no uartF */ + &i2cDeviceManager, + &spiDeviceManager, + &analogIn, + &storageDriver, + &uartADriver, + &gpioDriver, + &rcinDriver, + &rcoutDriver, + &schedulerInstance, + &utilInstance, + &opticalFlowDriver, + nullptr + ) +{} + +static bool thread_running = false; /**< Daemon status flag */ +static thread_t* daemon_task; /**< Handle of daemon task / thread */ + +extern const AP_HAL::HAL& hal; + + +/* + set the priority of the main APM task + */ +void hal_chibios_set_priority(uint8_t priority) +{ + chSysLock(); + if ((daemon_task->prio == daemon_task->realprio) || (priority > daemon_task->prio)) { + daemon_task->prio = priority; + } + daemon_task->realprio = priority; + chSchRescheduleS(); + chSysUnlock(); +} + +thread_t* get_main_thread() +{ + return daemon_task; +} + +static AP_HAL::HAL::Callbacks* g_callbacks; +THD_WORKING_AREA(_main_thread_wa, APM_MAIN_THREAD_STACK_SIZE); +static THD_FUNCTION(main_loop,arg) +{ + daemon_task = chThdGetSelfX(); + + Shared_DMA::init(); + + hal.uartA->begin(115200); + hal.uartB->begin(38400); + hal.uartC->begin(57600); + hal.rcin->init(); + hal.rcout->init(); + hal.analogin->init(); + hal.gpio->init(); + hal.scheduler->init(); + + /* + run setup() at low priority to ensure CLI doesn't hang the + system, and to allow initial sensor read loops to run + */ + hal_chibios_set_priority(APM_STARTUP_PRIORITY); + + schedulerInstance.hal_initialized(); + + g_callbacks->setup(); + hal.scheduler->system_initialized(); + + thread_running = true; + daemon_task->name = SKETCHNAME; + /* + switch to high priority for main loop + */ + chThdSetPriority(APM_MAIN_PRIORITY); + + while (true) { + g_callbacks->loop(); + + /* + give up 250 microseconds of time, to ensure drivers get a + chance to run. + */ + hal.scheduler->delay_microseconds(250); + } + thread_running = false; +} + +void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const +{ + /* + * System initializations. + * - ChibiOS HAL initialization, this also initializes the configured device drivers + * and performs the board-specific initializations. + * - Kernel initialization, the main() function becomes a thread and the + * RTOS is active. + */ + hrt_init(); + //STDOUT Initialistion + SerialConfig stdoutcfg = + { + HAL_STDOUT_BAUDRATE, + 0, + USART_CR2_STOP1_BITS, + 0 + }; + sdStart((SerialDriver*)&HAL_STDOUT_SERIAL, &stdoutcfg); + + //Setup SD Card and Initialise FATFS bindings + /* + * Start SD Driver + */ +#ifdef USE_POSIX + FRESULT err; + sdcStart(&SDCD1, NULL); + + if(sdcConnect(&SDCD1) == HAL_FAILED) { + printf("Err: Failed to initialize SDIO!\n"); + } else { + err = f_mount(&SDC_FS, "/", 1); + if (err != FR_OK) { + printf("Err: Failed to mount SD Card!\n"); + sdcDisconnect(&SDCD1); + } else { + printf("Successfully mounted SDCard..\n"); + } + //Create APM Directory + mkdir("/APM", 0777); + } +#endif + assert(callbacks); + g_callbacks = callbacks; + + chThdCreateStatic(_main_thread_wa, + sizeof(_main_thread_wa), + APM_MAIN_PRIORITY, /* Initial priority. */ + main_loop, /* Thread function. */ + nullptr); /* Thread parameter. */ + chThdExit(0); +} + +const AP_HAL::HAL& AP_HAL::get_HAL() { + static const HAL_ChibiOS hal_chibios; + return hal_chibios; +} + +#endif diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h new file mode 100644 index 0000000000..626447144c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.h @@ -0,0 +1,39 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#pragma once + +#include + +#include +#include +#include +#ifdef USE_POSIX +#include +#endif +#include +#include "ch.h" +#include "hal.h" +#include "hrt.h" + +class HAL_ChibiOS : public AP_HAL::HAL { +public: + HAL_ChibiOS(); + void run(int argc, char* const* argv, Callbacks* callbacks) const override; +}; +void hal_chibios_set_priority(uint8_t priority); + +thread_t* get_main_thread(void); diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp new file mode 100644 index 0000000000..f91d617fb2 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp @@ -0,0 +1,195 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "I2CDevice.h" + +#include +#include +#include "Util.h" +#include "Scheduler.h" + +#include "ch.h" +#include "hal.h" + +static I2CDriver* I2CD[] = { + &I2CD2, + &I2CD1 +}; + +static uint8_t tx_dma_stream[] = { + STM32_I2C_I2C2_TX_DMA_STREAM, + STM32_I2C_I2C1_TX_DMA_STREAM +}; + +static uint8_t rx_dma_stream[] = { + STM32_I2C_I2C2_RX_DMA_STREAM, + STM32_I2C_I2C1_RX_DMA_STREAM +}; + +using namespace ChibiOS; + +DeviceBus I2CDevice::businfo[I2CDevice::num_buses]; + +I2CDevice::I2CDevice(uint8_t bus, uint8_t address) : + _retries(2), + _busnum(bus), + _address(address) +{ + set_device_bus(bus); + set_device_address(address); + asprintf(&pname, "I2C:%u:%02x", + (unsigned)bus, (unsigned)address); + if (businfo[_busnum].dma_handle == nullptr) { + businfo[_busnum].dma_handle = new Shared_DMA(tx_dma_stream[_busnum], rx_dma_stream[_busnum], + FUNCTOR_BIND_MEMBER(&I2CDevice::dma_allocate, void), + FUNCTOR_BIND_MEMBER(&I2CDevice::dma_deallocate, void)); + } +} + +I2CDevice::~I2CDevice() +{ + businfo[_busnum].dma_handle->unregister(); + printf("I2C device bus %u address 0x%02x closed\n", + (unsigned)_busnum, (unsigned)_address); + free(pname); +} + +/* + allocate DMA channel + */ +void I2CDevice::dma_allocate(void) +{ + i2cStart(I2CD[_busnum], &i2ccfg); +} + +/* + deallocate DMA channel + */ +void I2CDevice::dma_deallocate(void) +{ + i2cStop(I2CD[_busnum]); +} + +bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) +{ + struct DeviceBus &binfo = businfo[_busnum]; + + if (!init_done) { + i2ccfg.op_mode = OPMODE_I2C; + i2ccfg.clock_speed = 400000; + i2ccfg.duty_cycle = FAST_DUTY_CYCLE_2; + init_done = true; + } + + binfo.dma_handle->lock(); + + if (_split_transfers) { + /* + splitting the transfer() into two pieces avoids a stop condition + with SCL low which is not supported on some devices (such as + LidarLite blue label) + */ + if (send && send_len) { + if (!_transfer(send, send_len, nullptr, 0)) { + binfo.dma_handle->unlock(); + return false; + } + } + if (recv && recv_len) { + if (!_transfer(nullptr, 0, recv, recv_len)) { + binfo.dma_handle->unlock(); + return false; + } + } + } else { + // combined transfer + if (!_transfer(send, send_len, recv, recv_len)) { + binfo.dma_handle->unlock(); + return false; + } + } + + binfo.dma_handle->unlock(); + return true; +} + +bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) +{ + int ret; + for(uint8_t i=0 ; i <= _retries; i++) { + i2cAcquireBus(I2CD[_busnum]); + // calculate a timeout as twice the expected transfer time, and set as min of 4ms + uint32_t timeout_ms = 1+2*(((8*1000000UL/i2ccfg.clock_speed)*MAX(send_len, recv_len))/1000); + timeout_ms = MAX(timeout_ms, 4); + if(send_len == 0) { + ret = i2cMasterReceiveTimeout(I2CD[_busnum], _address,recv, recv_len, MS2ST(timeout_ms)); + } else { + ret = i2cMasterTransmitTimeout(I2CD[_busnum], _address, send, send_len, + recv, recv_len, MS2ST(timeout_ms)); + } + i2cReleaseBus(I2CD[_busnum]); + if (ret != MSG_OK){ + _errors = i2cGetErrors(I2CD[_busnum]); + //restart the bus + i2cStop(I2CD[_busnum]); + i2cStart(I2CD[_busnum], &i2ccfg); + } else { + return true; + } + } + return false; +} + +bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv, + uint32_t recv_len, uint8_t times) +{ + return false; +} + + +/* + register a periodic callback +*/ +AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) +{ + if (_busnum >= num_buses) { + return nullptr; + } + struct DeviceBus &binfo = businfo[_busnum]; + return binfo.register_periodic_callback(period_usec, cb, this); +} + + +/* + adjust a periodic callback +*/ +bool I2CDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + if (_busnum >= num_buses) { + return false; + } + + struct DeviceBus &binfo = businfo[_busnum]; + + return binfo.adjust_timer(h, period_usec); +} + +AP_HAL::OwnPtr +I2CDeviceManager::get_device(uint8_t bus, uint8_t address) +{ + auto dev = AP_HAL::OwnPtr(new I2CDevice(bus, address)); + return dev; +} diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.h b/libraries/AP_HAL_ChibiOS/I2CDevice.h new file mode 100644 index 0000000000..ce50e30238 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/I2CDevice.h @@ -0,0 +1,107 @@ +/* + * Copyright (C) 2015-2016 Intel Corporation. All rights reserved. + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL_ChibiOS by Andrew Tridgell and Siddharth Bharat Purohit + */ + +#pragma once + +#include + +#include +#include +#include +#include "Semaphores.h" +#include "Device.h" +#include "shared_dma.h" + +namespace ChibiOS { + +class I2CDevice : public AP_HAL::I2CDevice { +public: + static I2CDevice *from(AP_HAL::I2CDevice *dev) + { + return static_cast(dev); + } + + I2CDevice(uint8_t bus, uint8_t address); + ~I2CDevice(); + + /* See AP_HAL::I2CDevice::set_address() */ + void set_address(uint8_t address) override { _address = address; } + + /* See AP_HAL::I2CDevice::set_retries() */ + void set_retries(uint8_t retries) override { _retries = retries; } + + /* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */ + bool set_speed(enum Device::Speed speed) override { return true; } + + /* See AP_HAL::Device::transfer() */ + bool transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) override; + + bool read_registers_multiple(uint8_t first_reg, uint8_t *recv, + uint32_t recv_len, uint8_t times) override; + + /* See AP_HAL::Device::register_periodic_callback() */ + AP_HAL::Device::PeriodicHandle register_periodic_callback( + uint32_t period_usec, AP_HAL::Device::PeriodicCb) override; + + /* See AP_HAL::Device::adjust_periodic_callback() */ + bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override; + + AP_HAL::Semaphore* get_semaphore() override { + // if asking for invalid bus number use bus 0 semaphore + return &businfo[_busnum(i2c_mgr); + } + + AP_HAL::OwnPtr get_device(uint8_t bus, uint8_t address) override; +}; + +} diff --git a/libraries/AP_HAL_ChibiOS/RCInput.cpp b/libraries/AP_HAL_ChibiOS/RCInput.cpp new file mode 100644 index 0000000000..150e313476 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/RCInput.cpp @@ -0,0 +1,208 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include "RCInput.h" +#include "hal.h" +#include "hwdef/common/ppm.h" +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + +#if HAL_WITH_IO_MCU +#include +extern AP_IOMCU iomcu; +#endif + + +using namespace ChibiOS; +extern const AP_HAL::HAL& hal; +void ChibiRCInput::init() +{ + ppm_init(1000000, true); + chMtxObjectInit(&rcin_mutex); + _init = true; +} + +bool ChibiRCInput::new_input() +{ + if (!_init) { + return false; + } + chMtxLock(&rcin_mutex); + bool valid = _rcin_timestamp_last_signal != _last_read; + + if (_override_valid) { + // if we have RC overrides active, then always consider it valid + valid = true; + } + _last_read = _rcin_timestamp_last_signal; + _override_valid = false; + chMtxUnlock(&rcin_mutex); + +#ifdef HAL_RCINPUT_WITH_AP_RADIO + if (!_radio_init) { + _radio_init = true; + radio = AP_Radio::instance(); + if (radio) { + radio->init(); + } + } +#endif + return valid; +} + +uint8_t ChibiRCInput::num_channels() +{ + if (!_init) { + return 0; + } + chMtxLock(&rcin_mutex); + uint8_t n = _num_channels; + chMtxUnlock(&rcin_mutex); + return n; +} + +uint16_t ChibiRCInput::read(uint8_t channel) +{ + if (!_init) { + return 0; + } + if (channel >= RC_INPUT_MAX_CHANNELS) { + return 0; + } + chMtxLock(&rcin_mutex); + if (_override[channel]) { + uint16_t v = _override[channel]; + chMtxUnlock(&rcin_mutex); + return v; + } + if (channel >= _num_channels) { + chMtxUnlock(&rcin_mutex); + return 0; + } + uint16_t v = _rc_values[channel]; + chMtxUnlock(&rcin_mutex); +#ifdef HAL_RCINPUT_WITH_AP_RADIO + if (radio && channel == 0) { + // hook to allow for update of radio on main thread, for mavlink sends + radio->update(); + } +#endif + return v; +} + +uint8_t ChibiRCInput::read(uint16_t* periods, uint8_t len) +{ + if (!_init) { + return false; + } + + if (len > RC_INPUT_MAX_CHANNELS) { + len = RC_INPUT_MAX_CHANNELS; + } + for (uint8_t i = 0; i < len; i++){ + periods[i] = read(i); + } + return len; +} + +bool ChibiRCInput::set_overrides(int16_t *overrides, uint8_t len) +{ + if (!_init) { + return false; + } + + bool res = false; + for (uint8_t i = 0; i < len; i++) { + res |= set_override(i, overrides[i]); + } + return res; +} + +bool ChibiRCInput::set_override(uint8_t channel, int16_t override) +{ + if (!_init) { + return false; + } + + if (override < 0) { + return false; /* -1: no change. */ + } + if (channel >= RC_INPUT_MAX_CHANNELS) { + return false; + } + _override[channel] = override; + if (override != 0) { + _override_valid = true; + return true; + } + return false; +} + +void ChibiRCInput::clear_overrides() +{ + for (uint8_t i = 0; i < RC_INPUT_MAX_CHANNELS; i++) { + set_override(i, 0); + } +} + + +void ChibiRCInput::_timer_tick(void) +{ + if (!_init) { + return; + } + if (ppm_available()) { + chMtxLock(&rcin_mutex); + _num_channels = ppm_read_bulk(_rc_values, RC_INPUT_MAX_CHANNELS); + _rcin_timestamp_last_signal = AP_HAL::micros(); + chMtxUnlock(&rcin_mutex); + } + +#ifdef HAL_RCINPUT_WITH_AP_RADIO + if (radio && radio->last_recv_us() != last_radio_us) { + last_radio_us = radio->last_recv_us(); + chMtxLock(&rcin_mutex); + _rcin_timestamp_last_signal = last_radio_us; + _num_channels = radio->num_channels(); + for (uint8_t i=0; i<_num_channels; i++) { + _rc_values[i] = radio->read(i); + } + chMtxUnlock(&rcin_mutex); + } +#endif + +#if HAL_WITH_IO_MCU + chMtxLock(&rcin_mutex); + if (iomcu.check_rcinput(last_iomcu_us, _num_channels, _rc_values, RC_INPUT_MAX_CHANNELS)) { + _rcin_timestamp_last_signal = last_iomcu_us; + } + chMtxUnlock(&rcin_mutex); +#endif + + + // note, we rely on the vehicle code checking new_input() + // and a timeout for the last valid input to handle failsafe +} + +bool ChibiRCInput::rc_bind(int dsmMode) +{ +#if HAL_RCINPUT_WITH_AP_RADIO + if (radio) { + radio->start_recv_bind(); + } +#endif + return true; +} +#endif //#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS diff --git a/libraries/AP_HAL_ChibiOS/RCInput.h b/libraries/AP_HAL_ChibiOS/RCInput.h new file mode 100644 index 0000000000..17ecbfaed5 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/RCInput.h @@ -0,0 +1,68 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#pragma once + +#include "AP_HAL_ChibiOS.h" +#ifdef HAL_RCINPUT_WITH_AP_RADIO +#include +#endif + +#ifndef RC_INPUT_MAX_CHANNELS +#define RC_INPUT_MAX_CHANNELS 18 +#endif + +class ChibiOS::ChibiRCInput : public AP_HAL::RCInput { +public: + void init() override; + bool new_input() override; + uint8_t num_channels() override; + uint16_t read(uint8_t ch) override; + uint8_t read(uint16_t* periods, uint8_t len) override; + + int16_t get_rssi(void) override { + return _rssi; + } + + + bool set_overrides(int16_t *overrides, uint8_t len) override; + bool set_override(uint8_t channel, int16_t override) override; + void clear_overrides() override; + + void _timer_tick(void); + bool rc_bind(int dsmMode) override; + +private: + /* override state */ + uint16_t _override[RC_INPUT_MAX_CHANNELS]; + uint16_t _rc_values[RC_INPUT_MAX_CHANNELS] = {0}; + + uint64_t _last_read; + bool _override_valid; + uint8_t _num_channels; + mutex_t rcin_mutex; + int16_t _rssi = -1; + uint32_t _rcin_timestamp_last_signal; + bool _init; +#ifdef HAL_RCINPUT_WITH_AP_RADIO + bool _radio_init; + AP_Radio *radio; + uint32_t last_radio_us; +#endif +#if HAL_WITH_IO_MCU + uint32_t last_iomcu_us; +#endif +}; diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp new file mode 100644 index 0000000000..82b4a2d7ba --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -0,0 +1,325 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include "RCOutput.h" +#include + +using namespace ChibiOS; + +extern const AP_HAL::HAL& hal; + +#if HAL_WITH_IO_MCU +#include +extern AP_IOMCU iomcu; +#endif + +#define PWM_CLK_FREQ 8000000 +#define PWM_US_WIDTH_FROM_CLK(x) ((PWM_CLK_FREQ/1000000)*x) +const struct ChibiRCOutput::pwm_group ChibiRCOutput::pwm_group_list[] = +{ + //Group 1 Config + { //Channels in the Group and respective mapping + {PWM_CHAN_MAP(0) , PWM_CHAN_MAP(1) , PWM_CHAN_MAP(2) , PWM_CHAN_MAP(3)}, + //Group Initial Config + { + 8000000, /* 8MHz PWM clock frequency. */ + 160000, /* Initial PWM period 20ms. */ + NULL, + { + //Channel Config + {PWM_OUTPUT_ACTIVE_HIGH, NULL}, + {PWM_OUTPUT_ACTIVE_HIGH, NULL}, + {PWM_OUTPUT_ACTIVE_HIGH, NULL}, + {PWM_OUTPUT_ACTIVE_HIGH, NULL} + }, + 0, + 0 + }, +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_FMUV3 + &PWMD1 +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 + &PWMD3 +#endif + } +}; + +void ChibiRCOutput::init() +{ + _num_groups = sizeof(pwm_group_list)/sizeof(pwm_group); + for (uint8_t i = 0; i < _num_groups; i++ ) { + //Start Pwm groups + pwmStart(pwm_group_list[i].pwm_drv, &pwm_group_list[i].pwm_cfg); + } +#if HAL_WITH_IO_MCU + iomcu.init(); + + // with IOMCU the local channels start at 8 + chan_offset = 8; +#endif +} + +void ChibiRCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) +{ + //check if the request spans accross any of the channel groups + uint8_t update_mask = 0; + uint32_t grp_ch_mask; + // greater than 400 doesn't give enough room at higher periods for + // the down pulse + if (freq_hz > 400 && _output_mode != MODE_PWM_BRUSHED) { + freq_hz = 400; + } + +#if HAL_WITH_IO_MCU + iomcu.set_freq(chmask, freq_hz); +#endif + + chmask >>= chan_offset; + if (chmask == 0) { + return; + } + + for (uint8_t i = 0; i < _num_groups; i++ ) { + grp_ch_mask = PWM_CHAN_MAP(0) | PWM_CHAN_MAP(1) | PWM_CHAN_MAP(2) | PWM_CHAN_MAP(3); + if ((grp_ch_mask & chmask) == grp_ch_mask) { + update_mask |= grp_ch_mask; + pwmChangePeriod(pwm_group_list[i].pwm_drv, + pwm_group_list[i].pwm_cfg.frequency/freq_hz); + } + } + if (chmask != update_mask) { + hal.console->printf("RCOutput: Failed to set PWM frequency req %x set %x\n", (unsigned)chmask, (unsigned)update_mask); + } +} + +uint16_t ChibiRCOutput::get_freq(uint8_t chan) +{ +#if HAL_WITH_IO_MCU + if (chan < chan_offset) { + return iomcu.get_freq(chan); + } +#endif + chan -= chan_offset; + + for (uint8_t i = 0; i < _num_groups; i++ ) { + for (uint8_t j = 0; j < 4; j++) { + if (pwm_group_list[i].chan[j] == chan) { + return pwm_group_list[i].pwm_drv->config->frequency / pwm_group_list[i].pwm_drv->period; + } + } + } + // assume 50Hz default + return 50; +} + +void ChibiRCOutput::enable_ch(uint8_t chan) +{ + if (chan < chan_offset) { + return; + } + chan -= chan_offset; + + for (uint8_t i = 0; i < _num_groups; i++ ) { + for (uint8_t j = 0; j < 4; j++) { + if ((pwm_group_list[i].chan[j] == chan) && !(en_mask & 1<= _esc_pwm_max) { + period_us = PWM_FRACTION_TO_WIDTH(pwm_group_list[i].pwm_drv, 1, 1); + } else { + period_us = PWM_FRACTION_TO_WIDTH(pwm_group_list[i].pwm_drv,\ + (_esc_pwm_max - _esc_pwm_min), (period_us - _esc_pwm_min)); + } + pwmEnableChannel(pwm_group_list[i].pwm_drv, j, period_us); + } else { + pwmEnableChannel(pwm_group_list[i].pwm_drv, j, PWM_US_WIDTH_FROM_CLK(period_us)); + } + } + } + } +} + +uint16_t ChibiRCOutput::read(uint8_t chan) +{ +#if HAL_WITH_IO_MCU + if (chan < chan_offset) { + return iomcu.read_channel(chan); + } +#endif + chan -= chan_offset; + return period[chan]; +} + +void ChibiRCOutput::read(uint16_t* period_us, uint8_t len) +{ +#if HAL_WITH_IO_MCU + for (uint8_t i=0; i. + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#pragma once + +#include "AP_HAL_ChibiOS.h" +#include "ch.h" +#include "hal.h" +#define PWM_CHAN_MAP(n) (1 << n) + +class ChibiOS::ChibiRCOutput : public AP_HAL::RCOutput { +public: + void init(); + void set_freq(uint32_t chmask, uint16_t freq_hz); + uint16_t get_freq(uint8_t ch); + void enable_ch(uint8_t ch); + void disable_ch(uint8_t ch); + void write(uint8_t ch, uint16_t period_us); + uint16_t read(uint8_t ch); + void read(uint16_t* period_us, uint8_t len); + uint16_t read_last_sent(uint8_t ch) override; + void read_last_sent(uint16_t* period_us, uint8_t len) override; + void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override { + _esc_pwm_min = min_pwm; + _esc_pwm_max = max_pwm; + } + void set_output_mode(enum output_mode mode) override; + + void cork(void) override; + void push(void) override; + + /* + force the safety switch on, disabling PWM output from the IO board + */ + bool force_safety_on(void) override; + + /* + force the safety switch off, enabling PWM output from the IO board + */ + void force_safety_off(void) override; + + bool enable_px4io_sbus_out(uint16_t rate_hz) override; + +private: + struct pwm_group { + uint8_t chan[4]; // chan number, zero based + PWMConfig pwm_cfg; + PWMDriver* pwm_drv; + }; + enum output_mode _output_mode = MODE_PWM_NORMAL; + + static const pwm_group pwm_group_list[]; + uint16_t _esc_pwm_min; + uint16_t _esc_pwm_max; + uint8_t _num_groups; + + // offset of first local channel + uint8_t chan_offset; + + // last sent values are for all channels + uint16_t last_sent[16]; + + // these values are for the local channels. Non-local channels are handled by IOMCU + uint32_t en_mask; + uint16_t period[16]; + uint8_t num_channels; + bool corked; + + // push out values to local PWM + void push_local(void); +}; diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp new file mode 100644 index 0000000000..7eea23102d --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp @@ -0,0 +1,343 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "SPIDevice.h" + +#include +#include +#include "Scheduler.h" +#include "Semaphores.h" +#include + +using namespace ChibiOS; + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 +#define SPI_BUS_SENSORS 1 +#define SPI_BUS_SENSORS2 0 +#define SPI_BUS_EXT 2 + +#define SPIDEV_CS_MPU GPIOB, 12 +#define SPIDEV_CS_CYRF GPIOA, 4 +#define SPIDEV_CS_FLOW GPIOA, 15 + +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_FMUV3 +#define SPI_BUS_SENSORS 0 +#define SPI_BUS_RAMTRON 1 +#define SPI_BUS_RADIO 1 +#define SPI_BUS_EXT 2 + +#define SPIDEV_CS_MS5611 GPIOD, 7 +#define SPIDEV_CS_EXT_MS5611 GPIOC, 14 +#define SPIDEV_CS_MPU GPIOC, 2 +#define SPIDEV_CS_EXT_MPU GPIOE, 4 +#define SPIDEV_CS_EXT_LSM9DS0_G GPIOC, 13 +#define SPIDEV_CS_EXT_LSM9DS0_AM GPIOC, 15 +#define SPIDEV_CS_RAMTRON GPIOD, 10 +#define SPIDEV_CS_RADIO GPIOD, 10 +#define SPIDEV_CS_FLOW GPIOE, 4 + +#endif // CONFIG_HAL_BOARD_SUBTYPE + +#define SPI1_CLOCK STM32_PCLK2 +#define SPI2_CLOCK STM32_PCLK1 +#define SPI3_CLOCK STM32_PCLK1 +#define SPI4_CLOCK STM32_PCLK2 + +static struct SPIDriverInfo { + SPIDriver *driver; + uint8_t dma_channel_rx; + uint8_t dma_channel_tx; +} spi_devices[] = { + { &SPID1, STM32_SPI_SPI1_RX_DMA_STREAM, STM32_SPI_SPI1_TX_DMA_STREAM }, + { &SPID2, STM32_SPI_SPI2_RX_DMA_STREAM, STM32_SPI_SPI2_TX_DMA_STREAM }, +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_FMUV3 + { &SPID4, STM32_SPI_SPI4_RX_DMA_STREAM, STM32_SPI_SPI4_TX_DMA_STREAM }, +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 + { &SPID3, STM32_SPI_SPI3_RX_DMA_STREAM, STM32_SPI_SPI3_TX_DMA_STREAM }, +#endif +}; + +#define MHZ (1000U*1000U) +#define KHZ (1000U) +SPIDesc SPIDeviceManager::device_table[] = { +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 + SPIDesc("mpu6000", SPI_BUS_SENSORS, SPIDEV_MPU, SPIDEV_CS_MPU, SPIDEV_MODE3, 500*KHZ, 8*MHZ), + SPIDesc("cypress", SPI_BUS_SENSORS2, SPIDEV_CYRF, SPIDEV_CS_CYRF, SPIDEV_MODE0, 2*MHZ, 2*MHZ), + SPIDesc("pixartflow", SPI_BUS_EXT, SPIDEV_FLOW, SPIDEV_CS_FLOW, SPIDEV_MODE3, 2*MHZ, 2*MHZ), +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_FMUV3 + SPIDesc("ms5611", SPI_BUS_SENSORS, SPIDEV_MS5611, SPIDEV_CS_MS5611, SPIDEV_MODE3, 20*MHZ, 20*MHZ ), + SPIDesc("ms5611_ext", SPI_BUS_EXT, SPIDEV_EXT_MS5611, SPIDEV_CS_EXT_MS5611, SPIDEV_MODE3, 20*MHZ, 20*MHZ), + SPIDesc("mpu9250", SPI_BUS_SENSORS, SPIDEV_MPU, SPIDEV_CS_MPU, SPIDEV_MODE3, 1*MHZ, 8*MHZ ), + SPIDesc("mpu9250_ext", SPI_BUS_EXT, SPIDEV_EXT_MPU , SPIDEV_CS_EXT_MPU, SPIDEV_MODE3, 1*MHZ, 8*MHZ ), + SPIDesc("lsm9ds0_ext_g", SPI_BUS_EXT, SPIDEV_EXT_LSM9DS0_G , SPIDEV_CS_EXT_LSM9DS0_G, SPIDEV_MODE3, 11*MHZ, 11*MHZ ), + SPIDesc("lsm9ds0_ext_am", SPI_BUS_EXT, SPIDEV_EXT_LSM9DS0_AM , SPIDEV_CS_EXT_LSM9DS0_AM, SPIDEV_MODE3, 11*MHZ, 11*MHZ ), + SPIDesc("ramtron", SPI_BUS_RAMTRON, SPIDEV_RAMTROM, SPIDEV_CS_RAMTRON, SPIDEV_MODE3, 8*MHZ, 8*MHZ ), + SPIDesc("cypress", SPI_BUS_RADIO, SPIDEV_CYRF, SPIDEV_CS_RADIO, SPIDEV_MODE0, 2*MHZ, 2*MHZ), +#endif +}; + +SPIBus::SPIBus(uint8_t _bus) : + DeviceBus(APM_SPI_PRIORITY), + bus(_bus) +{ + // allow for sharing of DMA channels with other peripherals + dma_handle = new Shared_DMA(spi_devices[bus].dma_channel_rx, + spi_devices[bus].dma_channel_tx, + FUNCTOR_BIND_MEMBER(&SPIBus::dma_allocate, void), + FUNCTOR_BIND_MEMBER(&SPIBus::dma_deallocate, void)); + +} + +/* + allocate DMA channel + */ +void SPIBus::dma_allocate(void) +{ + // nothing to do as we call spiStart() on each transaction +} + +/* + deallocate DMA channel + */ +void SPIBus::dma_deallocate(void) +{ + // another non-SPI peripheral wants one of our DMA channels + spiStop(spi_devices[bus].driver); +} + + +SPIDevice::SPIDevice(SPIBus &_bus, SPIDesc &_device_desc) + : bus(_bus) + , device_desc(_device_desc) +{ + set_device_bus(_bus.bus); + set_device_address(_device_desc.device); + freq_flag_low = derive_freq_flag(device_desc.lowspeed); + freq_flag_high = derive_freq_flag(device_desc.highspeed); + + set_speed(AP_HAL::Device::SPEED_LOW); + + asprintf(&pname, "SPI:%s:%u:%u", + device_desc.name, + (unsigned)bus.bus, (unsigned)device_desc.device); + //printf("SPI device %s on %u:%u at speed %u mode %u\n", + // device_desc.name, + // (unsigned)bus.bus, (unsigned)device_desc.device, + // (unsigned)frequency, (unsigned)device_desc.mode); +} + +SPIDevice::~SPIDevice() +{ + //printf("SPI device %s on %u:%u closed\n", device_desc.name, + // (unsigned)bus.bus, (unsigned)device_desc.device); + bus.dma_handle->unregister(); + free(pname); +} + +bool SPIDevice::set_speed(AP_HAL::Device::Speed speed) +{ + switch (speed) { + case AP_HAL::Device::SPEED_HIGH: + freq_flag = freq_flag_high; + break; + case AP_HAL::Device::SPEED_LOW: + freq_flag = freq_flag_low; + break; + } + return true; +} + +/* + low level transfer function + */ +void SPIDevice::do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len) +{ + bool old_cs_forced = cs_forced; + + if (!set_chip_select(true)) { + return; + } + spiExchange(spi_devices[device_desc.bus].driver, len, send, recv); + set_chip_select(old_cs_forced); +} + +uint16_t SPIDevice::derive_freq_flag(uint32_t _frequency) +{ + uint8_t i; + uint32_t spi_clock_freq; + switch(device_desc.bus) { + case 0: + spi_clock_freq = SPI1_CLOCK; + break; + case 1: + spi_clock_freq = SPI2_CLOCK; + break; + case 2: + spi_clock_freq = SPI4_CLOCK; + break; + case 3: + spi_clock_freq = SPI4_CLOCK; + break; + default: + spi_clock_freq = SPI1_CLOCK; + break; + } + + for(i = 0; i <= 7; i++) { + spi_clock_freq /= 2; + if (spi_clock_freq <= _frequency) { + break; + } + } + switch(i) { + case 0: //PCLK DIV 2 + return 0; + case 1: //PCLK DIV 4 + return SPI_CR1_BR_0; + case 2: //PCLK DIV 8 + return SPI_CR1_BR_1; + case 3: //PCLK DIV 16 + return SPI_CR1_BR_1 | SPI_CR1_BR_0; + case 4: //PCLK DIV 32 + return SPI_CR1_BR_2; + case 5: //PCLK DIV 64 + return SPI_CR1_BR_2 | SPI_CR1_BR_0; + case 6: //PCLK DIV 128 + return SPI_CR1_BR_2 | SPI_CR1_BR_1; + case 7: //PCLK DIV 256 + default: + return SPI_CR1_BR_2 | SPI_CR1_BR_1 | SPI_CR1_BR_0; + } +} + +bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) +{ + if (send_len == recv_len && send == recv) { + // simplest cases, needed for DMA + do_transfer(send, recv, recv_len); + return true; + } + uint32_t buf_aligned[1+((send_len+recv_len)/4)]; + uint8_t *buf = (uint8_t *)&buf_aligned[0]; + if (send_len > 0) { + memcpy(buf, send, send_len); + } + if (recv_len > 0) { + memset(&buf[send_len], 0, recv_len); + } + do_transfer((uint8_t *)buf, (uint8_t *)buf, send_len+recv_len); + if (recv_len > 0) { + memcpy(recv, &buf[send_len], recv_len); + } + return true; +} + +bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len) +{ + uint8_t buf[len]; + memcpy(buf, send, len); + do_transfer(buf, buf, len); + memcpy(recv, buf, len); + return true; +} + +AP_HAL::Semaphore *SPIDevice::get_semaphore() +{ + return &bus.semaphore; +} + + +AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) +{ + return bus.register_periodic_callback(period_usec, cb, this); +} + +bool SPIDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + return bus.adjust_timer(h, period_usec); +} + +/* + allow for control of SPI chip select pin + */ +bool SPIDevice::set_chip_select(bool set) +{ + if (set && cs_forced) { + return true; + } + if (!set && !cs_forced) { + return false; + } + if (!set && cs_forced) { + spiUnselectI(spi_devices[device_desc.bus].driver); /* Slave Select de-assertion. */ + spiReleaseBus(spi_devices[device_desc.bus].driver); /* Ownership release. */ + cs_forced = false; + bus.dma_handle->unlock(); + } else { + bus.dma_handle->lock(); + spiAcquireBus(spi_devices[device_desc.bus].driver); /* Acquire ownership of the bus. */ + bus.spicfg.end_cb = nullptr; + bus.spicfg.ssport = device_desc.port; + bus.spicfg.sspad = device_desc.pin; + bus.spicfg.cr1 = (uint16_t)(freq_flag | device_desc.mode); + bus.spicfg.cr2 = 0; + spiStart(spi_devices[device_desc.bus].driver, &bus.spicfg); /* Setup transfer parameters. */ + spiSelectI(spi_devices[device_desc.bus].driver); /* Slave Select assertion. */ + cs_forced = true; + } + return true; +} + +/* + return a SPIDevice given a string device name + */ +AP_HAL::OwnPtr +SPIDeviceManager::get_device(const char *name) +{ + /* Find the bus description in the table */ + uint8_t i; + for (i = 0; i(nullptr); + } + + SPIDesc &desc = device_table[i]; + + // find the bus + SPIBus *busp; + for (busp = buses; busp; busp = (SPIBus *)busp->next) { + if (busp->bus == desc.bus) { + break; + } + } + if (busp == nullptr) { + // create a new one + busp = new SPIBus(desc.bus); + if (busp == nullptr) { + return nullptr; + } + busp->next = buses; + busp->bus = desc.bus; + + buses = busp; + } + + return AP_HAL::OwnPtr(new SPIDevice(*busp, desc)); +} diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.h b/libraries/AP_HAL_ChibiOS/SPIDevice.h new file mode 100644 index 0000000000..fafa90f95a --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.h @@ -0,0 +1,139 @@ +/* + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published + * by the Free Software Foundation, either version 3 of the License, + * or (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once + +#include +#include +#include +#include "Semaphores.h" +#include "Scheduler.h" +#include "Device.h" + +#define SPIDEV_BMP280 0 +#define SPIDEV_LSM303D 1 +#define SPIDEV_L3GD20H 2 +#define SPIDEV_MS5611 3 +#define SPIDEV_EXT_MS5611 4 +#define SPIDEV_MPU 5 +#define SPIDEV_EXT_MPU 6 +#define SPIDEV_EXT_LSM9DS0_G 7 +#define SPIDEV_EXT_LSM9DS0_AM 8 +#define SPIDEV_CYRF 9 +#define SPIDEV_FLOW 10 +#define SPIDEV_RAMTROM 11 + +#define SPIDEV_MODE0 0 +#define SPIDEV_MODE1 SPI_CR1_CPHA +#define SPIDEV_MODE2 SPI_CR1_CPOL +#define SPIDEV_MODE3 SPI_CR1_CPOL | SPI_CR1_CPHA + + +namespace ChibiOS { + +class SPIDesc; + +class SPIBus : public DeviceBus { +public: + SPIBus(uint8_t bus); + struct spi_dev_s *dev; + uint8_t bus; + SPIConfig spicfg; + void dma_allocate(void); + void dma_deallocate(void); +}; + +struct SPIDesc { + SPIDesc(const char *_name, uint8_t _bus, + uint8_t _device, ioportid_t _port, uint8_t _pin, + uint16_t _mode, uint32_t _lowspeed, uint32_t _highspeed) + : name(_name), bus(_bus), device(_device), + port(_port), pin(_pin), mode(_mode), + lowspeed(_lowspeed), highspeed(_highspeed) + { + } + + const char *name; + uint8_t bus; + uint8_t device; + ioportid_t port; + uint8_t pin; + uint16_t mode; + uint32_t lowspeed; + uint32_t highspeed; +}; + + +class SPIDevice : public AP_HAL::SPIDevice { +public: + SPIDevice(SPIBus &_bus, SPIDesc &_device_desc); + + virtual ~SPIDevice(); + + /* See AP_HAL::Device::set_speed() */ + bool set_speed(AP_HAL::Device::Speed speed) override; + + // low level transfer function + void do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len); + + /* See AP_HAL::Device::transfer() */ + bool transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) override; + + /* See AP_HAL::SPIDevice::transfer_fullduplex() */ + bool transfer_fullduplex(const uint8_t *send, uint8_t *recv, + uint32_t len) override; + + /* See AP_HAL::Device::get_semaphore() */ + AP_HAL::Semaphore *get_semaphore() override; + + /* See AP_HAL::Device::register_periodic_callback() */ + AP_HAL::Device::PeriodicHandle register_periodic_callback( + uint32_t period_usec, AP_HAL::Device::PeriodicCb) override; + + /* See AP_HAL::Device::adjust_periodic_callback() */ + bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override; + + bool set_chip_select(bool set) override; + +private: + SPIBus &bus; + SPIDesc &device_desc; + uint32_t frequency; + uint16_t freq_flag; + uint16_t freq_flag_low; + uint16_t freq_flag_high; + char *pname; + bool cs_forced; + static void *spi_thread(void *arg); + uint16_t derive_freq_flag(uint32_t _frequency); +}; + +class SPIDeviceManager : public AP_HAL::SPIDeviceManager { +public: + friend class SPIDevice; + + static SPIDeviceManager *from(AP_HAL::SPIDeviceManager *spi_mgr) + { + return static_cast(spi_mgr); + } + + AP_HAL::OwnPtr get_device(const char *name); + +private: + static SPIDesc device_table[]; + SPIBus *buses; +}; + +} diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp new file mode 100644 index 0000000000..65c0ac52c7 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -0,0 +1,349 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + +#include "AP_HAL_ChibiOS.h" +#include "Scheduler.h" + +#include +#include +#include +#include +#include + +#include +#include + +using namespace ChibiOS; + +extern const AP_HAL::HAL& hal; +THD_WORKING_AREA(_timer_thread_wa, 2048); +THD_WORKING_AREA(_io_thread_wa, 2048); +THD_WORKING_AREA(_storage_thread_wa, 2048); +THD_WORKING_AREA(_uart_thread_wa, 2048); + +#if HAL_WITH_IO_MCU +extern ChibiOS::ChibiUARTDriver uart_io; +#endif + +ChibiScheduler::ChibiScheduler() +{} + +void ChibiScheduler::init() +{ + // setup the timer thread - this will call tasks at 1kHz + _timer_thread_ctx = chThdCreateStatic(_timer_thread_wa, + sizeof(_timer_thread_wa), + APM_TIMER_PRIORITY, /* Initial priority. */ + _timer_thread, /* Thread function. */ + this); /* Thread parameter. */ + + // the UART thread runs at a medium priority + _uart_thread_ctx = chThdCreateStatic(_uart_thread_wa, + sizeof(_uart_thread_wa), + APM_UART_PRIORITY, /* Initial priority. */ + _uart_thread, /* Thread function. */ + this); /* Thread parameter. */ + + // the IO thread runs at lower priority + _io_thread_ctx = chThdCreateStatic(_io_thread_wa, + sizeof(_io_thread_wa), + APM_IO_PRIORITY, /* Initial priority. */ + _io_thread, /* Thread function. */ + this); /* Thread parameter. */ + + // the storage thread runs at just above IO priority + _storage_thread_ctx = chThdCreateStatic(_storage_thread_wa, + sizeof(_storage_thread_wa), + APM_STORAGE_PRIORITY, /* Initial priority. */ + _storage_thread, /* Thread function. */ + this); /* Thread parameter. */ +} + +void ChibiScheduler::delay_microseconds(uint16_t usec) +{ + if (usec == 0) { //chibios faults with 0us sleep + return; + } + chThdSleepMicroseconds(usec); //Suspends Thread for desired microseconds +} + +/* + wrapper around sem_post that boosts main thread priority + */ +static void set_high_priority() +{ +#if APM_MAIN_PRIORITY_BOOST != APM_MAIN_PRIORITY + hal_chibios_set_priority(APM_MAIN_PRIORITY_BOOST); +#endif +} + +/* + return the main thread to normal priority + */ +static void set_normal_priority() +{ +#if APM_MAIN_PRIORITY_BOOST != APM_MAIN_PRIORITY + hal_chibios_set_priority(APM_MAIN_PRIORITY); +#endif +} + +/* + a variant of delay_microseconds that boosts priority to + APM_MAIN_PRIORITY_BOOST for APM_MAIN_PRIORITY_BOOST_USEC + microseconds when the time completes. This significantly improves + the regularity of timing of the main loop as it takes + */ +void ChibiScheduler::delay_microseconds_boost(uint16_t usec) +{ + delay_microseconds(usec); //Suspends Thread for desired microseconds + set_high_priority(); + delay_microseconds(APM_MAIN_PRIORITY_BOOST_USEC); + set_normal_priority(); +} + +void ChibiScheduler::delay(uint16_t ms) +{ + if (!in_main_thread()) { + //chprintf("ERROR: delay() from timer process\n"); + return; + } + uint64_t start = AP_HAL::micros64(); + + while ((AP_HAL::micros64() - start)/1000 < ms) { + delay_microseconds(1000); + if (_min_delay_cb_ms <= ms) { + if (_delay_cb) { + _delay_cb(); + } + } + } +} + +void ChibiScheduler::register_delay_callback(AP_HAL::Proc proc, + uint16_t min_time_ms) +{ + _delay_cb = proc; + _min_delay_cb_ms = min_time_ms; +} + +void ChibiScheduler::register_timer_process(AP_HAL::MemberProc proc) +{ + for (uint8_t i = 0; i < _num_timer_procs; i++) { + if (_timer_proc[i] == proc) { + return; + } + } + + if (_num_timer_procs < CHIBIOS_SCHEDULER_MAX_TIMER_PROCS) { + _timer_proc[_num_timer_procs] = proc; + _num_timer_procs++; + } else { + hal.console->printf("Out of timer processes\n"); + } +} + +void ChibiScheduler::register_io_process(AP_HAL::MemberProc proc) +{ + for (uint8_t i = 0; i < _num_io_procs; i++) { + if (_io_proc[i] == proc) { + return; + } + } + + if (_num_io_procs < CHIBIOS_SCHEDULER_MAX_TIMER_PROCS) { + _io_proc[_num_io_procs] = proc; + _num_io_procs++; + } else { + hal.console->printf("Out of IO processes\n"); + } +} + +void ChibiScheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us) +{ + _failsafe = failsafe; +} + +void ChibiScheduler::suspend_timer_procs() +{ + _timer_suspended = true; +} + +void ChibiScheduler::resume_timer_procs() +{ + _timer_suspended = false; + if (_timer_event_missed == true) { + _run_timers(false); + _timer_event_missed = false; + } +} +extern void Reset_Handler(); +void ChibiScheduler::reboot(bool hold_in_bootloader) +{ + // disarm motors to ensure they are off during a bootloader upload + hal.rcout->force_safety_on(); + hal.rcout->force_safety_no_wait(); + + // delay to ensure the async force_saftey operation completes + delay(500); + + // disable interrupts during reboot + chSysDisable(); + + // reboot + NVIC_SystemReset(); +} + +void ChibiScheduler::_run_timers(bool called_from_timer_thread) +{ + if (_in_timer_proc) { + return; + } + _in_timer_proc = true; + + if (!_timer_suspended) { + // now call the timer based drivers + for (int i = 0; i < _num_timer_procs; i++) { + if (_timer_proc[i]) { + _timer_proc[i](); + } + } + } else if (called_from_timer_thread) { + _timer_event_missed = true; + } + + // and the failsafe, if one is setup + if (_failsafe != nullptr) { + _failsafe(); + } + + // process analog input + ((ChibiAnalogIn *)hal.analogin)->_timer_tick(); + + _in_timer_proc = false; +} + +void ChibiScheduler::_timer_thread(void *arg) +{ + ChibiScheduler *sched = (ChibiScheduler *)arg; + sched->_timer_thread_ctx->name = "apm_timer"; + + while (!sched->_hal_initialized) { + sched->delay_microseconds(1000); + } + while (true) { + sched->delay_microseconds(1000); + + // run registered timers + sched->_run_timers(true); + + // process any pending RC output requests + //hal.rcout->timer_tick(); + + // process any pending RC input requests + ((ChibiRCInput *)hal.rcin)->_timer_tick(); + } +} + +void ChibiScheduler::_run_io(void) +{ + if (_in_io_proc) { + return; + } + _in_io_proc = true; + + if (!_timer_suspended) { + // now call the IO based drivers + for (int i = 0; i < _num_io_procs; i++) { + if (_io_proc[i]) { + _io_proc[i](); + } + } + } + + _in_io_proc = false; +} + +void ChibiScheduler::_uart_thread(void* arg) +{ + ChibiScheduler *sched = (ChibiScheduler *)arg; + sched->_uart_thread_ctx->name = "apm_uart"; + while (!sched->_hal_initialized) { + sched->delay_microseconds(1000); + } + while (true) { + sched->delay_microseconds(1000); + + // process any pending serial bytes + ((ChibiUARTDriver *)hal.uartA)->_timer_tick(); + ((ChibiUARTDriver *)hal.uartB)->_timer_tick(); + ((ChibiUARTDriver *)hal.uartC)->_timer_tick(); + /*((ChibiUARTDriver *)hal.uartD)->_timer_tick(); + ((ChibiUARTDriver *)hal.uartE)->_timer_tick(); + ((ChibiUARTDriver *)hal.uartF)->_timer_tick();*/ +#if HAL_WITH_IO_MCU + uart_io._timer_tick(); +#endif + } +} + +void ChibiScheduler::_io_thread(void* arg) +{ + ChibiScheduler *sched = (ChibiScheduler *)arg; + sched->_io_thread_ctx->name = "apm_io"; + while (!sched->_hal_initialized) { + sched->delay_microseconds(1000); + } + while (true) { + sched->delay_microseconds(1000); + + // run registered IO processes + sched->_run_io(); + } +} + +void ChibiScheduler::_storage_thread(void* arg) +{ + ChibiScheduler *sched = (ChibiScheduler *)arg; + sched->_storage_thread_ctx->name = "apm_storage"; + while (!sched->_hal_initialized) { + sched->delay_microseconds(10000); + } + while (true) { + sched->delay_microseconds(10000); + + // process any pending storage writes + ((ChibiStorage *)hal.storage)->_timer_tick(); + } +} + +bool ChibiScheduler::in_main_thread() const +{ + return get_main_thread() == chThdGetSelfX(); +} + +void ChibiScheduler::system_initialized() +{ + if (_initialized) { + AP_HAL::panic("PANIC: scheduler::system_initialized called" + "more than once"); + } + _initialized = true; +} + +#endif diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h new file mode 100644 index 0000000000..0be5caf279 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/Scheduler.h @@ -0,0 +1,108 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#pragma once + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#include "AP_HAL_ChibiOS_Namespace.h" + +#define CHIBIOS_SCHEDULER_MAX_TIMER_PROCS 8 + +#define APM_MAIN_PRIORITY_BOOST 180 // same as normal for now +#define APM_MAIN_PRIORITY 180 +#define APM_TIMER_PRIORITY 178 +#define APM_SPI_PRIORITY 179 +#define APM_CAN_PRIORITY 177 +#define APM_I2C_PRIORITY 176 +#define APM_UART_PRIORITY 60 +#define APM_STORAGE_PRIORITY 59 +#define APM_IO_PRIORITY 58 +#define APM_SHELL_PRIORITY 57 +#define APM_STARTUP_PRIORITY 10 + +/* how long to boost priority of the main thread for each main + loop. This needs to be long enough for all interrupt-level drivers + (mostly SPI drivers) to run, and for the main loop of the vehicle + code to start the AHRS update. + + Priority boosting of the main thread in delay_microseconds_boost() + avoids the problem that drivers in hpwork all happen to run right + at the start of the period where the main vehicle loop is calling + wait_for_sample(). That causes main loop timing jitter, which + reduces performance. Using the priority boost the main loop + temporarily runs at a priority higher than hpwork and the timer + thread, which results in much more consistent loop timing. +*/ +#define APM_MAIN_PRIORITY_BOOST_USEC 150 + +#define APM_MAIN_THREAD_STACK_SIZE 8192 + +/* Scheduler implementation: */ +class ChibiOS::ChibiScheduler : public AP_HAL::Scheduler { +public: + ChibiScheduler(); + /* AP_HAL::Scheduler methods */ + + + void init(); + void delay(uint16_t ms); + void delay_microseconds(uint16_t us); + void delay_microseconds_boost(uint16_t us); + void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms); + void register_timer_process(AP_HAL::MemberProc); + void register_io_process(AP_HAL::MemberProc); + void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us); + void suspend_timer_procs(); + void resume_timer_procs(); + void reboot(bool hold_in_bootloader); + + bool in_main_thread() const override; + void system_initialized(); + void hal_initialized() { _hal_initialized = true; } + +private: + bool _initialized; + volatile bool _hal_initialized; + AP_HAL::Proc _delay_cb; + uint16_t _min_delay_cb_ms; + AP_HAL::Proc _failsafe; + + volatile bool _timer_suspended; + + AP_HAL::MemberProc _timer_proc[CHIBIOS_SCHEDULER_MAX_TIMER_PROCS]; + uint8_t _num_timer_procs; + volatile bool _in_timer_proc; + + AP_HAL::MemberProc _io_proc[CHIBIOS_SCHEDULER_MAX_TIMER_PROCS]; + uint8_t _num_io_procs; + volatile bool _in_io_proc; + + volatile bool _timer_event_missed; + + thread_t* _timer_thread_ctx; + thread_t* _io_thread_ctx; + thread_t* _storage_thread_ctx; + thread_t* _uart_thread_ctx; + + static void _timer_thread(void *arg); + static void _io_thread(void *arg); + static void _storage_thread(void *arg); + static void _uart_thread(void *arg); + void _run_timers(bool called_from_timer_thread); + void _run_io(void); +}; +#endif diff --git a/libraries/AP_HAL_ChibiOS/Semaphores.cpp b/libraries/AP_HAL_ChibiOS/Semaphores.cpp new file mode 100644 index 0000000000..ab80184aee --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/Semaphores.cpp @@ -0,0 +1,57 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + +#include "Semaphores.h" + +extern const AP_HAL::HAL& hal; + +using namespace ChibiOS; + +bool Semaphore::give() +{ + chMtxUnlock(&_lock); + return true; +} + +bool Semaphore::take(uint32_t timeout_ms) +{ + if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) { + chMtxLock(&_lock); + return true; + } + if (take_nonblocking()) { + return true; + } + uint64_t start = AP_HAL::micros64(); + do { + hal.scheduler->delay_microseconds(200); + if (take_nonblocking()) { + return true; + } + } while ((AP_HAL::micros64() - start) < timeout_ms*1000); + return false; +} + +bool Semaphore::take_nonblocking() +{ + return chMtxTryLock(&_lock); +} + +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_ChibiOS/Semaphores.h b/libraries/AP_HAL_ChibiOS/Semaphores.h new file mode 100644 index 0000000000..15b7150acc --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/Semaphores.h @@ -0,0 +1,35 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#pragma once + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#include "AP_HAL_ChibiOS.h" + +class ChibiOS::Semaphore : public AP_HAL::Semaphore { +public: + Semaphore() { + chMtxObjectInit(&_lock); + } + bool give(); + bool take(uint32_t timeout_ms); + bool take_nonblocking(); +private: + mutex_t _lock; +}; +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp new file mode 100644 index 0000000000..d06e025900 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -0,0 +1,201 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + +#include + +#include "Storage.h" +#include "hwdef/common/flash.h" + +using namespace ChibiOS; + + +extern const AP_HAL::HAL& hal; + +void ChibiStorage::_storage_open(void) +{ + if (_initialised) { + return; + } + + _dirty_mask.clearall(); + +#if HAL_WITH_RAMTRON + using_fram = fram.init(); + if (using_fram) { + if (!fram.read(0, _buffer, CH_STORAGE_SIZE)) { + return; + } + _initialised = true; + return; + } + // allow for FMUv3 with no FRAM chip, fall through to flash storage +#endif + + // load from storage backend + _flash_load(); + + _initialised = true; +} + +/* + mark some lines as dirty. Note that there is no attempt to avoid + the race condition between this code and the _timer_tick() code + below, which both update _dirty_mask. If we lose the race then the + result is that a line is written more than once, but it won't result + in a line not being written. +*/ +void ChibiStorage::_mark_dirty(uint16_t loc, uint16_t length) +{ + uint16_t end = loc + length; + for (uint16_t line=loc>>CH_STORAGE_LINE_SHIFT; + line <= end>>CH_STORAGE_LINE_SHIFT; + line++) { + _dirty_mask.set(line); + } +} + +void ChibiStorage::read_block(void *dst, uint16_t loc, size_t n) +{ + if (loc >= sizeof(_buffer)-(n-1)) { + return; + } + _storage_open(); + memcpy(dst, &_buffer[loc], n); +} + +void ChibiStorage::write_block(uint16_t loc, const void *src, size_t n) +{ + if (loc >= sizeof(_buffer)-(n-1)) { + return; + } + if (memcmp(src, &_buffer[loc], n) != 0) { + _storage_open(); + memcpy(&_buffer[loc], src, n); + _mark_dirty(loc, n); + } +} + +void ChibiStorage::_timer_tick(void) +{ + if (!_initialised || _dirty_mask.empty()) { + return; + } + + + // write out the first dirty line. We don't write more + // than one to keep the latency of this call to a minimum + uint16_t i; + for (i=0; iprintf("Storage: Using flash pages %u and %u\n", _flash_page, _flash_page+1); + + if (!_flash.init()) { + AP_HAL::panic("unable to init flash storage"); + } +} + +/* + write one storage line. This also updates _dirty_mask. +*/ +void ChibiStorage::_flash_write(uint16_t line) +{ + if (_flash.write(line*CH_STORAGE_LINE_SIZE, CH_STORAGE_LINE_SIZE)) { + // mark the line clean + _dirty_mask.clear(line); + } +} + +/* + callback to write data to flash + */ +bool ChibiStorage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length) +{ + size_t base_address = stm32_flash_getpageaddr(_flash_page+sector); + bool ret = stm32_flash_write(base_address+offset, data, length) == length; + if (!ret && _flash_erase_ok()) { + // we are getting flash write errors while disarmed. Try + // re-writing all of flash + uint32_t now = AP_HAL::millis(); + if (now - _last_re_init_ms > 5000) { + _last_re_init_ms = now; + bool ok = _flash.re_initialise(); + hal.console->printf("Storage: failed at %u:%u for %u - re-init %u\n", + (unsigned)sector, (unsigned)offset, (unsigned)length, (unsigned)ok); + } + } + return ret; +} + +/* + callback to read data from flash + */ +bool ChibiStorage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length) +{ + size_t base_address = stm32_flash_getpageaddr(_flash_page+sector); + const uint8_t *b = ((const uint8_t *)base_address)+offset; + memcpy(data, b, length); + return true; +} + +/* + callback to erase flash sector + */ +bool ChibiStorage::_flash_erase_sector(uint8_t sector) +{ + return stm32_flash_erasepage(_flash_page+sector); +} + +/* + callback to check if erase is allowed + */ +bool ChibiStorage::_flash_erase_ok(void) +{ + // only allow erase while disarmed + return !hal.util->get_soft_armed(); +} + +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_ChibiOS/Storage.h b/libraries/AP_HAL_ChibiOS/Storage.h new file mode 100644 index 0000000000..bb1209ccb7 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/Storage.h @@ -0,0 +1,73 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#pragma once + +#include +#include "AP_HAL_ChibiOS_Namespace.h" +#include +#include +#include "hwdef/common/flash.h" +#include + +#define CH_STORAGE_SIZE HAL_STORAGE_SIZE + +// when using flash storage we use a small line size to make storage +// compact and minimise the number of erase cycles needed +#define CH_STORAGE_LINE_SHIFT 3 + +#define CH_STORAGE_LINE_SIZE (1<. + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#include "UARTDriver.h" +#include "GPIO.h" +#include +#include "shared_dma.h" + +extern const AP_HAL::HAL& hal; + +using namespace ChibiOS; + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_FMUV3 +#define HAVE_USB_SERIAL +#endif + +static ChibiUARTDriver::SerialDef _serial_tab[] = { +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_FMUV3 + {(BaseSequentialStream*) &SDU1, true, false, 0, 0, false, 0, 0}, //Serial 0, USB + UART4_CONFIG, // Serial 1, GPS + USART2_CONFIG, // Serial 2, telem1 + USART3_CONFIG, // Serial 3, telem2 + UART8_CONFIG, // Serial 4, GPS2 + //UART7_CONFIG, // Serial 5, debug console +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 + USART1_CONFIG, // Serial 0, debug console + USART6_CONFIG, // Serial 1, GPS + USART2_CONFIG, // Serial 2, sonix +#endif +#if HAL_WITH_IO_MCU + USART6_CONFIG, // IO MCU +#endif +}; + +// event used to wake up waiting thread +#define EVT_DATA EVENT_MASK(0) + +ChibiUARTDriver::ChibiUARTDriver(uint8_t serial_num) : +tx_bounce_buf_ready(true), +_serial_num(serial_num), +_baudrate(57600), +_is_usb(false), +_in_timer(false), +_initialised(false) +{ + _serial = _serial_tab[serial_num].serial; + _is_usb = _serial_tab[serial_num].is_usb; + _dma_rx = _serial_tab[serial_num].dma_rx; + _dma_tx = _serial_tab[serial_num].dma_tx; + chMtxObjectInit(&_write_mutex); +} + +void ChibiUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) +{ + hal.gpio->pinMode(2, HAL_GPIO_OUTPUT); + hal.gpio->pinMode(3, HAL_GPIO_OUTPUT); + if (_serial == nullptr) { + return; + } + bool was_initialised = _initialised; + uint16_t min_tx_buffer = 4096; + uint16_t min_rx_buffer = 1024; + // on PX4 we have enough memory to have a larger transmit and + // receive buffer for all ports. This means we don't get delays + // while waiting to write GPS config packets + if (txS < min_tx_buffer) { + txS = min_tx_buffer; + } + if (rxS < min_rx_buffer) { + rxS = min_rx_buffer; + } + + /* + allocate the read buffer + we allocate buffers before we successfully open the device as we + want to allocate in the early stages of boot, and cause minimum + thrashing of the heap once we are up. The ttyACM0 driver may not + connect for some time after boot + */ + if (rxS != _readbuf.get_size()) { + _initialised = false; + while (_in_timer) { + hal.scheduler->delay(1); + } + + _readbuf.set_size(rxS); + } + + if (b != 0) { + _baudrate = b; + } + + /* + allocate the write buffer + */ + if (txS != _writebuf.get_size()) { + _initialised = false; + while (_in_timer) { + hal.scheduler->delay(1); + } + _writebuf.set_size(txS); + } + + if (_is_usb) { +#ifdef HAVE_USB_SERIAL + /* + * Initializes a serial-over-USB CDC driver. + */ + if (!was_initialised) { + sduObjectInit((SerialUSBDriver*)_serial); + sduStart((SerialUSBDriver*)_serial, &serusbcfg); + /* + * Activates the USB driver and then the USB bus pull-up on D+. + * Note, a delay is inserted in order to not have to disconnect the cable + * after a reset. + */ + usbDisconnectBus(serusbcfg.usbp); + hal.scheduler->delay_microseconds(1500); + usbStart(serusbcfg.usbp, &usbcfg); + usbConnectBus(serusbcfg.usbp); + } +#endif + } else { + if (_baudrate != 0) { + //setup Rx DMA + if(!was_initialised) { + if(_dma_rx) { + rxdma = STM32_DMA_STREAM(_serial_tab[_serial_num].dma_rx_stream_id); + bool dma_allocated = dmaStreamAllocate(rxdma, + 12, //IRQ Priority + (stm32_dmaisr_t)rxbuff_full_irq, + (void *)this); + osalDbgAssert(!dma_allocated, "stream already allocated"); + dmaStreamSetPeripheral(rxdma, &((SerialDriver*)_serial)->usart->DR); + } + if (_dma_tx) { + // we only allow for sharing of the TX DMA channel, not the RX + // DMA channel, as the RX side is active all the time, so + // cannot be shared + dma_handle = new Shared_DMA(_serial_tab[_serial_num].dma_tx_stream_id, + SHARED_DMA_NONE, + FUNCTOR_BIND_MEMBER(&ChibiUARTDriver::dma_tx_allocate, void), + FUNCTOR_BIND_MEMBER(&ChibiUARTDriver::dma_tx_deallocate, void)); + } + } + sercfg.speed = _baudrate; + if (!_dma_tx && !_dma_rx) { + sercfg.cr1 = 0; + sercfg.cr3 = 0; + } else { + if (_dma_rx) { + sercfg.cr1 = USART_CR1_IDLEIE; + sercfg.cr3 = USART_CR3_DMAR; + } + if (_dma_tx) { + sercfg.cr3 |= USART_CR3_DMAT; + } + } + sercfg.cr2 = USART_CR2_STOP1_BITS; + sercfg.irq_cb = rx_irq_cb; + sercfg.ctx = (void*)this; + + sdStart((SerialDriver*)_serial, &sercfg); + if(_dma_rx) { + //Configure serial driver to skip handling RX packets + //because we will handle them via DMA + ((SerialDriver*)_serial)->usart->CR1 &= ~USART_CR1_RXNEIE; + //Start DMA + if(!was_initialised) { + uint32_t dmamode = STM32_DMA_CR_DMEIE | STM32_DMA_CR_TEIE; + dmamode |= STM32_DMA_CR_CHSEL(STM32_DMA_GETCHANNEL(_serial_tab[_serial_num].dma_rx_stream_id, + _serial_tab[_serial_num].dma_rx_channel_id)); + dmamode |= STM32_DMA_CR_PL(0); + dmaStreamSetMemory0(rxdma, rx_bounce_buf); + dmaStreamSetTransactionSize(rxdma, RX_BOUNCE_BUFSIZE); + dmaStreamSetMode(rxdma, dmamode | STM32_DMA_CR_DIR_P2M | + STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE); + dmaStreamEnable(rxdma); + } + } + } + } + + if (_writebuf.get_size() && _readbuf.get_size()) { + _initialised = true; + } + _uart_owner_thd = chThdGetSelfX(); +} + +void ChibiUARTDriver::dma_tx_allocate(void) +{ + osalDbgAssert(txdma == nullptr, "double DMA allocation"); + txdma = STM32_DMA_STREAM(_serial_tab[_serial_num].dma_tx_stream_id); + bool dma_allocated = dmaStreamAllocate(txdma, + 12, //IRQ Priority + (stm32_dmaisr_t)tx_complete, + (void *)this); + osalDbgAssert(!dma_allocated, "stream already allocated"); + dmaStreamSetPeripheral(txdma, &((SerialDriver*)_serial)->usart->DR); +} + +void ChibiUARTDriver::dma_tx_deallocate(void) +{ + chSysLock(); + dmaStreamRelease(txdma); + txdma = nullptr; + chSysUnlock(); +} + +void ChibiUARTDriver::tx_complete(void* self, uint32_t flags) +{ + ChibiUARTDriver* uart_drv = (ChibiUARTDriver*)self; + if (uart_drv->_dma_tx) { + uart_drv->dma_handle->unlock_from_IRQ(); + } + uart_drv->tx_bounce_buf_ready = true; +} + + +void ChibiUARTDriver::rx_irq_cb(void* self) +{ + ChibiUARTDriver* uart_drv = (ChibiUARTDriver*)self; + if (!uart_drv->_dma_rx) { + return; + } + volatile uint16_t sr = ((SerialDriver*)(uart_drv->_serial))->usart->SR; + if(sr & USART_SR_IDLE) { + volatile uint16_t dr = ((SerialDriver*)(uart_drv->_serial))->usart->DR; + (void)dr; + //disable dma, triggering DMA transfer complete interrupt + uart_drv->rxdma->stream->CR &= ~STM32_DMA_CR_EN; + } +} + +void ChibiUARTDriver::rxbuff_full_irq(void* self, uint32_t flags) +{ + ChibiUARTDriver* uart_drv = (ChibiUARTDriver*)self; + if (uart_drv->_lock_rx_in_timer_tick) { + return; + } + if (!uart_drv->_dma_rx) { + return; + } + uint8_t len = RX_BOUNCE_BUFSIZE - uart_drv->rxdma->stream->NDTR; + if (len == 0) { + return; + } + uart_drv->_readbuf.write(uart_drv->rx_bounce_buf, len); + //restart the DMA transfers + dmaStreamSetMemory0(uart_drv->rxdma, uart_drv->rx_bounce_buf); + dmaStreamSetTransactionSize(uart_drv->rxdma, RX_BOUNCE_BUFSIZE); + dmaStreamEnable(uart_drv->rxdma); + if (uart_drv->_wait.thread_ctx && uart_drv->_readbuf.available() >= uart_drv->_wait.n) { + chSysLockFromISR(); + chEvtSignalI(uart_drv->_wait.thread_ctx, EVT_DATA); + chSysUnlockFromISR(); + } +} + +void ChibiUARTDriver::begin(uint32_t b) +{ + begin(b, 0, 0); +} + +void ChibiUARTDriver::end() +{ + _initialised = false; + while (_in_timer) hal.scheduler->delay(1); + + if (_is_usb) { +#ifdef HAVE_USB_SERIAL + + sduStop((SerialUSBDriver*)_serial); +#endif + } else { + sdStop((SerialDriver*)_serial); + } + _readbuf.set_size(0); + _writebuf.set_size(0); +} + +void ChibiUARTDriver::flush() +{ + if (_is_usb) { +#ifdef HAVE_USB_SERIAL + + sduSOFHookI((SerialUSBDriver*)_serial); +#endif + } else { + //TODO: Handle this for other serial ports + } +} + +bool ChibiUARTDriver::is_initialized() +{ + return _initialised; +} + +void ChibiUARTDriver::set_blocking_writes(bool blocking) +{ + _nonblocking_writes = !blocking; +} + +bool ChibiUARTDriver::tx_pending() { return false; } + +/* Empty implementations of Stream virtual methods */ +uint32_t ChibiUARTDriver::available() { + if (!_initialised) { + return 0; + } + if (_is_usb) { +#ifdef HAVE_USB_SERIAL + + if (((SerialUSBDriver*)_serial)->config->usbp->state != USB_ACTIVE) { + return 0; + } +#endif + } + return _readbuf.available(); +} + +uint32_t ChibiUARTDriver::txspace() +{ + if (!_initialised) { + return 0; + } + return _writebuf.space(); +} + +int16_t ChibiUARTDriver::read() +{ + if (_uart_owner_thd != chThdGetSelfX()){ + return -1; + } + if (!_initialised) { + return -1; + } + + uint8_t byte; + if (!_readbuf.read_byte(&byte)) { + return -1; + } + + return byte; +} + +/* Empty implementations of Print virtual methods */ +size_t ChibiUARTDriver::write(uint8_t c) +{ + if (!chMtxTryLock(&_write_mutex)) { + return -1; + } + + if (!_initialised) { + chMtxUnlock(&_write_mutex); + return 0; + } + + while (_writebuf.space() == 0) { + if (_nonblocking_writes) { + chMtxUnlock(&_write_mutex); + return 0; + } + hal.scheduler->delay(1); + } + size_t ret = _writebuf.write(&c, 1); + chMtxUnlock(&_write_mutex); + return ret; +} + +size_t ChibiUARTDriver::write(const uint8_t *buffer, size_t size) +{ + if (!_initialised) { + return 0; + } + + if (!chMtxTryLock(&_write_mutex)) { + return -1; + } + + if (!_nonblocking_writes) { + /* + use the per-byte delay loop in write() above for blocking writes + */ + chMtxUnlock(&_write_mutex); + size_t ret = 0; + while (size--) { + if (write(*buffer++) != 1) break; + ret++; + } + return ret; + } + + size_t ret = _writebuf.write(buffer, size); + chMtxUnlock(&_write_mutex); + return ret; +} + +/* + wait for data to arrive, or a timeout. Return true if data has + arrived, false on timeout + */ +bool ChibiUARTDriver::wait_timeout(uint16_t n, uint32_t timeout_ms) +{ + chEvtGetAndClearEvents(EVT_DATA); + if (available() >= n) { + return true; + } + _wait.n = n; + _wait.thread_ctx = chThdGetSelfX(); + eventmask_t mask = chEvtWaitAnyTimeout(EVT_DATA, MS2ST(timeout_ms)); + return (mask & EVT_DATA) != 0; +} + +/* + push any pending bytes to/from the serial port. This is called at + 1kHz in the timer thread. Doing it this way reduces the system call + overhead in the main task enormously. + */ +void ChibiUARTDriver::_timer_tick(void) +{ + int ret; + uint32_t n; + + if (!_initialised) return; + + if (_dma_rx && rxdma) { + _lock_rx_in_timer_tick = true; + //Check if DMA is enabled + //if not, it might be because the DMA interrupt was silenced + //let's handle that here so that we can continue receiving + if (!(rxdma->stream->CR & STM32_DMA_CR_EN)) { + uint8_t len = RX_BOUNCE_BUFSIZE - rxdma->stream->NDTR; + if (len != 0) { + _readbuf.write(rx_bounce_buf, len); + if (_wait.thread_ctx && _readbuf.available() >= _wait.n) { + chEvtSignal(_wait.thread_ctx, EVT_DATA); + } + } + //DMA disabled by idle interrupt never got a chance to be handled + //we will enable it here + dmaStreamSetMemory0(rxdma, rx_bounce_buf); + dmaStreamSetTransactionSize(rxdma, RX_BOUNCE_BUFSIZE); + dmaStreamEnable(rxdma); + } + _lock_rx_in_timer_tick = false; + } + + // don't try IO on a disconnected USB port + if (_is_usb) { +#ifdef HAVE_USB_SERIAL + if (((SerialUSBDriver*)_serial)->config->usbp->state != USB_ACTIVE) { + return; + } +#endif + } + if(_is_usb) { +#ifdef HAVE_USB_SERIAL + ((ChibiGPIO *)hal.gpio)->set_usb_connected(); +#endif + } + _in_timer = true; + + { + // try to fill the read buffer + ByteBuffer::IoVec vec[2]; + + const auto n_vec = _readbuf.reserve(vec, _readbuf.space()); + for (int i = 0; i < n_vec; i++) { + //Do a non-blocking read + if (_is_usb) { + ret = 0; + #ifdef HAVE_USB_SERIAL + ret = chnReadTimeout((SerialUSBDriver*)_serial, vec[i].data, vec[i].len, TIME_IMMEDIATE); + #endif + } else if(!_dma_rx){ + ret = 0; + ret = chnReadTimeout((SerialDriver*)_serial, vec[i].data, vec[i].len, TIME_IMMEDIATE); + } else { + ret = 0; + } + if (ret < 0) { + break; + } + _readbuf.commit((unsigned)ret); + + /* stop reading as we read less than we asked for */ + if ((unsigned)ret < vec[i].len) { + break; + } + } + } + + // write any pending bytes + n = _writebuf.available(); + if (n > 0) { + if(!_dma_tx) { + ByteBuffer::IoVec vec[2]; + const auto n_vec = _writebuf.peekiovec(vec, n); + for (int i = 0; i < n_vec; i++) { + if (_is_usb) { + ret = 0; + #ifdef HAVE_USB_SERIAL + ret = chnWriteTimeout((SerialUSBDriver*)_serial, vec[i].data, vec[i].len, TIME_IMMEDIATE); + #endif + } else { + ret = chnWriteTimeout((SerialDriver*)_serial, vec[i].data, vec[i].len, TIME_IMMEDIATE); + } + if (ret < 0) { + break; + } + _writebuf.advance(ret); + + /* We wrote less than we asked for, stop */ + if ((unsigned)ret != vec[i].len) { + break; + } + } + } else { + if(tx_bounce_buf_ready) { + /* TX DMA channel preparation.*/ + _writebuf.advance(tx_len); + tx_len = _writebuf.peekbytes(tx_bounce_buf, TX_BOUNCE_BUFSIZE); + if (tx_len == 0) { + goto end; + } + dma_handle->lock(); + tx_bounce_buf_ready = false; + osalDbgAssert(txdma != nullptr, "UART TX DMA allocation failed"); + dmaStreamSetMemory0(txdma, tx_bounce_buf); + dmaStreamSetTransactionSize(txdma, tx_len); + uint32_t dmamode = STM32_DMA_CR_DMEIE | STM32_DMA_CR_TEIE; + dmamode |= STM32_DMA_CR_CHSEL(STM32_DMA_GETCHANNEL(_serial_tab[_serial_num].dma_tx_stream_id, + _serial_tab[_serial_num].dma_tx_channel_id)); + dmamode |= STM32_DMA_CR_PL(0); + dmaStreamSetMode(txdma, dmamode | STM32_DMA_CR_DIR_M2P | + STM32_DMA_CR_MINC | STM32_DMA_CR_TCIE); + dmaStreamEnable(txdma); + } else if (_dma_tx && txdma) { + if (!(txdma->stream->CR & STM32_DMA_CR_EN)) { + if (txdma->stream->NDTR == 0) { + tx_bounce_buf_ready = true; + dma_handle->unlock(); + } + } + } + } + } +end: + _in_timer = false; +} +#endif //CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h new file mode 100644 index 0000000000..ee9f1595f4 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -0,0 +1,100 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#pragma once + +#include + +#include "AP_HAL_ChibiOS.h" +#include "shared_dma.h" + +#define RX_BOUNCE_BUFSIZE 128 +#define TX_BOUNCE_BUFSIZE 64 + +class ChibiOS::ChibiUARTDriver : public AP_HAL::UARTDriver { +public: + ChibiUARTDriver(uint8_t serial_num); + + 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(); + + + uint32_t available() override; + uint32_t txspace() override; + int16_t read() override; + void _timer_tick(void); + + + size_t write(uint8_t c); + size_t write(const uint8_t *buffer, size_t size); + struct SerialDef { + BaseSequentialStream* serial; + bool is_usb; + bool dma_rx; + uint8_t dma_rx_stream_id; + uint32_t dma_rx_channel_id; + bool dma_tx; + uint8_t dma_tx_stream_id; + uint32_t dma_tx_channel_id; + }; + + bool wait_timeout(uint16_t n, uint32_t timeout_ms) override; + +private: + bool _dma_rx; + bool _dma_tx; + bool tx_bounce_buf_ready; + uint8_t _serial_num; + uint32_t _baudrate; + uint16_t tx_len; + BaseSequentialStream* _serial; + SerialConfig sercfg; + bool _is_usb; + const thread_t* _uart_owner_thd; + + struct { + // thread waiting for data + thread_t *thread_ctx; + // number of bytes needed + uint16_t n; + } _wait; + + // we use in-task ring buffers to reduce the system call cost + // of ::read() and ::write() in the main loop + uint8_t rx_bounce_buf[RX_BOUNCE_BUFSIZE]; + uint8_t tx_bounce_buf[TX_BOUNCE_BUFSIZE]; + ByteBuffer _readbuf{0}; + ByteBuffer _writebuf{0}; + mutex_t _write_mutex; + const stm32_dma_stream_t* rxdma; + const stm32_dma_stream_t* txdma; + bool _in_timer; + bool _nonblocking_writes; + bool _initialised; + bool _lock_rx_in_timer_tick = false; + Shared_DMA *dma_handle; + static void rx_irq_cb(void* sd); + static void rxbuff_full_irq(void* self, uint32_t flags); + static void tx_complete(void* self, uint32_t flags); + + void dma_tx_allocate(void); + void dma_tx_deallocate(void); +}; diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp new file mode 100644 index 0000000000..0e132e14f1 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -0,0 +1,107 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#include "Util.h" +#include + +#if HAL_WITH_IO_MCU +#include +extern AP_IOMCU iomcu; +#endif + +using namespace ChibiOS; +/** + how much free memory do we have in bytes. +*/ +uint32_t ChibiUtil::available_memory(void) +{ + size_t totalp = 0; + // get memory available on heap + chHeapStatus(nullptr, &totalp, nullptr); + + // we also need to add in memory that is not yet allocated to the heap + totalp += chCoreGetStatusX(); + + return totalp; +} + +/* + get safety switch state + */ +ChibiUtil::safety_state ChibiUtil::safety_switch_state(void) +{ +#if HAL_WITH_IO_MCU + return iomcu.get_safety_switch_state(); +#else + return SAFETY_NONE; +#endif +} + +void ChibiUtil::set_imu_temp(float current) +{ +#if HAL_WITH_IO_MCU && HAL_HAVE_IMU_HEATER + if (!heater.target || *heater.target == -1) { + return; + } + + // average over temperatures to remove noise + heater.count++; + heater.sum += current; + + // update once a second + uint32_t now = AP_HAL::millis(); + if (now - heater.last_update_ms < 1000) { + return; + } + heater.last_update_ms = now; + + current = heater.sum / heater.count; + heater.sum = 0; + heater.count = 0; + + // experimentally tweaked for Pixhawk2 + const float kI = 0.3f; + const float kP = 200.0f; + float target = (float)(*heater.target); + + // limit to 65 degrees to prevent damage + target = constrain_float(target, 0, 65); + + float err = target - current; + + heater.integrator += kI * err; + heater.integrator = constrain_float(heater.integrator, 0, 70); + + float output = constrain_float(kP * err + heater.integrator, 0, 100); + + // hal.console->printf("integrator %.1f out=%.1f temp=%.2f err=%.2f\n", heater.integrator, output, current, err); + + iomcu.set_heater_duty_cycle(output); +#endif // HAL_WITH_IO_MCU && HAL_HAVE_IMU_HEATER +} + +void ChibiUtil::set_imu_target_temp(int8_t *target) +{ +#if HAL_WITH_IO_MCU && HAL_HAVE_IMU_HEATER + heater.target = target; +#endif +} + +#endif //CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h new file mode 100644 index 0000000000..19cd919a90 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -0,0 +1,49 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#pragma once + +#include +#include "AP_HAL_ChibiOS_Namespace.h" +#include "Semaphores.h" + +class ChibiOS::ChibiUtil : public AP_HAL::Util { +public: + bool run_debug_shell(AP_HAL::BetterStream *stream) { return false; } + AP_HAL::Semaphore *new_semaphore(void) override { return new ChibiOS::Semaphore; } + uint32_t available_memory() override; + + /* + return state of safety switch, if applicable + */ + enum safety_state safety_switch_state(void) override; + + // IMU temperature control + void set_imu_temp(float current); + void set_imu_target_temp(int8_t *target); + +private: + +#if HAL_WITH_IO_MCU && HAL_HAVE_IMU_HEATER + struct { + int8_t *target; + float integrator; + uint16_t count; + float sum; + uint32_t last_update_ms; + } heater; +#endif +}; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_common.mk b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_common.mk new file mode 100644 index 0000000000..10d3b7bca7 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_common.mk @@ -0,0 +1,331 @@ +# ARM Cortex-Mx common makefile scripts and rules. +############################################################################## +# Processing options coming from the upper Makefile. +# + +# Compiler options +OPT := $(USE_OPT) +COPT := $(USE_COPT) +CPPOPT := $(USE_CPPOPT) + +# Garbage collection +ifeq ($(USE_LINK_GC),yes) + OPT += -ffunction-sections -fdata-sections -fno-common + LDOPT := ,--gc-sections +else + LDOPT := +endif + +# Linker extra options +ifneq ($(USE_LDOPT),) + LDOPT := $(LDOPT),$(USE_LDOPT) +endif + +# Link time optimizations +ifeq ($(USE_LTO),yes) + OPT += -flto +endif + +# FPU options default (Cortex-M4 and Cortex-M7 single precision). +ifeq ($(USE_FPU_OPT),) + USE_FPU_OPT = -mfloat-abi=$(USE_FPU) -mfpu=fpv4-sp-d16 -fsingle-precision-constant +endif + +# FPU-related options +ifeq ($(USE_FPU),) + USE_FPU = no +endif +ifneq ($(USE_FPU),no) + OPT += $(USE_FPU_OPT) + DDEFS += -DCORTEX_USE_FPU=TRUE + DADEFS += -DCORTEX_USE_FPU=TRUE +else + DDEFS += -DCORTEX_USE_FPU=FALSE + DADEFS += -DCORTEX_USE_FPU=FALSE +endif + +# Process stack size +ifeq ($(USE_PROCESS_STACKSIZE),) + LDOPT := $(LDOPT),--defsym=__process_stack_size__=0x400 +else + LDOPT := $(LDOPT),--defsym=__process_stack_size__=$(USE_PROCESS_STACKSIZE) +endif + +# Exceptions stack size +ifeq ($(USE_EXCEPTIONS_STACKSIZE),) + LDOPT := $(LDOPT),--defsym=__main_stack_size__=0x400 +else + LDOPT := $(LDOPT),--defsym=__main_stack_size__=$(USE_EXCEPTIONS_STACKSIZE) +endif + +# Output directory and files +ifeq ($(BUILDDIR),) + BUILDDIR = build +endif +ifeq ($(BUILDDIR),.) + BUILDDIR = build +endif +OUTFILES := $(BUILDDIR)/$(PROJECT).elf \ + $(BUILDDIR)/$(PROJECT).hex \ + $(BUILDDIR)/$(PROJECT).bin \ + $(BUILDDIR)/$(PROJECT).dmp \ + $(BUILDDIR)/$(PROJECT).list + +ifdef SREC + OUTFILES += $(BUILDDIR)/$(PROJECT).srec +endif + +# Source files groups and paths +ifeq ($(USE_THUMB),yes) + TCSRC += $(CSRC) + TCPPSRC += $(CPPSRC) +else + ACSRC += $(CSRC) + ACPPSRC += $(CPPSRC) +endif +ASRC := $(ACSRC) $(ACPPSRC) +TSRC := $(TCSRC) $(TCPPSRC) +SRCPATHS := $(sort $(dir $(ASMXSRC)) $(dir $(ASMSRC)) $(dir $(ASRC)) $(dir $(TSRC))) + +# Various directories +OBJDIR := $(BUILDDIR)/obj +LSTDIR := $(BUILDDIR)/lst + +# Object files groups +ACOBJS := $(addprefix $(OBJDIR)/, $(notdir $(ACSRC:.c=.o))) +ACPPOBJS := $(addprefix $(OBJDIR)/, $(notdir $(ACPPSRC:.cpp=.o))) +TCOBJS := $(addprefix $(OBJDIR)/, $(notdir $(TCSRC:.c=.o))) +TCPPOBJS := $(addprefix $(OBJDIR)/, $(notdir $(TCPPSRC:.cpp=.o))) +ASMOBJS := $(addprefix $(OBJDIR)/, $(notdir $(ASMSRC:.s=.o))) +ASMXOBJS := $(addprefix $(OBJDIR)/, $(notdir $(ASMXSRC:.S=.o))) +OBJS := $(ASMXOBJS) $(ASMOBJS) $(ACOBJS) $(TCOBJS) $(ACPPOBJS) $(TCPPOBJS) + +# Paths +IINCDIR := $(patsubst %,-I%,$(INCDIR) $(DINCDIR) $(UINCDIR)) +LLIBDIR := $(patsubst %,-L%,$(DLIBDIR) $(ULIBDIR)) +LLIBDIR += -L$(dir $(LDSCRIPT)) + +# Macros +DEFS := $(DDEFS) $(UDEFS) +ADEFS := $(DADEFS) $(UADEFS) + +# Libs +LIBS := $(DLIBS) $(ULIBS) + +# Various settings +MCFLAGS := -mcpu=$(MCU) +ODFLAGS = -x --syms +ASFLAGS = $(MCFLAGS) -Wa,-amhls=$(LSTDIR)/$(notdir $(<:.s=.lst)) $(ADEFS) +ASXFLAGS = $(MCFLAGS) -Wa,-amhls=$(LSTDIR)/$(notdir $(<:.S=.lst)) $(ADEFS) +CFLAGS = $(MCFLAGS) $(OPT) $(COPT) $(CWARN) -Wa,-alms=$(LSTDIR)/$(notdir $(<:.c=.lst)) $(DEFS) +CPPFLAGS = $(MCFLAGS) $(OPT) $(CPPOPT) $(CPPWARN) -Wa,-alms=$(LSTDIR)/$(notdir $(<:.cpp=.lst)) $(DEFS) +LDFLAGS = $(MCFLAGS) $(OPT) -nostartfiles $(LLIBDIR) -Wl,-Map=$(BUILDDIR)/$(PROJECT).map,--cref,--no-warn-mismatch,--library-path=$(RULESPATH)/ld,--script=$(LDSCRIPT)$(LDOPT) + +# provide a marker for ArduPilot build options in ChibiOS +CFLAGS += -D_ARDUPILOT_ + +# Thumb interwork enabled only if needed because it kills performance. +ifneq ($(strip $(TSRC)),) + CFLAGS += -DTHUMB_PRESENT + CPPFLAGS += -DTHUMB_PRESENT + ASFLAGS += -DTHUMB_PRESENT + ASXFLAGS += -DTHUMB_PRESENT + ifneq ($(strip $(ASRC)),) + # Mixed ARM and THUMB mode. + CFLAGS += -mthumb-interwork + CPPFLAGS += -mthumb-interwork + ASFLAGS += -mthumb-interwork + ASXFLAGS += -mthumb-interwork + LDFLAGS += -mthumb-interwork + else + # Pure THUMB mode, THUMB C code cannot be called by ARM asm code directly. + CFLAGS += -mno-thumb-interwork -DTHUMB_NO_INTERWORKING + CPPFLAGS += -mno-thumb-interwork -DTHUMB_NO_INTERWORKING + ASFLAGS += -mno-thumb-interwork -DTHUMB_NO_INTERWORKING -mthumb + ASXFLAGS += -mno-thumb-interwork -DTHUMB_NO_INTERWORKING -mthumb + LDFLAGS += -mno-thumb-interwork -mthumb + endif +else + # Pure ARM mode + CFLAGS += -mno-thumb-interwork + CPPFLAGS += -mno-thumb-interwork + ASFLAGS += -mno-thumb-interwork + ASXFLAGS += -mno-thumb-interwork + LDFLAGS += -mno-thumb-interwork +endif + +# Generate dependency information +ASFLAGS += -MD -MP -MF .dep/$(@F).d +ASXFLAGS += -MD -MP -MF .dep/$(@F).d +CFLAGS += -MD -MP -MF .dep/$(@F).d +CPPFLAGS += -MD -MP -MF .dep/$(@F).d + +# Paths where to search for sources +VPATH = $(SRCPATHS) + + +ifndef ECHO +T := $(shell $(MAKE) $(MAKECMDGOALS) --no-print-directory \ + -nrRf $(firstword $(MAKEFILE_LIST)) \ + ECHO="COUNTTHIS" | grep -c "COUNTTHIS") + +N := x +C = $(words $N)$(eval N := x $N) +ECHO = echo "[$C/$T] ChibiOS:" +endif +all: PRE_MAKE_ALL_RULE_HOOK $(OBJS) $(OUTFILES) POST_MAKE_ALL_RULE_HOOK + +PRE_MAKE_ALL_RULE_HOOK: + +POST_MAKE_ALL_RULE_HOOK: + +$(OBJS): | $(BUILDDIR) $(OBJDIR) $(LSTDIR) + +$(BUILDDIR): +ifneq ($(USE_VERBOSE_COMPILE),yes) + @echo Compiler Options + @echo $(CC) -c $(CFLAGS) -I. $(IINCDIR) main.c -o main.o + @echo +endif + @mkdir -p $(BUILDDIR) + +$(OBJDIR): + @mkdir -p $(OBJDIR) + +$(LSTDIR): + @mkdir -p $(LSTDIR) + +$(ACPPOBJS) : $(OBJDIR)/%.o : %.cpp +ifeq ($(USE_VERBOSE_COMPILE),yes) + @echo + $(CPPC) -c $(CPPFLAGS) $(AOPT) -I. $(IINCDIR) $< -o $@ +else + @$(ECHO) Compiling $( $@ + $(SZ) $< +else + @echo Creating $@ + @$(OD) $(ODFLAGS) $< > $@ + @echo + @$(SZ) $< +endif + +%.list: %.elf +ifeq ($(USE_VERBOSE_COMPILE),yes) + $(OD) -S $< > $@ +else + @echo Creating $@ + @$(OD) -S $< > $@ + @echo + @echo Done +endif + +lib: $(OBJS) $(BUILDDIR)/lib$(PROJECT).a pass + +$(BUILDDIR)/lib$(PROJECT).a: $(OBJS) + @$(AR) -r $@ $^ + @echo + @echo ChibiOS: Done! + +pass: $(BUILDDIR) + @echo $(foreach f,$(IINCDIR),"$(f);") > $(BUILDDIR)/include_dirs + +clean: CLEAN_RULE_HOOK + @echo Cleaning + -rm -fR .dep $(BUILDDIR) + @echo + @echo Done + +CLEAN_RULE_HOOK: + +# +# Include the dependency files, should be the last of the makefile +# +-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*) + +# *** EOF *** diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c new file mode 100644 index 0000000000..eb003148b0 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c @@ -0,0 +1,324 @@ +/************************************************************************************ + * Copyright (C) 2011 Uros Platise. All rights reserved. + * Author: Uros Platise + * + * 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 NuttX 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 OWNER 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. + * + ************************************************************************************/ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ + +#include "flash.h" +#include "hal.h" + +// #pragma GCC optimize("O0") + +/* + this driver has been tested with STM32F427 and STM32F412 + */ + +#ifndef BOARD_FLASH_SIZE +#error "You must define BOARD_FLASH_SIZE in kbyte" +#endif + +#define KB(x) ((x*1024)) +// Refer Flash memory map in the User Manual to fill the following fields per microcontroller +#define STM32_FLASH_BASE 0x08000000 +#define STM32_FLASH_SIZE KB(BOARD_FLASH_SIZE) + +// optionally disable interrupts during flash writes +#define STM32_FLASH_DISABLE_ISR 0 + +// the 2nd bank of flash needs to be handled differently +#define STM32_FLASH_BANK2_START (STM32_FLASH_BASE+0x00080000) + + +#if BOARD_FLASH_SIZE == 512 +#define STM32_FLASH_NPAGES 7 +static const uint32_t flash_memmap[STM32_FLASH_NPAGES] = { KB(16), KB(16), KB(16), KB(16), KB(64), + KB(128), KB(128), KB(128) }; + +#elif BOARD_FLASH_SIZE == 1024 +#define STM32_FLASH_NPAGES 12 +static const uint32_t flash_memmap[STM32_FLASH_NPAGES] = { KB(16), KB(16), KB(16), KB(16), KB(64), + KB(128), KB(128), KB(128), KB(128), KB(128), KB(128), KB(128) }; + +#elif BOARD_FLASH_SIZE == 2048 +#define STM32_FLASH_NPAGES 24 +static const uint32_t flash_memmap[STM32_FLASH_NPAGES] = { KB(16), KB(16), KB(16), KB(16), KB(64), + KB(128), KB(128), KB(128), KB(128), KB(128), KB(128), KB(128), + KB(16), KB(16), KB(16), KB(16), KB(64), + KB(128), KB(128), KB(128), KB(128), KB(128), KB(128), KB(128)}; +#endif + +// keep a cache of the page addresses +static uint32_t flash_pageaddr[STM32_FLASH_NPAGES]; +static bool flash_pageaddr_initialised; + + +#define FLASH_KEY1 0x45670123 +#define FLASH_KEY2 0xCDEF89AB +/* Some compiler options will convert short loads and stores into byte loads + * and stores. We don't want this to happen for IO reads and writes! + */ +/* # define getreg16(a) (*(volatile uint16_t *)(a)) */ +static inline uint16_t getreg16(unsigned int addr) +{ + uint16_t retval; + __asm__ __volatile__("\tldrh %0, [%1]\n\t" : "=r"(retval) : "r"(addr)); + return retval; +} + +/* define putreg16(v,a) (*(volatile uint16_t *)(a) = (v)) */ +static inline void putreg16(uint16_t val, unsigned int addr) +{ + __asm__ __volatile__("\tstrh %0, [%1]\n\t": : "r"(val), "r"(addr)); +} + +static void stm32_flash_wait_idle(void) +{ + while (FLASH->SR & FLASH_SR_BSY) { + // nop + } +} + +static void stm32_flash_unlock(void) +{ + stm32_flash_wait_idle(); + + if (FLASH->CR & FLASH_CR_LOCK) { + /* Unlock sequence */ + FLASH->KEYR = FLASH_KEY1; + FLASH->KEYR = FLASH_KEY2; + } + + // disable the data cache - see stm32 errata 2.1.11 + FLASH->ACR &= ~FLASH_ACR_DCEN; +} + +void stm32_flash_lock(void) +{ + stm32_flash_wait_idle(); + FLASH->CR |= FLASH_CR_LOCK; + + // reset and re-enable the data cache - see stm32 errata 2.1.11 + FLASH->ACR |= FLASH_ACR_DCRST; + FLASH->ACR &= ~FLASH_ACR_DCRST; + FLASH->ACR |= FLASH_ACR_DCEN; +} + + + +/* + get the memory address of a page + */ +uint32_t stm32_flash_getpageaddr(uint32_t page) +{ + if (page >= STM32_FLASH_NPAGES) { + return 0; + } + + if (!flash_pageaddr_initialised) { + uint32_t address = STM32_FLASH_BASE; + uint8_t i; + + for (i = 0; i < STM32_FLASH_NPAGES; i++) { + flash_pageaddr[i] = address; + address += stm32_flash_getpagesize(i); + } + flash_pageaddr_initialised = true; + } + + return flash_pageaddr[page]; +} + +/* + get size in bytes of a page + */ +uint32_t stm32_flash_getpagesize(uint32_t page) +{ + return flash_memmap[page]; +} + +/* + return total number of pages + */ +uint32_t stm32_flash_getnumpages() +{ + return STM32_FLASH_NPAGES; +} + +static bool stm32_flash_ispageerased(uint32_t page) +{ + uint32_t addr; + uint32_t count; + + if (page >= STM32_FLASH_NPAGES) { + return false; + } + + for (addr = stm32_flash_getpageaddr(page), count = stm32_flash_getpagesize(page); + count; count--, addr++) { + if ((*(volatile uint8_t *)(addr)) != 0xff) { + return false; + } + } + + return true; +} + +/* + erase a page + */ +bool stm32_flash_erasepage(uint32_t page) +{ + if (page >= STM32_FLASH_NPAGES) { + return false; + } + +#if STM32_FLASH_DISABLE_ISR + syssts_t sts = chSysGetStatusAndLockX(); +#endif + stm32_flash_wait_idle(); + stm32_flash_unlock(); + + // clear any previous errors + FLASH->SR = 0xF3; + + stm32_flash_wait_idle(); + + // the snb mask is not contiguous, calculate the mask for the page + uint8_t snb = (((page % 12) << 3) | ((page / 12) << 7)); + + FLASH->CR = FLASH_CR_PSIZE_1 | snb | FLASH_CR_SER; + FLASH->CR |= FLASH_CR_STRT; + + stm32_flash_wait_idle(); + + if (FLASH->SR) { + // an error occurred + FLASH->SR = 0xF3; + stm32_flash_lock(); +#if STM32_FLASH_DISABLE_ISR + chSysRestoreStatusX(sts); +#endif + return false; + } + + stm32_flash_lock(); +#if STM32_FLASH_DISABLE_ISR + chSysRestoreStatusX(sts); +#endif + + return stm32_flash_ispageerased(page); +} + + +int32_t stm32_flash_write(uint32_t addr, const void *buf, uint32_t count) +{ + uint16_t *hword = (uint16_t *)buf; + uint32_t written = count; + + /* STM32 requires half-word access */ + if (count & 1) { + return -1; + } + + if ((addr+count) >= STM32_FLASH_BASE+STM32_FLASH_SIZE) { + return -1; + } + + /* Get flash ready and begin flashing */ + + if (!(RCC->CR & RCC_CR_HSION)) { + return -1; + } + +#if STM32_FLASH_DISABLE_ISR + syssts_t sts = chSysGetStatusAndLockX(); +#endif + + stm32_flash_unlock(); + + // clear previous errors + FLASH->SR = 0xF3; + + /* TODO: implement up_progmem_write() to support other sizes than 16-bits */ + FLASH->CR &= ~(FLASH_CR_PSIZE); + FLASH->CR |= FLASH_CR_PSIZE_0 | FLASH_CR_PG; + + for (;count; count -= 2, hword++, addr += 2) { + /* Write half-word and wait to complete */ + + putreg16(*hword, addr); + + stm32_flash_wait_idle(); + + if (FLASH->SR) { + // we got an error + FLASH->SR = 0xF3; + FLASH->CR &= ~(FLASH_CR_PG); + goto failed; + } + + if (getreg16(addr) != *hword) { + FLASH->CR &= ~(FLASH_CR_PG); + goto failed; + } + } + + FLASH->CR &= ~(FLASH_CR_PG); + + stm32_flash_lock(); +#if STM32_FLASH_DISABLE_ISR + chSysRestoreStatusX(sts); +#endif + return written; + +failed: + stm32_flash_lock(); +#if STM32_FLASH_DISABLE_ISR + chSysRestoreStatusX(sts); +#endif + return -1; +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h new file mode 100644 index 0000000000..84dde1c935 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.h @@ -0,0 +1,30 @@ + + +/* + * Copyright (C) Siddharth Bharat Purohit 2017 + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "hal.h" + +#ifdef __cplusplus +extern "C" { +#endif +uint32_t stm32_flash_getpageaddr(uint32_t page); +uint32_t stm32_flash_getpagesize(uint32_t page); +uint32_t stm32_flash_getnumpages(void); +bool stm32_flash_erasepage(uint32_t page); +int32_t stm32_flash_write(uint32_t addr, const void *buf, uint32_t count); +#ifdef __cplusplus +} +#endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c new file mode 100644 index 0000000000..0725f1e28a --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c @@ -0,0 +1,67 @@ +/* + * Copyright (C) Siddharth Bharat Purohit 2017 + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +// High Resolution Timer + +#include "ch.h" +#include "hal.h" +#include "hrt.h" +#include +/* + * HRT GPT configuration + */ + + +//static void hrt_cb(GPTDriver*); +static uint64_t timer_base = 0; +static const GPTConfig hrtcfg = { + 1000000, /* 1MHz timer clock.*/ + NULL, /* Timer callback.*/ + 0, + 0 +}; + + +void hrt_init() +{ + gptStart(&HRT_TIMER, &hrtcfg); + gptStartContinuous(&HRT_TIMER, 0xFFFF); +} + +uint64_t hrt_micros() +{ + static volatile uint64_t last_micros = 0; + static volatile uint64_t micros; + + syssts_t sts = chSysGetStatusAndLockX(); + micros = timer_base + (uint64_t)gptGetCounterX(&HRT_TIMER); + // we are doing this to avoid an additional interupt routing + // since we are definitely going to get called atleast once in + // a full timer period + if(last_micros > micros) { + timer_base += 0x10000; + micros += 0x10000; + } + last_micros = micros; + chSysRestoreStatusX(sts); + return micros; +} + +/* +static void hrt_cb(GPTDriver* gptd) +{ + timer_base += 0x10000; +} +*/ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h new file mode 100644 index 0000000000..302c685241 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h @@ -0,0 +1,11 @@ +#pragma once + + +#ifdef __cplusplus +extern "C" { +#endif +void hrt_init(); +uint64_t hrt_micros(); +#if __cplusplus +} +#endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c new file mode 100644 index 0000000000..167a74646b --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c @@ -0,0 +1,54 @@ +/* + * Copyright (C) Siddharth Bharat Purohit 2017 + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +/* + wrappers for allocation functions + + Relies on linker wrap options + + Note that not all functions that have been wrapped are implemented + here. The others are wrapped to ensure the function is not used + without an implementation. If we need them then we can implement as + needed. + */ + +#include +#include +#include +#include +#include + +#define MIN_ALIGNMENT 8 + +void *malloc(size_t size) +{ + return chHeapAllocAligned(NULL, size, MIN_ALIGNMENT); +} + +void *calloc(size_t nmemb, size_t size) +{ + void *p = chHeapAllocAligned(NULL, nmemb*size, MIN_ALIGNMENT); + if (p != NULL) { + memset(p, 0, nmemb*size); + } + return p; +} + +void free(void *ptr) +{ + if(ptr != NULL) { + chHeapFree(ptr); + } +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/posix.c b/libraries/AP_HAL_ChibiOS/hwdef/common/posix.c new file mode 100644 index 0000000000..21ebcce6fe --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/posix.c @@ -0,0 +1,2521 @@ +/** + @file posix.c + + @brief POSIX wrapper for FatFS + - Provides many of the common Posix/linux functions + - POSIX character I/O functions + - isatty + - fgetc + - fputc + - getchar + - putc + - putchar + - ungetc + + - POSIX string I/O functions + - fgets + - fputs + - puts + + - POSIX file position functions + - feof + - fgetpos + - fseek + - fsetpos + - ftell + - lseek + - rewind + + - POSIX file functions + - close + - fileno + - fileno_to_stream NOT POSIX + - fopen + - fread + - ftruncate + - fwrite + - open + - read + - sync + - syncfs + - truncate + - write + - fclose + + - POSIX file information functions + - dump_stat - NOT POSIX + - fstat + - mctime - NOT POSIX + - stat + + - POSIX file and directory manipulation + - basename + - baseext - NOT POSIX + - chmod + - chdir + - dirname + - getcwd + - mkdir + - rename + - rmdir + - unlink + - utime + + - POSIX - directory scanning functions + - closedir + - opendir + - readdir + + - POSIX error functions + - clrerror + - ferror + - perror + - strerror + - strerror_r + + - Device open functions + - fdevopen - NOT POSIX + + - FatFS to POSIX bridge functions - NOT POSIX + - fatfs_getc + - fatfs_putc + - fatfs_to_errno + - fatfs_to_fileno + - fat_time_to_unix + - fileno_to_fatfs + - free_file_descriptor + - new_file_descriptor + - mkfs + - posix_fopen_modes_to_open + - unix_time_to_fat + + @par Copyright © 2015 Mike Gore, GPL License + @par You are free to use this code under the terms of GPL + please retain a copy of this notice in any code you use it in. + +This is free software: you can redistribute it and/or modify it under the +terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) +any later version. + +This software is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . +*/ + +#include +#include "posix.h" +#undef strerror_r + +/// Note: fdevopen assigns stdin,stdout,stderr + +///@brief POSIX errno. +int errno; + + +///@brief POSIX fileno to POSIX FILE stream table +/// +/// - Note: the index of __iob[] is reffered to "fileno". +/// - Reference: libc/avr-libc-1.8.0/libc/stdio. +/// - stdin = __iob[0]. +/// - __iob[1] = stdout. +/// - __iob[2] = stderr. +FILE *__iob[MAX_FILES]; + +/// @brief POSIX error messages for each errno value. +/// +/// - man page errno (3) +const char *sys_errlist[] = +{ + "OK", + "Operation not permitted", + "No such file or directory", + "No such process", + "Interrupted system call", + "I/O error", + "No such device or address", + "Argument list too long", + "Exec format error", + "Bad file number", + "No child processes", + "Try again", + "Out of memory", + "Permission denied", + "Bad address", + "Block device required", + "Device or resource busy", + "File exists", + "Cross-device link", + "No such device", + "Not a directory", + "Is a directory", + "Invalid argument", + "File table overflow", + "Too many open files", + "Not a typewriter", + "Text file busy", + "File too large", + "No space left on device", + "Illegal seek", + "Read-only file system", + "Too many links", + "Broken pipe", + "Math argument out of domain of func", + "Math result not representable", + "Bad Message", + NULL +}; + +// ============================================= +/// - POSIX character I/O functions +/// @brief Test POSIX fileno if it is a Serial Console/TTY. +/// +/// - man page isatty (3). +/// +/// @param[in] fileno: POSIX fileno of open file. +/// +/// @return 1 if fileno is a serial TTY/Console (uart in avr-libc terms). +/// @return 0 if POSIX fileno is NOT a Serial TTY. + +int isatty(int fileno) +{ +/// @todo Perhaps we should verify console functions have been added ? + if(fileno >= 0 && fileno <= 2) + return(1); + return 0; +} + +/// @brief Get byte from a TTY device or FatFs file stream +/// open() or fopen() sets stream->get = fatfs_getc() for FatFs functions +/// See fdevopen() sets stream->get for TTY devices +/// +/// - man page fgetc (3). +/// +/// @param[in] stream: POSIX stream pointer. +/// +/// @return character. +/// @return EOF on error with errno set. + +int +fgetc(FILE *stream) +{ + int c; + + if(stream == NULL) + { + errno = EBADF; // Bad File Number + return(EOF); + } + + if ((stream->flags & __SRD) == 0) + return EOF; + + if ((stream->flags & __SUNGET) != 0) { + stream->flags &= ~__SUNGET; + stream->len++; + return stream->unget; + } + + if (stream->flags & __SSTR) { + c = *stream->buf; + if (c == '\0') { + stream->flags |= __SEOF; + return EOF; + } else { + stream->buf++; + } + } else { + if(!stream->get) + { + printf("fgetc stream->get NULL\n"); + return(EOF); + } + // get character from device or file + c = stream->get(stream); + if (c < 0) { + /* if != _FDEV_ERR, assume its _FDEV_EOF */ + stream->flags |= (c == _FDEV_ERR)? __SERR: __SEOF; + return EOF; + } + } + + stream->len++; + return (c); +} + +/// @brief Put a byte to TTY device or FatFs file stream +/// open() or fopen() sets stream->put = fatfs_outc() for FatFs functions +/// See fdevopen() sets stream->put get for TTY devices +/// +/// - man page fputc (3). +/// +/// @param[in] stream: POSIX stream pointer. +/// +/// @return character. + +int +fputc(int c, FILE *stream) +{ + errno = 0; + int ret; + + if(stream == NULL) + { + errno = EBADF; // Bad File Number + return(EOF); + } + + + if(stream != stdout && stream != stderr) + { + return(fatfs_putc(c,stream)); + } + + // TTY outputs + + if ((stream->flags & __SWR) == 0) + return EOF; + + if (stream->flags & __SSTR) { + if (stream->len < stream->size) + *stream->buf++ = c; + stream->len++; + return c; + } else { + if(!stream->put) + { + printf("fputc stream->put NULL\n"); + return(EOF); + } + ret = stream->put(c, stream); + if(ret != EOF) + stream->len++; + return(ret); + } +} + + + +///@brief functions normally defined as macros +#ifndef IO_MACROS +/// @brief get a character from stdin +/// See fdevopen() sets stream->get for TTY devices +/// +/// - man page getchar (3). +/// +/// @param[in] stream: POSIX stream pointer. +/// +/// @return character. + +int +getchar() +{ + return(fgetc(stdin)); +} + +/// @brief put a character to stdout +/// See fdevopen() sets stream->put get for TTY devices +/// +/// - man page putchar (3). +/// +/// @param[in] stream: POSIX stream pointer. +/// +/// @return character. + +int +putchar(int c) +{ + return(fputc(c,stdout)); +} +#endif + +/// @brief Un-Get byte from a TTY device or FatFs file stream +/// +/// - man page ungetc (3). +/// +/// @param[in] c: Character to unget +/// @param[in] stream: POSIX stream pointer. +/// +/// @return character. +/// @return EOF on error with errno set. +/* +int +ungetc(int c, FILE *stream) +{ + int fd = fileno(stream); + if(!isatty(fd)) + return(EOF); + + if(c == EOF) + return EOF; + if((stream->flags & __SUNGET) != 0 ) + return EOF; + if ((stream->flags & __SRD) == 0 ) + return EOF; + + stream->flags |= __SUNGET; + stream->flags &= ~__SEOF; + + stream->unget = c; + stream->len--; + + return (c); +} +*/ +#ifndef IO_MACROS +// ============================================= +/// @brief Put a character to a stream +/// See fdevopen() sets stream->put get for TTY devices +/// +/// - man page putc (3). +/// +/// @param[in] stream: POSIX stream pointer. +/// + +int +putc(int c, FILE *stream) +{ + return(fputc(c, stream)); +} + +#endif + +// ============================================= +/// POSIX string I/O +/// @brief get a string from stdin +/// See fdevopen() sets stream->put get for TTY devices +/// +/// - man page fgets (3). +/// +/// @param[in] stream: POSIX stream pointer. +/// +/// @return character. + +char * +fgets(char *str, int size, FILE *stream) +{ + int c; + int ind = 0; + while(size--) + { + c = fgetc(stream); + if(c == EOF) + { + if( ind == 0) + return(NULL); + break; + } + if(c == '\n') + break; + if(c == 0x08) + { + if(ind > 0) + --ind; + continue; + } + str[ind++] = c; + } + str[ind] = 0; + return(str); +} + +/// @brief put a string to stdout +/// See fdevopen() sets stream->put get for TTY devices +/// +/// - man page fputs (3). +/// +/// @param[in] stream: POSIX stream pointer. +/// +/// @return character. + +int +fputs(const char *str, FILE *stream) +{ + while(*str) + { + if(fputc(*str, stream) == EOF) + return(EOF); + ++str; + } + return(0); +} + + +#ifndef IO_MACROS +/// @brief put a string to stdout +/// See fdevopen() sets stream->put get for TTY devices +/// +/// - man page puts (3). +/// +/// @param[in] stream: POSIX stream pointer. +/// +/// @return character. + +int +puts(const char *str) +{ + while(*str) + { + if(fputc(*str, stdout) == EOF) + return(EOF); + ++str; + } + return ( fputc('\n',stdout) ); +} + +#endif + + +// ============================================= +// ============================================= +/// - POSIX file position functions +// ============================================= +// ============================================= +/// @brief feof reports if the stream is at EOF +/// - man page feof (3). +/// +/// @param[in] stream: POSIX stream pointer. +/// @return 1 if EOF set, 0 otherwise. + +int feof(FILE *stream) +{ + if(stream->flags & __SEOF) + return(1); + return(0); +} + +/// @brief POSIX get position of file stream. +/// +/// - man page fgetpos (3). +/// +/// @param[in] stream: POSIX file stream. +/// @param[in] pos: position pointer for return. +/// +/// @return 0 on sucess. +/// @return -1 on error with errno set. + +int fgetpos(FILE *stream, size_t *pos) +{ + long offset = ftell(stream); + *pos = offset; + if(offset == -1) + return(-1); + return( 0 ); +} + +/// @brief POSIX seek to file possition. +/// +/// - man page fseek (3). +/// +/// @param[in] stream: POSIX file stream. +/// @param[in] offset: offset to seek to. +/// @param[in] whence: +/// - SEEK_SET The offset is set to offset bytes. +/// - SEEK_CUR The offset is set to its current location plus offset bytes. +/// - SEEK_END The offset is set to the size of the file plus offset bytes. +/// +/// @return file position on sucess. +/// @return -1 on error. + +int fseek(FILE *stream, long offset, int whence) +{ + long ret; + + int fn = fileno(stream); + if(fn < 0) + return(-1); + + ret = lseek(fn, offset, whence); + + if(ret == -1) + return(-1); + + return(0); +} + +/// @brief POSIX set position of file stream. +/// +/// - man page fsetpos (3). +/// +/// @param[in] stream: POSIX file stream. +/// @param[in] pos: position pointer. +/// +/// @return 0 with *pos set to position on sucess. +/// @return -1 on error with errno set. + +int fsetpos(FILE *stream, size_t *pos) +{ + return (fseek(stream, (size_t) *pos, SEEK_SET) ); +} + +/// @brief POSIX file position of open stream. +/// +/// - man page fteel (3). +/// +/// @param[in] stream: POSIX file stream. +/// +/// @return file position on sucess. +/// @return -1 on error with errno set. + +long ftell(FILE *stream) +{ + errno = 0; + + int fn = fileno(stream); + if(isatty(fn)) + return(-1); + // fileno_to_fatfs checks for fd out of bounds + FIL *fh = fileno_to_fatfs(fn); + if ( fh == NULL ) + { + errno = EBADF; + return(-1); + } + + return( fh->fptr ); +} + +/// @brief POSIX seek to file position. +/// +/// - man page lseek (2). +/// +/// @param[in] fileno: POSIX fileno of open file. +/// @param[in] position: offset to seek to. +/// @param[in] whence +/// - SEEK_SET The offset is set to offset bytes. +/// - SEEK_CUR The offset is set to its current location plus offset bytes. +/// - SEEK_END The offset is set to the size of the file plus offset bytes. +/// +/// @return file position on sucess. +/// @return -1 on error. + +off_t lseek(int fileno, off_t position, int whence) +{ + FRESULT res; + FIL *fh; + errno = 0; + FILE *stream; + + + // fileno_to_fatfs checks for fd out of bounds + fh = fileno_to_fatfs(fileno); + if(fh == NULL) + { + errno = EMFILE; + return(-1); + } + if(isatty(fileno)) + return(-1); + + + stream = fileno_to_stream(fileno); + stream->flags |= __SUNGET; + + if(whence == SEEK_END) + position += f_size(fh); + else if(whence==SEEK_CUR) + position += fh->fptr; + + res = f_lseek(fh, position); + if(res) + { + errno = fatfs_to_errno(res); + return -1; + } + return (fh->fptr); +} + +/// @brief POSIX rewind file to the beginning. +/// +/// - man page rewind (3). +/// +/// @param[in] stream: POSIX file stream. +/// +/// @return void. + +void rewind( FILE *stream) +{ + fseek(stream, 0L, SEEK_SET); +} + +// ============================================= +// ============================================= +/// - POSIX file functions +// ============================================= +// ============================================= +/// @brief POSIX Close a file with fileno handel. +/// +/// - man page close (2). +/// +/// @param[in] fileno: fileno of file. +/// +/// @return 0 on sucess. +/// @return -1 on error with errno set. + +int close(int fileno) +{ + FILE *stream; + FIL *fh; + int res; + + errno = 0; + + // checks if fileno out of bounds + stream = fileno_to_stream(fileno); + if(stream == NULL) + { + return(-1); + } + + // fileno_to_fatfs checks for fileno out of bounds + fh = fileno_to_fatfs(fileno); + if(fh == NULL) + { + return(-1); + } + res = f_close(fh); + free_file_descriptor(fileno); + if (res != FR_OK) + { + errno = fatfs_to_errno(res); + return(-1); + } + return(0); +} + +/// @brief Convert POSIX stream pointer to POSIX fileno (index of __iob[]) +/// +/// - man page fileno (3) +/// @param[in] stream: stream pointer +/// +/// @return int fileno on success +/// @return -1 with errno = EBAFD if stream is NULL or not found + +int fileno(FILE *stream) +{ + int fileno; + + if(stream == NULL) + { + errno = EBADF; + return(-1); + } + + for(fileno=0; fileno= MAX_FILES) + { + errno = EBADF; + return(NULL); + } + + stream = __iob[fileno]; + if(stream == NULL) + { + errno = EBADF; + return(NULL); + } + return(stream); +} + +///@brief POSIX Open a file with path name and ascii file mode string. +/// +/// - man page fopen(3). +/// +/// @param[in] path: filename string. +/// @param[in] mode: POSIX open mode strings. +/// +/// @return stream * on success. +/// @return NULL on error with errno set. + +FILE *fopen(const char *path, const char *mode) +{ + int flags = posix_fopen_modes_to_open(mode); + int fileno = open(path, flags); + + // checks if fileno out of bounds + return( fileno_to_stream(fileno) ); +} + +/// @brief POSIX read nmemb elements from buf, size bytes each, to the stream fd. +/// +/// - man page fread (3). +/// +/// @param[in] ptr: buffer. +/// @param[in] nmemb: number of items to read. +/// @param[in] size: size of each item in bytes. +/// @param[in] stream: POSIX file stream. +/// +/// @return count on sucess. +/// @return 0 or < size on error with errno set. + +size_t fread(void *ptr, size_t size, size_t nmemb, FILE *stream) +{ + size_t count = size * nmemb; + int fn = fileno(stream); + ssize_t ret; + + // read() checks for fn out of bounds + ret = read(fn, ptr, count); + if(ret < 0) + return(0); + + return((size_t) ret); +} + +/// @brief POSIX truncate open file to length. +/// +/// - man page ftruncate (3). +/// +/// @param[in] fd: open file number. +/// @param[in] length: length to truncate to. +/// +/// @return 0 on success. +/// @return -1 on fail. + +int ftruncate(int fd, off_t length) +{ + errno = 0; + FIL *fh; + FRESULT rc; + + if(isatty(fd)) + return(-1); + // fileno_to_fatfs checks for fd out of bounds + fh = fileno_to_fatfs(fd); + if(fh == NULL) + { + return(-1); + } + rc = f_lseek(fh, length); + if (rc != FR_OK) + { + errno = fatfs_to_errno(rc); + return(-1); + } + rc = f_truncate(fh); + if (rc != FR_OK) + { + errno = fatfs_to_errno(rc); + return(-1); + } + return(0); +} + +/// @brief POSIX write nmemb elements from buf, size bytes each, to the stream fd. +/// +/// - man page write (2). +/// +/// @param[in] ptr: buffer. +/// @param[in] nmemb: number of items to write. +/// @param[in] size: size of each item in bytes. +/// @param[in] stream: POSIX file stream. +/// +/// @return count written on sucess. +/// @return 0 or < size on error with errno set. + +size_t fwrite(const void *ptr, size_t size, size_t nmemb, FILE *stream) +{ + size_t count = size * nmemb; + int fn = fileno(stream); + ssize_t ret; + + // write () checks for fn out of bounds + ret = write(fn, ptr, count); + + if(ret < 0) + return(0); + + return((size_t) ret); +} + + + +/// @brief POSIX Open a file with integer mode flags. +/// +/// - man page open (2). +/// +/// @param[in] pathname: filename string. +/// @param[in] flags: POSIX open modes. +/// +/// @return fileno on success. +/// @return -1 on error with errno set. + +int open(const char *pathname, int flags) +{ + int fileno; + int fatfs_modes; + FILE *stream; + FIL *fh; + int res; + + errno = 0; +// FIXME Assume here that the disk interface mmc_init was already called +#if 0 +// Checks Disk status + res = mmc_init(0); + + if(res != RES_OK) + { + errno = fatfs_to_errno(res); + return(-1); + } +#endif + + if((flags & O_ACCMODE) == O_RDWR) + fatfs_modes = FA_READ | FA_WRITE; + else if((flags & O_ACCMODE) == O_RDONLY) + fatfs_modes = FA_READ; + else + fatfs_modes = FA_WRITE; + + if(flags & O_CREAT) + { + if(flags & O_TRUNC) + fatfs_modes |= FA_CREATE_ALWAYS; + else + fatfs_modes |= FA_OPEN_ALWAYS; + } + + fileno = new_file_descriptor(); + + // checks if fileno out of bounds + stream = fileno_to_stream(fileno); + if(stream == NULL) + { + free_file_descriptor(fileno); + return(-1); + } + + // fileno_to_fatfs checks for fileno out of bounds + fh = fileno_to_fatfs(fileno); + if(fh == NULL) + { + free_file_descriptor(fileno); + errno = EBADF; + return(-1); + } + res = f_open(fh, pathname, (BYTE) (fatfs_modes & 0xff)); + if(res != FR_OK) + { + errno = fatfs_to_errno(res); + free_file_descriptor(fileno); + return(-1); + } + if(flags & O_APPEND) + { +/// Seek to end of the file + res = f_lseek(fh, f_size(fh)); + if (res != FR_OK) + { + errno = fatfs_to_errno(res); + f_close(fh); + free_file_descriptor(fileno); + return(-1); + } + } + + if((flags & O_ACCMODE) == O_RDWR) + { + // FIXME fdevopen should do this + stream->put = fatfs_putc; + stream->get = fatfs_getc; + stream->flags = _FDEV_SETUP_RW; + } + else if((flags & O_ACCMODE) == O_RDONLY) + { + // FIXME fdevopen should do this + stream->put = NULL; + stream->get = fatfs_getc; + stream->flags = _FDEV_SETUP_READ; + } + else + { + // FIXME fdevopen should do this + stream->put = fatfs_putc; + stream->get = NULL; + stream->flags = _FDEV_SETUP_WRITE; + } + + return(fileno); +} + +/// @brief POSIX read count bytes from *buf to fileno fd. +/// +/// - man page read (2). +/// +/// @param[in] fd: POSIX fileno. +/// @param[in] buf: buffer. +/// @param[in] count: number of bytes to write. +/// +/// @return count on sucess. +/// @return -1 on error with errno set. + +ssize_t read(int fd, void *buf, size_t count) +{ + UINT size; + UINT bytes = count; + int res; + int ret; + FIL *fh; + FILE *stream; + + //FIXME + *(char *) buf = 0; + + errno = 0; + + // TTY read function + // FIXME should we really be blocking ??? + stream = fileno_to_stream(fd); + if(stream == stdin) + { + char *ptr = (char *) buf; + // ungetc is undefined for read + stream->flags |= __SUNGET; + size = 0; + while(count--) + { + ret = fgetc(stream); + if(ret < 0) + break; + + *ptr++ = ret; + ++size; + } + return(size); + } + if(stream == stdout || stream == stderr) + { + return(-1); + } + + // fileno_to_fatfs checks for fd out of bounds + fh = fileno_to_fatfs(fd); + if ( fh == NULL ) + { + errno = EBADF; + return(-1); + } + + res = f_read(fh, (void *) buf, bytes, &size); + if(res != FR_OK) + { + errno = fatfs_to_errno(res); + return(-1); + } + return ((ssize_t) size); +} + + +/// @brief POSIX Sync all pending file changes and metadata on ALL files. +/// +/// - man page sync (2). +/// +/// @return void. + +void sync(void) +{ + FIL *fh; + int i; + + for(i=0;iflags |= __SUNGET; + + // fileno_to_fatfs checks for fd out of bounds + fh = fileno_to_fatfs(fd); + if(fh == NULL) + { + errno = EBADF; + return(-1); + } + + res = f_sync ( fh ); + if (res != FR_OK) + { + errno = fatfs_to_errno(res); + return(-1); + } + return(0); +} + + + +/// @brief POSIX truncate named file to length. +/// +/// - man page truncate (2). +/// +/// @param[in] path: file name to truncate. +/// @param[in] length: length to truncate to. +/// +/// @return 0 on sucess. +/// @return -1 n fail. + +int truncate(const char *path, off_t length) +{ + errno = 0; + FIL fh; + FRESULT rc; + + rc = f_open(&fh , path, FA_OPEN_EXISTING | FA_READ | FA_WRITE); + if (rc != FR_OK) + { + errno = fatfs_to_errno(rc); + return(-1); + } + rc = f_lseek(&fh, length); + if (rc != FR_OK) + { + errno = fatfs_to_errno(rc); + return(-1); + } + rc = f_truncate(&fh); + if (rc != FR_OK) + { + errno = fatfs_to_errno(rc); + return(-1); + } + return(0); +} + + +/// @brief POSIX Write count bytes from *buf to fileno fd. +/// +/// - man page write (2). +/// +/// @param[in] fd: POSIX fileno. +/// @param[in] buf: buffer. +/// @param[in] count: number of bytes to write. +/// @return count on sucess. +/// @return -1 on error with errno set. + +ssize_t write(int fd, const void *buf, size_t count) +{ + UINT size; + UINT bytes = count; + FRESULT res; + FIL *fh; + FILE *stream; + errno = 0; + + // TTY read function + stream = fileno_to_stream(fd); + if(stream == stdout || stream == stderr) + { + char *ptr = (char *) buf; + size = 0; + while(count--) + { + int c,ret; + c = *ptr++; + ret = fputc(c, stream); + if(c != ret) + break; + + ++size; + } + return(size); + } + if(stream == stdin) + { + return(-1); + } + + // fileno_to_fatfs checks for fd out of bounds + fh = fileno_to_fatfs(fd); + if ( fh == NULL ) + { + errno = EBADF; + return(-1); + } + + res = f_write(fh, buf, bytes, &size); + if(res != FR_OK) + { + errno = fatfs_to_errno(res); + return(-1); + } + return ((ssize_t) size); +} + + +/// @brief POSIX close a file stream. +/// +/// - man page flose (3). +/// +/// @param[in] stream: POSIX stream pointer. + +/// @return 0 on sucess. +/// @return -1 on error witrh errno set. +/* +int fclose(FILE *stream) +{ + int fn = fileno(stream); + if(fn < 0) + return(EOF); + + return( close(fn) ); +} +*/ +// ============================================= +// ============================================= +/// - POSIX file information functions +// ============================================= +// ============================================= + +/// @brief Display struct stat, from POSIX stat(0 or fstat(), in ASCII. +/// NOT POSIX +/// @param[in] sp: struct stat pointer. +/// +/// @return void. +/* +void dump_stat(struct stat *sp) +{ + mode_t mode = sp->st_mode; + + printf("\tSize: %lu\n", (uint32_t)sp->st_size); + + printf("\tType: "); + if(S_ISDIR(mode)) + printf("DIR\n"); + else if(S_ISREG(mode)) + printf("File\n"); + else + printf("Unknown\n"); + + + printf("\tMode: %lo\n", (uint32_t)sp->st_mode); + printf("\tUID: %lu\n", (uint32_t)sp->st_uid); + printf("\tGID: %lu\n", (uint32_t)sp->st_gid); + printf("\tatime: %s\n",mctime((time_t)sp->st_atime)); + printf("\tmtime: %s\n",mctime((time_t)sp->st_mtime)); + printf("\tctime: %s\n",mctime((time_t)sp->st_ctime)); +} +*/ +#if 0 +/// @brief POSIX fstat of open file. +/// FatFS does not have a function that will map to this +/// +/// - man page (2). +/// +/// @param[in] fd: POSIX fileno of open file. +/// @param[in] buf: struct stat buffer to return results in. +/// +/// @return 0 on success. +/// @return -1 on error with errno set. +/// +/// @todo needs fileno to filename lookup in order to work. +/// - We may be able to work out the directory pointer from the FatFS data? +/// - or - cache the filename on open ??? + +int fstat(int fd, struct stat *buf) +{ + FIL *fh; + FRESULT rc; + + if(isatty(fd)) + return(-1); + + //FIXME TODO + return(-1); + +} +#endif + +/// @brief POSIX stat - get file status of named file. +/// +/// - man page (2). +/// +/// @param[in] name: file name. +/// @param[in] buf: struct stat buffer to return results in. +/// +/// @return 0 on success. +/// @return -1 on error with errno set. + +int stat(const char *name, struct stat *buf) +{ + FILINFO info; + int res; + time_t epoch; + uint16_t mode; + errno = 0; + + // f_stat does not handle / or . as root directory + if (strcmp(name,"/") == 0 || strcmp(name,".") == 0) + { + buf->st_atime = 0; + buf->st_mtime = 0; + buf->st_ctime = 0; + buf->st_uid= 0; + buf->st_gid= 0; + buf->st_size = 0; + buf->st_mode = S_IFDIR; + return(0); + } + + res = f_stat(name, &info); + if(res != FR_OK) + { + errno = fatfs_to_errno(res); + return(-1); + } + + buf->st_size = info.fsize; + epoch = fat_time_to_unix(info.fdate, info.ftime); + buf->st_atime = epoch; // Access time + buf->st_mtime = epoch; // Modification time + buf->st_ctime = epoch; // Creation time + + // We only handle read only case + mode = (FATFS_R | FATFS_X); + if( !(info.fattrib & AM_RDO)) + mode |= (FATFS_W); // enable write if NOT read only + + if(info.fattrib & AM_SYS) + { + buf->st_uid= 0; + buf->st_gid= 0; + } + { + buf->st_uid=1000; + buf->st_gid=1000; + } + + if(info.fattrib & AM_DIR) + mode |= S_IFDIR; + else + mode |= S_IFREG; + buf->st_mode = mode; + + return(0); +} + +///@brief Set Modification and Access time of a file +///@param[in] filename: file name +///@param[in *times: access and modication utimbuf structure, if NULL use current time +///@return 0 if ok, -1 on error + +int utime(const char *filename, const struct utimbuf *times) +{ + + FILINFO fno; + uint16_t fdate,ftime; + time_t ut = 0; + int res; + + if(times) + ut = times->modtime; + + + unix_time_to_fat(ut, (uint16_t *) &fdate, (uint16_t *) &ftime); + + + fno.fdate = fdate; + fno.ftime = ftime; + + res = f_utime(filename, (FILINFO *) &fno); + + return( fatfs_to_errno(res) ); +} + +int64_t fs_getfree() { + FATFS *fs; + DWORD fre_clust, fre_sect, tot_sect; + + + /* Get volume information and free clusters of drive 1 */ + FRESULT res = f_getfree("/", &fre_clust, &fs); + if (res) return(res); + + /* Get total sectors and free sectors */ + fre_sect = fre_clust * fs->csize; + return (int64_t)(fre_sect)*512; +} + + +int64_t fs_gettotal() { + FATFS *fs; + DWORD fre_clust, fre_sect, tot_sect; + + + /* Get volume information and free clusters of drive 1 */ + FRESULT res = f_getfree("/", &fre_clust, &fs); + if (res) return(res); + + /* Get total sectors and free sectors */ + tot_sect = (fs->n_fatent - 2) * fs->csize; + return (int64_t)(tot_sect)*512; +} +// ============================================= +// ============================================= +/// - POSIX file and directory manipulation +// ============================================= +// ============================================= +/// @brief POSIX Basename of filename. +/// +/// - man page (3). +/// +/// @param[in] str: string to find basename in. +/// +/// @return pointer to basename of string. + +char *basename(const char *str) +{ + const char *base = str; + if(!str) + return(""); + while(*str) + { + if(*str++ == '/') + base = str; + } + + return (char*)base; +} + +/// @brief File extention of a file name. +/// NOT POSIX +/// +/// @param[in] str: string to find extension in. +/// +/// @return pointer to basename extension. + +char *baseext(char *str) +{ + char *ext = ""; + + while(*str) + { + if(*str++ == '.') + ext = str; + } + return(ext); +} + + +/// @brief POSIX change directory. +/// +/// - man page chdir (2). +/// +/// @param[in] pathname: directory to change to +/// +/// @return 0 on sucess. +/// @return -1 on error with errno set. + +int chdir(const char *pathname) +{ + errno = 0; + + int res = f_chdir(pathname); + if(res != FR_OK) + { + errno = fatfs_to_errno(res); + return(-1); + } + return(0); +} + +/// @brief POSIX chmod function - change file access permission +/// Unfortunately file f_open modes and f_chmod modes are not the same +/// Files that are open have way more options - but only while the file is open. +/// - so this is a bit of a hack - we can only set read only - if on one has write perms +/// +/// - man page chmod (2). +/// +/// @param[in] pathname: filename string. +/// @param[in] mode: POSIX chmod modes. +/// +/// @return fileno on success. + +int chmod(const char *pathname, mode_t mode) +{ + int rc; + errno = 0; + + // FIXME for now we combine user,group and other perms and ask if anyone has write perms ? + + // Read only ??? + if ( !( mode & ( S_IWUSR | S_IWGRP | S_IWOTH))) + { + // file is read only + rc = f_chmod(pathname, AM_RDO, AM_RDO); + if (rc != FR_OK) + { + errno = fatfs_to_errno(rc); + return(-1); + } + } + + return(0); +} + +/// @brief POSIX directory name of a filename. +/// Return the index of the last '/' character. +/// +/// - Example: +/// @code +/// dir[0] = 0; +/// ret = dirname(path) +/// if(ret) +/// strncpy(dir,path,ret); +/// @endcode +/// +/// @param[in] str: string to examine. +/// @return 0 if no directory part. +/// @return index of last '/' character. +/// + +int dirname(char *str) +{ + int end = 0; + int ind = 0; + + if(!str) + return(0); + + while(*str) + { + if(*str == '/') + end = ind; + ++str; + ++ind; + } + return(end); +} + +#if 0 +/// @brief POSIX fchmod function - change file access permission +/// FatFS does not have a function that will map to this +/// +/// - man page fchmod (2). +/// +/// @param[in] fd: file handle +/// @param[in] mode: POSIX chmod modes. +/// +/// @return fileno on success. + +int fchmod(int fd, mode_t mode) +{ + //FIXME TODO + return (-1); +} +#endif + +/// @brief POSIX get current working directory +/// +/// - man page getcwd (2). +/// +/// @param[in] pathname: directory to change to +/// +/// @return 0 on sucess. +/// @return -1 on error with errno set. + +char *getcwd(char *pathname, int len) +{ + int res; + errno = 0; + + res = f_getcwd(pathname, len); + if(res != FR_OK) + { + errno = fatfs_to_errno(res); + return(NULL); + } + return(pathname); +} + +/// @brief POSIX make a directory. +/// +/// - man page mkdir (2). +/// +/// @param[in] pathname: directory to create +/// +/// @return 0 on sucess. +/// @return -1 on error with errno set. + +int mkdir(const char *pathname, mode_t mode) +{ + errno = 0; + + if(mode) + { + if(chmod(pathname, mode)) + return(-1); + } + + int res = f_mkdir(pathname); + if(res != FR_OK) + { + errno = fatfs_to_errno(res); + return(-1); + } + return(0); +} + +/// @brief POSIX rename a file by name. +/// +/// - man page (2). +/// +/// @param[in] oldpath: original name. +/// @param[in] newpath: new name. +/// +/// @return 0 on success. +/// @return -1 on error with errno set. + +int rename(const char *oldpath, const char *newpath) +{ +/* Rename an object */ + int rc; + errno = 0; + rc = f_rename(oldpath, newpath); + if(rc) + { + errno = fatfs_to_errno(rc); + return(-1); + } + return(0); +} + +/// @brief POSIX delete a directory. +/// +/// - man page rmdir (2). +/// +/// @param[in] pathname: directory to delete. +/// +/// @return 0 on sucess. +/// @return -1 on error with errno set. + +int rmdir(const char *pathname) +{ + errno = 0; + int res = f_unlink(pathname); + if(res != FR_OK) + { + errno = fatfs_to_errno(res); + return(-1); + } + return(0); +} + + +/// @brief POSIX delete a file. +/// +/// - man page unlink (2). +/// +/// @param[in] pathname: filename to delete. +/// +/// @return 0 on sucess. +/// @return -1 on error with errno set. + +int unlink(const char *pathname) +{ + errno = 0; + int res = f_unlink(pathname); + if(res != FR_OK) + { + errno = fatfs_to_errno(res); + return(-1); + } + return(0); +} + +// ============================================= +// ============================================= +/// - POSIX - directory scanning functions +// ============================================= +// ============================================= +/// @brief POSIX closedir +/// - man page closedir (2). +/// +/// @param[in] dirp: DIR * directory handle +/// +/// @return 0 on sucess. +/// @return -1 on error with errno set. +int closedir(DIR *dirp) +{ + int res = f_closedir (dirp); + if(res != FR_OK) + { + errno = fatfs_to_errno(res); + return(-1); + } + return(0); +} + +/// @brief POSIX opendir +/// - man page opendir (2). +/// +/// @param[in] pathname: directory to delete. +/// +/// @return DIR * on sucess. +/// @return NULL on error with errno set. +static DIR _dp; +DIR *opendir(const char *pathdir) +{ + int res = f_opendir((DIR *) &_dp, pathdir); + if(res != FR_OK) + { + errno = fatfs_to_errno(res); + return(NULL); + } + return ((DIR *) &_dp); +} + +/// @brief POSIX opendir +/// - man page readdir(2). +/// +/// @param[in] dirp: DIR * directory handle +/// +/// @return DIR * on sucess. +/// @return NULL on error with errno set. +static dirent_t _de; +dirent_t * readdir(DIR *dirp) +{ + FILINFO fno; + int len; + int res; + + _de.d_name[0] = 0; + res = f_readdir ( dirp, &fno ); + if(res != FR_OK || fno.fname[0] == 0) + { + errno = fatfs_to_errno(res); + return(NULL); + } + len = strlen(fno.fname); + strncpy(_de.d_name,fno.fname,len); + _de.d_name[len] = 0; + return( (dirent_t *) &_de); +} + +// ============================================= +// ============================================= +/// - POSIX error functions +// ============================================= +// ============================================= + +/// @brief clrerror resets stream EOF and error flags +/// - man page clrerror(3). +/// +/// @param[in] stream: POSIX stream pointer. +/// @return EOF on error with errno set. + +void clrerror(FILE *stream) +{ + stream->flags &= ~__SEOF; + stream->flags &= ~__SERR; +} + +/// @brief ferror reports if the stream has an error flag set +/// - man page ferror (3). +/// +/// @param[in] stream: POSIX stream pointer. +/// @return 1 if EOF set, 0 otherwise. + +int ferror(FILE *stream) +{ + if(stream->flags & __SERR) + return(1); + return(0); +} + +/// @brief POSIX perror() - convert POSIX errno to text with user message. +/// +/// - man page errno (3). +/// +/// @param[in] s: User message displayed before the error message +/// +/// @see sys_errlist[]. +/// @return void. + +void perror(const char *s) +{ + const char *ptr = NULL; + + + if(errno >=0 && errno < EBADMSG) + ptr = sys_errlist[errno]; + else + ptr = sys_errlist[EBADMSG]; + + if(s && *s) + printf("%s: %s\n", s, ptr); + else + printf("%s\n", ptr); +} + +/// @brief POSIX strerror() - convert POSIX errno to text with user message. +/// +/// - man page strerror (3). +/// +/// @param[in] errnum: index for sys_errlist[] +/// +/// @see sys_errlist[]. +/// @return char * + +char *strerror(int errnum) +{ + return( (char *)sys_errlist[errnum] ); +} + + +/// @brief POSIX strerror_r() - convert POSIX errno to text with user message. +/// +/// - man page strerror (3). +/// +/// @param[in] errnum: index for sys_errlist[] +/// @param[in] buf: user buffer for error message +/// @param[in] buflen: length of user buffer for error message +/// +/// @see sys_errlist[]. +/// @return char * + +char *__wrap_strerror_r(int errnum, char *buf, size_t buflen) +{ + strncpy(buf, sys_errlist[errnum], buflen); + return(buf); +} + + +// ============================================= +// ============================================= +/// Device open functions +// ============================================= +// ============================================= +/// @brief Assign stdin,stdout,stderr or any use defined I/O +/// NOT POSIX +/// +/// @param[in] *put: uart putc function pointer +/// @param[in] *get: uart gutc function pointer + +FILE * +fdevopen(int (*put)(char, FILE *), int (*get)(FILE *)) +{ + FILE *s; + + if (put == 0 && get == 0) + return 0; + + if ((s = calloc(1, sizeof(FILE))) == 0) + return 0; + + s->flags = __SMALLOC; + + if (get != 0) { + s->get = get; + s->flags |= __SRD; + // We assign the first device with a read discriptor to stdin + // Only assign once + if (stdin == 0) + stdin = s; + } + + if (put != 0) { + s->put = put; + s->flags |= __SWR; + // NOTE: We assign the first device with a write to both STDOUT andd STDERR + + // Only assign in unassigned + if (stdout == 0) + stdout = s; + if (stderr == 0) + stderr = s; + } + + return s; +} + + +// ============================================= +// ============================================= +/// - FatFS to POSIX bridge functions +// ============================================= +// ============================================= + +/// @brief Formt SD card +/// @param[in] *name: device name +/// @retrun void +/* +int mkfs(char *name) +{ + FATFS fs; + uint8_t *mem; + int res; + int len; + int c; + char dev[4]; + + len = MATCH(name,"/dev/sd"); + if(!len) + { + printf("Expected /dev/sda .. /dev/sdj\n"); + return(0); + } + // Convert /dev/sd[a-j] to 0: .. 9: + dev[1] = ':'; + dev[2] = 0; + c = tolower( name[len-1] ); + if(c >= 'a' && c <= ('a' + 9)) + dev[0] = (c - 'a'); + dev[3] = 0; + + // Register work area to the logical drive 0: + res = f_mount(&fs, dev, 0); + if(!res) + { + put_rc(res); + return(0); + } + + // Allocate memory for mkfs function + mem = malloc(1024); + if(!mem) + return(0); + + // Create FAT volume on the logical drive 0 + // 2nd argument is ignored. + res = f_mkfs(dev, FM_FAT32, 0, mem, 1024); + if(res) + { + put_rc(res); + free(mem); + return(0); + } + free(mem); + return(1); +} +*/ +/// @brief Private FatFs function called by fgetc() to get a byte from file stream +/// FIXME buffer this function call +/// NOT POSIX +/// open() assigns stream->get = fatfs_getc() +/// +/// - man page fgetc (3). +/// - Notes: fgetc does all tests prior to caling us, including ungetc. +/// +/// @param[in] stream: POSIX stream pointer. +/// +/// @return character. +/// @return EOF on error with errno set. + +int fatfs_getc(FILE *stream) +{ + FIL *fh; + UINT size; + int res; + uint8_t c; + long pos; + + errno = 0; + + if(stream == NULL) + { + errno = EBADF; // Bad File Number + return(EOF); + } + + fh = (FIL *) fdev_get_udata(stream); + if(fh == NULL) + { + errno = EBADF; // Bad File Number + return(EOF); + } + + res = f_read(fh, &c, 1, (UINT *) &size); + if( res != FR_OK || size != 1) + { + errno = fatfs_to_errno(res); + stream->flags |= __SEOF; + return(EOF); + } + + // AUTOMATIC end of line METHOD detection + // ALWAYS return '\n' for ALL methods + // History: End of line (EOL) characters sometimes differ, mostly legacy systems, and modern UNIX (which uses just '\n') + // '\r' ONLY + // '\r\n' + // '\n' + // The difference was mostly from the way old mechanical printers were controlled. + // '\n' (New Line = NL) advanced the line + // '\r' (Charage Return = CR) moved the print head to start of line + // '\t' (Tabstop = TAB) + // '\f' (Form feed = FF) + // The problem with mechanical devices is that each had differing control and time delays to deal with. + // (TAB, CR, NL and FF) did different things and took differing times depending on the device. + // + // Long before DOS UNIX took the position that controlling physical devices must be a device drivers problem only. + // They reasoned if users had to worry about all the ugly controll and timing issues no code would be portable. + // Therefore they made NL just a SYMBOL for the driver to determine what to do. + // This design philosophy argued if you needed better control its better to use a real designed purposed tool for it. + // (ie. like curses or termcap). + + // Here to deal with those other old ugly stupid pointless EOL methods we convert to just a symbol '\n' + // FROM '\n' OR '\r'char OR '\r\n' TO '\n' + // Note: char != '\n' + if(c == '\r') + { + // PEEK forward 1 character + pos = f_tell(fh); + // Check for trailing '\n' or EOF + res = f_read(fh, &c, 1, (UINT *) &size); + if(res != FR_OK || size != 1) + { + // '\r' with EOF impiles '\n' + return('\n'); + } + // This file must be '\r' ONLY for end of line + if(c != '\n') + { + // Not '\n' or EOF o move file pointer back to just after the '\r' + f_lseek(fh, pos); + return('\n'); + } + c = '\n'; + } + return(c & 0xff); +} + +/// @brief Private FatFs function called by fputc() to put a byte from file stream +/// NOT POSIX +/// open() assigns stream->put = fatfs_putc() +/// +/// - man page fputc (3). +/// - Notes: fputc does all tests prior to caling us. +/// +/// @param[in] c: character. +/// @param[in] stream: POSIX stream pointer. +/// +/// @return character +/// @return EOF on error with errno set. + +int fatfs_putc(char c, FILE *stream) +{ + int res; + FIL *fh; + UINT size; + + errno = 0; + if(stream == NULL) + { + errno = EBADF; // Bad File Number + return(EOF); + } + + fh = (FIL *) fdev_get_udata(stream); + if(fh == NULL) + { + errno = EBADF; // Bad File Number + return(EOF); + } + + res = f_write(fh, &c, 1, (UINT *) &size); + if( res != FR_OK || size != 1) + { + errno = fatfs_to_errno(res); + stream->flags |= __SEOF; + return(EOF); + } + return(c); +} + +/// @brief Convert FafFs error result to POSIX errno. +/// NOT POSIX +/// +/// - man page errno (3). +/// +/// @param[in] Result: FatFs Result code. +/// +/// @return POSIX errno. +/// @return EBADMSG if no conversion possible. + +int fatfs_to_errno( FRESULT Result ) +{ + switch( Result ) + { + case FR_OK: /* FatFS (0) Succeeded */ + return (0); /* POSIX OK */ + case FR_DISK_ERR: /* FatFS (1) A hard error occurred in the low level disk I/O layer */ + return (EIO); /* POSIX Input/output error (POSIX.1) */ + + case FR_INT_ERR: /* FatFS (2) Assertion failed */ + return (EPERM); /* POSIX Operation not permitted (POSIX.1) */ + + case FR_NOT_READY: /* FatFS (3) The physical drive cannot work */ + return (EBUSY); /* POSIX Device or resource busy (POSIX.1) */ + + case FR_NO_FILE: /* FatFS (4) Could not find the file */ + return (ENOENT); /* POSIX No such file or directory (POSIX.1) */ + + case FR_NO_PATH: /* FatFS (5) Could not find the path */ + return (ENOENT); /* POSIX No such file or directory (POSIX.1) */ + + case FR_INVALID_NAME: /* FatFS (6) The path name format is invalid */ + return (EINVAL); /* POSIX Invalid argument (POSIX.1) */ + + case FR_DENIED: /* FatFS (7) Access denied due to prohibited access or directory full */ + return (EACCES); /* POSIX Permission denied (POSIX.1) */ + case FR_EXIST: /* FatFS (8) Access denied due to prohibited access */ + return (EACCES); /* POSIX Permission denied (POSIX.1) */ + + case FR_INVALID_OBJECT: /* FatFS (9) The file/directory object is invalid */ + return (EINVAL); /* POSIX Invalid argument (POSIX.1) */ + + case FR_WRITE_PROTECTED: /* FatFS (10) The physical drive is write protected */ + return(EROFS); /* POSIX Read-only filesystem (POSIX.1) */ + + case FR_INVALID_DRIVE: /* FatFS (11) The logical drive number is invalid */ + return(ENXIO); /* POSIX No such device or address (POSIX.1) */ + + case FR_NOT_ENABLED: /* FatFS (12) The volume has no work area */ + return (ENOSPC); /* POSIX No space left on device (POSIX.1) */ + + case FR_NO_FILESYSTEM: /* FatFS (13) There is no valid FAT volume */ + return(ENXIO); /* POSIX No such device or address (POSIX.1) */ + + case FR_MKFS_ABORTED: /* FatFS (14) The f_mkfs() aborted due to any parameter error */ + return (EINVAL); /* POSIX Invalid argument (POSIX.1) */ + + case FR_TIMEOUT: /* FatFS (15) Could not get a grant to access the volume within defined period */ + return (EBUSY); /* POSIX Device or resource busy (POSIX.1) */ + + case FR_LOCKED: /* FatFS (16) The operation is rejected according to the file sharing policy */ + return (EBUSY); /* POSIX Device or resource busy (POSIX.1) */ + + + case FR_NOT_ENOUGH_CORE: /* FatFS (17) LFN working buffer could not be allocated */ + return (ENOMEM); /* POSIX Not enough space (POSIX.1) */ + + case FR_TOO_MANY_OPEN_FILES:/* FatFS (18) Number of open files > _FS_SHARE */ + return (EMFILE); /* POSIX Too many open files (POSIX.1) */ + + case FR_INVALID_PARAMETER:/* FatFS (19) Given parameter is invalid */ + return (EINVAL); /* POSIX Invalid argument (POSIX.1) */ + + } + return (EBADMSG); /* POSIX Bad message (POSIX.1) */ +} + + +/// @brief Convert FatFS file handle to POSIX fileno. +/// NOT POSIX +/// +/// @param[in] fh: FatFS file pointer. +/// +/// @return fileno on success. +/// @return -1 on error with errno set to EBADF. + +int fatfs_to_fileno(FIL *fh) +{ + int i; + + FILE *stream; + + if(fh == NULL) + { + errno = EBADF; + return(-1); + } + + for(i=0;itm_year < 70) { + return((time_t)-1); + } + + n = t->tm_year + 1900 - 1; + epoch = (t->tm_year - 70) * YEAR + + ((n / 4 - n / 100 + n / 400) - (1969 / 4 - 1969 / 100 + 1969 / 400)) * DAY; + + y = t->tm_year + 1900; + m = 0; + + for(i = 0; i < t->tm_mon; i++) { + epoch += mon [m] * DAY; + if(m == 1 && y % 4 == 0 && (y % 100 != 0 || y % 400 == 0)) + epoch += DAY; + + if(++m > 11) { + m = 0; + y++; + } + } + + epoch += (t->tm_mday - 1) * DAY; + epoch += t->tm_hour * HOUR + t->tm_min * MINUTE + t->tm_sec; + + return epoch; +} + +/// @brief Convert FatFs file date and time to POSIX epoch seconds. +/// NOT POSIX +/// +/// - man page timegm (3). +/// +/// @param[in] date: FatFs date. +/// @param[in] time: FatFs time. +/// +/// @see timegm() +/// +/// @return epoch seconds + +time_t fat_time_to_unix(uint16_t date, uint16_t time) +{ + struct tm tp; + time_t unix; + + memset(&tp, 0, sizeof(struct tm)); + + tp.tm_sec = (time << 1) & 0x3e; // 2 second resolution + tp.tm_min = ((time >> 5) & 0x3f); + tp.tm_hour = ((time >> 11) & 0x1f); + tp.tm_mday = (date & 0x1f); + tp.tm_mon = ((date >> 5) & 0x0f) - 1; + tp.tm_year = ((date >> 9) & 0x7f) + 80; + unix = replace_mktime( &tp ); + return( unix ); +} + +/// @brief Convert Linux POSIX time_t to FAT32 date and time. +/// NOT POSIX +/// - man page gmtime (3). +/// @param[in] epoch: unix epoch seconds +/// @param[in] *date: fat32 date +/// @param[in] *time: fat32 time +/// @return void + +void unix_time_to_fat(time_t epoch, uint16_t *date, uint16_t *time) +{ + struct tm *t = gmtime((time_t *) &epoch); + +/* Pack date and time into a uint32_t variable */ + *date = ((uint16_t)(t->tm_year - 80) << 9) + | (((uint16_t)t->tm_mon+1) << 5) + | (((uint16_t)t->tm_mday)); + + *time = ((uint16_t)t->tm_hour << 11) + | ((uint16_t)t->tm_min << 5) + | ((uint16_t)t->tm_sec >> 1); +} + +/// @brief Convert POSIX fileno to FatFS handle +/// NOT POSIX +/// +/// - FatFS file handle is pointed to by the avr-libc stream->udata. +/// +/// @param[in] fileno: fileno of file +/// +/// @return FIL * FatFS file handle on success. +/// @return NULL if POSIX fileno is invalid NULL + +FIL *fileno_to_fatfs(int fileno) +{ + FILE *stream; + FIL *fh; + + if(isatty( fileno )) + { + errno = EBADF; + return(NULL); + } + + // checks if fileno out of bounds + stream = fileno_to_stream(fileno); + if( stream == NULL ) + return(NULL); + + fh = fdev_get_udata(stream); + if(fh == NULL) + { + errno = EBADF; + return(NULL); + } + return(fh); +} + + + +/// @brief Free POSIX fileno FILE descriptor. +/// NOT POSIX +/// +/// @param[in] fileno: POSIX file number __iob[] index. +/// +/// @return fileno on success. +/// @return -1 on failure. + +int free_file_descriptor(int fileno) +{ + FILE *stream; + FIL *fh; + + if(isatty( fileno )) + { + errno = EBADF; + return(-1); + } + + // checks if fileno out of bounds + stream = fileno_to_stream(fileno); + if(stream == NULL) + { + return(-1); + } + + fh = fdev_get_udata(stream); + + if(fh != NULL) + { + free(fh); + } + + if(stream->buf != NULL && stream->flags & __SMALLOC) + { + free(stream->buf); + } + + __iob[fileno] = NULL; + free(stream); + return(fileno); +} + + + +// ============================================= +/// @brief Allocate a POSIX FILE descriptor. +/// NOT POSIX +/// +/// @return fileno on success. +/// @return -1 on failure with errno set. + +int new_file_descriptor( void ) +{ + int i; + FILE *stream; + FIL *fh; + + for(i=0;isent++; + fputc(ch, (FILE *) p->buffer); +} + +*/ +/// @brief fprintf function +/// Example user defined printf function using fputc for I/O +/// This method allows I/O to devices and strings without typical C++ overhead +/// @param[in] *fp: FILE stream pointer +/// @param[in] fmt: printf forat string +/// @param[in] ...: vararg list or arguments +/// @return size of printed result + +int +fprintf(FILE *fp, const char *fmt, ...) +{ + va_list va; + char* buf; + int16_t len, i; + va_start(va, fmt); + len = vasprintf(&buf, fmt, va); + if (len > 0) { + for(i = 0; i < len; i++) { + fputc(buf[i], fp); + } + } else { + return -1; + } + va_end(va); + + return len; +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/posix.h b/libraries/AP_HAL_ChibiOS/hwdef/common/posix.h new file mode 100644 index 0000000000..42239823be --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/posix.h @@ -0,0 +1,388 @@ +/** + @file fatfs/posix.h + + @brief POSIX wrapper for FatFS + + @par Copyright © 2015 Mike Gore, GPL License + @par You are free to use this code under the terms of GPL + please retain a copy of this notice in any code you use it in. + +This is free software: you can redistribute it and/or modify it under the +terms of the GNU General Public License as published by the Free Software +Foundation, either version 3 of the License, or (at your option) +any later version. + +This software is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . +*/ + +#ifndef _POSIX_H_ +#define _POSIX_H_ +#include +#ifdef USE_POSIX +#define POSIX +#pragma GCC diagnostic ignored "-Wshadow" +///@brief make sure we use our EDOM and ERANGE values +#undef EDOM +#undef ERANGE +#include +#include +#include +#include +#include +///@brief make sure we use our strerror_r function + +#ifdef __cplusplus +extern "C" { +#endif +// ============================================= +///@brief Standard POSIX typedefs. +/// +/// - Using these makes code portable accross many acrchitectures +//typedef uint32_t blkcnt_t; /*< blkcnt_t for this architecture */ +//typedef uint32_t blksize_t; /*< blksize_t for this architecture */ +extern int errno; +// ============================================= + +// @brief posix errno values +enum POSIX_errno +{ + EOK, /*< 0 NO ERROR */ + EPERM, /*< 1 Operation not permitted */ + ENOENT, /*< 2 No such file or directory */ + ESRCH, /*< 3 No such process */ + EINTR, /*< 4 Interrupted system call */ + EIO, /*< 5 I/O error */ + ENXIO, /*< 6 No such device or address */ + E2BIG, /*< 7 Argument list too long */ + ENOEXEC, /*< 8 Exec format error */ + EBADF, /*< 9 Bad file number */ + ECHILD, /*< 10 No child processes */ + EAGAIN, /*< 11 Try again */ + ENOMEM, /*< 12 Out of memory */ + EACCES, /*< 13 Permission denied */ + EFAULT, /*< 14 Bad address */ + ENOTBLK, /*< 15 Block device required */ + EBUSY, /*< 16 Device or resource busy */ + EEXIST, /*< 17 File exists */ + EXDEV, /*< 18 Cross-device link */ + ENODEV, /*< 19 No such device */ + ENOTDIR, /*< 20 Not a directory */ + EISDIR, /*< 21 Is a directory */ + EINVAL, /*< 22 Invalid argument */ + ENFILE, /*< 23 File table overflow */ + EMFILE, /*< 24 Too many open files */ + ENOTTY, /*< 25 Not a typewriter */ + ETXTBSY, /*< 26 Text file busy */ + EFBIG, /*< 27 File too large */ + ENOSPC, /*< 28 No space left on device */ + ESPIPE, /*< 29 Illegal seek */ + EROFS, /*< 30 Read-only file system */ + EMLINK, /*< 31 Too many links */ + EPIPE, /*< 32 Broken pipe */ + EDOM, /*< 33 Math argument out of domain of func */ + ERANGE, /*< 34 Math result not representable */ + EBADMSG /*< 35 Bad Message */ +}; +// ============================================= + +///@brief POSIX stat structure +///@see stat() +///@see fstat() +struct stat +{ + dev_t st_dev; /*< ID of device containing file */ + ino_t st_ino; /*< inode number */ + mode_t st_mode; /*< protection */ + nlink_t st_nlink; /*< number of hard links */ + uid_t st_uid; /*< user ID of owner */ + gid_t st_gid; /*< group ID of owner */ + dev_t st_rdev; /*< device ID (if special file) */ + off_t st_size; /*< total size, in bytes */ + uint32_t st_blksize;/*< blocksize for filesystem I/O */ + uint32_t st_blocks; /*< number of 512B blocks allocated */ + time_t st_atime; /*< time of last access */ + time_t st_mtime; /*< time of last modification */ + time_t st_ctime; /*< time of last status change */ +}; + +///@brief POSIX utimbuf structure +///@see utime() +typedef struct utimbuf +{ + time_t actime; /* access time */ + time_t modtime; /* modification time */ +} utime_t; + +#if _USE_LFN != 0 +#define MAX_NAME_LEN _MAX_LFN +#else +#define MAX_NAME_LEN 13 +#endif + +struct dirent { +#if 0 // unsupported + ino_t d_ino; /* inode number */ + off_t d_off; /* not an offset; see NOTES */ + unsigned short d_reclen; /* length of this record */ + unsigned char d_type; /* type of file; not supported + by all filesystem types */ +#endif + char d_name[MAX_NAME_LEN]; /* filename */ +}; + +typedef struct dirent dirent_t; + + +///@brief POSIX lstat() +///@see stat() +#define lstat stat +// ============================================= +///@brief FILE type structure +struct __file { + char *buf; /* buffer pointer */ + unsigned char unget; /* ungetc() buffer */ + uint8_t flags; /* flags, see below */ +#define __SRD 0x0001 /* OK to read */ +#define __SWR 0x0002 /* OK to write */ +#define __SSTR 0x0004 /* this is an sprintf/snprintf string */ +#define __SPGM 0x0008 /* fmt string is in progmem */ +#define __SERR 0x0010 /* found error */ +#define __SEOF 0x0020 /* found EOF */ +#define __SUNGET 0x040 /* ungetc() happened */ +#define __SMALLOC 0x80 /* handle is malloc()ed */ +#if 0 + /* possible future extensions, will require uint16_t flags */ + #define __SRW 0x0100 /* open for reading & writing */ + #define __SLBF 0x0200 /* line buffered */ + #define __SNBF 0x0400 /* unbuffered */ + #define __SMBF 0x0800 /* buf is from malloc */ +#endif + int size; /* size of buffer */ + int len; /* characters read or written so far */ + int (*put)(char, struct __file *); /* write one char to device */ + int (*get)(struct __file *); /* read one char from device */ +// FIXME add all low level functions here like _open, _close, ... like newlib does + void *udata; /* User defined and accessible data. */ +}; +// ============================================= +///@brief POSIX open modes - no other combination are allowed. +/// - man page open(2) +/// - Note: The POSIX correct test of O_RDONLY is: (mode & O_ACCMODE) == O_RDONLY. +#define O_ACCMODE 00000003 /*< read, write, read-write modes */ +#define O_RDONLY 00000000 /*< Read only */ +#define O_WRONLY 00000001 /*< Write only */ +#define O_RDWR 00000002 /*< Read/Write */ +#define O_CREAT 00000100 /*< Create file only if it does not exist */ +#define O_EXCL 00000200 /*< O_CREAT option, Create fails if file exists +*/ +#define O_NOCTTY 00000400 /*< @todo */ +#define O_TRUNC 00001000 /*< Truncate if exists */ +#define O_APPEND 00002000 /*< All writes are to EOF */ +#define O_NONBLOCK 00004000 /*< @todo */ +#define O_BINARY 00000004 /*< Binary */ +#define O_TEXT 00000004 /*< Text End Of Line translation */ +#define O_CLOEXEC 00000000 +///@brief POSIX File types, see fstat and stat. +#define S_IFMT 0170000 /*< These bits determine file type. */ +#define S_IFDIR 0040000 /*< Directory. */ +#define S_IFCHR 0020000 /*< Character device. */ +#define S_IFBLK 0060000 /*< Block device. */ +#define S_IFREG 0100000 /*< Regular file. */ +#define S_IFIFO 0010000 /*< FIFO. */ +#define S_IFLNK 0120000 /*< Symbolic link. */ +#define S_IFSOCK 0140000 /*< Socket. */ +#define S_IREAD 0400 /*< Read by owner. */ +#define S_IWRITE 0200 /*< Write by owner. */ +#define S_IEXEC 0100 /*< Execute by owner. */ + +///@brief POSIX File type test macros. +#define S_ISTYPE(mode, mask) (((mode) & S_IFMT) == (mask)) +#define S_ISDIR(mode) S_ISTYPE((mode), S_IFDIR) +#define S_ISCHR(mode) S_ISTYPE((mode), S_IFCHR) +#define S_ISBLK(mode) S_ISTYPE((mode), S_IFBLK) +#define S_ISREG(mode) S_ISTYPE((mode), S_IFREG) + +//@brief POSIX File permissions, see fstat and stat +#define S_IRUSR S_IREAD /*< Read by owner. */ +#define S_IWUSR S_IWRITE /*< Write by owner. */ +#define S_IXUSR S_IEXEC /*< Execute by owner. */ +#define S_IRWXU (S_IREAD|S_IWRITE|S_IEXEC) /*< Read,Write,Execute by owner */ + +#define S_IRGRP (S_IRUSR >> 3) /*< Read by group. */ +#define S_IWGRP (S_IWUSR >> 3) /*< Write by group. */ +#define S_IXGRP (S_IXUSR >> 3) /*< Execute by group. */ +#define S_IRWXG (S_IRWXU >> 3) /*< Read,Write,Execute by user */ + +#define S_IROTH (S_IRGRP >> 3) /*< Read by others. */ +#define S_IWOTH (S_IWGRP >> 3) /*< Write by others. */ +#define S_IXOTH (S_IXGRP >> 3) /*< Execute by others. */ +#define S_IRWXO (S_IRWXG >> 3) /*< Read,Write,Execute by other */ +// ============================================= + +///@brief used in posix.c to compare to ascii file modes +#define modecmp(str, pat) (strcmp(str, pat) == 0 ? 1: 0) + +// ============================================= +///@brief FATFS open modes +#define FATFS_R (S_IRUSR | S_IRGRP | S_IROTH) /*< FatFs Read perms */ +#define FATFS_W (S_IWUSR | S_IWGRP | S_IWOTH) /*< FatFs Write perms */ +#define FATFS_X (S_IXUSR | S_IXGRP | S_IXOTH) /*< FatFs Execute perms */ + +// ============================================= +///@brief End of file or device read +#define EOF (-1) + +///@brief Seek offset macros +#define SEEK_SET 0 +#define SEEK_CUR 1 +#define SEEK_END 2 + +// ============================================= +///@brief define FILE type +typedef struct __file FILE; + +///@brief Maximum number of POSIX file handles. +#define MAX_FILES 16 +extern FILE *__iob[MAX_FILES]; + +///@brief define stdin, stdout and stderr +#undef stdin +#undef stdout +#undef stderr + +// Hard coded stdin,stdout and stderr locations +#define stdin (__iob[0]) +#define stdout (__iob[1]) +#define stderr (__iob[2]) + +// ============================================= +//#define IO_MACROS +#ifdef IO_MACROS +///@briefdefine putc, putchar and getc in terms of the posix fgetc() and fputc() interface +/// MAKE SURE that fdevopen() has been called BEFORE any input/output is processed +/// @see uart.c for the fdevopen() call +#define putc(__c, __stream) fputc(__c, __stream) +#define getc(__stream) fgetc(__stream) +/** + The macro \c putchar sends character \c c to \c stdout. +*/ +#define putchar(__c) fputc(__c,stdout) + +#define puts(__str) fputs(__str,stdout) +#endif + +// ============================================= +///@brief device IO udata +#define fdev_set_udata(stream, u) do { (stream)->udata = u; } while(0) +#define fdev_get_udata(stream) ((stream)->udata) + +///@brief device status flags +#define _FDEV_EOF (-1) +#define _FDEV_ERR (-2) +//@brief device read/write flags +#define _FDEV_SETUP_READ __SRD /**< fdev_setup_stream() with read intent */ +#define _FDEV_SETUP_WRITE __SWR /**< fdev_setup_stream() with write intent */ +#define _FDEV_SETUP_RW (__SRD|__SWR) /**< fdev_setup_stream() with read/write intent */ + +// ============================================= + + +/* posix.c */ +int isatty ( int fileno ); +int fgetc ( FILE *stream ); +int fputc ( int c , FILE *stream ); +#ifndef IO_MACROS +int getchar ( void ); +int putchar ( int c ); +#endif +//int ungetc ( int c , FILE *stream ); +#ifndef IO_MACROS +int putc ( int c , FILE *stream ); +#endif +char *fgets ( char *str , int size , FILE *stream ); +int fputs ( const char *str , FILE *stream ); +#ifndef IO_MACROS +int puts ( const char *str ); +#endif +int feof ( FILE *stream ); +int fgetpos ( FILE *stream , size_t *pos ); +int fseek ( FILE *stream , long offset , int whence ); +int fsetpos ( FILE *stream , size_t *pos ); +long ftell ( FILE *stream ); +off_t lseek ( int fileno , off_t position , int whence ); +void rewind ( FILE *stream ); +int close ( int fileno ); +int fileno ( FILE *stream ); +FILE *fileno_to_stream ( int fileno ); +FILE *fopen ( const char *path , const char *mode ); +size_t fread ( void *ptr , size_t size , size_t nmemb , FILE *stream ); +int ftruncate ( int fd , off_t length ); +size_t fwrite ( const void *ptr , size_t size , size_t nmemb , FILE *stream ); +int open (const char *pathname, int flags); +ssize_t read ( int fd , void *buf , size_t count ); +void sync ( void ); +int syncfs(int fd); +int fsync ( int fd ); +int truncate ( const char *path , off_t length ); +ssize_t write ( int fd , const void *buf , size_t count ); +//int fclose ( FILE *stream ); +//void dump_stat ( struct stat *sp ); + +#if 0 +int fstat ( int fd , struct stat *buf ); +#endif +int64_t fs_getfree(void); +int64_t fs_gettotal(void); +int stat ( const char *name , struct stat *buf ); +char *basename (const char *str ); +char *baseext ( char *str ); +int chdir ( const char *pathname ); +int chmod ( const char *pathname , mode_t mode ); +int dirname ( char *str ); +//int utime(const char *filename, const struct utimbuf *times); + +#if 0 + int fchmod ( int fd , mode_t mode ); +#endif + +char *getcwd ( char *pathname , int len ); +int mkdir ( const char *pathname , mode_t mode ); +int rename ( const char *oldpath , const char *newpath ); +int rmdir ( const char *pathname ); +int unlink ( const char *pathname ); +int closedir ( DIR *dirp ); +DIR *opendir ( const char *pathdir ); +struct dirent *readdir ( DIR *dirp ); +void clrerror ( FILE *stream ); +int ferror ( FILE *stream ); +void perror ( const char *s ); +char *strerror ( int errnum ); +char *__wrap_strerror_r ( int errnum , char *buf , size_t buflen ); +FILE *fdevopen ( int (*put )(char ,FILE *), int (*get )(FILE *)); +//int mkfs(char *name ); +int fatfs_getc ( FILE *stream ); +int fatfs_putc ( char c , FILE *stream ); +int fatfs_to_errno ( FRESULT Result ); +int fatfs_to_fileno ( FIL *fh ); +time_t fat_time_to_unix ( uint16_t date , uint16_t time ); +void unix_time_to_fat(time_t epoch, uint16_t *date, uint16_t *time); +FIL *fileno_to_fatfs ( int fileno ); +int free_file_descriptor ( int fileno ); +int new_file_descriptor ( void ); +int posix_fopen_modes_to_open ( const char *mode ); + +int fprintf(FILE *fp, const char *format, ...); + +#if __cplusplus +} +#endif + +// ============================================= +#endif //USE_POSIX +#endif //_POSIX_H_ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/ppm.c b/libraries/AP_HAL_ChibiOS/hwdef/common/ppm.c new file mode 100644 index 0000000000..83c51afa87 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/ppm.c @@ -0,0 +1,101 @@ +/* + * Copyright (C) Siddharth Bharat Purohit 2017 + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include "ppm.h" + +static ICUConfig icucfg; //Input Capture Unit Config +static uint16_t ppm_buffer[10] = {0}; +static bool updated[10] = {0}; +static bool available; +static uint8_t buf_ptr = 0; +static uint8_t num_channels = 0; +static void ppm_measurement_cb(ICUDriver*); + +//Initiallise ppm ICU with requested configuration +bool ppm_init(uint32_t freq, bool active_high) +{ + icumode_t ppm_active_mode; + + if (active_high) { + ppm_active_mode = ICU_INPUT_ACTIVE_HIGH; + } else { + ppm_active_mode = ICU_INPUT_ACTIVE_LOW; + } + + icucfg.mode = ppm_active_mode; + icucfg.frequency = freq; + icucfg.channel = PPM_ICU_CHANNEL; + icucfg.width_cb = NULL; + icucfg.period_cb = ppm_measurement_cb; + icucfg.overflow_cb = NULL; + icucfg.dier = 0; + + icuStart(&PPM_ICU_TIMER, &icucfg); + icuStartCapture(&PPM_ICU_TIMER); + icuEnableNotifications(&PPM_ICU_TIMER); + return true; +} + +uint16_t ppm_read(uint8_t channel) +{ + //return 0 if channel requested is out range + if(channel >= num_channels) { + return 0; + } + updated[channel] = false; + return ppm_buffer[channel]; +} + +uint8_t ppm_read_bulk(uint16_t periods[], uint8_t len) +{ + uint8_t i; + for(i = 0; (i < num_channels) && (i < len); i++) { + periods[i] = ppm_buffer[i]; + } + return i; +} + +bool ppm_available() +{ + uint8_t i; + for (i = 0; i < 10; i++) { + if (updated[i]) { + return true; + } + } + return false; +} + +uint8_t ppm_num_channels() +{ + return num_channels; +} + +static void ppm_measurement_cb(ICUDriver *icup) +{ + uint16_t period = icuGetPeriodX(icup); + if (period >= 2700 || buf_ptr >= 10) { + //This is a sync pulse let's reset buffer pointer + num_channels = buf_ptr + 1; + buf_ptr = 0; + } else { + if(period > 900) { + updated[buf_ptr] = true; + ppm_buffer[buf_ptr] = period; + } + buf_ptr++; + } +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/ppm.h b/libraries/AP_HAL_ChibiOS/hwdef/common/ppm.h new file mode 100644 index 0000000000..a0345cc544 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/ppm.h @@ -0,0 +1,28 @@ +/* + * Copyright (C) Siddharth Bharat Purohit 2017 + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "hal.h" + +#ifdef __cplusplus +extern "C" { +#endif +bool ppm_init(uint32_t freq, bool active_high); +uint16_t ppm_read(uint8_t chan); + +uint8_t ppm_read_bulk(uint16_t periods[], uint8_t len); +bool ppm_available(); +#if __cplusplus +} +#endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c new file mode 100644 index 0000000000..ede70f9794 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c @@ -0,0 +1,313 @@ +/* + * Copyright (C) Siddharth Bharat Purohit 2017 + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +/* + wrappers for stdio functions + + Relies on linker wrap options + + Note that not all functions that have been wrapped are implemented + here. The others are wrapped to ensure the function is not used + without an implementation. If we need them then we can implement as + needed. + */ +#include +#include +#include +#include +#include + +int vsnprintf(char *str, size_t size, const char *fmt, va_list ap) +{ + MemoryStream ms; + BaseSequentialStream *chp; + size_t size_wo_nul; + int retval; + + if (size > 0) + size_wo_nul = size - 1; + else + size_wo_nul = 0; + + /* Memory stream object to be used as a string writer, reserving one + byte for the final zero.*/ + msObjectInit(&ms, (uint8_t *)str, size_wo_nul, 0); + + /* Performing the print operation using the common code.*/ + chp = (BaseSequentialStream *)(void *)&ms; + + retval = chvprintf(chp, fmt, ap); + + + /* Terminate with a zero, unless size==0.*/ + if (ms.eos < size) + str[ms.eos] = 0; + + /* Return number of bytes that would have been written.*/ + return retval; +} + +int snprintf(char *str, size_t size, const char *fmt, ...) +{ + va_list arg; + int done; + + va_start (arg, fmt); + done = vsnprintf(str, size, fmt, arg); + va_end (arg); + + return done; +} + +int vasprintf(char **strp, const char *fmt, va_list ap) +{ + int len = vsnprintf(NULL, 0, fmt, ap); + if (len <= 0) { + return -1; + } + char *buf = calloc(len+1, 1); + if (!buf) { + return -1; + } + vsnprintf(buf, len+1, fmt, ap); + (*strp) = buf; + return len; +} + +int asprintf(char **strp, const char *fmt, ...) +{ + va_list ap; + va_start(ap, fmt); + int ret = vasprintf(strp, fmt, ap); + va_end(ap); + return ret; +} + +int vprintf(const char *fmt, va_list arg) +{ + return chvprintf ((BaseSequentialStream*)&HAL_STDOUT_SERIAL, fmt, arg); +} + +int printf(const char *fmt, ...) +{ + va_list arg; + int done; + + va_start (arg, fmt); + done = vprintf(fmt, arg); + va_end (arg); + + return done; +} + +#define MAXLN 128 +#define ISSPACE " \t\n\r\f\v" + +/* + * sscanf(buf,fmt,va_alist) + */ +int +sscanf (const char *buf, const char *fmt, ...) +{ + int count; + va_list ap; + + va_start (ap, fmt); + count = vsscanf (buf, fmt, ap); + va_end (ap); + return (count); +} + +static char * +_getbase(char *p, int *basep) +{ + if (p[0] == '0') { + switch (p[1]) { + case 'x': + *basep = 16; + break; + case 't': case 'n': + *basep = 10; + break; + case 'o': + *basep = 8; + break; + default: + *basep = 10; + return (p); + } + return (p + 2); + } + *basep = 10; + return (p); +} + +static int16_t +_atob (uint32_t *vp, char *p, int base) +{ + uint32_t value, v1, v2; + char *q, tmp[20]; + int digit; + + if (p[0] == '0' && (p[1] == 'x' || p[1] == 'X')) { + base = 16; + p += 2; + } + + if (base == 16 && (q = strchr (p, '.')) != 0) { + if (q - p > sizeof(tmp) - 1) + return (0); + + strncpy (tmp, p, q - p); + tmp[q - p] = '\0'; + if (!_atob (&v1, tmp, 16)) + return (0); + + q++; + if (strchr (q, '.')) + return (0); + + if (!_atob (&v2, q, 16)) + return (0); + *vp = (v1 << 16) + v2; + return (1); + } + + value = *vp = 0; + for (; *p; p++) { + if (*p >= '0' && *p <= '9') + digit = *p - '0'; + else if (*p >= 'a' && *p <= 'f') + digit = *p - 'a' + 10; + else if (*p >= 'A' && *p <= 'F') + digit = *p - 'A' + 10; + else + return (0); + + if (digit >= base) + return (0); + value *= base; + value += digit; + } + *vp = value; + return (1); +} + +/* + * atob(vp,p,base) + * converts p to binary result in vp, rtn 1 on success + */ +int16_t +atob(uint32_t *vp, char *p, int base) +{ + uint32_t v; + + if (base == 0) + p = _getbase (p, &base); + if (_atob (&v, p, base)) { + *vp = v; + return (1); + } + return (0); +} + + +/* + * vsscanf(buf,fmt,ap) + */ +int +vsscanf (const char *buf, const char *s, va_list ap) +{ + int count, noassign, width, base, lflag; + const char *tc; + char *t, tmp[MAXLN]; + + count = noassign = width = lflag = 0; + while (*s && *buf) { + while (isspace ((unsigned char)(*s))) + s++; + if (*s == '%') { + s++; + for (; *s; s++) { + if (strchr ("dibouxcsefg%", *s)) + break; + if (*s == '*') + noassign = 1; + else if (*s == 'l' || *s == 'L') + lflag = 1; + else if (*s >= '1' && *s <= '9') { + for (tc = s; isdigit (*s); s++); + strncpy (tmp, tc, s - tc); + tmp[s - tc] = '\0'; + atob (&width, tmp, 10); + s--; + } + } + if (*s == 's') { + while (isspace ((unsigned char)(*buf))) + buf++; + if (!width) + width = strcspn (buf, ISSPACE); + if (!noassign) { + strncpy (t = va_arg (ap, char *), buf, width); + t[width] = '\0'; + } + buf += width; + } else if (*s == 'c') { + if (!width) + width = 1; + if (!noassign) { + strncpy (t = va_arg (ap, char *), buf, width); + t[width] = '\0'; + } + buf += width; + } else if (strchr ("dobxu", *s)) { + while (isspace ((unsigned char)(*buf))) + buf++; + if (*s == 'd' || *s == 'u') + base = 10; + else if (*s == 'x') + base = 16; + else if (*s == 'o') + base = 8; + else if (*s == 'b') + base = 2; + if (!width) { + if (isspace ((unsigned char)(*(s + 1))) || *(s + 1) == 0) + width = strcspn (buf, ISSPACE); + else + width = strchr (buf, *(s + 1)) - buf; + } + strncpy (tmp, buf, width); + tmp[width] = '\0'; + buf += width; + if (!noassign) + atob (va_arg (ap, uint32_t *), tmp, base); + } + if (!noassign) + count++; + width = noassign = lflag = 0; + s++; + } else { + while (isspace ((unsigned char)(*buf))) + buf++; + if (*s != *buf) + break; + else + s++, buf++; + } + } + return (count); +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h new file mode 100644 index 0000000000..f2b40131fc --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h @@ -0,0 +1,39 @@ +/* + * Copyright (C) Siddharth Bharat Purohit 2017 + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "posix.h" +#include +#include +#include +#ifdef __cplusplus +extern "C" { +#endif + +int vsnprintf(char *str, size_t size, const char *fmt, va_list ap); +int snprintf(char *str, size_t size, const char *fmt, ...); +int vasprintf(char **strp, const char *fmt, va_list ap); +int asprintf(char **strp, const char *fmt, ...); +int vprintf(const char *fmt, va_list arg); +int printf(const char *fmt, ...); + + +int sscanf (const char *buf, const char *fmt, ...); +int vsscanf (const char *buf, const char *s, va_list ap); +void *malloc(size_t size); +void *calloc(size_t nmemb, size_t size); +void free(void *ptr); +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stubs.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stubs.c new file mode 100644 index 0000000000..a429dc6b29 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stubs.c @@ -0,0 +1,222 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +/* +* **** This file incorporates work covered by the following copyright and **** +* **** permission notice: **** +* +* Copyright (c) 2009 by Michael Fischer. 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 OWNER 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. +* +**************************************************************************** +* History: +* +* 28.03.09 mifi First Version, based on the original syscall.c from +* newlib version 1.17.0 +* 17.08.09 gdisirio Modified the file for use under ChibiOS/RT +* 15.11.09 gdisirio Added read and write handling +****************************************************************************/ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include +#include +#include +#include +#include + +#include "ch.h" +#if defined(STDOUT_SD) || defined(STDIN_SD) +#include "hal.h" +#endif + +uint8_t _before_main = 1; + +/***************************************************************************/ + +__attribute__((used)) +int _read(struct _reent *r, int file, char * ptr, int len) +{ + (void)r; +#if defined(STDIN_SD) + if (!len || (file != 0)) { + __errno_r(r) = EINVAL; + return -1; + } + len = sdRead(&STDIN_SD, (uint8_t *)ptr, (size_t)len); + return len; +#else + (void)file; + (void)ptr; + (void)len; + __errno_r(r) = EINVAL; + return -1; +#endif +} + +/***************************************************************************/ + +__attribute__((used)) +int _lseek(struct _reent *r, int file, int ptr, int dir) +{ + (void)r; + (void)file; + (void)ptr; + (void)dir; + + return 0; +} + +/***************************************************************************/ + +__attribute__((used)) +int _write(struct _reent *r, int file, char * ptr, int len) +{ + (void)r; + (void)file; + (void)ptr; +#if defined(STDOUT_SD) + if (file != 1) { + __errno_r(r) = EINVAL; + return -1; + } + sdWrite(&STDOUT_SD, (uint8_t *)ptr, (size_t)len); +#endif + return len; +} + +/***************************************************************************/ + +__attribute__((used)) +int _close(struct _reent *r, int file) +{ + (void)r; + (void)file; + + return 0; +} + +/***************************************************************************/ + +__attribute__((used)) +caddr_t _sbrk(struct _reent *r, int incr) +{ +#if CH_CFG_USE_MEMCORE + void *p; + + chDbgCheck(incr >= 0); + p = chHeapAlloc(NULL, (size_t)incr); + if (p == NULL) { + __errno_r(r) = ENOMEM; + return (caddr_t)-1; + } + return (caddr_t)p; +#else + (void)incr; + __errno_r(r) = ENOMEM; + return (caddr_t)-1; +#endif +} + + +/***************************************************************************/ + +__attribute__((used)) +int _fstat(struct _reent *r, int file, struct stat * st) +{ + (void)r; + (void)file; + + memset(st, 0, sizeof(*st)); + st->st_mode = S_IFCHR; + return 0; +} + +/***************************************************************************/ + +__attribute__((used)) +int _isatty(struct _reent *r, int fd) +{ + (void)r; + (void)fd; + + return 1; +} + +__attribute__((used)) +pid_t _getpid() +{ + return 0; +} + +__attribute__((used)) +void _exit( int status ) +{ + (void)status; + while( 1 ); +} + +__attribute__((used)) +void _fini(void) +{ +} + +__attribute__((used)) +int _kill( int pid, int sig ) +{ + (void)pid; (void)sig; + return -1; +} + +/*** EOF ***/ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c new file mode 100644 index 0000000000..785f48e8ec --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.c @@ -0,0 +1,393 @@ +/* + ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + http://www.apache.org/licenses/LICENSE-2.0 + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include "hal.h" +#include "hwdef.h" + +#include + +#pragma GCC optimize("O0") + +#ifdef HAL_USB_PRODUCT_ID + +/* Virtual serial port over USB.*/ +SerialUSBDriver SDU1; + +/* + * Endpoints to be used for USBD1. + */ +#define USBD1_DATA_REQUEST_EP 1 +#define USBD1_DATA_AVAILABLE_EP 1 +#define USBD1_INTERRUPT_REQUEST_EP 2 + +/* + * USB Device Descriptor. + */ +static const uint8_t vcom_device_descriptor_data[18] = { + USB_DESC_DEVICE( + 0x0110, /* bcdUSB (1.1). */ + 0x02, /* bDeviceClass (CDC). */ + 0x00, /* bDeviceSubClass. */ + 0x00, /* bDeviceProtocol. */ + 0x40, /* bMaxPacketSize. */ + HAL_USB_VENDOR_ID, /* idVendor (ST). */ + HAL_USB_PRODUCT_ID, /* idProduct. */ + 0x0200, /* bcdDevice. */ + 1, /* iManufacturer. */ + 2, /* iProduct. */ + 3, /* iSerialNumber. */ + 1) /* bNumConfigurations. */ +}; + +/* + * Device Descriptor wrapper. + */ +static const USBDescriptor vcom_device_descriptor = { + sizeof vcom_device_descriptor_data, + vcom_device_descriptor_data +}; + +/* Configuration Descriptor tree for a CDC.*/ +static const uint8_t vcom_configuration_descriptor_data[67] = { + /* Configuration Descriptor.*/ + USB_DESC_CONFIGURATION(67, /* wTotalLength. */ + 0x02, /* bNumInterfaces. */ + 0x01, /* bConfigurationValue. */ + 0, /* iConfiguration. */ + 0xC0, /* bmAttributes (self powered). */ + 50), /* bMaxPower (100mA). */ + /* Interface Descriptor.*/ + USB_DESC_INTERFACE (0x00, /* bInterfaceNumber. */ + 0x00, /* bAlternateSetting. */ + 0x01, /* bNumEndpoints. */ + 0x02, /* bInterfaceClass (Communications + Interface Class, CDC section + 4.2). */ + 0x02, /* bInterfaceSubClass (Abstract + Control Model, CDC section 4.3). */ + 0x01, /* bInterfaceProtocol (AT commands, + CDC section 4.4). */ + 0), /* iInterface. */ + /* Header Functional Descriptor (CDC section 5.2.3).*/ + USB_DESC_BYTE (5), /* bLength. */ + USB_DESC_BYTE (0x24), /* bDescriptorType (CS_INTERFACE). */ + USB_DESC_BYTE (0x00), /* bDescriptorSubtype (Header + Functional Descriptor. */ + USB_DESC_BCD (0x0110), /* bcdCDC. */ + /* Call Management Functional Descriptor. */ + USB_DESC_BYTE (5), /* bFunctionLength. */ + USB_DESC_BYTE (0x24), /* bDescriptorType (CS_INTERFACE). */ + USB_DESC_BYTE (0x01), /* bDescriptorSubtype (Call Management + Functional Descriptor). */ + USB_DESC_BYTE (0x00), /* bmCapabilities (D0+D1). */ + USB_DESC_BYTE (0x01), /* bDataInterface. */ + /* ACM Functional Descriptor.*/ + USB_DESC_BYTE (4), /* bFunctionLength. */ + USB_DESC_BYTE (0x24), /* bDescriptorType (CS_INTERFACE). */ + USB_DESC_BYTE (0x02), /* bDescriptorSubtype (Abstract + Control Management Descriptor). */ + USB_DESC_BYTE (0x02), /* bmCapabilities. */ + /* Union Functional Descriptor.*/ + USB_DESC_BYTE (5), /* bFunctionLength. */ + USB_DESC_BYTE (0x24), /* bDescriptorType (CS_INTERFACE). */ + USB_DESC_BYTE (0x06), /* bDescriptorSubtype (Union + Functional Descriptor). */ + USB_DESC_BYTE (0x00), /* bMasterInterface (Communication + Class Interface). */ + USB_DESC_BYTE (0x01), /* bSlaveInterface0 (Data Class + Interface). */ + /* Endpoint 2 Descriptor.*/ + USB_DESC_ENDPOINT (USBD1_INTERRUPT_REQUEST_EP|0x80, + 0x03, /* bmAttributes (Interrupt). */ + 0x0008, /* wMaxPacketSize. */ + 0xFF), /* bInterval. */ + /* Interface Descriptor.*/ + USB_DESC_INTERFACE (0x01, /* bInterfaceNumber. */ + 0x00, /* bAlternateSetting. */ + 0x02, /* bNumEndpoints. */ + 0x0A, /* bInterfaceClass (Data Class + Interface, CDC section 4.5). */ + 0x00, /* bInterfaceSubClass (CDC section + 4.6). */ + 0x00, /* bInterfaceProtocol (CDC section + 4.7). */ + 0x00), /* iInterface. */ + /* Endpoint 3 Descriptor.*/ + USB_DESC_ENDPOINT (USBD1_DATA_AVAILABLE_EP, /* bEndpointAddress.*/ + 0x02, /* bmAttributes (Bulk). */ + 0x0040, /* wMaxPacketSize. */ + 0x00), /* bInterval. */ + /* Endpoint 1 Descriptor.*/ + USB_DESC_ENDPOINT (USBD1_DATA_REQUEST_EP|0x80, /* bEndpointAddress.*/ + 0x02, /* bmAttributes (Bulk). */ + 0x0040, /* wMaxPacketSize. */ + 0x00) /* bInterval. */ +}; + +/* + * Configuration Descriptor wrapper. + */ +static const USBDescriptor vcom_configuration_descriptor = { + sizeof vcom_configuration_descriptor_data, + vcom_configuration_descriptor_data +}; + +/* + * U.S. English language identifier. + */ +static const uint8_t vcom_string0[] = { + USB_DESC_BYTE(4), /* bLength. */ + USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType. */ + USB_DESC_WORD(0x0409) /* wLANGID (U.S. English). */ +}; + +/* + * Vendor string. + */ +static const uint8_t vcom_string1[] = { + USB_DESC_BYTE(20), /* bLength. */ + USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType. */ + 'A', 0, 'r', 0, 'd', 0, 'u', 0, 'P', 0, 'i', 0, 'l', 0, 'o', 0, + 't', 0 +}; + +/* + * Device Description string. + */ +static const uint8_t vcom_string2[] = { + USB_DESC_BYTE(56), /* bLength. */ + USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType. */ + 'C', 0, 'h', 0, 'i', 0, 'b', 0, 'i', 0, 'O', 0, 'S', 0, '/', 0, + 'R', 0, 'T', 0, ' ', 0, 'V', 0, 'i', 0, 'r', 0, 't', 0, 'u', 0, + 'a', 0, 'l', 0, ' ', 0, 'C', 0, 'O', 0, 'M', 0, ' ', 0, 'P', 0, + 'o', 0, 'r', 0, 't', 0 +}; + +/* + * Serial Number string. + */ +static const uint8_t vcom_string3[] = { + USB_DESC_BYTE(8), /* bLength. */ + USB_DESC_BYTE(USB_DESCRIPTOR_STRING), /* bDescriptorType. */ + '0' + CH_KERNEL_MAJOR, 0, + '0' + CH_KERNEL_MINOR, 0, + '0' + CH_KERNEL_PATCH, 0 +}; + +/* + * Strings wrappers array. The strings are created dynamically to + * allow them to be setup with apj_tool + */ +static USBDescriptor vcom_strings[] = { + {sizeof vcom_string0, vcom_string0}, + {0, NULL}, // manufacturer + {0, NULL}, // product + {0, NULL}, // version +}; + +/* + dynamically allocate a USB descriptor string + */ +static void setup_usb_string(USBDescriptor *desc, const char *str) +{ + uint8_t len = strlen(str); + desc->ud_size = 2+2*len; + uint8_t *b = (uint8_t *)malloc(desc->ud_size); + desc->ud_string = (const char *)b; + b[0] = USB_DESC_BYTE(desc->ud_size); + b[1] = USB_DESC_BYTE(USB_DESCRIPTOR_STRING); + uint8_t i; + for (i=0; i. + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ +#pragma once + +#if HAL_USE_SERIAL_USB +extern const USBConfig usbcfg; +extern SerialUSBConfig serusbcfg; +extern SerialUSBDriver SDU1; +#endif +/** @} */ \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/board.c b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/board.c new file mode 100644 index 0000000000..cc2a3a9d06 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/board.c @@ -0,0 +1,180 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ + +#include "hal.h" + +#if HAL_USE_PAL || defined(__DOXYGEN__) +/** + * @brief PAL setup. + * @details Digital I/O ports static configuration as defined in @p board.h. + * This variable is used by the HAL when initializing the PAL driver. + */ +const PALConfig pal_default_config = { + + {VAL_GPIOA_MODER, VAL_GPIOA_OTYPER, VAL_GPIOA_OSPEEDR, VAL_GPIOA_PUPDR, VAL_GPIOA_ODR, VAL_GPIOA_AFRL, VAL_GPIOA_AFRH}, + {VAL_GPIOB_MODER, VAL_GPIOB_OTYPER, VAL_GPIOB_OSPEEDR, VAL_GPIOB_PUPDR, VAL_GPIOB_ODR, VAL_GPIOB_AFRL, VAL_GPIOB_AFRH}, + {VAL_GPIOC_MODER, VAL_GPIOC_OTYPER, VAL_GPIOC_OSPEEDR, VAL_GPIOC_PUPDR, VAL_GPIOC_ODR, VAL_GPIOC_AFRL, VAL_GPIOC_AFRH}, + {VAL_GPIOD_MODER, VAL_GPIOD_OTYPER, VAL_GPIOD_OSPEEDR, VAL_GPIOD_PUPDR, VAL_GPIOD_ODR, VAL_GPIOD_AFRL, VAL_GPIOD_AFRH}, + {VAL_GPIOE_MODER, VAL_GPIOE_OTYPER, VAL_GPIOE_OSPEEDR, VAL_GPIOE_PUPDR, VAL_GPIOE_ODR, VAL_GPIOE_AFRL, VAL_GPIOE_AFRH}, + {VAL_GPIOF_MODER, VAL_GPIOF_OTYPER, VAL_GPIOF_OSPEEDR, VAL_GPIOF_PUPDR, VAL_GPIOF_ODR, VAL_GPIOF_AFRL, VAL_GPIOF_AFRH}, + {VAL_GPIOG_MODER, VAL_GPIOG_OTYPER, VAL_GPIOG_OSPEEDR, VAL_GPIOG_PUPDR, VAL_GPIOG_ODR, VAL_GPIOG_AFRL, VAL_GPIOG_AFRH}, + {VAL_GPIOH_MODER, VAL_GPIOH_OTYPER, VAL_GPIOH_OSPEEDR, VAL_GPIOH_PUPDR, VAL_GPIOH_ODR, VAL_GPIOH_AFRL, VAL_GPIOH_AFRH}, + {VAL_GPIOI_MODER, VAL_GPIOI_OTYPER, VAL_GPIOI_OSPEEDR, VAL_GPIOI_PUPDR, VAL_GPIOI_ODR, VAL_GPIOI_AFRL, VAL_GPIOI_AFRH} +}; +#endif + +/** + * @brief Early initialization code. + * @details This initialization must be performed just after stack setup + * and before any other initialization. + */ +void __early_init(void) { + stm32_clock_init(); +} + +void __late_init(void) { + halInit(); + chSysInit(); +#ifdef HAL_USB_PRODUCT_ID + setup_usb_strings(); +#endif +} + +#if HAL_USE_SDC || defined(__DOXYGEN__) +/** + * @brief SDC card detection. + */ +bool sdc_lld_is_card_inserted(SDCDriver *sdcp) { + static bool last_status = false; + + if (blkIsTransferring(sdcp)) + return last_status; + return last_status = (bool)palReadPad(GPIOC, 11); +} + +/** + * @brief SDC card write protection detection. + */ +bool sdc_lld_is_write_protected(SDCDriver *sdcp) { + + (void)sdcp; + return false; +} +#endif /* HAL_USE_SDC */ + +#if HAL_USE_MMC_SPI || defined(__DOXYGEN__) +/** + * @brief MMC_SPI card detection. + */ +bool mmc_lld_is_card_inserted(MMCDriver *mmcp) { + + (void)mmcp; + /* TODO: Fill the implementation.*/ + return true; +} + +/** + * @brief MMC_SPI card write protection detection. + */ +bool mmc_lld_is_write_protected(MMCDriver *mmcp) { + + (void)mmcp; + /* TODO: Fill the implementation.*/ + return false; +} +#endif + +/** + * @brief Board-specific initialization code. + * @todo Add your board-specific code, if any. + */ +void boardInit(void) { + + //Setup ADC pins for Voltage and Current Sensing + palSetPadMode(GPIOA, 2, PAL_MODE_INPUT_ANALOG); //Pin PA2 + palSetPadMode(GPIOA, 3, PAL_MODE_INPUT_ANALOG); //Pin PA3 + palSetPadMode(GPIOA, 4, PAL_MODE_INPUT_ANALOG); //Pin PA4 + + palSetPadMode(GPIOE, 12, PAL_STM32_MODE_OUTPUT | PAL_STM32_OTYPE_OPENDRAIN | PAL_STM32_OSPEED_MID2); + palClearPad(GPIOE, 12); + + /* External interrupts */ + // GPIO_GYRO_DRDY + palSetPadMode(GPIOB, 0, PAL_STM32_MODE_INPUT | PAL_STM32_PUPDR_FLOATING); + // GPIO_MAG_DRDY + palSetPadMode(GPIOB, 1, PAL_STM32_MODE_INPUT | PAL_STM32_PUPDR_FLOATING); + + // GPIO_ACCEL_DRDY + palSetPadMode(GPIOB, 4, PAL_STM32_MODE_INPUT | PAL_STM32_PUPDR_FLOATING); + + // GPIOI_MPU_DRDY + palSetPadMode(GPIOD, 15, PAL_STM32_MODE_INPUT | PAL_STM32_PUPDR_FLOATING); + + + + /* SPI chip selects */ + // // SPIDEV_CS_MS5611 + //(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) + palSetPadMode(GPIOD, 7, PAL_STM32_MODE_OUTPUT | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_LOWEST); + palSetPad(GPIOD, 7); + // GPIO_SPI_CS_FRAM + //(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) + palSetPadMode(GPIOD, 10, PAL_STM32_MODE_OUTPUT | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_LOWEST); + palSetPad(GPIOD, 10); + + // SPIDEV_CS_MPU + //(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) + palSetPadMode(GPIOC, 2, PAL_STM32_MODE_OUTPUT | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_LOWEST); + palSetPad(GPIOC, 2); + // SPIDEV_CS_EXT_MPU + //(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) + palSetPadMode(GPIOE, 4, PAL_STM32_MODE_OUTPUT | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_LOWEST); + palSetPad(GPIOE, 4); + // SPIDEV_CS_EXT_MS5611 + //(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) + palSetPadMode(GPIOC, 14, PAL_STM32_MODE_OUTPUT | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_LOWEST); + palSetPad(GPIOC, 14); + // SPIDEV_CS_EXT_LSM9DS0_AM + //(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) + palSetPadMode(GPIOC, 15, PAL_STM32_MODE_OUTPUT | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_LOWEST); + palSetPad(GPIOC, 15); + // SPIDEV_CS_EXT_LSM9DS0_G + //(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) + palSetPadMode(GPIOC, 13, PAL_STM32_MODE_OUTPUT | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_LOWEST); + palSetPad(GPIOC, 13); + + //PWM O/P Setup + + // GPIO_TIM1_CH1OUT + // (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN9) + palSetPadMode(GPIOE, 9, PAL_STM32_MODE_ALTERNATE | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_MID2 | PAL_STM32_ALTERNATE(1)); + palClearPad(GPIOE, 9); + // GPIO_TIM1_CH2OUT + // (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN11) + palSetPadMode(GPIOE, 11, PAL_STM32_MODE_ALTERNATE | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_MID2 | PAL_STM32_ALTERNATE(1)); + palClearPad(GPIOE, 11); + // GPIO_TIM1_CH3OUT + // (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN13) + palSetPadMode(GPIOE, 13, PAL_STM32_MODE_ALTERNATE | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_MID2 | PAL_STM32_ALTERNATE(1)); + palClearPad(GPIOE, 13); + // GPIO_TIM1_CH4OUT + // (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PUSHPULL|GPIO_PORTE|GPIO_PIN14) + palSetPadMode(GPIOE, 14, PAL_STM32_MODE_ALTERNATE | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_OSPEED_MID2 | PAL_STM32_ALTERNATE(1)); + palClearPad(GPIOE, 14); + //call extra board specific initialisation method + HAL_BOARD_INIT_HOOK_CALL +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/board.h b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/board.h new file mode 100644 index 0000000000..8229e61ffd --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/board.h @@ -0,0 +1,47 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#pragma once + +/* + * APM HW Defines + */ +#define PPM_ICU_TIMER ICUD4 +#define PPM_ICU_CHANNEL ICU_CHANNEL_2 + +#define HRT_TIMER GPTD5 +#define LINE_LED1 PAL_LINE(GPIOE,12) + +#include "hwdef.h" + + +#ifndef HAL_BOARD_INIT_HOOK_DEFINE +#define HAL_BOARD_INIT_HOOK_DEFINE +#endif +#ifndef HAL_BOARD_INIT_HOOK_CALL +#define HAL_BOARD_INIT_HOOK_CALL +#endif +#if !defined(_FROM_ASM_) +#ifdef __cplusplus +extern "C" { +#endif + void boardInit(void); + HAL_BOARD_INIT_HOOK_DEFINE +#ifdef __cplusplus +} +#endif +#endif /* _FROM_ASM_ */ + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/chconf.h b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/chconf.h new file mode 100644 index 0000000000..8be81d2dbf --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/chconf.h @@ -0,0 +1,519 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ + +/** + * @file templates/chconf.h + * @brief Configuration file template. + * @details A copy of this file must be placed in each project directory, it + * contains the application specific kernel settings. + * + * @addtogroup config + * @details Kernel related settings and hooks. + * @{ + */ + +#pragma once + +#define _CHIBIOS_RT_CONF_ + +/*===========================================================================*/ +/** + * @name System timers settings + * @{ + */ +/*===========================================================================*/ + +/** + * @brief System time counter resolution. + * @note Allowed values are 16 or 32 bits. + */ +#define CH_CFG_ST_RESOLUTION 32 + +/** + * @brief System tick frequency. + * @details Frequency of the system timer that drives the system ticks. This + * setting also defines the system tick time unit. + */ +#define CH_CFG_ST_FREQUENCY 10000 + +/** + * @brief Time delta constant for the tick-less mode. + * @note If this value is zero then the system uses the classic + * periodic tick. This value represents the minimum number + * of ticks that is safe to specify in a timeout directive. + * The value one is not valid, timeouts are rounded up to + * this value. + */ +#define CH_CFG_ST_TIMEDELTA 2 + +/** @} */ + +/*===========================================================================*/ +/** + * @name Kernel parameters and options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief Round robin interval. + * @details This constant is the number of system ticks allowed for the + * threads before preemption occurs. Setting this value to zero + * disables the preemption for threads with equal priority and the + * round robin becomes cooperative. Note that higher priority + * threads can still preempt, the kernel is always preemptive. + * @note Disabling the round robin preemption makes the kernel more compact + * and generally faster. + * @note The round robin preemption is not supported in tickless mode and + * must be set to zero in that case. + */ +#define CH_CFG_TIME_QUANTUM 0 + +/** + * @brief Managed RAM size. + * @details Size of the RAM area to be managed by the OS. If set to zero + * then the whole available RAM is used. The core memory is made + * available to the heap allocator and/or can be used directly through + * the simplified core memory allocator. + * + * @note In order to let the OS manage the whole RAM the linker script must + * provide the @p __heap_base__ and @p __heap_end__ symbols. + * @note Requires @p CH_CFG_USE_MEMCORE. + */ +#define CH_CFG_MEMCORE_SIZE 0 + +/** + * @brief Idle thread automatic spawn suppression. + * @details When this option is activated the function @p chSysInit() + * does not spawn the idle thread. The application @p main() + * function becomes the idle thread and must implement an + * infinite loop. + */ +#define CH_CFG_NO_IDLE_THREAD FALSE + +/** @} */ + +/*===========================================================================*/ +/** + * @name Performance options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief OS optimization. + * @details If enabled then time efficient rather than space efficient code + * is used when two possible implementations exist. + * + * @note This is not related to the compiler optimization options. + * @note The default is @p TRUE. + */ +#define CH_CFG_OPTIMIZE_SPEED TRUE + +/** @} */ + +/*===========================================================================*/ +/** + * @name Subsystem options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief Time Measurement APIs. + * @details If enabled then the time measurement APIs are included in + * the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_TM TRUE + +/** + * @brief Threads registry APIs. + * @details If enabled then the registry APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_REGISTRY TRUE + +/** + * @brief Threads synchronization APIs. + * @details If enabled then the @p chThdWait() function is included in + * the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_WAITEXIT TRUE + +/** + * @brief Semaphores APIs. + * @details If enabled then the Semaphores APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_SEMAPHORES TRUE + +/** + * @brief Semaphores queuing mode. + * @details If enabled then the threads are enqueued on semaphores by + * priority rather than in FIFO order. + * + * @note The default is @p FALSE. Enable this if you have special + * requirements. + * @note Requires @p CH_CFG_USE_SEMAPHORES. + */ +#define CH_CFG_USE_SEMAPHORES_PRIORITY FALSE + +/** + * @brief Mutexes APIs. + * @details If enabled then the mutexes APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_MUTEXES TRUE + +/** + * @brief Enables recursive behavior on mutexes. + * @note Recursive mutexes are heavier and have an increased + * memory footprint. + * + * @note The default is @p FALSE. + * @note Requires @p CH_CFG_USE_MUTEXES. + */ +#define CH_CFG_USE_MUTEXES_RECURSIVE FALSE + +/** + * @brief Conditional Variables APIs. + * @details If enabled then the conditional variables APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_CFG_USE_MUTEXES. + */ +#define CH_CFG_USE_CONDVARS TRUE + +/** + * @brief Conditional Variables APIs with timeout. + * @details If enabled then the conditional variables APIs with timeout + * specification are included in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_CFG_USE_CONDVARS. + */ +#define CH_CFG_USE_CONDVARS_TIMEOUT TRUE + +/** + * @brief Events Flags APIs. + * @details If enabled then the event flags APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_EVENTS TRUE + +/** + * @brief Events Flags APIs with timeout. + * @details If enabled then the events APIs with timeout specification + * are included in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_CFG_USE_EVENTS. + */ +#define CH_CFG_USE_EVENTS_TIMEOUT TRUE + +/** + * @brief Synchronous Messages APIs. + * @details If enabled then the synchronous messages APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_MESSAGES TRUE + +/** + * @brief Synchronous Messages queuing mode. + * @details If enabled then messages are served by priority rather than in + * FIFO order. + * + * @note The default is @p FALSE. Enable this if you have special + * requirements. + * @note Requires @p CH_CFG_USE_MESSAGES. + */ +#define CH_CFG_USE_MESSAGES_PRIORITY FALSE + +/** + * @brief Mailboxes APIs. + * @details If enabled then the asynchronous messages (mailboxes) APIs are + * included in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_CFG_USE_SEMAPHORES. + */ +#define CH_CFG_USE_MAILBOXES TRUE + +/** + * @brief Core Memory Manager APIs. + * @details If enabled then the core memory manager APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_MEMCORE TRUE + +/** + * @brief Heap Allocator APIs. + * @details If enabled then the memory heap allocator APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_CFG_USE_MEMCORE and either @p CH_CFG_USE_MUTEXES or + * @p CH_CFG_USE_SEMAPHORES. + * @note Mutexes are recommended. + */ +#define CH_CFG_USE_HEAP TRUE + +/** + * @brief Memory Pools Allocator APIs. + * @details If enabled then the memory pools allocator APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_MEMPOOLS TRUE + +/** + * @brief Dynamic Threads APIs. + * @details If enabled then the dynamic threads creation APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_CFG_USE_WAITEXIT. + * @note Requires @p CH_CFG_USE_HEAP and/or @p CH_CFG_USE_MEMPOOLS. + */ +#define CH_CFG_USE_DYNAMIC TRUE + +/** @} */ + +/*===========================================================================*/ +/** + * @name Debug options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief Debug option, kernel statistics. + * + * @note The default is @p FALSE. + */ +#define CH_DBG_STATISTICS TRUE + +/** + * @brief Debug option, system state check. + * @details If enabled the correct call protocol for system APIs is checked + * at runtime. + * + * @note The default is @p FALSE. + */ +#define CH_DBG_SYSTEM_STATE_CHECK TRUE + +/** + * @brief Debug option, parameters checks. + * @details If enabled then the checks on the API functions input + * parameters are activated. + * + * @note The default is @p FALSE. + */ +#define CH_DBG_ENABLE_CHECKS TRUE + +/** + * @brief Debug option, consistency checks. + * @details If enabled then all the assertions in the kernel code are + * activated. This includes consistency checks inside the kernel, + * runtime anomalies and port-defined checks. + * + * @note The default is @p FALSE. + */ +#define CH_DBG_ENABLE_ASSERTS TRUE + +/** + * @brief Debug option, trace buffer. + * @details If enabled then the trace buffer is activated. + * + * @note The default is @p CH_DBG_TRACE_MASK_DISABLED. + */ +#define CH_DBG_TRACE_MASK CH_DBG_TRACE_MASK_NONE + +/** + * @brief Trace buffer entries. + * @note The trace buffer is only allocated if @p CH_DBG_TRACE_MASK is + * different from @p CH_DBG_TRACE_MASK_DISABLED. + */ +#define CH_DBG_TRACE_BUFFER_SIZE 128 + +/** + * @brief Debug option, stack checks. + * @details If enabled then a runtime stack check is performed. + * + * @note The default is @p FALSE. + * @note The stack check is performed in a architecture/port dependent way. + * It may not be implemented or some ports. + * @note The default failure mode is to halt the system with the global + * @p panic_msg variable set to @p NULL. + */ +#define CH_DBG_ENABLE_STACK_CHECK TRUE + +/** + * @brief Debug option, stacks initialization. + * @details If enabled then the threads working area is filled with a byte + * value when a thread is created. This can be useful for the + * runtime measurement of the used stack. + * + * @note The default is @p FALSE. + */ +#define CH_DBG_FILL_THREADS TRUE + +/** + * @brief Debug option, threads profiling. + * @details If enabled then a field is added to the @p thread_t structure that + * counts the system ticks occurred while executing the thread. + * + * @note The default is @p FALSE. + * @note This debug option is not currently compatible with the + * tickless mode. + */ +#define CH_DBG_THREADS_PROFILING FALSE + +/** @} */ + +/*===========================================================================*/ +/** + * @name Kernel hooks + * @{ + */ +/*===========================================================================*/ + +/** + * @brief Threads descriptor structure extension. + * @details User fields added to the end of the @p thread_t structure. + */ +#define CH_CFG_THREAD_EXTRA_FIELDS \ + /* Add threads custom fields here.*/ + +/** + * @brief Threads initialization hook. + * @details User initialization code added to the @p chThdInit() API. + * + * @note It is invoked from within @p chThdInit() and implicitly from all + * the threads creation APIs. + */ +#define CH_CFG_THREAD_INIT_HOOK(tp) { \ + /* Add threads initialization code here.*/ \ +} + +/** + * @brief Threads finalization hook. + * @details User finalization code added to the @p chThdExit() API. + */ +#define CH_CFG_THREAD_EXIT_HOOK(tp) { \ + /* Add threads finalization code here.*/ \ +} + +/** + * @brief Context switch hook. + * @details This hook is invoked just before switching between threads. + */ +#define CH_CFG_CONTEXT_SWITCH_HOOK(ntp, otp) { \ + /* Context switch code here.*/ \ +} + +/** + * @brief ISR enter hook. + */ +#define CH_CFG_IRQ_PROLOGUE_HOOK() { \ + /* IRQ prologue code here.*/ \ +} + +/** + * @brief ISR exit hook. + */ +#define CH_CFG_IRQ_EPILOGUE_HOOK() { \ + /* IRQ epilogue code here.*/ \ +} + +/** + * @brief Idle thread enter hook. + * @note This hook is invoked within a critical zone, no OS functions + * should be invoked from here. + * @note This macro can be used to activate a power saving mode. + */ +#define CH_CFG_IDLE_ENTER_HOOK() { \ + /* Idle-enter code here.*/ \ +} + +/** + * @brief Idle thread leave hook. + * @note This hook is invoked within a critical zone, no OS functions + * should be invoked from here. + * @note This macro can be used to deactivate a power saving mode. + */ +#define CH_CFG_IDLE_LEAVE_HOOK() { \ + /* Idle-leave code here.*/ \ +} + +/** + * @brief Idle Loop hook. + * @details This hook is continuously invoked by the idle thread loop. + */ +#define CH_CFG_IDLE_LOOP_HOOK() { \ + /* Idle loop code here.*/ \ +} + +/** + * @brief System tick event hook. + * @details This hook is invoked in the system tick handler immediately + * after processing the virtual timers queue. + */ +#define CH_CFG_SYSTEM_TICK_HOOK() { \ + /* System tick event code here.*/ \ +} + +/** + * @brief System halt hook. + * @details This hook is invoked in case to a system halting error before + * the system is halted. + */ +#define CH_CFG_SYSTEM_HALT_HOOK(reason) { \ + /* System halt code here.*/ \ +} + +/** + * @brief Trace hook. + * @details This hook is invoked each time a new record is written in the + * trace buffer. + */ +#define CH_CFG_TRACE_HOOK(tep) { \ + /* Trace code here.*/ \ +} + +/** @} */ + +/*===========================================================================*/ +/* Port-specific settings (override port settings defaulted in chcore.h). */ +/*===========================================================================*/ + + +/** @} */ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/chibios_board.mk b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/chibios_board.mk new file mode 100644 index 0000000000..a47d0b0a8b --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/chibios_board.mk @@ -0,0 +1,228 @@ +############################################################################## +# Build global options +# NOTE: Can be overridden externally. +# + +# Compiler options here. +ifeq ($(USE_OPT),) + USE_OPT = -Os -g -fomit-frame-pointer -falign-functions=16 -DCHPRINTF_USE_FLOAT=1 +endif + +# C specific options here (added to USE_OPT). +ifeq ($(USE_COPT),) + USE_COPT = +endif + +# C++ specific options here (added to USE_OPT). +ifeq ($(USE_CPPOPT),) + USE_CPPOPT = -fno-rtti +endif + +# Enable this if you want the linker to remove unused code and data +ifeq ($(USE_LINK_GC),) + USE_LINK_GC = yes +endif + +# Linker extra options here. +ifeq ($(USE_LDOPT),) + USE_LDOPT = +endif + +# Enable this if you want link time optimizations (LTO) +ifeq ($(USE_LTO),) + USE_LTO = no +endif + +# If enabled, this option allows to compile the application in THUMB mode. +ifeq ($(USE_THUMB),) + USE_THUMB = yes +endif + +# Enable this if you want to see the full log while compiling. +ifeq ($(USE_VERBOSE_COMPILE),) + USE_VERBOSE_COMPILE = no +endif + +# If enabled, this option makes the build process faster by not compiling +# modules not used in the current configuration. +ifeq ($(USE_SMART_BUILD),) + USE_SMART_BUILD = no +endif + +# +# Build global options +############################################################################## + +############################################################################## +# Architecture or project specific options +# +HWDEF = $(AP_HAL)/hwdef +# Stack size to be allocated to the Cortex-M process stack. This stack is +# the stack used by the main() thread. +ifeq ($(USE_PROCESS_STACKSIZE),) + USE_PROCESS_STACKSIZE = 0x400 +endif + +# Stack size to the allocated to the Cortex-M main/exceptions stack. This +# stack is used for processing interrupts and exceptions. +ifeq ($(USE_EXCEPTIONS_STACKSIZE),) + USE_EXCEPTIONS_STACKSIZE = 0x400 +endif + +# Enables the use of FPU (no, softfp, hard). +ifeq ($(USE_FPU),) + USE_FPU = hard +endif + +# +# Architecture or project specific options +############################################################################## + +############################################################################## +# Project, sources and paths +# + +# Define project name here +PROJECT = ch + +# Imported source files and paths +# Startup files. +include $(CHIBIOS)/os/common/startup/ARMCMx/compilers/GCC/mk/startup_stm32f4xx.mk +# HAL-OSAL files (optional). +include $(CHIBIOS)/os/hal/hal.mk +include $(CHIBIOS)/os/hal/ports/STM32/STM32F4xx/platform.mk +include $(CHIBIOS)/os/hal/osal/rt/osal.mk +# RTOS files (optional). +include $(CHIBIOS)/os/rt/rt.mk +include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk +# Other files (optional). +#include $(CHIBIOS)/test/rt/test.mk +include $(CHIBIOS)/os/hal/lib/streams/streams.mk +include $(CHIBIOS)/os/various/fatfs_bindings/fatfs.mk + +VARIOUSSRC = $(STREAMSSRC) + +VARIOUSINC = $(STREAMSINC) +# C sources that can be compiled in ARM or THUMB mode depending on the global +# setting. +CSRC = $(STARTUPSRC) \ + $(KERNSRC) \ + $(PORTSRC) \ + $(OSALSRC) \ + $(HALSRC) \ + $(PLATFORMSRC) \ + $(VARIOUSSRC) \ + $(FATFSSRC) \ + $(HWDEF)/common/stubs.c \ + $(HWDEF)/fmuv3/board.c \ + $(HWDEF)/common/usbcfg.c \ + $(HWDEF)/common/ppm.c \ + $(HWDEF)/common/flash.c \ + $(HWDEF)/common/malloc.c \ + $(HWDEF)/common/stdio.c \ + $(HWDEF)/common/posix.c \ + $(HWDEF)/common/hrt.c + + +# $(TESTSRC) \ +# test.c + +# C++ sources that can be compiled in ARM or THUMB mode depending on the global +# setting. +CPPSRC = + +# C sources to be compiled in ARM mode regardless of the global setting. +# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler +# option that results in lower performance and larger code size. +ACSRC = + +# C++ sources to be compiled in ARM mode regardless of the global setting. +# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler +# option that results in lower performance and larger code size. +ACPPSRC = + +# C sources to be compiled in THUMB mode regardless of the global setting. +# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler +# option that results in lower performance and larger code size. +TCSRC = + +# C sources to be compiled in THUMB mode regardless of the global setting. +# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler +# option that results in lower performance and larger code size. +TCPPSRC = + +# List ASM source files here +ASMSRC = +ASMXSRC = $(STARTUPASM) $(PORTASM) $(OSALASM) + +INCDIR = $(CHIBIOS)/os/license \ + $(STARTUPINC) $(KERNINC) $(PORTINC) $(OSALINC) $(FATFSINC) \ + $(HALINC) $(PLATFORMINC) $(BOARDINC) $(TESTINC) $(VARIOUSINC) \ + $(HWDEF)/common $(HWDEF)/fmuv3 + +# +# Project, sources and paths +############################################################################## + +############################################################################## +# Compiler settings +# + +MCU = cortex-m4 + +#TRGT = arm-elf- +TRGT = arm-none-eabi- +CC = $(TRGT)gcc +CPPC = $(TRGT)g++ +# Enable loading with g++ only if you need C++ runtime support. +# NOTE: You can use C++ even without C++ support if you are careful. C++ +# runtime support makes code size explode. +LD = $(TRGT)gcc +#LD = $(TRGT)g++ +CP = $(TRGT)objcopy +AS = $(TRGT)gcc -x assembler-with-cpp +AR = $(TRGT)ar +OD = $(TRGT)objdump +SZ = $(TRGT)size +HEX = $(CP) -O ihex +BIN = $(CP) -O binary + +# ARM-specific options here +AOPT = + +# THUMB-specific options here +TOPT = -mthumb -DTHUMB + +# Define C warning options here +CWARN = -Wall -Wextra -Wundef -Wstrict-prototypes + +# Define C++ warning options here +CPPWARN = -Wall -Wextra -Wundef + +# +# Compiler settings +############################################################################## + +############################################################################## +# Start of user section +# + +# List all user C define here, like -D_DEBUG=1 +UDEFS = -DBOARD_FLASH_SIZE=2048 + +# Define ASM defines here +UADEFS = + +# List all user directories here +UINCDIR = + +# List the user directory to look for the libraries here +ULIBDIR = + +# List all user libraries here +ULIBS = + +# +# End of user defines +############################################################################## +include $(HWDEF)/common/chibios_common.mk diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/ffconf.h b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/ffconf.h new file mode 100644 index 0000000000..c3d2b53f2c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/ffconf.h @@ -0,0 +1,272 @@ +#pragma once + +/* CHIBIOS FIX */ +#include "ch.h" + +/*---------------------------------------------------------------------------/ +/ FatFs - FAT file system module configuration file +/---------------------------------------------------------------------------*/ + +#define _FFCONF 68020 /* Revision ID */ + +/*---------------------------------------------------------------------------/ +/ Function Configurations +/---------------------------------------------------------------------------*/ + +#define _FS_READONLY 0 +/* This option switches read-only configuration. (0:Read/Write or 1:Read-only) +/ Read-only configuration removes writing API functions, f_write(), f_sync(), +/ f_unlink(), f_mkdir(), f_chmod(), f_rename(), f_truncate(), f_getfree() +/ and optional writing functions as well. */ + + +#define _FS_MINIMIZE 0 +/* This option defines minimization level to remove some basic API functions. +/ +/ 0: All basic functions are enabled. +/ 1: f_stat(), f_getfree(), f_unlink(), f_mkdir(), f_truncate() and f_rename() +/ are removed. +/ 2: f_opendir(), f_readdir() and f_closedir() are removed in addition to 1. +/ 3: f_lseek() function is removed in addition to 2. */ + + +#define _USE_STRFUNC 0 +/* This option switches string functions, f_gets(), f_putc(), f_puts() and +/ f_printf(). +/ +/ 0: Disable string functions. +/ 1: Enable without LF-CRLF conversion. +/ 2: Enable with LF-CRLF conversion. */ + + +#define _USE_FIND 0 +/* This option switches filtered directory read functions, f_findfirst() and +/ f_findnext(). (0:Disable, 1:Enable 2:Enable with matching altname[] too) */ + + +#define _USE_MKFS 0 +/* This option switches f_mkfs() function. (0:Disable or 1:Enable) */ + + +#define _USE_FASTSEEK 0 +/* This option switches fast seek function. (0:Disable or 1:Enable) */ + + +#define _USE_EXPAND 0 +/* This option switches f_expand function. (0:Disable or 1:Enable) */ + + +#define _USE_CHMOD 1 +/* This option switches attribute manipulation functions, f_chmod() and f_utime(). +/ (0:Disable or 1:Enable) Also _FS_READONLY needs to be 0 to enable this option. */ + + +#define _USE_LABEL 0 +/* This option switches volume label functions, f_getlabel() and f_setlabel(). +/ (0:Disable or 1:Enable) */ + + +#define _USE_FORWARD 0 +/* This option switches f_forward() function. (0:Disable or 1:Enable) */ + + +/*---------------------------------------------------------------------------/ +/ Locale and Namespace Configurations +/---------------------------------------------------------------------------*/ + +#define _CODE_PAGE 850 +/* This option specifies the OEM code page to be used on the target system. +/ Incorrect setting of the code page can cause a file open failure. +/ +/ 1 - ASCII (No extended character. Non-LFN cfg. only) +/ 437 - U.S. +/ 720 - Arabic +/ 737 - Greek +/ 771 - KBL +/ 775 - Baltic +/ 850 - Latin 1 +/ 852 - Latin 2 +/ 855 - Cyrillic +/ 857 - Turkish +/ 860 - Portuguese +/ 861 - Icelandic +/ 862 - Hebrew +/ 863 - Canadian French +/ 864 - Arabic +/ 865 - Nordic +/ 866 - Russian +/ 869 - Greek 2 +/ 932 - Japanese (DBCS) +/ 936 - Simplified Chinese (DBCS) +/ 949 - Korean (DBCS) +/ 950 - Traditional Chinese (DBCS) +*/ + + +#define _USE_LFN 3 +#define _MAX_LFN 255 +/* The _USE_LFN switches the support of long file name (LFN). +/ +/ 0: Disable support of LFN. _MAX_LFN has no effect. +/ 1: Enable LFN with static working buffer on the BSS. Always NOT thread-safe. +/ 2: Enable LFN with dynamic working buffer on the STACK. +/ 3: Enable LFN with dynamic working buffer on the HEAP. +/ +/ To enable the LFN, Unicode handling functions (option/unicode.c) must be added +/ to the project. The working buffer occupies (_MAX_LFN + 1) * 2 bytes and +/ additional 608 bytes at exFAT enabled. _MAX_LFN can be in range from 12 to 255. +/ It should be set 255 to support full featured LFN operations. +/ When use stack for the working buffer, take care on stack overflow. When use heap +/ memory for the working buffer, memory management functions, ff_memalloc() and +/ ff_memfree(), must be added to the project. */ + + +#define _LFN_UNICODE 0 +/* This option switches character encoding on the API. (0:ANSI/OEM or 1:UTF-16) +/ To use Unicode string for the path name, enable LFN and set _LFN_UNICODE = 1. +/ This option also affects behavior of string I/O functions. */ + + +#define _STRF_ENCODE 3 +/* When _LFN_UNICODE == 1, this option selects the character encoding ON THE FILE to +/ be read/written via string I/O functions, f_gets(), f_putc(), f_puts and f_printf(). +/ +/ 0: ANSI/OEM +/ 1: UTF-16LE +/ 2: UTF-16BE +/ 3: UTF-8 +/ +/ This option has no effect when _LFN_UNICODE == 0. */ + + +#define _FS_RPATH 1 +/* This option configures support of relative path. +/ +/ 0: Disable relative path and remove related functions. +/ 1: Enable relative path. f_chdir() and f_chdrive() are available. +/ 2: f_getcwd() function is available in addition to 1. +*/ + + +/*---------------------------------------------------------------------------/ +/ Drive/Volume Configurations +/---------------------------------------------------------------------------*/ + +#define _VOLUMES 1 +/* Number of volumes (logical drives) to be used. */ + + +#define _STR_VOLUME_ID 0 +#define _VOLUME_STRS "RAM","NAND","CF","SD","SD2","USB","USB2","USB3" +/* _STR_VOLUME_ID switches string support of volume ID. +/ When _STR_VOLUME_ID is set to 1, also pre-defined strings can be used as drive +/ number in the path name. _VOLUME_STRS defines the drive ID strings for each +/ logical drives. Number of items must be equal to _VOLUMES. Valid characters for +/ the drive ID strings are: A-Z and 0-9. */ + + +#define _MULTI_PARTITION 0 +/* This option switches support of multi-partition on a physical drive. +/ By default (0), each logical drive number is bound to the same physical drive +/ number and only an FAT volume found on the physical drive will be mounted. +/ When multi-partition is enabled (1), each logical drive number can be bound to +/ arbitrary physical drive and partition listed in the VolToPart[]. Also f_fdisk() +/ funciton will be available. */ + + +#define _MIN_SS 512 +#define _MAX_SS 512 +/* These options configure the range of sector size to be supported. (512, 1024, +/ 2048 or 4096) Always set both 512 for most systems, all type of memory cards and +/ harddisk. But a larger value may be required for on-board flash memory and some +/ type of optical media. When _MAX_SS is larger than _MIN_SS, FatFs is configured +/ to variable sector size and GET_SECTOR_SIZE command must be implemented to the +/ disk_ioctl() function. */ + + +#define _USE_TRIM 0 +/* This option switches support of ATA-TRIM. (0:Disable or 1:Enable) +/ To enable Trim function, also CTRL_TRIM command should be implemented to the +/ disk_ioctl() function. */ + + +#define _FS_NOFSINFO 0 +/* If you need to know correct free space on the FAT32 volume, set bit 0 of this +/ option, and f_getfree() function at first time after volume mount will force +/ a full FAT scan. Bit 1 controls the use of last allocated cluster number. +/ +/ bit0=0: Use free cluster count in the FSINFO if available. +/ bit0=1: Do not trust free cluster count in the FSINFO. +/ bit1=0: Use last allocated cluster number in the FSINFO if available. +/ bit1=1: Do not trust last allocated cluster number in the FSINFO. +*/ + + + +/*---------------------------------------------------------------------------/ +/ System Configurations +/---------------------------------------------------------------------------*/ + +#define _FS_TINY 0 +/* This option switches tiny buffer configuration. (0:Normal or 1:Tiny) +/ At the tiny configuration, size of file object (FIL) is reduced _MAX_SS bytes. +/ Instead of private sector buffer eliminated from the file object, common sector +/ buffer in the file system object (FATFS) is used for the file data transfer. */ + + +#define _FS_EXFAT 1 +/* This option switches support of exFAT file system. (0:Disable or 1:Enable) +/ When enable exFAT, also LFN needs to be enabled. (_USE_LFN >= 1) +/ Note that enabling exFAT discards C89 compatibility. */ + + +#define _FS_NORTC 0 +#define _NORTC_MON 1 +#define _NORTC_MDAY 1 +#define _NORTC_YEAR 2016 +/* The option _FS_NORTC switches timestamp functiton. If the system does not have +/ any RTC function or valid timestamp is not needed, set _FS_NORTC = 1 to disable +/ the timestamp function. All objects modified by FatFs will have a fixed timestamp +/ defined by _NORTC_MON, _NORTC_MDAY and _NORTC_YEAR in local time. +/ To enable timestamp function (_FS_NORTC = 0), get_fattime() function need to be +/ added to the project to get current time form real-time clock. _NORTC_MON, +/ _NORTC_MDAY and _NORTC_YEAR have no effect. +/ These options have no effect at read-only configuration (_FS_READONLY = 1). */ + + +#define _FS_LOCK 0 +/* The option _FS_LOCK switches file lock function to control duplicated file open +/ and illegal operation to open objects. This option must be 0 when _FS_READONLY +/ is 1. +/ +/ 0: Disable file lock function. To avoid volume corruption, application program +/ should avoid illegal open, remove and rename to the open objects. +/ >0: Enable file lock function. The value defines how many files/sub-directories +/ can be opened simultaneously under file lock control. Note that the file +/ lock control is independent of re-entrancy. */ + + +#define _FS_REENTRANT 0 +#define _FS_TIMEOUT MS2ST(1000) +#define _SYNC_t semaphore_t* +/* The option _FS_REENTRANT switches the re-entrancy (thread safe) of the FatFs +/ module itself. Note that regardless of this option, file access to different +/ volume is always re-entrant and volume control functions, f_mount(), f_mkfs() +/ and f_fdisk() function, are always not re-entrant. Only file/directory access +/ to the same volume is under control of this function. +/ +/ 0: Disable re-entrancy. _FS_TIMEOUT and _SYNC_t have no effect. +/ 1: Enable re-entrancy. Also user provided synchronization handlers, +/ ff_req_grant(), ff_rel_grant(), ff_del_syncobj() and ff_cre_syncobj() +/ function, must be added to the project. Samples are available in +/ option/syscall.c. +/ +/ The _FS_TIMEOUT defines timeout period in unit of time tick. +/ The _SYNC_t defines O/S dependent sync object type. e.g. HANDLE, ID, OS_EVENT*, +/ SemaphoreHandle_t and etc.. A header file for O/S definitions needs to be +/ included somewhere in the scope of ff.h. */ + +/* #include // O/S definitions */ + + +/*--- End of configuration options ---*/ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/halconf.h b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/halconf.h new file mode 100644 index 0000000000..29f62535fc --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/halconf.h @@ -0,0 +1,401 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ + +/** + * @file templates/halconf.h + * @brief HAL configuration header. + * @details HAL configuration file, this file allows to enable or disable the + * various device drivers from your application. You may also use + * this file in order to override the device drivers default settings. + * + * @addtogroup HAL_CONF + * @{ + */ + +#pragma once +#include "mcuconf.h" + +/** + * @brief Enables the PAL subsystem. + */ +#if !defined(HAL_USE_PAL) || defined(__DOXYGEN__) +#define HAL_USE_PAL TRUE +#endif + +/** + * @brief Enables the ADC subsystem. + */ +#if !defined(HAL_USE_ADC) || defined(__DOXYGEN__) +#define HAL_USE_ADC TRUE +#endif + +/** + * @brief Enables the CAN subsystem. + */ +#if !defined(HAL_USE_CAN) || defined(__DOXYGEN__) +#define HAL_USE_CAN FALSE +#endif + +/** + * @brief Enables the DAC subsystem. + */ +#if !defined(HAL_USE_DAC) || defined(__DOXYGEN__) +#define HAL_USE_DAC FALSE +#endif + +/** + * @brief Enables the EXT subsystem. + */ +#if !defined(HAL_USE_EXT) || defined(__DOXYGEN__) +#define HAL_USE_EXT TRUE +#endif + +/** + * @brief Enables the GPT subsystem. + */ +#if !defined(HAL_USE_GPT) || defined(__DOXYGEN__) +#define HAL_USE_GPT TRUE +#endif + +/** + * @brief Enables the I2C subsystem. + */ +#if !defined(HAL_USE_I2C) || defined(__DOXYGEN__) +#define HAL_USE_I2C TRUE +#endif + +/** + * @brief Enables the I2S subsystem. + */ +#if !defined(HAL_USE_I2S) || defined(__DOXYGEN__) +#define HAL_USE_I2S FALSE +#endif + +/** + * @brief Enables the ICU subsystem. + */ +#if !defined(HAL_USE_ICU) || defined(__DOXYGEN__) +#define HAL_USE_ICU TRUE +#endif + +/** + * @brief Enables the MAC subsystem. + */ +#if !defined(HAL_USE_MAC) || defined(__DOXYGEN__) +#define HAL_USE_MAC FALSE +#endif + +/** + * @brief Enables the MMC_SPI subsystem. + */ +#if !defined(HAL_USE_MMC_SPI) || defined(__DOXYGEN__) +#define HAL_USE_MMC_SPI FALSE +#endif + +/** + * @brief Enables the PWM subsystem. + */ +#if !defined(HAL_USE_PWM) || defined(__DOXYGEN__) +#define HAL_USE_PWM TRUE +#endif + +/** + * @brief Enables the QSPI subsystem. + */ +#if !defined(HAL_USE_QSPI) || defined(__DOXYGEN__) +#define HAL_USE_QSPI FALSE +#endif + +/** + * @brief Enables the RTC subsystem. + */ +#if !defined(HAL_USE_RTC) || defined(__DOXYGEN__) +#define HAL_USE_RTC FALSE +#endif + +/** + * @brief Enables the SDC subsystem. + */ +#if !defined(HAL_USE_SDC) || defined(__DOXYGEN__) +#define HAL_USE_SDC TRUE +#endif + +/** + * @brief Enables the SERIAL subsystem. + */ +#if !defined(HAL_USE_SERIAL) || defined(__DOXYGEN__) +#define HAL_USE_SERIAL TRUE +#endif + +/** + * @brief Enables the SERIAL over USB subsystem. + */ +#if !defined(HAL_USE_SERIAL_USB) || defined(__DOXYGEN__) +#define HAL_USE_SERIAL_USB TRUE +#endif + +/** + * @brief Enables the SPI subsystem. + */ +#if !defined(HAL_USE_SPI) || defined(__DOXYGEN__) +#define HAL_USE_SPI TRUE +#endif + +/** + * @brief Enables the UART subsystem. + */ +#if !defined(HAL_USE_UART) || defined(__DOXYGEN__) +#define HAL_USE_UART FALSE +#endif + +/** + * @brief Enables the USB subsystem. + */ +#if !defined(HAL_USE_USB) || defined(__DOXYGEN__) +#define HAL_USE_USB TRUE +#endif + +/** + * @brief Enables the WDG subsystem. + */ +#if !defined(HAL_USE_WDG) || defined(__DOXYGEN__) +#define HAL_USE_WDG FALSE +#endif + +/*===========================================================================*/ +/* ADC driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables synchronous APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(ADC_USE_WAIT) || defined(__DOXYGEN__) +#define ADC_USE_WAIT TRUE +#endif + +/** + * @brief Enables the @p adcAcquireBus() and @p adcReleaseBus() APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(ADC_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) +#define ADC_USE_MUTUAL_EXCLUSION TRUE +#endif + +/*===========================================================================*/ +/* CAN driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Sleep mode related APIs inclusion switch. + */ +#if !defined(CAN_USE_SLEEP_MODE) || defined(__DOXYGEN__) +#define CAN_USE_SLEEP_MODE TRUE +#endif + +/*===========================================================================*/ +/* I2C driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables the mutual exclusion APIs on the I2C bus. + */ +#if !defined(I2C_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) +#define I2C_USE_MUTUAL_EXCLUSION TRUE +#endif + +/*===========================================================================*/ +/* MAC driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables an event sources for incoming packets. + */ +#if !defined(MAC_USE_ZERO_COPY) || defined(__DOXYGEN__) +#define MAC_USE_ZERO_COPY FALSE +#endif + +/** + * @brief Enables an event sources for incoming packets. + */ +#if !defined(MAC_USE_EVENTS) || defined(__DOXYGEN__) +#define MAC_USE_EVENTS TRUE +#endif + +/*===========================================================================*/ +/* MMC_SPI driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Delays insertions. + * @details If enabled this options inserts delays into the MMC waiting + * routines releasing some extra CPU time for the threads with + * lower priority, this may slow down the driver a bit however. + * This option is recommended also if the SPI driver does not + * use a DMA channel and heavily loads the CPU. + */ +#if !defined(MMC_NICE_WAITING) || defined(__DOXYGEN__) +#define MMC_NICE_WAITING TRUE +#endif + +/*===========================================================================*/ +/* SDC driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Number of initialization attempts before rejecting the card. + * @note Attempts are performed at 10mS intervals. + */ +#if !defined(SDC_INIT_RETRY) || defined(__DOXYGEN__) +#define SDC_INIT_RETRY 100 +#endif + +/** + * @brief Include support for MMC cards. + * @note MMC support is not yet implemented so this option must be kept + * at @p FALSE. + */ +#if !defined(SDC_MMC_SUPPORT) || defined(__DOXYGEN__) +#define SDC_MMC_SUPPORT FALSE +#endif + +/** + * @brief Delays insertions. + * @details If enabled this options inserts delays into the MMC waiting + * routines releasing some extra CPU time for the threads with + * lower priority, this may slow down the driver a bit however. + */ +#if !defined(SDC_NICE_WAITING) || defined(__DOXYGEN__) +#define SDC_NICE_WAITING TRUE +#endif + +/*===========================================================================*/ +/* SERIAL driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Default bit rate. + * @details Configuration parameter, this is the baud rate selected for the + * default configuration. + */ +#if !defined(SERIAL_DEFAULT_BITRATE) || defined(__DOXYGEN__) +#define SERIAL_DEFAULT_BITRATE 38400 +#endif + +/** + * @brief Serial buffers size. + * @details Configuration parameter, you can change the depth of the queue + * buffers depending on the requirements of your application. + * @note The default is 16 bytes for both the transmission and receive + * buffers. + */ +#if !defined(SERIAL_BUFFERS_SIZE) || defined(__DOXYGEN__) +#define SERIAL_BUFFERS_SIZE 1024 +#endif + +/*===========================================================================*/ +/* SERIAL_USB driver related setting. */ +/*===========================================================================*/ + +/** + * @brief Serial over USB buffers size. + * @details Configuration parameter, the buffer size must be a multiple of + * the USB data endpoint maximum packet size. + * @note The default is 256 bytes for both the transmission and receive + * buffers. + */ +#if !defined(SERIAL_USB_BUFFERS_SIZE) || defined(__DOXYGEN__) +#define SERIAL_USB_BUFFERS_SIZE 256 +#endif + +/** + * @brief Serial over USB number of buffers. + * @note The default is 2 buffers. + */ +#if !defined(SERIAL_USB_BUFFERS_NUMBER) || defined(__DOXYGEN__) +#define SERIAL_USB_BUFFERS_NUMBER 2 +#endif + +/*===========================================================================*/ +/* SPI driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables synchronous APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(SPI_USE_WAIT) || defined(__DOXYGEN__) +#define SPI_USE_WAIT TRUE +#endif + +/** + * @brief Enables the @p spiAcquireBus() and @p spiReleaseBus() APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(SPI_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) +#define SPI_USE_MUTUAL_EXCLUSION TRUE +#endif + +/*===========================================================================*/ +/* UART driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables synchronous APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(UART_USE_WAIT) || defined(__DOXYGEN__) +#define UART_USE_WAIT FALSE +#endif + +/** + * @brief Enables the @p uartAcquireBus() and @p uartReleaseBus() APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(UART_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) +#define UART_USE_MUTUAL_EXCLUSION FALSE +#endif + +/*===========================================================================*/ +/* USB driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables synchronous APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(USB_USE_WAIT) || defined(__DOXYGEN__) +#define USB_USE_WAIT FALSE +#endif + + +/** @} */ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat new file mode 100644 index 0000000000..46978dff89 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat @@ -0,0 +1,114 @@ +# hw definition file for processing by chibios_pins.py +# for FMUv3 hardware + +# MCU class and specific type +MCU STM32F4xx STM32F427xx + +# board ID for firmware load +APJ_BOARD_ID 9 + +# crystal frequency +OSCILLATOR_HZ 24000000 + +# board voltage +STM32_VDD 330U + +# serial port for stdout +STDOUT_SERIAL SD7 +STDOUT_BAUDRATE 115200 + +# USB setup +USB_VENDOR 0x0483 # ST +USB_PRODUCT 0x5740 +USB_STRING_MANUFACTURER "ArduPilot" +USB_STRING_PRODUCT "ChibiOS/RT Virtual COM Port" +USB_STRING_SERIAL "100" + +# UART4 serial GPS +PA0 UART4_TX UART4 +PA1 UART4_RX UART4 +PA2 BATT_VOLTAGE_SENS ADC1 +PA3 BATT_CURRENT_SENS ADC1 +PA4 VDD_5V_SENS ADC1 +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 + +PA8 VDD_5V_PERIPH_EN OUTPUT LOW +PA9 VBUS INPUT OPENDRAIN +# PA10 IO-debug-console +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD +# PA15 ALARM + +PB0 EXTERN_DRDY INPUT +PB1 EXTERN_CS INPUT +PB2 BOOT1 INPUT +PB3 FMU_SW0 INPUT +PB8 I2C1_SCL I2C1 +PB9 I2C1_SDA I2C1 +PB10 I2C2_SCL I2C2 +PB11 I2C2_SDA I2C2 +PB13 SPI2_SCK SPI2 +PB14 SPI2_MISO SPI2 +PB15 SPI2_MOSI SPI2 + +PC0 VBUS_VALID INPUT +PC1 MAG_CS SPI1 CS +PC2 MPU_CS SPI1 CS +PC3 AUX_POWER ADC1 +PC4 AUX_ADC2 ADC1 +PC5 PRESSURE_SENS ADC1 +# USART6 to IO +PC6 USART6_TX USART6 +PC7 USART6_RX USART6 + +PC8 SDIO_D0 SDIO +PC9 SDIO_D1 SDIO +PC10 SDIO_D2 SDIO +PC11 SDIO_D3 SDIO +PC12 SDIO_CK SDIO +PC13 GYRO_EXT_CS CS +PC14 BARO_EXT_CS CS +PC15 ACCEL_EXT_CS CS + +# PD0 CAN1_RX +# PD1 CAN1_TX +PD2 SDIO_CMD SDIO +# USART2 serial2 telem1 +PD3 USART2_CTS USART2 +PD4 USART2_RTS USART2 +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 +PD7 BARO_CS CS + +# USART3 serial3 telem2 +PD8 USART3_TX USART3 +PD9 USART3_RX USART3 +PD10 FRAM_CS CS +PD11 USART3_CTS USART3 +PD12 USART3_RTS USART3 +PD13 TIM4_CH2 TIM4 +PD14 TIM4_CH3 TIM4 +PD15 MPU_DRDY INPUT + +# UART8 serial4 GPS2 +PE0 UART8_RX UART8 +PE1 UART8_TX UART8 +PE2 SPI4_SCK SPI4 +PE3 VDD_3V3_SENSORS_EN OUTPUT HIGH +PE4 MPU_EXT_CS CS +PE5 SPI4_MISO SPI4 +PE6 SPI4_MOSI SPI4 +# UART7 debug console +PE7 UART7_RX UART7 +PE8 UART7_TX UART7 +PE9 TIM1_CH1 TIM1 +PE10 VDD_5V_HIPOWER_OC INPUT +PE11 TIM1_CH2 TIM1 +PE12 FMU_LED_AMBER OUTPUT +PE13 TIM1_CH3 TIM1 +PE14 TIM1_CH4 TIM1 +PE15 VDD_5V_PERIPH_OC INPUT diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/ldscript.ld b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/ldscript.ld new file mode 100644 index 0000000000..dd9df8df38 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/ldscript.ld @@ -0,0 +1,393 @@ +/* + ChibiOS - Copyright (C) 2006..2015 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +/* + * ST32F429xI memory setup. + * Note: Use of ram1, ram2 and ram3 is mutually exclusive with use of ram0. + */ +MEMORY +{ + flash : org = 0x08004000, len = 2032K + ram0 : org = 0x20000000, len = 192k /* SRAM1 + SRAM2 + SRAM3 */ + ram1 : org = 0x20000000, len = 112k /* SRAM1 */ + ram2 : org = 0x2001C000, len = 16k /* SRAM2 */ + ram3 : org = 0x20020000, len = 64k /* SRAM3 */ + ram4 : org = 0x10000000, len = 64k /* CCM SRAM */ + ram5 : org = 0x40024000, len = 4k /* BCKP SRAM */ + ram6 : org = 0x00000000, len = 0 + ram7 : org = 0x00000000, len = 0 +} + +/* RAM region to be used for Main stack. This stack accommodates the processing + of all exceptions and interrupts*/ +REGION_ALIAS("MAIN_STACK_RAM", ram0); + +/* RAM region to be used for the process stack. This is the stack used by + the main() function.*/ +REGION_ALIAS("PROCESS_STACK_RAM", ram0); + +/* RAM region to be used for data segment.*/ +REGION_ALIAS("DATA_RAM", ram0); + +/* RAM region to be used for BSS segment.*/ +REGION_ALIAS("BSS_RAM", ram0); + +/* RAM region to be used for the default heap.*/ +REGION_ALIAS("HEAP_RAM", ram0); + +__ram0_start__ = ORIGIN(ram0); +__ram0_size__ = LENGTH(ram0); +__ram0_end__ = __ram0_start__ + __ram0_size__; +__ram1_start__ = ORIGIN(ram1); +__ram1_size__ = LENGTH(ram1); +__ram1_end__ = __ram1_start__ + __ram1_size__; +__ram2_start__ = ORIGIN(ram2); +__ram2_size__ = LENGTH(ram2); +__ram2_end__ = __ram2_start__ + __ram2_size__; +__ram3_start__ = ORIGIN(ram3); +__ram3_size__ = LENGTH(ram3); +__ram3_end__ = __ram3_start__ + __ram3_size__; +__ram4_start__ = ORIGIN(ram4); +__ram4_size__ = LENGTH(ram4); +__ram4_end__ = __ram4_start__ + __ram4_size__; +__ram5_start__ = ORIGIN(ram5); +__ram5_size__ = LENGTH(ram5); +__ram5_end__ = __ram5_start__ + __ram5_size__; +__ram6_start__ = ORIGIN(ram6); +__ram6_size__ = LENGTH(ram6); +__ram6_end__ = __ram6_start__ + __ram6_size__; +__ram7_start__ = ORIGIN(ram7); +__ram7_size__ = LENGTH(ram7); +__ram7_end__ = __ram7_start__ + __ram7_size__; + +ENTRY(Reset_Handler) + +SECTIONS +{ + . = 0; + _text = .; + + startup : ALIGN(16) SUBALIGN(16) + { + KEEP(*(.vectors)) + } > flash + + constructors : ALIGN(4) SUBALIGN(4) + { + __init_array_start = .; + KEEP(*(SORT(.init_array.*))) + KEEP(*(.init_array)) + __init_array_end = .; + } > flash + + destructors : ALIGN(4) SUBALIGN(4) + { + __fini_array_start = .; + KEEP(*(.fini_array)) + KEEP(*(SORT(.fini_array.*))) + __fini_array_end = .; + } > flash + + .text : ALIGN(16) SUBALIGN(16) + { + *(.text) + *(.text.*) + *(.rodata) + *(.rodata.*) + *(.glue_7t) + *(.glue_7) + *(.gcc*) + } > flash + + .ARM.extab : + { + *(.ARM.extab* .gnu.linkonce.armextab.*) + } > flash + + .ARM.exidx : { + __exidx_start = .; + *(.ARM.exidx* .gnu.linkonce.armexidx.*) + __exidx_end = .; + } > flash + + .eh_frame_hdr : + { + *(.eh_frame_hdr) + } > flash + + .eh_frame : ONLY_IF_RO + { + *(.eh_frame) + } > flash + + .textalign : ONLY_IF_RO + { + . = ALIGN(8); + } > flash + + /* Legacy symbol, not used anywhere.*/ + . = ALIGN(4); + PROVIDE(_etext = .); + + /* Special section for exceptions stack.*/ + .mstack : + { + . = ALIGN(8); + __main_stack_base__ = .; + . += __main_stack_size__; + . = ALIGN(8); + __main_stack_end__ = .; + } > MAIN_STACK_RAM + + /* Special section for process stack.*/ + .pstack : + { + __process_stack_base__ = .; + __main_thread_stack_base__ = .; + . += __process_stack_size__; + . = ALIGN(8); + __process_stack_end__ = .; + __main_thread_stack_end__ = .; + } > PROCESS_STACK_RAM + + .data : ALIGN(4) + { + . = ALIGN(4); + PROVIDE(_textdata = LOADADDR(.data)); + PROVIDE(_data = .); + _textdata_start = LOADADDR(.data); + _data_start = .; + *(.data) + *(.data.*) + *(.ramtext) + . = ALIGN(4); + PROVIDE(_edata = .); + _data_end = .; + } > DATA_RAM AT > flash + + .bss (NOLOAD) : ALIGN(4) + { + . = ALIGN(4); + _bss_start = .; + *(.bss) + *(.bss.*) + *(COMMON) + . = ALIGN(4); + _bss_end = .; + PROVIDE(end = .); + } > BSS_RAM + + .ram0_init : ALIGN(4) + { + . = ALIGN(4); + __ram0_init_text__ = LOADADDR(.ram0_init); + __ram0_init__ = .; + *(.ram0_init) + *(.ram0_init.*) + . = ALIGN(4); + } > ram0 AT > flash + + .ram0 (NOLOAD) : ALIGN(4) + { + . = ALIGN(4); + __ram0_clear__ = .; + *(.ram0_clear) + *(.ram0_clear.*) + . = ALIGN(4); + __ram0_noinit__ = .; + *(.ram0) + *(.ram0.*) + . = ALIGN(4); + __ram0_free__ = .; + } > ram0 + + .ram1_init : ALIGN(4) + { + . = ALIGN(4); + __ram1_init_text__ = LOADADDR(.ram1_init); + __ram1_init__ = .; + *(.ram1_init) + *(.ram1_init.*) + . = ALIGN(4); + } > ram1 AT > flash + + .ram1 (NOLOAD) : ALIGN(4) + { + . = ALIGN(4); + __ram1_clear__ = .; + *(.ram1_clear) + *(.ram1_clear.*) + . = ALIGN(4); + __ram1_noinit__ = .; + *(.ram1) + *(.ram1.*) + . = ALIGN(4); + __ram1_free__ = .; + } > ram1 + + .ram2_init : ALIGN(4) + { + . = ALIGN(4); + __ram2_init_text__ = LOADADDR(.ram2_init); + __ram2_init__ = .; + *(.ram2_init) + *(.ram2_init.*) + . = ALIGN(4); + } > ram2 AT > flash + + .ram2 (NOLOAD) : ALIGN(4) + { + . = ALIGN(4); + __ram2_clear__ = .; + *(.ram2_clear) + *(.ram2_clear.*) + . = ALIGN(4); + __ram2_noinit__ = .; + *(.ram2) + *(.ram2.*) + . = ALIGN(4); + __ram2_free__ = .; + } > ram2 + + .ram3_init : ALIGN(4) + { + . = ALIGN(4); + __ram3_init_text__ = LOADADDR(.ram3_init); + __ram3_init__ = .; + *(.ram3_init) + *(.ram3_init.*) + . = ALIGN(4); + } > ram3 AT > flash + + .ram3 (NOLOAD) : ALIGN(4) + { + . = ALIGN(4); + __ram3_clear__ = .; + *(.ram3_clear) + *(.ram3_clear.*) + . = ALIGN(4); + __ram3_noinit__ = .; + *(.ram3) + *(.ram3.*) + . = ALIGN(4); + __ram3_free__ = .; + } > ram3 + + .ram4_init : ALIGN(4) + { + . = ALIGN(4); + __ram4_init_text__ = LOADADDR(.ram4_init); + __ram4_init__ = .; + *(.ram4_init) + *(.ram4_init.*) + . = ALIGN(4); + } > ram4 AT > flash + + .ram4 (NOLOAD) : ALIGN(4) + { + . = ALIGN(4); + __ram4_clear__ = .; + *(.ram4_clear) + *(.ram4_clear.*) + . = ALIGN(4); + __ram4_noinit__ = .; + *(.ram4) + *(.ram4.*) + . = ALIGN(4); + __ram4_free__ = .; + } > ram4 + + .ram5_init : ALIGN(4) + { + . = ALIGN(4); + __ram5_init_text__ = LOADADDR(.ram5_init); + __ram5_init__ = .; + *(.ram5_init) + *(.ram5_init.*) + . = ALIGN(4); + } > ram5 AT > flash + + .ram5 (NOLOAD) : ALIGN(4) + { + . = ALIGN(4); + __ram5_clear__ = .; + *(.ram5_clear) + *(.ram5_clear.*) + . = ALIGN(4); + __ram5_noinit__ = .; + *(.ram5) + *(.ram5.*) + . = ALIGN(4); + __ram5_free__ = .; + } > ram5 + + .ram6_init : ALIGN(4) + { + . = ALIGN(4); + __ram6_init_text__ = LOADADDR(.ram6_init); + __ram6_init__ = .; + *(.ram6_init) + *(.ram6_init.*) + . = ALIGN(4); + } > ram6 AT > flash + + .ram6 (NOLOAD) : ALIGN(4) + { + . = ALIGN(4); + __ram6_clear__ = .; + *(.ram6_clear) + *(.ram6_clear.*) + . = ALIGN(4); + __ram6_noinit__ = .; + *(.ram6) + *(.ram6.*) + . = ALIGN(4); + __ram6_free__ = .; + } > ram6 + + .ram7_init : ALIGN(4) + { + . = ALIGN(4); + __ram7_init_text__ = LOADADDR(.ram7_init); + __ram7_init__ = .; + *(.ram7_init) + *(.ram7_init.*) + . = ALIGN(4); + } > ram7 AT > flash + + .ram7 (NOLOAD) : ALIGN(4) + { + . = ALIGN(4); + __ram7_clear__ = .; + *(.ram7_clear) + *(.ram7_clear.*) + . = ALIGN(4); + __ram7_noinit__ = .; + *(.ram7) + *(.ram7.*) + . = ALIGN(4); + __ram7_free__ = .; + } > ram7 + + /* The default heap uses the (statically) unused part of a RAM section.*/ + .heap (NOLOAD) : + { + . = ALIGN(8); + __heap_base__ = .; + . = ORIGIN(HEAP_RAM) + LENGTH(HEAP_RAM); + __heap_end__ = .; + } > HEAP_RAM +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/mcuconf.h new file mode 100644 index 0000000000..8fdc6f8bab --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/mcuconf.h @@ -0,0 +1,306 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ + +#pragma once +/* + * STM32F4xx drivers configuration. + * The following settings override the default settings present in + * the various device driver implementation headers. + * Note that the settings for each driver only have effect if the whole + * driver is enabled in halconf.h. + * + * IRQ priorities: + * 15...0 Lowest...Highest. + * + * DMA priorities: + * 0...3 Lowest...Highest. + */ + +/* + * HAL driver system settings. + */ +#define STM32_NO_INIT FALSE +#define STM32_HSI_ENABLED TRUE +#define STM32_LSI_ENABLED TRUE +#define STM32_HSE_ENABLED TRUE +#define STM32_LSE_ENABLED FALSE +#define STM32_CLOCK48_REQUIRED TRUE +#define STM32_SW STM32_SW_PLL +#define STM32_PLLSRC STM32_PLLSRC_HSE +#define STM32_PLLM_VALUE 24 +#define STM32_PLLN_VALUE 336 +#define STM32_PLLP_VALUE 2 +#define STM32_PLLQ_VALUE 7 +#define STM32_HPRE STM32_HPRE_DIV1 +#define STM32_PPRE1 STM32_PPRE1_DIV4 +#define STM32_PPRE2 STM32_PPRE2_DIV2 +#define STM32_RTCSEL STM32_RTCSEL_LSI +#define STM32_RTCPRE_VALUE 8 +#define STM32_MCO1SEL STM32_MCO1SEL_HSI +#define STM32_MCO1PRE STM32_MCO1PRE_DIV1 +#define STM32_MCO2SEL STM32_MCO2SEL_SYSCLK +#define STM32_MCO2PRE STM32_MCO2PRE_DIV5 +#define STM32_I2SSRC STM32_I2SSRC_CKIN +#define STM32_PLLI2SN_VALUE 192 +#define STM32_PLLI2SR_VALUE 5 +#define STM32_PVD_ENABLE FALSE +#define STM32_PLS STM32_PLS_LEV0 +#define STM32_BKPRAM_ENABLE FALSE + +/* + * ADC driver system settings. + */ +#define STM32_ADC_ADCPRE ADC_CCR_ADCPRE_DIV4 +#define STM32_ADC_USE_ADC1 TRUE +#define STM32_ADC_USE_ADC2 FALSE +#define STM32_ADC_USE_ADC3 FALSE +#define STM32_ADC_ADC1_DMA_PRIORITY 2 +#define STM32_ADC_ADC2_DMA_PRIORITY 2 +#define STM32_ADC_ADC3_DMA_PRIORITY 2 +#define STM32_ADC_IRQ_PRIORITY 6 +#define STM32_ADC_ADC1_DMA_IRQ_PRIORITY 6 +#define STM32_ADC_ADC2_DMA_IRQ_PRIORITY 6 +#define STM32_ADC_ADC3_DMA_IRQ_PRIORITY 6 + +/* + * CAN driver system settings. + */ +#define STM32_CAN_USE_CAN1 FALSE +#define STM32_CAN_USE_CAN2 FALSE +#define STM32_CAN_CAN1_IRQ_PRIORITY 11 +#define STM32_CAN_CAN2_IRQ_PRIORITY 11 + +/* + * DAC driver system settings. + */ +#define STM32_DAC_DUAL_MODE FALSE +#define STM32_DAC_USE_DAC1_CH1 FALSE +#define STM32_DAC_USE_DAC1_CH2 FALSE +#define STM32_DAC_DAC1_CH1_IRQ_PRIORITY 10 +#define STM32_DAC_DAC1_CH2_IRQ_PRIORITY 10 +#define STM32_DAC_DAC1_CH1_DMA_PRIORITY 2 +#define STM32_DAC_DAC1_CH2_DMA_PRIORITY 2 + +/* + * EXT driver system settings. + */ +#define STM32_EXT_EXTI0_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI1_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI2_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI3_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI4_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI5_9_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI10_15_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI16_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI17_IRQ_PRIORITY 15 +#define STM32_EXT_EXTI18_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI19_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI20_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI21_IRQ_PRIORITY 15 +#define STM32_EXT_EXTI22_IRQ_PRIORITY 15 + +/* + * GPT driver system settings. + */ +#define STM32_GPT_USE_TIM1 FALSE +#define STM32_GPT_USE_TIM2 FALSE +#define STM32_GPT_USE_TIM3 FALSE +#define STM32_GPT_USE_TIM4 FALSE +#define STM32_GPT_USE_TIM5 TRUE +#define STM32_GPT_USE_TIM6 FALSE +#define STM32_GPT_USE_TIM7 FALSE +#define STM32_GPT_USE_TIM8 FALSE +#define STM32_GPT_USE_TIM9 FALSE +#define STM32_GPT_USE_TIM11 FALSE +#define STM32_GPT_USE_TIM12 FALSE +#define STM32_GPT_USE_TIM14 FALSE +#define STM32_GPT_TIM1_IRQ_PRIORITY 7 +#define STM32_GPT_TIM2_IRQ_PRIORITY 7 +#define STM32_GPT_TIM3_IRQ_PRIORITY 7 +#define STM32_GPT_TIM4_IRQ_PRIORITY 7 +#define STM32_GPT_TIM5_IRQ_PRIORITY 7 +#define STM32_GPT_TIM6_IRQ_PRIORITY 7 +#define STM32_GPT_TIM7_IRQ_PRIORITY 7 +#define STM32_GPT_TIM8_IRQ_PRIORITY 7 +#define STM32_GPT_TIM9_IRQ_PRIORITY 7 +#define STM32_GPT_TIM11_IRQ_PRIORITY 7 +#define STM32_GPT_TIM12_IRQ_PRIORITY 7 +#define STM32_GPT_TIM14_IRQ_PRIORITY 7 + +/* + * I2C driver system settings. + */ +#define STM32_I2C_BUSY_TIMEOUT 50 +#define STM32_I2C_I2C1_IRQ_PRIORITY 5 +#define STM32_I2C_I2C2_IRQ_PRIORITY 5 +#define STM32_I2C_I2C3_IRQ_PRIORITY 5 +#define STM32_I2C_I2C1_DMA_PRIORITY 3 +#define STM32_I2C_I2C2_DMA_PRIORITY 3 +#define STM32_I2C_I2C3_DMA_PRIORITY 3 +#define STM32_I2C_DMA_ERROR_HOOK(i2cp) osalSysHalt("DMA failure") + +/* + * I2S driver system settings. + */ +#define STM32_I2S_SPI2_IRQ_PRIORITY 10 +#define STM32_I2S_SPI3_IRQ_PRIORITY 10 +#define STM32_I2S_SPI2_DMA_PRIORITY 1 +#define STM32_I2S_SPI3_DMA_PRIORITY 1 +#define STM32_I2S_DMA_ERROR_HOOK(i2sp) osalSysHalt("DMA failure") + +/* + * ICU driver system settings. + */ +#define STM32_ICU_USE_TIM1 FALSE +#define STM32_ICU_USE_TIM2 FALSE +#define STM32_ICU_USE_TIM3 FALSE +#define STM32_ICU_USE_TIM4 TRUE +#define STM32_ICU_USE_TIM5 FALSE +#define STM32_ICU_USE_TIM8 FALSE +#define STM32_ICU_USE_TIM9 FALSE +#define STM32_ICU_TIM1_IRQ_PRIORITY 7 +#define STM32_ICU_TIM2_IRQ_PRIORITY 7 +#define STM32_ICU_TIM3_IRQ_PRIORITY 7 +#define STM32_ICU_TIM4_IRQ_PRIORITY 7 +#define STM32_ICU_TIM5_IRQ_PRIORITY 7 +#define STM32_ICU_TIM8_IRQ_PRIORITY 7 +#define STM32_ICU_TIM9_IRQ_PRIORITY 7 + +/* + * MAC driver system settings. + */ +#define STM32_MAC_TRANSMIT_BUFFERS 2 +#define STM32_MAC_RECEIVE_BUFFERS 4 +#define STM32_MAC_BUFFERS_SIZE 1522 +#define STM32_MAC_PHY_TIMEOUT 100 +#define STM32_MAC_ETH1_CHANGE_PHY_STATE TRUE +#define STM32_MAC_ETH1_IRQ_PRIORITY 13 +#define STM32_MAC_IP_CHECKSUM_OFFLOAD 0 + +/* + * PWM driver system settings. + */ +#define STM32_PWM_USE_ADVANCED FALSE +#define STM32_PWM_USE_TIM1 TRUE +#define STM32_PWM_USE_TIM2 FALSE +#define STM32_PWM_USE_TIM3 FALSE +#define STM32_PWM_USE_TIM4 FALSE +#define STM32_PWM_USE_TIM5 FALSE +#define STM32_PWM_USE_TIM8 FALSE +#define STM32_PWM_USE_TIM9 FALSE +#define STM32_PWM_TIM1_IRQ_PRIORITY 7 +#define STM32_PWM_TIM2_IRQ_PRIORITY 7 +#define STM32_PWM_TIM3_IRQ_PRIORITY 7 +#define STM32_PWM_TIM4_IRQ_PRIORITY 7 +#define STM32_PWM_TIM5_IRQ_PRIORITY 7 +#define STM32_PWM_TIM8_IRQ_PRIORITY 7 +#define STM32_PWM_TIM9_IRQ_PRIORITY 7 + +/* + * SDC driver system settings. + */ +#define STM32_SDC_SDIO_DMA_PRIORITY 3 +#define STM32_SDC_SDIO_IRQ_PRIORITY 9 +#define STM32_SDC_WRITE_TIMEOUT_MS 1000 +#define STM32_SDC_READ_TIMEOUT_MS 1000 +#define STM32_SDC_CLOCK_ACTIVATION_DELAY 10 +#define STM32_SDC_SDIO_UNALIGNED_SUPPORT TRUE + +/* + * SERIAL driver system settings. + */ +#define STM32_SERIAL_USART1_PRIORITY 11 +#define STM32_SERIAL_USART2_PRIORITY 11 +#define STM32_SERIAL_USART3_PRIORITY 11 +#define STM32_SERIAL_UART4_PRIORITY 11 +#define STM32_SERIAL_UART5_PRIORITY 11 +#define STM32_SERIAL_USART6_PRIORITY 11 +#define STM32_SERIAL_UART7_PRIORITY 11 +#define STM32_SERIAL_UART8_PRIORITY 11 + +/* + * SPI driver system settings. + */ +#define STM32_SPI_SPI1_DMA_PRIORITY 1 +#define STM32_SPI_SPI2_DMA_PRIORITY 1 +#define STM32_SPI_SPI3_DMA_PRIORITY 1 +#define STM32_SPI_SPI4_DMA_PRIORITY 1 +#define STM32_SPI_SPI1_IRQ_PRIORITY 10 +#define STM32_SPI_SPI2_IRQ_PRIORITY 10 +#define STM32_SPI_SPI3_IRQ_PRIORITY 10 +#define STM32_SPI_SPI4_IRQ_PRIORITY 10 +#define STM32_SPI_DMA_ERROR_HOOK(spip) osalSysHalt("DMA failure") + +/* + * ST driver system settings. + */ +#define STM32_ST_IRQ_PRIORITY 8 +#define STM32_ST_USE_TIMER 2 + +/* + * UART driver system settings. + */ +#define STM32_UART_USART1_IRQ_PRIORITY 12 +#define STM32_UART_USART2_IRQ_PRIORITY 12 +#define STM32_UART_USART3_IRQ_PRIORITY 12 +#define STM32_UART_UART4_IRQ_PRIORITY 12 +#define STM32_UART_UART5_IRQ_PRIORITY 12 +#define STM32_UART_USART6_IRQ_PRIORITY 12 +#define STM32_UART_USART1_DMA_PRIORITY 0 +#define STM32_UART_USART2_DMA_PRIORITY 0 +#define STM32_UART_USART3_DMA_PRIORITY 0 +#define STM32_UART_UART4_DMA_PRIORITY 0 +#define STM32_UART_UART5_DMA_PRIORITY 0 +#define STM32_UART_USART6_DMA_PRIORITY 0 +#define STM32_UART_DMA_ERROR_HOOK(uartp) osalSysHalt("DMA failure") + +/* + * USB driver system settings. + */ +#define STM32_USB_USE_OTG1 TRUE +#define STM32_USB_USE_OTG2 FALSE +#define STM32_USB_OTG1_IRQ_PRIORITY 14 +#define STM32_USB_OTG2_IRQ_PRIORITY 14 +#define STM32_USB_OTG1_RX_FIFO_SIZE 512 +#define STM32_USB_OTG2_RX_FIFO_SIZE 1024 +#define STM32_USB_OTG_THREAD_PRIO LOWPRIO +#define STM32_USB_OTG_THREAD_STACK_SIZE 128 +#define STM32_USB_OTGFIFO_FILL_BASEPRI 0 + +/* + * WDG driver system settings. + */ +#define STM32_WDG_USE_IWDG FALSE + +// include generated config +#include "hwdef.h" + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F405xx.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F405xx.py new file mode 100644 index 0000000000..0b5b83be34 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F405xx.py @@ -0,0 +1,964 @@ +AltFunction_map = { + # format is PIN:FUNCTION : AFNUM + # extracted from tabula-AF-F405.csv + "PA0:ETH_MII_CRS" : 11, + "PA0:EVENTOUT" : 15, + "PA0:TIM2_CH1_ETR" : 1, + "PA0:TIM5_CH1" : 2, + "PA0:TIM8_ETR" : 3, + "PA0:UART4_TX" : 8, + "PA0:USART2_CTS" : 7, + "PA10:DCMI_D1" : 13, + "PA10:EVENTOUT" : 15, + "PA10:OTG_FS_ID" : 10, + "PA10:TIM1_CH3" : 1, + "PA10:USART1_RX" : 7, + "PA11:CAN1_RX" : 9, + "PA11:EVENTOUT" : 15, + "PA11:OTG_FS_DM" : 10, + "PA11:TIM1_CH4" : 1, + "PA11:USART1_CTS" : 7, + "PA12:CAN1_TX" : 9, + "PA12:EVENTOUT" : 15, + "PA12:OTG_FS_DP" : 10, + "PA12:TIM1_ETR" : 1, + "PA12:USART1_RTS" : 7, + "PA13:EVENTOUT" : 15, + "PA13:JTMS-SWDIO" : 0, + "PA14:EVENTOUT" : 15, + "PA14:JTCK-SWCLK" : 0, + "PA1:ETH_MII_RX_CLK" : 11, + "PA1:ETH_RMII__REF_CLK" : 11, + "PA1:EVENTOUT" : 15, + "PA1:TIM2_CH2" : 1, + "PA1:TIM5_CH2" : 2, + "PA1:UART4_RX" : 8, + "PA1:USART2_RTS" : 7, + "PA2:ETH_MDIO" : 11, + "PA2:EVENTOUT" : 15, + "PA2:TIM2_CH3" : 1, + "PA2:TIM5_CH3" : 2, + "PA2:TIM9_CH1" : 3, + "PA2:USART2_TX" : 7, + "PA3:ETH_MII_COL" : 11, + "PA3:EVENTOUT" : 15, + "PA3:OTG_HS_ULPI_D0" : 10, + "PA3:TIM2_CH4" : 1, + "PA3:TIM5_CH4" : 2, + "PA3:TIM9_CH2" : 3, + "PA3:USART2_RX" : 7, + "PA4:DCMI_HSYNC" : 13, + "PA4:EVENTOUT" : 15, + "PA4:OTG_HS_SOF" : 12, + "PA4:SPI1_NSS" : 5, + "PA4:SPI3_NSSI2S3_WS" : 6, + "PA4:USART2_CK" : 7, + "PA5:EVENTOUT" : 15, + "PA5:OTG_HS_ULPI_CK" : 10, + "PA5:SPI1_SCK" : 5, + "PA5:TIM2_CH1_ETR" : 1, + "PA5:TIM8_CH1N" : 3, + "PA6:DCMI_PIXCK" : 13, + "PA6:EVENTOUT" : 15, + "PA6:SPI1_MISO" : 5, + "PA6:TIM13_CH1" : 9, + "PA6:TIM1_BKIN" : 1, + "PA6:TIM3_CH1" : 2, + "PA6:TIM8_BKIN" : 3, + "PA7:ETH_MII_RX_DV" : 11, + "PA7:ETH_RMII_CRS_DV" : 11, + "PA7:EVENTOUT" : 15, + "PA7:SPI1_MOSI" : 5, + "PA7:TIM14_CH1" : 9, + "PA7:TIM1_CH1N" : 1, + "PA7:TIM3_CH2" : 2, + "PA7:TIM8_CH1N" : 3, + "PA8:EVENTOUT" : 15, + "PA8:I2C3_SCL" : 4, + "PA8:MCO1" : 0, + "PA8:OTG_FS_SOF" : 10, + "PA8:TIM1_CH1" : 1, + "PA8:USART1_CK" : 7, + "PA9:DCMI_D0" : 13, + "PA9:EVENTOUT" : 15, + "PA9:I2C3_SMBA" : 4, + "PA9:TIM1_CH2" : 1, + "PA9:USART1_TX" : 7, + "PB0:ETH_MII_RXD2" : 11, + "PB0:EVENTOUT" : 15, + "PB0:OTG_HS_ULPI_D1" : 10, + "PB0:TIM1_CH2N" : 1, + "PB0:TIM3_CH3" : 2, + "PB0:TIM8_CH2N" : 3, + "PB10:ETH_MII_RX_ER" : 11, + "PB10:EVENTOUT" : 15, + "PB10:I2C2_SCL" : 4, + "PB10:OTG_HS_ULPI_D3" : 10, + "PB10:SPI2_SCKI2S2_CK" : 5, + "PB10:TIM2_CH3" : 1, + "PB10:USART3_TX" : 7, + "PB11:ETH_MII_TX_EN" : 11, + "PB11:ETH_RMII_TX_EN" : 11, + "PB11:EVENTOUT" : 15, + "PB11:I2C2_SDA" : 4, + "PB11:OTG_HS_ULPI_D4" : 10, + "PB11:TIM2_CH4" : 1, + "PB11:USART3_RX" : 7, + "PB12:CAN2_RX" : 9, + "PB12:ETH_MII_TXD0" : 11, + "PB12:ETH_RMII_TXD0" : 11, + "PB12:EVENTOUT" : 15, + "PB12:I2C2_SMBA" : 4, + "PB12:OTG_HS_ID" : 12, + "PB12:OTG_HS_ULPI_D5" : 10, + "PB12:SPI2_NSSI2S2_WS" : 5, + "PB12:TIM1_BKIN" : 1, + "PB12:USART3_CK" : 7, + "PB13:CAN2_TX" : 9, + "PB13:ETH_MII_TXD1" : 11, + "PB13:ETH_RMII_TXD1" : 11, + "PB13:EVENTOUT" : 15, + "PB13:OTG_HS_ULPI_D6" : 10, + "PB13:SPI2_SCKI2S2_CK" : 5, + "PB13:TIM1_CH1N" : 1, + "PB13:USART3_CTS" : 7, + "PB14:EVENTOUT" : 15, + "PB14:I2S2EXT_SD" : 6, + "PB14:OTG_HS_DM" : 12, + "PB14:SPI2_MISO" : 5, + "PB14:TIM12_CH1" : 9, + "PB14:TIM1_CH2N" : 1, + "PB14:TIM8_CH2N" : 3, + "PB14:USART3_RTS" : 7, + "PB1:ETH_MII_RXD3" : 11, + "PB1:EVENTOUT" : 15, + "PB1:OTG_HS_ULPI_D2" : 10, + "PB1:TIM1_CH3N" : 1, + "PB1:TIM3_CH4" : 2, + "PB1:TIM8_CH3N" : 3, + "PB2:EVENTOUT" : 15, + "PB3:EVENTOUT" : 15, + "PB3:JTDO" : 0, + "PB3:SPI1_SCK" : 5, + "PB3:SPI3_SCKI2S3_CK" : 6, + "PB3:TIM2_CH2" : 1, + "PB3:TRACESWO" : 0, + "PB4:EVENTOUT" : 15, + "PB4:I2S3EXT_SD" : 7, + "PB4:NJTRST" : 0, + "PB4:SPI1_MISO" : 5, + "PB4:SPI3_MISO" : 6, + "PB4:TIM3_CH1" : 2, + "PB5:CAN2_RX" : 9, + "PB5:DCMI_D10" : 13, + "PB5:ETH_PPS_OUT" : 11, + "PB5:EVENTOUT" : 15, + "PB5:I2C1_SMBA" : 4, + "PB5:OTG_HS_ULPI_D7" : 10, + "PB5:SPI1_MOSI" : 5, + "PB5:SPI3_MOSII2S3_SD" : 6, + "PB5:TIM3_CH2" : 2, + "PB6:CAN2_TX" : 9, + "PB6:DCMI_D5" : 13, + "PB6:EVENTOUT" : 15, + "PB6:I2C1_SCL" : 4, + "PB6:TIM4_CH1" : 2, + "PB6:USART1_TX" : 7, + "PB7:DCMI_VSYNC" : 13, + "PB7:EVENTOUT" : 15, + "PB7:FSMC_NL" : 12, + "PB7:I2C1_SDA" : 4, + "PB7:TIM4_CH2" : 2, + "PB7:USART1_RX" : 7, + "PB8:CAN1_RX" : 9, + "PB8:DCMI_D6" : 13, + "PB8:ETH_MII_TXD3" : 11, + "PB8:EVENTOUT" : 15, + "PB8:I2C1_SCL" : 4, + "PB8:SDIO_D4" : 12, + "PB8:TIM10_CH1" : 3, + "PB8:TIM4_CH3" : 2, + "PB9:CAN1_TX" : 9, + "PB9:DCMI_D7" : 13, + "PB9:EVENTOUT" : 15, + "PB9:I2C1_SDA" : 4, + "PB9:SDIO_D5" : 12, + "PB9:SPI2_NSSI2S2_WS" : 5, + "PB9:TIM11_CH1" : 3, + "PB9:TIM4_CH4" : 2, + "PC0:EVENTOUT" : 15, + "PC0:OTG_HS_ULPI_STP" : 10, + "PC10:DCMI_D8" : 13, + "PC10:EVENTOUT" : 15, + "PC10:I2S3_CK" : 6, + "PC10:SDIO_D2" : 12, + "PC10:SPI3_SCK" : 6, + "PC10:UART4_TX" : 8, + "PC10:USART3_TX" : 7, + "PC11:DCMI_D4" : 13, + "PC11:EVENTOUT" : 15, + "PC11:I2S3EXT_SD" : 5, + "PC11:SDIO_D3" : 12, + "PC11:SPI3_MISO" : 6, + "PC11:UART4_RX" : 8, + "PC11:USART3_RX" : 7, + "PC12:DCMI_D9" : 13, + "PC12:EVENTOUT" : 15, + "PC12:SDIO_CK" : 12, + "PC12:SPI3_MOSII2S3_SD" : 6, + "PC12:UART5_TX" : 8, + "PC12:USART3_CK" : 7, + "PC13:EVENTOUT" : 15, + "PC14:EVENTOUT" : 15, + "PC1:ETH_MDC" : 11, + "PC1:EVENTOUT" : 15, + "PC2:ETH_MII_TXD2" : 11, + "PC2:EVENTOUT" : 15, + "PC2:I2S2EXT_SD" : 6, + "PC2:OTG_HS_ULPI_DIR" : 10, + "PC2:SPI2_MISO" : 5, + "PC3:ETH_MII_TX_CLK" : 11, + "PC3:EVENTOUT" : 15, + "PC3:OTG_HS_ULPI_NXT" : 10, + "PC3:SPI2_MOSII2S2_SD" : 5, + "PC4:ETH_MII_RXD0" : 11, + "PC4:ETH_RMII_RXD0" : 11, + "PC4:EVENTOUT" : 15, + "PC5:ETH_MII_RXD1" : 11, + "PC5:ETH_RMII_RXD1" : 11, + "PC5:EVENTOUT" : 15, + "PC6:DCMI_D0" : 13, + "PC6:EVENTOUT" : 15, + "PC6:I2S2_MCK" : 5, + "PC6:SDIO_D6" : 12, + "PC6:TIM3_CH1" : 2, + "PC6:TIM8_CH1" : 3, + "PC6:USART6_TX" : 8, + "PC7:DCMI_D1" : 13, + "PC7:EVENTOUT" : 15, + "PC7:I2S3_MCK" : 6, + "PC7:SDIO_D7" : 12, + "PC7:TIM3_CH2" : 2, + "PC7:TIM8_CH2" : 3, + "PC7:USART6_RX" : 8, + "PC8:DCMI_D2" : 13, + "PC8:EVENTOUT" : 15, + "PC8:SDIO_D0" : 12, + "PC8:TIM3_CH3" : 2, + "PC8:TIM8_CH3" : 3, + "PC8:USART6_CK" : 8, + "PC9:DCMI_D3" : 13, + "PC9:EVENTOUT" : 15, + "PC9:I2C3_SDA" : 4, + "PC9:I2S_CKIN" : 5, + "PC9:MCO2" : 0, + "PC9:SDIO_D1" : 12, + "PC9:TIM3_CH4" : 2, + "PC9:TIM8_CH4" : 3, + "PD10:EVENTOUT" : 15, + "PD10:FSMC_D15" : 12, + "PD10:USART3_CK" : 7, + "PD11:EVENTOUT" : 15, + "PD11:FSMC_A16" : 12, + "PD11:USART3_CTS" : 7, + "PD12:EVENTOUT" : 15, + "PD12:FSMC_A17" : 12, + "PD12:TIM4_CH1" : 2, + "PD12:USART3_RTS" : 7, + "PD13:EVENTOUT" : 15, + "PD13:FSMC_A18" : 12, + "PD13:TIM4_CH2" : 2, + "PD14:EVENTOUT" : 15, + "PD14:FSMC_D0" : 12, + "PD14:TIM4_CH3" : 2, + "PD15:EVENTOUT" : 15, + "PD15:FSMC_D1" : 12, + "PD15:TIM4_CH4" : 2, + "PD1:CAN1_TX" : 9, + "PD1:EVENTOUT" : 15, + "PD1:FSMC_D3" : 12, + "PD2:DCMI_D11" : 13, + "PD2:EVENTOUT" : 15, + "PD2:SDIO_CMD" : 12, + "PD2:TIM3_ETR" : 2, + "PD2:UART5_RX" : 8, + "PD3:EVENTOUT" : 15, + "PD3:FSMC_CLK" : 12, + "PD3:USART2_CTS" : 7, + "PD4:EVENTOUT" : 15, + "PD4:FSMC_NOE" : 12, + "PD4:USART2_RTS" : 7, + "PD5:EVENTOUT" : 15, + "PD5:FSMC_NWE" : 12, + "PD5:USART2_TX" : 7, + "PD6:EVENTOUT" : 15, + "PD6:FSMC_NWAIT" : 12, + "PD6:USART2_RX" : 7, + "PD7:EVENTOUT" : 15, + "PD7:FSMC_NCE2" : 12, + "PD7:FSMC_NE1" : 12, + "PD7:USART2_CK" : 7, + "PD8:EVENTOUT" : 15, + "PD8:FSMC_D13" : 12, + "PD8:USART3_TX" : 7, + "PD9:EVENTOUT" : 15, + "PD9:FSMC_D14" : 12, + "PD9:USART3_RX" : 7, + "PE0:DCMI_D2" : 13, + "PE0:EVENTOUT" : 15, + "PE0:FSMC_NBL0" : 12, + "PE0:TIM4_ETR" : 2, + "PE10:EVENTOUT" : 15, + "PE10:FSMC_D7" : 12, + "PE10:TIM1_CH2N" : 1, + "PE11:EVENTOUT" : 15, + "PE11:FSMC_D8" : 12, + "PE11:TIM1_CH2" : 1, + "PE12:EVENTOUT" : 15, + "PE12:FSMC_D9" : 12, + "PE12:TIM1_CH3N" : 1, + "PE13:EVENTOUT" : 15, + "PE13:FSMC_D10" : 12, + "PE13:TIM1_CH3" : 1, + "PE14:EVENTOUT" : 15, + "PE14:FSMC_D11" : 12, + "PE14:TIM1_CH4" : 1, + "PE1:DCMI_D3" : 13, + "PE1:EVENTOUT" : 15, + "PE1:FSMC_NBL1" : 12, + "PE2:ETH_MII_TXD3" : 11, + "PE2:EVENTOUT" : 15, + "PE2:FSMC_A23" : 12, + "PE2:TRACECLK" : 0, + "PE3:EVENTOUT" : 15, + "PE3:FSMC_A19" : 12, + "PE3:TRACED0" : 0, + "PE4:DCMI_D4" : 13, + "PE4:EVENTOUT" : 15, + "PE4:FSMC_A20" : 12, + "PE4:TRACED1" : 0, + "PE5:DCMI_D6" : 13, + "PE5:EVENTOUT" : 15, + "PE5:FSMC_A21" : 12, + "PE5:TIM9_CH1" : 3, + "PE5:TRACED2" : 0, + "PE6:DCMI_D7" : 13, + "PE6:EVENTOUT" : 15, + "PE6:FSMC_A22" : 12, + "PE6:TIM9_CH2" : 3, + "PE6:TRACED3" : 0, + "PE7:EVENTOUT" : 15, + "PE7:FSMC_D4" : 12, + "PE7:TIM1_ETR" : 1, + "PE8:EVENTOUT" : 15, + "PE8:FSMC_D5" : 12, + "PE8:TIM1_CH1N" : 1, + "PE9:EVENTOUT" : 15, + "PE9:FSMC_D6" : 12, + "PE9:TIM1_CH1" : 1, + "PF0:EVENTOUT" : 15, + "PF0:FSMC_A0" : 12, + "PF0:I2C2_SDA" : 4, + "PF10:EVENTOUT" : 15, + "PF10:FSMC_INTR" : 12, + "PF11:DCMI_D12" : 13, + "PF11:EVENTOUT" : 15, + "PF12:EVENTOUT" : 15, + "PF12:FSMC_A6" : 12, + "PF13:EVENTOUT" : 15, + "PF13:FSMC_A7" : 12, + "PF14:EVENTOUT" : 15, + "PF14:FSMC_A8" : 12, + "PF1:EVENTOUT" : 15, + "PF1:FSMC_A1" : 12, + "PF1:I2C2_SCL" : 4, + "PF2:EVENTOUT" : 15, + "PF2:FSMC_A2" : 12, + "PF2:I2C2_SMBA" : 4, + "PF3:EVENTOUT" : 15, + "PF3:FSMC_A3" : 12, + "PF4:EVENTOUT" : 15, + "PF4:FSMC_A4" : 12, + "PF5:EVENTOUT" : 15, + "PF5:FSMC_A5" : 12, + "PF6:EVENTOUT" : 15, + "PF6:FSMC_NIORD" : 12, + "PF6:TIM10_CH1" : 3, + "PF7:EVENTOUT" : 15, + "PF7:FSMC_NREG" : 12, + "PF7:TIM11_CH1" : 3, + "PF8:EVENTOUT" : 15, + "PF8:FSMC_NIOWR" : 12, + "PF8:TIM13_CH1" : 9, + "PF9:EVENTOUT" : 15, + "PF9:FSMC_CD" : 12, + "PF9:TIM14_CH1" : 9, + "PG0:EVENTOUT" : 15, + "PG0:FSMC_A10" : 12, + "PG10:EVENTOUT" : 15, + "PG10:FSMC_NCE4_1" : 12, + "PG10:FSMC_NE3" : 12, + "PG11:ETH_MII_TX_EN" : 11, + "PG11:ETH_RMII_TX_EN" : 11, + "PG11:EVENTOUT" : 15, + "PG11:FSMC_NCE4_2" : 12, + "PG12:EVENTOUT" : 15, + "PG12:FSMC_NE4" : 12, + "PG12:USART6_RTS" : 8, + "PG13:ETH_MII_TXD0" : 11, + "PG13:ETH_RMII_TXD0" : 11, + "PG13:EVENTOUT" : 15, + "PG13:FSMC_A24" : 12, + "PG13:UART6_CTS" : 8, + "PG14:ETH_MII_TXD1" : 11, + "PG14:ETH_RMII_TXD1" : 11, + "PG14:EVENTOUT" : 15, + "PG14:FSMC_A25" : 12, + "PG14:USART6_TX" : 8, + "PG1:EVENTOUT" : 15, + "PG1:FSMC_A11" : 12, + "PG2:EVENTOUT" : 15, + "PG2:FSMC_A12" : 12, + "PG3:EVENTOUT" : 15, + "PG3:FSMC_A13" : 12, + "PG4:EVENTOUT" : 15, + "PG4:FSMC_A14" : 12, + "PG5:EVENTOUT" : 15, + "PG5:FSMC_A15" : 12, + "PG6:EVENTOUT" : 15, + "PG6:FSMC_INT2" : 12, + "PG7:EVENTOUT" : 15, + "PG7:FSMC_INT3" : 12, + "PG7:USART6_CK" : 8, + "PG8:ETH_PPS_OUT" : 11, + "PG8:EVENTOUT" : 15, + "PG8:USART6_RTS" : 8, + "PG9:EVENTOUT" : 15, + "PG9:FSMC_NCE3" : 12, + "PG9:FSMC_NE2" : 12, + "PG9:USART6_RX" : 8, + "PH10:DCMI_D1" : 13, + "PH10:EVENTOUT" : 15, + "PH10:TIM5_CH1" : 2, + "PH11:DCMI_D2" : 13, + "PH11:EVENTOUT" : 15, + "PH11:TIM5_CH2" : 2, + "PH12:DCMI_D3" : 13, + "PH12:EVENTOUT" : 15, + "PH12:TIM5_CH3" : 2, + "PH13:CAN1_TX" : 9, + "PH13:EVENTOUT" : 15, + "PH13:TIM8_CH1N" : 3, + "PH14:DCMI_D4" : 13, + "PH14:EVENTOUT" : 15, + "PH14:TIM8_CH2N" : 3, + "PH15:DCMI_D11" : 13, + "PH15:EVENTOUT" : 15, + "PH15:TIM8_CH3N" : 3, + "PH1:EVENTOUT" : 15, + "PH2:ETH_MII_CRS" : 11, + "PH2:EVENTOUT" : 15, + "PH3:ETH_MII_COL" : 11, + "PH3:EVENTOUT" : 15, + "PH4:EVENTOUT" : 15, + "PH4:I2C2_SCL" : 4, + "PH4:OTG_HS_ULPI_NXT" : 10, + "PH5:EVENTOUT" : 15, + "PH5:I2C2_SDA" : 4, + "PH6:ETH_MII_RXD2" : 11, + "PH6:EVENTOUT" : 15, + "PH6:I2C2_SMBA" : 4, + "PH6:TIM12_CH1" : 9, + "PH7:ETH_MII_RXD3" : 11, + "PH7:EVENTOUT" : 15, + "PH7:I2C3_SCL" : 4, + "PH8:DCMI_HSYNC" : 13, + "PH8:EVENTOUT" : 15, + "PH8:I2C3_SDA" : 4, + "PH9:DCMI_D0" : 13, + "PH9:EVENTOUT" : 15, + "PH9:I2C3_SMBA" : 4, + "PH9:TIM12_CH2" : 9, +} +AltFunction_map = { + # format is PIN:FUNCTION : AFNUM + # extracted from tabula-AF-F405.csv + "PA0:ETH_MII_CRS" : 11, + "PA0:EVENTOUT" : 15, + "PA0:TIM2_CH1_ETR" : 1, + "PA0:TIM5_CH1" : 2, + "PA0:TIM8_ETR" : 3, + "PA0:UART4_TX" : 8, + "PA0:USART2_CTS" : 7, + "PA10:DCMI_D1" : 13, + "PA10:EVENTOUT" : 15, + "PA10:OTG_FS_ID" : 10, + "PA10:TIM1_CH3" : 1, + "PA10:USART1_RX" : 7, + "PA11:CAN1_RX" : 9, + "PA11:EVENTOUT" : 15, + "PA11:OTG_FS_DM" : 10, + "PA11:TIM1_CH4" : 1, + "PA11:USART1_CTS" : 7, + "PA12:CAN1_TX" : 9, + "PA12:EVENTOUT" : 15, + "PA12:OTG_FS_DP" : 10, + "PA12:TIM1_ETR" : 1, + "PA12:USART1_RTS" : 7, + "PA13:EVENTOUT" : 15, + "PA13:JTMS-SWDIO" : 0, + "PA14:EVENTOUT" : 15, + "PA14:JTCK-SWCLK" : 0, + "PA1:ETH_MII_RX_CLK" : 11, + "PA1:ETH_RMII__REF_CLK" : 11, + "PA1:EVENTOUT" : 15, + "PA1:TIM2_CH2" : 1, + "PA1:TIM5_CH2" : 2, + "PA1:UART4_RX" : 8, + "PA1:USART2_RTS" : 7, + "PA2:ETH_MDIO" : 11, + "PA2:EVENTOUT" : 15, + "PA2:TIM2_CH3" : 1, + "PA2:TIM5_CH3" : 2, + "PA2:TIM9_CH1" : 3, + "PA2:USART2_TX" : 7, + "PA3:ETH_MII_COL" : 11, + "PA3:EVENTOUT" : 15, + "PA3:OTG_HS_ULPI_D0" : 10, + "PA3:TIM2_CH4" : 1, + "PA3:TIM5_CH4" : 2, + "PA3:TIM9_CH2" : 3, + "PA3:USART2_RX" : 7, + "PA4:DCMI_HSYNC" : 13, + "PA4:EVENTOUT" : 15, + "PA4:OTG_HS_SOF" : 12, + "PA4:SPI1_NSS" : 5, + "PA4:SPI3_NSSI2S3_WS" : 6, + "PA4:USART2_CK" : 7, + "PA5:EVENTOUT" : 15, + "PA5:OTG_HS_ULPI_CK" : 10, + "PA5:SPI1_SCK" : 5, + "PA5:TIM2_CH1_ETR" : 1, + "PA5:TIM8_CH1N" : 3, + "PA6:DCMI_PIXCK" : 13, + "PA6:EVENTOUT" : 15, + "PA6:SPI1_MISO" : 5, + "PA6:TIM13_CH1" : 9, + "PA6:TIM1_BKIN" : 1, + "PA6:TIM3_CH1" : 2, + "PA6:TIM8_BKIN" : 3, + "PA7:ETH_MII_RX_DV" : 11, + "PA7:ETH_RMII_CRS_DV" : 11, + "PA7:EVENTOUT" : 15, + "PA7:SPI1_MOSI" : 5, + "PA7:TIM14_CH1" : 9, + "PA7:TIM1_CH1N" : 1, + "PA7:TIM3_CH2" : 2, + "PA7:TIM8_CH1N" : 3, + "PA8:EVENTOUT" : 15, + "PA8:I2C3_SCL" : 4, + "PA8:MCO1" : 0, + "PA8:OTG_FS_SOF" : 10, + "PA8:TIM1_CH1" : 1, + "PA8:USART1_CK" : 7, + "PA9:DCMI_D0" : 13, + "PA9:EVENTOUT" : 15, + "PA9:I2C3_SMBA" : 4, + "PA9:TIM1_CH2" : 1, + "PA9:USART1_TX" : 7, + "PB0:ETH_MII_RXD2" : 11, + "PB0:EVENTOUT" : 15, + "PB0:OTG_HS_ULPI_D1" : 10, + "PB0:TIM1_CH2N" : 1, + "PB0:TIM3_CH3" : 2, + "PB0:TIM8_CH2N" : 3, + "PB10:ETH_MII_RX_ER" : 11, + "PB10:EVENTOUT" : 15, + "PB10:I2C2_SCL" : 4, + "PB10:OTG_HS_ULPI_D3" : 10, + "PB10:SPI2_SCKI2S2_CK" : 5, + "PB10:TIM2_CH3" : 1, + "PB10:USART3_TX" : 7, + "PB11:ETH_MII_TX_EN" : 11, + "PB11:ETH_RMII_TX_EN" : 11, + "PB11:EVENTOUT" : 15, + "PB11:I2C2_SDA" : 4, + "PB11:OTG_HS_ULPI_D4" : 10, + "PB11:TIM2_CH4" : 1, + "PB11:USART3_RX" : 7, + "PB12:CAN2_RX" : 9, + "PB12:ETH_MII_TXD0" : 11, + "PB12:ETH_RMII_TXD0" : 11, + "PB12:EVENTOUT" : 15, + "PB12:I2C2_SMBA" : 4, + "PB12:OTG_HS_ID" : 12, + "PB12:OTG_HS_ULPI_D5" : 10, + "PB12:SPI2_NSSI2S2_WS" : 5, + "PB12:TIM1_BKIN" : 1, + "PB12:USART3_CK" : 7, + "PB13:CAN2_TX" : 9, + "PB13:ETH_MII_TXD1" : 11, + "PB13:ETH_RMII_TXD1" : 11, + "PB13:EVENTOUT" : 15, + "PB13:OTG_HS_ULPI_D6" : 10, + "PB13:SPI2_SCKI2S2_CK" : 5, + "PB13:TIM1_CH1N" : 1, + "PB13:USART3_CTS" : 7, + "PB14:EVENTOUT" : 15, + "PB14:I2S2EXT_SD" : 6, + "PB14:OTG_HS_DM" : 12, + "PB14:SPI2_MISO" : 5, + "PB14:TIM12_CH1" : 9, + "PB14:TIM1_CH2N" : 1, + "PB14:TIM8_CH2N" : 3, + "PB14:USART3_RTS" : 7, + "PB1:ETH_MII_RXD3" : 11, + "PB1:EVENTOUT" : 15, + "PB1:OTG_HS_ULPI_D2" : 10, + "PB1:TIM1_CH3N" : 1, + "PB1:TIM3_CH4" : 2, + "PB1:TIM8_CH3N" : 3, + "PB2:EVENTOUT" : 15, + "PB3:EVENTOUT" : 15, + "PB3:JTDO" : 0, + "PB3:SPI1_SCK" : 5, + "PB3:SPI3_SCKI2S3_CK" : 6, + "PB3:TIM2_CH2" : 1, + "PB3:TRACESWO" : 0, + "PB4:EVENTOUT" : 15, + "PB4:I2S3EXT_SD" : 7, + "PB4:NJTRST" : 0, + "PB4:SPI1_MISO" : 5, + "PB4:SPI3_MISO" : 6, + "PB4:TIM3_CH1" : 2, + "PB5:CAN2_RX" : 9, + "PB5:DCMI_D10" : 13, + "PB5:ETH_PPS_OUT" : 11, + "PB5:EVENTOUT" : 15, + "PB5:I2C1_SMBA" : 4, + "PB5:OTG_HS_ULPI_D7" : 10, + "PB5:SPI1_MOSI" : 5, + "PB5:SPI3_MOSII2S3_SD" : 6, + "PB5:TIM3_CH2" : 2, + "PB6:CAN2_TX" : 9, + "PB6:DCMI_D5" : 13, + "PB6:EVENTOUT" : 15, + "PB6:I2C1_SCL" : 4, + "PB6:TIM4_CH1" : 2, + "PB6:USART1_TX" : 7, + "PB7:DCMI_VSYNC" : 13, + "PB7:EVENTOUT" : 15, + "PB7:FSMC_NL" : 12, + "PB7:I2C1_SDA" : 4, + "PB7:TIM4_CH2" : 2, + "PB7:USART1_RX" : 7, + "PB8:CAN1_RX" : 9, + "PB8:DCMI_D6" : 13, + "PB8:ETH_MII_TXD3" : 11, + "PB8:EVENTOUT" : 15, + "PB8:I2C1_SCL" : 4, + "PB8:SDIO_D4" : 12, + "PB8:TIM10_CH1" : 3, + "PB8:TIM4_CH3" : 2, + "PB9:CAN1_TX" : 9, + "PB9:DCMI_D7" : 13, + "PB9:EVENTOUT" : 15, + "PB9:I2C1_SDA" : 4, + "PB9:SDIO_D5" : 12, + "PB9:SPI2_NSSI2S2_WS" : 5, + "PB9:TIM11_CH1" : 3, + "PB9:TIM4_CH4" : 2, + "PC0:EVENTOUT" : 15, + "PC0:OTG_HS_ULPI_STP" : 10, + "PC10:DCMI_D8" : 13, + "PC10:EVENTOUT" : 15, + "PC10:I2S3_CK" : 6, + "PC10:SDIO_D2" : 12, + "PC10:SPI3_SCK" : 6, + "PC10:UART4_TX" : 8, + "PC10:USART3_TX" : 7, + "PC11:DCMI_D4" : 13, + "PC11:EVENTOUT" : 15, + "PC11:I2S3EXT_SD" : 5, + "PC11:SDIO_D3" : 12, + "PC11:SPI3_MISO" : 6, + "PC11:UART4_RX" : 8, + "PC11:USART3_RX" : 7, + "PC12:DCMI_D9" : 13, + "PC12:EVENTOUT" : 15, + "PC12:SDIO_CK" : 12, + "PC12:SPI3_MOSII2S3_SD" : 6, + "PC12:UART5_TX" : 8, + "PC12:USART3_CK" : 7, + "PC13:EVENTOUT" : 15, + "PC14:EVENTOUT" : 15, + "PC1:ETH_MDC" : 11, + "PC1:EVENTOUT" : 15, + "PC2:ETH_MII_TXD2" : 11, + "PC2:EVENTOUT" : 15, + "PC2:I2S2EXT_SD" : 6, + "PC2:OTG_HS_ULPI_DIR" : 10, + "PC2:SPI2_MISO" : 5, + "PC3:ETH_MII_TX_CLK" : 11, + "PC3:EVENTOUT" : 15, + "PC3:OTG_HS_ULPI_NXT" : 10, + "PC3:SPI2_MOSII2S2_SD" : 5, + "PC4:ETH_MII_RXD0" : 11, + "PC4:ETH_RMII_RXD0" : 11, + "PC4:EVENTOUT" : 15, + "PC5:ETH_MII_RXD1" : 11, + "PC5:ETH_RMII_RXD1" : 11, + "PC5:EVENTOUT" : 15, + "PC6:DCMI_D0" : 13, + "PC6:EVENTOUT" : 15, + "PC6:I2S2_MCK" : 5, + "PC6:SDIO_D6" : 12, + "PC6:TIM3_CH1" : 2, + "PC6:TIM8_CH1" : 3, + "PC6:USART6_TX" : 8, + "PC7:DCMI_D1" : 13, + "PC7:EVENTOUT" : 15, + "PC7:I2S3_MCK" : 6, + "PC7:SDIO_D7" : 12, + "PC7:TIM3_CH2" : 2, + "PC7:TIM8_CH2" : 3, + "PC7:USART6_RX" : 8, + "PC8:DCMI_D2" : 13, + "PC8:EVENTOUT" : 15, + "PC8:SDIO_D0" : 12, + "PC8:TIM3_CH3" : 2, + "PC8:TIM8_CH3" : 3, + "PC8:USART6_CK" : 8, + "PC9:DCMI_D3" : 13, + "PC9:EVENTOUT" : 15, + "PC9:I2C3_SDA" : 4, + "PC9:I2S_CKIN" : 5, + "PC9:MCO2" : 0, + "PC9:SDIO_D1" : 12, + "PC9:TIM3_CH4" : 2, + "PC9:TIM8_CH4" : 3, + "PD10:EVENTOUT" : 15, + "PD10:FSMC_D15" : 12, + "PD10:USART3_CK" : 7, + "PD11:EVENTOUT" : 15, + "PD11:FSMC_A16" : 12, + "PD11:USART3_CTS" : 7, + "PD12:EVENTOUT" : 15, + "PD12:FSMC_A17" : 12, + "PD12:TIM4_CH1" : 2, + "PD12:USART3_RTS" : 7, + "PD13:EVENTOUT" : 15, + "PD13:FSMC_A18" : 12, + "PD13:TIM4_CH2" : 2, + "PD14:EVENTOUT" : 15, + "PD14:FSMC_D0" : 12, + "PD14:TIM4_CH3" : 2, + "PD15:EVENTOUT" : 15, + "PD15:FSMC_D1" : 12, + "PD15:TIM4_CH4" : 2, + "PD1:CAN1_TX" : 9, + "PD1:EVENTOUT" : 15, + "PD1:FSMC_D3" : 12, + "PD2:DCMI_D11" : 13, + "PD2:EVENTOUT" : 15, + "PD2:SDIO_CMD" : 12, + "PD2:TIM3_ETR" : 2, + "PD2:UART5_RX" : 8, + "PD3:EVENTOUT" : 15, + "PD3:FSMC_CLK" : 12, + "PD3:USART2_CTS" : 7, + "PD4:EVENTOUT" : 15, + "PD4:FSMC_NOE" : 12, + "PD4:USART2_RTS" : 7, + "PD5:EVENTOUT" : 15, + "PD5:FSMC_NWE" : 12, + "PD5:USART2_TX" : 7, + "PD6:EVENTOUT" : 15, + "PD6:FSMC_NWAIT" : 12, + "PD6:USART2_RX" : 7, + "PD7:EVENTOUT" : 15, + "PD7:FSMC_NCE2" : 12, + "PD7:FSMC_NE1" : 12, + "PD7:USART2_CK" : 7, + "PD8:EVENTOUT" : 15, + "PD8:FSMC_D13" : 12, + "PD8:USART3_TX" : 7, + "PD9:EVENTOUT" : 15, + "PD9:FSMC_D14" : 12, + "PD9:USART3_RX" : 7, + "PE0:DCMI_D2" : 13, + "PE0:EVENTOUT" : 15, + "PE0:FSMC_NBL0" : 12, + "PE0:TIM4_ETR" : 2, + "PE10:EVENTOUT" : 15, + "PE10:FSMC_D7" : 12, + "PE10:TIM1_CH2N" : 1, + "PE11:EVENTOUT" : 15, + "PE11:FSMC_D8" : 12, + "PE11:TIM1_CH2" : 1, + "PE12:EVENTOUT" : 15, + "PE12:FSMC_D9" : 12, + "PE12:TIM1_CH3N" : 1, + "PE13:EVENTOUT" : 15, + "PE13:FSMC_D10" : 12, + "PE13:TIM1_CH3" : 1, + "PE14:EVENTOUT" : 15, + "PE14:FSMC_D11" : 12, + "PE14:TIM1_CH4" : 1, + "PE1:DCMI_D3" : 13, + "PE1:EVENTOUT" : 15, + "PE1:FSMC_NBL1" : 12, + "PE2:ETH_MII_TXD3" : 11, + "PE2:EVENTOUT" : 15, + "PE2:FSMC_A23" : 12, + "PE2:TRACECLK" : 0, + "PE3:EVENTOUT" : 15, + "PE3:FSMC_A19" : 12, + "PE3:TRACED0" : 0, + "PE4:DCMI_D4" : 13, + "PE4:EVENTOUT" : 15, + "PE4:FSMC_A20" : 12, + "PE4:TRACED1" : 0, + "PE5:DCMI_D6" : 13, + "PE5:EVENTOUT" : 15, + "PE5:FSMC_A21" : 12, + "PE5:TIM9_CH1" : 3, + "PE5:TRACED2" : 0, + "PE6:DCMI_D7" : 13, + "PE6:EVENTOUT" : 15, + "PE6:FSMC_A22" : 12, + "PE6:TIM9_CH2" : 3, + "PE6:TRACED3" : 0, + "PE7:EVENTOUT" : 15, + "PE7:FSMC_D4" : 12, + "PE7:TIM1_ETR" : 1, + "PE8:EVENTOUT" : 15, + "PE8:FSMC_D5" : 12, + "PE8:TIM1_CH1N" : 1, + "PE9:EVENTOUT" : 15, + "PE9:FSMC_D6" : 12, + "PE9:TIM1_CH1" : 1, + "PF0:EVENTOUT" : 15, + "PF0:FSMC_A0" : 12, + "PF0:I2C2_SDA" : 4, + "PF10:EVENTOUT" : 15, + "PF10:FSMC_INTR" : 12, + "PF11:DCMI_D12" : 13, + "PF11:EVENTOUT" : 15, + "PF12:EVENTOUT" : 15, + "PF12:FSMC_A6" : 12, + "PF13:EVENTOUT" : 15, + "PF13:FSMC_A7" : 12, + "PF14:EVENTOUT" : 15, + "PF14:FSMC_A8" : 12, + "PF1:EVENTOUT" : 15, + "PF1:FSMC_A1" : 12, + "PF1:I2C2_SCL" : 4, + "PF2:EVENTOUT" : 15, + "PF2:FSMC_A2" : 12, + "PF2:I2C2_SMBA" : 4, + "PF3:EVENTOUT" : 15, + "PF3:FSMC_A3" : 12, + "PF4:EVENTOUT" : 15, + "PF4:FSMC_A4" : 12, + "PF5:EVENTOUT" : 15, + "PF5:FSMC_A5" : 12, + "PF6:EVENTOUT" : 15, + "PF6:FSMC_NIORD" : 12, + "PF6:TIM10_CH1" : 3, + "PF7:EVENTOUT" : 15, + "PF7:FSMC_NREG" : 12, + "PF7:TIM11_CH1" : 3, + "PF8:EVENTOUT" : 15, + "PF8:FSMC_NIOWR" : 12, + "PF8:TIM13_CH1" : 9, + "PF9:EVENTOUT" : 15, + "PF9:FSMC_CD" : 12, + "PF9:TIM14_CH1" : 9, + "PG0:EVENTOUT" : 15, + "PG0:FSMC_A10" : 12, + "PG10:EVENTOUT" : 15, + "PG10:FSMC_NCE4_1" : 12, + "PG10:FSMC_NE3" : 12, + "PG11:ETH_MII_TX_EN" : 11, + "PG11:ETH_RMII_TX_EN" : 11, + "PG11:EVENTOUT" : 15, + "PG11:FSMC_NCE4_2" : 12, + "PG12:EVENTOUT" : 15, + "PG12:FSMC_NE4" : 12, + "PG12:USART6_RTS" : 8, + "PG13:ETH_MII_TXD0" : 11, + "PG13:ETH_RMII_TXD0" : 11, + "PG13:EVENTOUT" : 15, + "PG13:FSMC_A24" : 12, + "PG13:UART6_CTS" : 8, + "PG14:ETH_MII_TXD1" : 11, + "PG14:ETH_RMII_TXD1" : 11, + "PG14:EVENTOUT" : 15, + "PG14:FSMC_A25" : 12, + "PG14:USART6_TX" : 8, + "PG1:EVENTOUT" : 15, + "PG1:FSMC_A11" : 12, + "PG2:EVENTOUT" : 15, + "PG2:FSMC_A12" : 12, + "PG3:EVENTOUT" : 15, + "PG3:FSMC_A13" : 12, + "PG4:EVENTOUT" : 15, + "PG4:FSMC_A14" : 12, + "PG5:EVENTOUT" : 15, + "PG5:FSMC_A15" : 12, + "PG6:EVENTOUT" : 15, + "PG6:FSMC_INT2" : 12, + "PG7:EVENTOUT" : 15, + "PG7:FSMC_INT3" : 12, + "PG7:USART6_CK" : 8, + "PG8:ETH_PPS_OUT" : 11, + "PG8:EVENTOUT" : 15, + "PG8:USART6_RTS" : 8, + "PG9:EVENTOUT" : 15, + "PG9:FSMC_NCE3" : 12, + "PG9:FSMC_NE2" : 12, + "PG9:USART6_RX" : 8, + "PH10:DCMI_D1" : 13, + "PH10:EVENTOUT" : 15, + "PH10:TIM5_CH1" : 2, + "PH11:DCMI_D2" : 13, + "PH11:EVENTOUT" : 15, + "PH11:TIM5_CH2" : 2, + "PH12:DCMI_D3" : 13, + "PH12:EVENTOUT" : 15, + "PH12:TIM5_CH3" : 2, + "PH13:CAN1_TX" : 9, + "PH13:EVENTOUT" : 15, + "PH13:TIM8_CH1N" : 3, + "PH14:DCMI_D4" : 13, + "PH14:EVENTOUT" : 15, + "PH14:TIM8_CH2N" : 3, + "PH15:DCMI_D11" : 13, + "PH15:EVENTOUT" : 15, + "PH15:TIM8_CH3N" : 3, + "PH1:EVENTOUT" : 15, + "PH2:ETH_MII_CRS" : 11, + "PH2:EVENTOUT" : 15, + "PH3:ETH_MII_COL" : 11, + "PH3:EVENTOUT" : 15, + "PH4:EVENTOUT" : 15, + "PH4:I2C2_SCL" : 4, + "PH4:OTG_HS_ULPI_NXT" : 10, + "PH5:EVENTOUT" : 15, + "PH5:I2C2_SDA" : 4, + "PH6:ETH_MII_RXD2" : 11, + "PH6:EVENTOUT" : 15, + "PH6:I2C2_SMBA" : 4, + "PH6:TIM12_CH1" : 9, + "PH7:ETH_MII_RXD3" : 11, + "PH7:EVENTOUT" : 15, + "PH7:I2C3_SCL" : 4, + "PH8:DCMI_HSYNC" : 13, + "PH8:EVENTOUT" : 15, + "PH8:I2C3_SDA" : 4, + "PH9:DCMI_D0" : 13, + "PH9:EVENTOUT" : 15, + "PH9:I2C3_SMBA" : 4, + "PH9:TIM12_CH2" : 9, +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F412Rx.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F412Rx.py new file mode 100644 index 0000000000..25950cc43e --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F412Rx.py @@ -0,0 +1,672 @@ +#!/usr/bin/env python +''' +these tables are generated from the STM32 datasheet RM0402 in en.DM00180369.pdf for the +STM32F412 +''' + +DMA_Map = { + # format is [DMA_TABLE, StreamNum] + # extracted from tabula-stm32f412-ref-manual-196.csv and tabula-stm32f412-ref-manual-196(1).csv + "ADC1" : [(2,0),(2,4)], + "DFSDM1_FLT0" : [(2,6),(2,0)], + "DFSDM1_FLT1" : [(2,1),(2,4)], + "I2C1_RX" : [(1,0),(1,5)], + "I2C1_TX" : [(1,1),(1,6),(1,7)], + "I2C2_RX" : [(1,2),(1,3)], + "I2C2_TX" : [(1,7)], + "I2C3_RX" : [(1,1),(1,2)], + "I2C3_TX" : [(1,4),(1,5)], + "I2CFMP1_RX" : [(1,3),(1,0)], + "I2CFMP1_TX" : [(1,1),(1,7)], + "I2S2EXT_RX" : [(1,3)], + "I2S2_EXT_TX" : [(1,4)], + "I2S3_EXT_RX" : [(1,2),(1,0)], + "I2S3_EXT_TX" : [(1,5)], + "QUADSPI" : [(2,7)], + "SDIO" : [(2,3),(2,6)], + "SPI1_RX" : [(2,0),(2,2)], + "SPI1_TX" : [(2,2),(2,3),(2,5)], + "SPI2_RX" : [(1,3)], + "SPI2_TX" : [(1,4)], + "SPI3_RX" : [(1,0),(1,2)], + "SPI3_TX" : [(1,5),(1,7)], + "SPI4_RX" : [(2,0),(2,4),(2,3)], + "SPI4_TX" : [(2,1),(2,4)], + "SPI5_RX" : [(2,3),(2,5)], + "SPI5_TX" : [(2,4),(2,5),(2,6)], + "TIM1_CH1" : [(2,6),(2,1),(2,3)], + "TIM1_CH2" : [(2,6),(2,2)], + "TIM1_CH3" : [(2,6),(2,6)], + "TIM1_CH4" : [(2,4)], + "TIM1_COM" : [(2,4)], + "TIM1_TRIG" : [(2,0),(2,4)], + "TIM1_UP" : [(2,5)], + "TIM2_CH1" : [(1,5)], + "TIM2_CH2" : [(1,6)], + "TIM2_CH3" : [(1,1)], + "TIM2_CH4" : [(1,6),(1,7)], + "TIM2_UP" : [(1,1),(1,7)], + "TIM3_CH1" : [(1,4)], + "TIM3_CH2" : [(1,5)], + "TIM3_CH3" : [(1,7)], + "TIM3_CH4" : [(1,2)], + "TIM3_TRIG" : [(1,4)], + "TIM3_UP" : [(1,2)], + "TIM4_CH1" : [(1,0)], + "TIM4_CH2" : [(1,3)], + "TIM4_CH3" : [(1,7)], + "TIM4_UP" : [(1,6)], + "TIM5_CH1" : [(1,2)], + "TIM5_CH2" : [(1,4)], + "TIM5_CH3" : [(1,0)], + "TIM5_CH4" : [(1,1),(1,3)], + "TIM5_TRIG" : [(1,1),(1,3)], + "TIM5_UP" : [(1,0),(1,6)], + "TIM6_UP" : [(1,1)], + "TIM7_UP" : [(1,2),(1,4)], + "TIM8_CH1" : [(2,2),(2,2)], + "TIM8_CH2" : [(2,2),(2,3)], + "TIM8_CH3" : [(2,2),(2,4)], + "TIM8_CH4" : [(2,7)], + "TIM8_COM" : [(2,7)], + "TIM8_TRIG" : [(2,7)], + "TIM8_UP" : [(2,1)], + "USART1_RX" : [(2,2),(2,5)], + "USART1_TX" : [(2,7)], + "USART2_RX" : [(1,5),(1,7)], + "USART2_TX" : [(1,6)], + "USART3_RX" : [(1,1)], + "USART3_TX" : [(1,3),(1,4)], + "USART6_RX" : [(2,1),(2,2)], + "USART6_TX" : [(2,6),(2,7)], +} + +''' +tables generated using af_parse.py and tabula +''' +AltFunction_map = { + # format is PIN:FUNCTION : AFNUM + # extracted from tabula-AF-F412RG.csv + "PA0:EVENTOUT" : 15, + "PA0:TIM2_CH1" : 1, + "PA0:TIM2_ETR" : 1, + "PA0:TIM5_CH1" : 2, + "PA0:TIM8_ETR" : 3, + "PA0:USART2_CTS" : 7, + "PA10:EVENTOUT" : 15, + "PA10:I2S5_SD" : 6, + "PA10:SPI5_MOSI" : 6, + "PA10:TIM1_CH3" : 1, + "PA10:USART1_RX" : 7, + "PA10:USB_FS_ID" : 10, + "PA11:CAN1_RX" : 9, + "PA11:EVENTOUT" : 15, + "PA11:SPI4_MISO" : 6, + "PA11:TIM1_CH4" : 1, + "PA11:USART1_CTS" : 7, + "PA11:USART6_TX" : 8, + "PA11:USB_FS_DM" : 10, + "PA12:CAN1_TX" : 9, + "PA12:EVENTOUT" : 15, + "PA12:SPI5_MISO" : 6, + "PA12:TIM1_ETR" : 1, + "PA12:USART1_RTS" : 7, + "PA12:USART6_RX" : 8, + "PA12:USB_FS_DP" : 10, + "PA13:EVENTOUT" : 15, + "PA13:JTMS-SWDIO" : 0, + "PA14:EVENTOUT" : 15, + "PA14:JTCK-SWCLK" : 0, + "PA15:EVENTOUT" : 15, + "PA15:I2S1_WS" : 5, + "PA15:I2S3_WS" : 6, + "PA15:JTDI" : 0, + "PA15:SPI1_NSS" : 5, + "PA15:SPI3_NSS" : 6, + "PA15:TIM2_CH1" : 1, + "PA15:TIM2_ETR" : 1, + "PA15:USART1_TX" : 7, + "PA1:EVENTOUT" : 15, + "PA1:I2S4_SD" : 5, + "PA1:QUADSPI_BK1_IO3" : 9, + "PA1:SPI4_MOSI" : 5, + "PA1:TIM2_CH2" : 1, + "PA1:TIM5_CH2" : 2, + "PA1:USART2_RTS" : 7, + "PA2:EVENTOUT" : 15, + "PA2:FSMC_D4" : 12, + "PA2:I2S2_CKIN" : 5, + "PA2:TIM2_CH3" : 1, + "PA2:TIM5_CH3" : 2, + "PA2:TIM9_CH1" : 3, + "PA2:USART2_TX" : 7, + "PA3:EVENTOUT" : 15, + "PA3:FSMC_D5" : 12, + "PA3:I2S2_MCK" : 5, + "PA3:TIM2_CH4" : 1, + "PA3:TIM5_CH4" : 2, + "PA3:TIM9_CH2" : 3, + "PA3:USART2_RX" : 7, + "PA4:DFSDM1_DATIN1" : 8, + "PA4:EVENTOUT" : 15, + "PA4:FSMC_D6" : 12, + "PA4:I2S1_WS" : 5, + "PA4:I2S3_WS" : 6, + "PA4:SPI1_NSS" : 5, + "PA4:SPI3_NSS" : 6, + "PA4:USART2_CK" : 7, + "PA5:DFSDM1_CKIN1" : 8, + "PA5:EVENTOUT" : 15, + "PA5:FSMC_D7" : 12, + "PA5:I2S1_CK" : 5, + "PA5:SPI1_SCK" : 5, + "PA5:TIM2_CH1" : 1, + "PA5:TIM2_ETR" : 1, + "PA5:TIM8_CH1N" : 3, + "PA6:EVENTOUT" : 15, + "PA6:I2S2_MCK" : 6, + "PA6:QUADSPI_BK2_IO0" : 10, + "PA6:SDIO_CMD" : 12, + "PA6:SPI1_MISO" : 5, + "PA6:TIM13_CH1" : 9, + "PA6:TIM1_BKIN" : 1, + "PA6:TIM3_CH1" : 2, + "PA6:TIM8_BKIN" : 3, + "PA7:EVENTOUT" : 15, + "PA7:I2S1_SD" : 5, + "PA7:QUADSPI_BK2_IO1" : 10, + "PA7:SPI1_MOSI" : 5, + "PA7:TIM14_CH1" : 9, + "PA7:TIM1_CH1N" : 1, + "PA7:TIM3_CH2" : 2, + "PA7:TIM8_CH1N" : 3, + "PA8:EVENTOUT" : 15, + "PA8:I2C3_SCL" : 4, + "PA8:MCO_1" : 0, + "PA8:SDIO_D1" : 12, + "PA8:TIM1_CH1" : 1, + "PA8:USART1_CK" : 7, + "PA8:USB_FS_SOF" : 10, + "PA9:EVENTOUT" : 15, + "PA9:I2C3_SMBA" : 4, + "PA9:SDIO_D2" : 12, + "PA9:TIM1_CH2" : 1, + "PA9:USART1_TX" : 7, + "PA9:USB_FS_VBUS" : 10, + "PB0:EVENTOUT" : 15, + "PB0:I2S5_CK" : 6, + "PB0:SPI5_SCK" : 6, + "PB0:TIM1_CH2N" : 1, + "PB0:TIM3_CH3" : 2, + "PB0:TIM8_CH2N" : 3, + "PB10:EVENTOUT" : 15, + "PB10:I2C2_SCL" : 4, + "PB10:I2CFMP1_SCL" : 9, + "PB10:I2S2_CK" : 5, + "PB10:I2S3_MCK" : 6, + "PB10:SDIO_D7" : 12, + "PB10:SPI2_SCK" : 5, + "PB10:TIM2_CH3" : 1, + "PB10:USART3_TX" : 7, + "PB11:EVENTOUT" : 15, + "PB11:I2C2_SDA" : 4, + "PB11:I2S2_CKIN" : 5, + "PB11:TIM2_CH4" : 1, + "PB11:USART3_RX" : 7, + "PB12:CAN2_RX" : 9, + "PB12:DFSDM1_DATIN1" : 10, + "PB12:EVENTOUT" : 15, + "PB12:FSMC_D13" : 12, + "PB12:FSMC_DA13" : 12, + "PB12:I2C2_SMBA" : 4, + "PB12:I2S2_WS" : 5, + "PB12:I2S3_CK" : 7, + "PB12:I2S4_WS" : 6, + "PB12:SPI2_NSS" : 5, + "PB12:SPI3_SCK" : 7, + "PB12:SPI4_NSS" : 6, + "PB12:TIM1_BKIN" : 1, + "PB12:USART3_CK" : 8, + "PB13:CAN2_TX" : 9, + "PB13:DFSDM1_CKIN1" : 10, + "PB13:EVENTOUT" : 15, + "PB13:I2CFMP1_SMBA" : 4, + "PB13:I2S2_CK" : 5, + "PB13:I2S4_CK" : 6, + "PB13:SPI2_SCK" : 5, + "PB13:SPI4_SCK" : 6, + "PB13:TIM1_CH1N" : 1, + "PB13:USART3_CTS" : 8, + "PB14:DFSDM1_DATIN2" : 8, + "PB14:EVENTOUT" : 15, + "PB14:FSMC_D0" : 10, + "PB14:I2CFMP1_SDA" : 4, + "PB14:I2S2EXT_SD" : 6, + "PB14:SDIO_D6" : 12, + "PB14:SPI2_MISO" : 5, + "PB14:TIM12_CH1" : 9, + "PB14:TIM1_CH2N" : 1, + "PB14:TIM8_CH2N" : 3, + "PB14:USART3_RTS" : 7, + "PB15:DFSDM1_CKIN2" : 8, + "PB15:EVENTOUT" : 15, + "PB15:I2CFMP1_SCL" : 4, + "PB15:I2S2_SD" : 5, + "PB15:RTC_50HZ" : 0, + "PB15:SDIO_CK" : 12, + "PB15:SPI2_MOSI" : 5, + "PB15:TIM12_CH2" : 9, + "PB15:TIM1_CH3N" : 1, + "PB15:TIM8_CH3N" : 3, + "PB1:DFSDM1_DATIN0" : 8, + "PB1:EVENTOUT" : 15, + "PB1:I2S5_WS" : 6, + "PB1:QUADSPI_CLK" : 9, + "PB1:SPI5_NSS" : 6, + "PB1:TIM1_CH3N" : 1, + "PB1:TIM3_CH4" : 2, + "PB1:TIM8_CH3N" : 3, + "PB2:DFSDM1_CKIN0" : 6, + "PB2:EVENTOUT" : 15, + "PB2:QUADSPI_CLK" : 9, + "PB3:" : 3, + "PB3:EVENTOUT" : 15, + "PB3:I2C2_SDA" : 9, + "PB3:I2CFMP1_SDA" : 4, + "PB3:I2S1_CK" : 5, + "PB3:I2S3_CK" : 6, + "PB3:JTDO-SWO" : 0, + "PB3:SPI1_SCK" : 5, + "PB3:SPI3_SCK" : 6, + "PB3:TIM2_CH2" : 1, + "PB3:USART1_RX" : 7, + "PB4:EVENTOUT" : 15, + "PB4:I2C3_SDA" : 9, + "PB4:I2S3EXT_SD" : 7, + "PB4:JTRST" : 0, + "PB4:SDIO_D0" : 12, + "PB4:SPI1_MISO" : 5, + "PB4:SPI3_MISO" : 6, + "PB4:TIM3_CH1" : 2, + "PB5:CAN2_RX" : 9, + "PB5:EVENTOUT" : 15, + "PB5:I2C1_SMBA" : 4, + "PB5:I2S1_SD" : 5, + "PB5:I2S3_SD" : 6, + "PB5:SDIO_D3" : 12, + "PB5:SPI1_MOSI" : 5, + "PB5:SPI3_MOSI" : 6, + "PB5:TIM3_CH2" : 2, + "PB6:CAN2_TX" : 9, + "PB6:EVENTOUT" : 15, + "PB6:I2C1_SCL" : 4, + "PB6:QUADSPI_BK1_NCS" : 10, + "PB6:SDIO_D0" : 12, + "PB6:TIM4_CH1" : 2, + "PB6:USART1_TX" : 7, + "PB7:EVENTOUT" : 15, + "PB7:FSMC_NL" : 12, + "PB7:I2C1_SDA" : 4, + "PB7:TIM4_CH2" : 2, + "PB7:USART1_RX" : 7, + "PB8:CAN1_RX" : 8, + "PB8:EVENTOUT" : 15, + "PB8:I2C1_SCL" : 4, + "PB8:I2C3_SDA" : 9, + "PB8:I2S5_SD" : 6, + "PB8:SDIO_D4" : 12, + "PB8:SPI5_MOSI" : 6, + "PB8:TIM10_CH1" : 3, + "PB8:TIM4_CH3" : 2, + "PB9:CAN1_TX" : 8, + "PB9:EVENTOUT" : 15, + "PB9:I2C1_SDA" : 4, + "PB9:I2C2_SDA" : 9, + "PB9:I2S2_WS" : 5, + "PB9:SDIO_D5" : 12, + "PB9:SPI2_NSS" : 5, + "PB9:TIM11_CH1" : 3, + "PB9:TIM4_CH4" : 2, + "PC0:EVENTOUT" : 15, + "PC10:EVENTOUT" : 15, + "PC10:I2S3_CK" : 6, + "PC10:QUADSPI_BK1_IO1" : 9, + "PC10:SDIO_D2" : 12, + "PC10:SPI3_SCK" : 6, + "PC10:USART3_TX" : 7, + "PC11:EVENTOUT" : 15, + "PC11:FSMC_D2" : 10, + "PC11:I2S3EXT_SD" : 5, + "PC11:QUADSPI_BK2_NCS" : 9, + "PC11:SDIO_D3" : 12, + "PC11:SPI3_MISO" : 6, + "PC11:USART3_RX" : 7, + "PC12:EVENTOUT" : 15, + "PC12:FSMC_D3" : 10, + "PC12:I2S3_SD" : 6, + "PC12:SDIO_CK" : 12, + "PC12:SPI3_MOSI" : 6, + "PC12:USART3_CK" : 7, + "PC13:EVENTOUT" : 15, + "PC14:EVENTOUT" : 15, + "PC15:EVENTOUT" : 15, + "PC1:EVENTOUT" : 15, + "PC2:DFSDM1_CKOUT" : 8, + "PC2:EVENTOUT" : 15, + "PC2:FSMC_NWE" : 12, + "PC2:I2S2EXT_SD" : 6, + "PC2:SPI2_MISO" : 5, + "PC3:EVENTOUT" : 15, + "PC3:FSMC_A0" : 12, + "PC3:I2S2_SD" : 5, + "PC3:SPI2_MOSI" : 5, + "PC4:EVENTOUT" : 15, + "PC4:FSMC_NE4" : 12, + "PC4:I2S1_MCK" : 5, + "PC4:QUADSPI_BK2_IO2" : 10, + "PC5:EVENTOUT" : 15, + "PC5:FSMC_NOE" : 12, + "PC5:I2CFMP1_SMBA" : 4, + "PC5:QUADSPI_BK2_IO3" : 10, + "PC5:USART3_RX" : 7, + "PC6:DFSDM1_CKIN3" : 6, + "PC6:EVENTOUT" : 15, + "PC6:FSMC_D1" : 10, + "PC6:I2CFMP1_SCL" : 4, + "PC6:I2S2_MCK" : 5, + "PC6:SDIO_D6" : 12, + "PC6:TIM3_CH1" : 2, + "PC6:TIM8_CH1" : 3, + "PC6:USART6_TX" : 8, + "PC7:DFSDM1_DATIN3" : 10, + "PC7:EVENTOUT" : 15, + "PC7:I2CFMP1_SDA" : 4, + "PC7:I2S2_CK" : 5, + "PC7:I2S3_MCK" : 6, + "PC7:SDIO_D7" : 12, + "PC7:SPI2_SCK" : 5, + "PC7:TIM3_CH2" : 2, + "PC7:TIM8_CH2" : 3, + "PC7:USART6_RX" : 8, + "PC8:EVENTOUT" : 15, + "PC8:QUADSPI_BK1_IO2" : 9, + "PC8:SDIO_D0" : 12, + "PC8:TIM3_CH3" : 2, + "PC8:TIM8_CH3" : 3, + "PC8:USART6_CK" : 8, + "PC9:EVENTOUT" : 15, + "PC9:I2C3_SDA" : 4, + "PC9:I2S2_CKIN" : 5, + "PC9:MCO_2" : 0, + "PC9:QUADSPI_BK1_IO0" : 9, + "PC9:SDIO_D1" : 12, + "PC9:TIM3_CH4" : 2, + "PC9:TIM8_CH4" : 3, + "PD0:CAN1_RX" : 9, + "PD0:EVENTOUT" : 15, + "PD0:FSMC_D2" : 12, + "PD0:FSMC_DA2" : 12, + "PD10:EVENTOUT" : 15, + "PD10:FSMC_D15" : 12, + "PD10:FSMC_DA15" : 12, + "PD10:USART3_CK" : 7, + "PD11:EVENTOUT" : 15, + "PD11:FSMC_A16" : 12, + "PD11:I2CFMP1_SMBA" : 4, + "PD11:QUADSPI_BK1_IO0" : 9, + "PD11:USART3_CTS" : 7, + "PD12:" : 6, + "PD12:EVENTOUT" : 15, + "PD12:FSMC_A17" : 12, + "PD12:I2CFMP1_SCL" : 4, + "PD12:QUADSPI_BK1_IO1" : 9, + "PD12:TIM4_CH1" : 2, + "PD12:USART3_RTS" : 7, + "PD13:EVENTOUT" : 15, + "PD13:FSMC_A18" : 12, + "PD13:I2CFMP1_SDA" : 4, + "PD13:QUADSPI_BK1_IO3" : 9, + "PD13:TIM4_CH2" : 2, + "PD14:EVENTOUT" : 15, + "PD14:FSMC_D0" : 12, + "PD14:FSMC_DA0" : 12, + "PD14:I2CFMP1_SCL" : 4, + "PD14:TIM4_CH3" : 2, + "PD15:EVENTOUT" : 15, + "PD15:FSMC_D1" : 12, + "PD15:FSMC_DA1" : 12, + "PD15:I2CFMP1_SDA" : 4, + "PD15:TIM4_CH4" : 2, + "PD1:CAN1_TX" : 9, + "PD1:EVENTOUT" : 15, + "PD1:FSMC_D3" : 12, + "PD1:FSMC_DA3" : 12, + "PD2:EVENTOUT" : 15, + "PD2:FSMC_NWE" : 10, + "PD2:SDIO_CMD" : 12, + "PD2:TIM3_ETR" : 2, + "PD3:DFSDM1_DATIN0" : 6, + "PD3:EVENTOUT" : 15, + "PD3:FSMC_CLK" : 12, + "PD3:I2S2_CK" : 5, + "PD3:QUADSPI_CLK" : 9, + "PD3:SPI2_SCK" : 5, + "PD3:TRACED1" : 0, + "PD3:USART2_CTS" : 7, + "PD4:DFSDM1_CKIN0" : 6, + "PD4:EVENTOUT" : 15, + "PD4:FSMC_NOE" : 12, + "PD4:USART2_RTS" : 7, + "PD5:EVENTOUT" : 15, + "PD5:FSMC_NWE" : 12, + "PD5:USART2_TX" : 7, + "PD6:DFSDM1_DATIN1" : 6, + "PD6:EVENTOUT" : 15, + "PD6:FSMC_NWAIT" : 12, + "PD6:I2S3_SD" : 5, + "PD6:SPI3_MOSI" : 5, + "PD6:USART2_RX" : 7, + "PD7:DFSDM1_CKIN1" : 6, + "PD7:EVENTOUT" : 15, + "PD7:FSMC_NE1" : 12, + "PD7:USART2_CK" : 7, + "PD8:EVENTOUT" : 15, + "PD8:FSMC_D13" : 12, + "PD8:FSMC_DA13" : 12, + "PD8:USART3_TX" : 7, + "PD9:EVENTOUT" : 15, + "PD9:FSMC_D14" : 12, + "PD9:FSMC_DA14" : 12, + "PD9:USART3_RX" : 7, + "PE0:EVENTOUT" : 15, + "PE0:FSMC_NBL0" : 12, + "PE0:TIM4_ETR" : 2, + "PE10:EVENTOUT" : 15, + "PE10:FSMC_D7" : 12, + "PE10:FSMC_DA7" : 12, + "PE10:QUADSPI_BK2_IO3" : 10, + "PE10:TIM1_CH2N" : 1, + "PE11:" : 10, + "PE11:EVENTOUT" : 15, + "PE11:FSMC_D8" : 12, + "PE11:FSMC_DA8" : 12, + "PE11:I2S4_WS" : 5, + "PE11:I2S5_WS" : 6, + "PE11:SPI4_NSS" : 5, + "PE11:SPI5_NSS" : 6, + "PE11:TIM1_CH2" : 1, + "PE12:EVENTOUT" : 15, + "PE12:FSMC_D9" : 12, + "PE12:FSMC_DA9" : 12, + "PE12:I2S4_CK" : 5, + "PE12:I2S5_CK" : 6, + "PE12:SPI4_SCK" : 5, + "PE12:SPI5_SCK" : 6, + "PE12:TIM1_CH3N" : 1, + "PE13:EVENTOUT" : 15, + "PE13:FSMC_D10" : 12, + "PE13:FSMC_DA10" : 12, + "PE13:SPI4_MISO" : 5, + "PE13:SPI5_MISO" : 6, + "PE13:TIM1_CH3" : 1, + "PE14:EVENTOUT" : 15, + "PE14:FSMC_D11" : 12, + "PE14:FSMC_DA11" : 12, + "PE14:I2S4_SD" : 5, + "PE14:I2S5_SD" : 6, + "PE14:SPI4_MOSI" : 5, + "PE14:SPI5_MOSI" : 6, + "PE14:TIM1_CH4" : 1, + "PE15:EVENTOUT" : 15, + "PE15:FSMC_D12" : 12, + "PE15:FSMC_DA12" : 12, + "PE15:TIM1_BKIN" : 1, + "PE1:EVENTOUT" : 15, + "PE1:FSMC_NBL1" : 12, + "PE2:EVENTOUT" : 15, + "PE2:FSMC_A23" : 12, + "PE2:I2S4_CK" : 5, + "PE2:I2S5_CK" : 6, + "PE2:QUADSPI_BK1_IO2" : 9, + "PE2:SPI4_SCK" : 5, + "PE2:SPI5_SCK" : 6, + "PE2:TRACECLK" : 0, + "PE3:EVENTOUT" : 15, + "PE3:FSMC_A19" : 12, + "PE3:TRACED0" : 0, + "PE4:DFSDM1_DATIN3" : 8, + "PE4:EVENTOUT" : 15, + "PE4:FSMC_A20" : 12, + "PE4:I2S4_WS" : 5, + "PE4:I2S5_WS" : 6, + "PE4:SPI4_NSS" : 5, + "PE4:SPI5_NSS" : 6, + "PE4:TRACED1" : 0, + "PE5:DFSDM1_CKIN3" : 8, + "PE5:EVENTOUT" : 15, + "PE5:FSMC_A21" : 12, + "PE5:SPI4_MISO" : 5, + "PE5:SPI5_MISO" : 6, + "PE5:TIM9_CH1" : 3, + "PE5:TRACED2" : 0, + "PE6:EVENTOUT" : 15, + "PE6:FSMC_A22" : 12, + "PE6:I2S4_SD" : 5, + "PE6:I2S5_SD" : 6, + "PE6:SPI4_MOSI" : 5, + "PE6:SPI5_MOSI" : 6, + "PE6:TIM9_CH2" : 3, + "PE6:TRACED3" : 0, + "PE7:DFSDM1_DATIN2" : 6, + "PE7:EVENTOUT" : 15, + "PE7:FSMC_D4" : 12, + "PE7:FSMC_DA4" : 12, + "PE7:QUADSPI_BK2_IO0" : 10, + "PE7:TIM1_ETR" : 1, + "PE8:DFSDM1_CKIN2" : 6, + "PE8:EVENTOUT" : 15, + "PE8:FSMC_D5" : 12, + "PE8:FSMC_DA5" : 12, + "PE8:QUADSPI_BK2_IO1" : 10, + "PE8:TIM1_CH1N" : 1, + "PE9:DFSDM1_CKOUT" : 6, + "PE9:EVENTOUT" : 15, + "PE9:FSMC_D6" : 12, + "PE9:FSMC_DA6" : 12, + "PE9:QUADSPI_BK2_IO2" : 10, + "PE9:TIM1_CH1" : 1, + "PF0:EVENTOUT" : 15, + "PF0:FSMC_A0" : 12, + "PF0:I2C2_SDA" : 4, + "PF10:EVENTOUT" : 15, + "PF10:TIM1_ETR" : 1, + "PF10:TIM5_CH4" : 2, + "PF11:EVENTOUT" : 15, + "PF11:TIM8_ETR" : 3, + "PF12:EVENTOUT" : 15, + "PF12:FSMC_A6" : 12, + "PF12:TIM8_BKIN" : 3, + "PF13:EVENTOUT" : 15, + "PF13:FSMC_A7" : 12, + "PF13:I2CFMP1_SMBA" : 4, + "PF14:EVENTOUT" : 15, + "PF14:FSMC_A8" : 12, + "PF14:I2CFMP1_SCL" : 4, + "PF15:EVENTOUT" : 15, + "PF15:FSMC_A9" : 12, + "PF15:I2CFMP1_SDA" : 4, + "PF1:EVENTOUT" : 15, + "PF1:FSMC_A1" : 12, + "PF1:I2C2_SCL" : 4, + "PF2:EVENTOUT" : 15, + "PF2:FSMC_A2" : 12, + "PF2:I2C2_SMBA" : 4, + "PF3:EVENTOUT" : 15, + "PF3:FSMC_A3" : 12, + "PF3:TIM5_CH1" : 2, + "PF4:EVENTOUT" : 15, + "PF4:FSMC_A4" : 12, + "PF4:TIM5_CH2" : 2, + "PF5:EVENTOUT" : 15, + "PF5:FSMC_A5" : 12, + "PF5:TIM5_CH3" : 2, + "PF6:EVENTOUT" : 15, + "PF6:QUADSPI_BK1_IO3" : 9, + "PF6:TIM10_CH1" : 3, + "PF6:TRACED0" : 0, + "PF7:EVENTOUT" : 15, + "PF7:QUADSPI_BK1_IO2" : 9, + "PF7:TIM11_CH1" : 3, + "PF7:TRACED1" : 0, + "PF8:EVENTOUT" : 15, + "PF8:QUADSPI_BK1_IO0" : 10, + "PF8:TIM13_CH1" : 9, + "PF9:EVENTOUT" : 15, + "PF9:QUADSPI_BK1_IO1" : 10, + "PF9:TIM14_CH1" : 9, + "PG0:CAN1_RX" : 9, + "PG0:EVENTOUT" : 15, + "PG0:FSMC_A10" : 12, + "PG10:EVENTOUT" : 15, + "PG10:FSMC_NE3" : 12, + "PG11:CAN2_RX" : 9, + "PG11:EVENTOUT" : 15, + "PG12:CAN2_TX" : 9, + "PG12:EVENTOUT" : 15, + "PG12:FSMC_NE4" : 12, + "PG12:USART6_RTS" : 8, + "PG13:EVENTOUT" : 15, + "PG13:FSMC_A24" : 12, + "PG13:TRACED2" : 0, + "PG13:USART6_CTS" : 8, + "PG14:EVENTOUT" : 15, + "PG14:FSMC_A25" : 12, + "PG14:QUADSPI_BK2_IO3" : 9, + "PG14:TRACED3" : 0, + "PG14:USART6_TX" : 8, + "PG15:EVENTOUT" : 15, + "PG15:USART6_CTS" : 8, + "PG1:CAN1_TX" : 9, + "PG1:EVENTOUT" : 15, + "PG1:FSMC_A11" : 12, + "PG2:EVENTOUT" : 15, + "PG2:FSMC_A12" : 12, + "PG3:EVENTOUT" : 15, + "PG3:FSMC_A13" : 12, + "PG4:EVENTOUT" : 15, + "PG4:FSMC_A14" : 12, + "PG5:EVENTOUT" : 15, + "PG5:FSMC_A15" : 12, + "PG6:EVENTOUT" : 15, + "PG6:QUADSPI_BK1_NCS" : 10, + "PG7:EVENTOUT" : 15, + "PG7:USART6_CK" : 8, + "PG8:EVENTOUT" : 15, + "PG8:USART6_RTS" : 8, + "PG9:EVENTOUT" : 15, + "PG9:FSMC_NE2" : 12, + "PG9:QUADSPI_BK2_IO2" : 9, + "PG9:USART6_RX" : 8, + "PH0:EVENTOUT" : 15, + "PH1:EVENTOUT" : 15, +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F427xx.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F427xx.py new file mode 100644 index 0000000000..25a5352983 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F427xx.py @@ -0,0 +1,739 @@ +#!/usr/bin/env python +''' +these tables are generated from the STM32 datasheet DM00071990.pdf for the +STM32F427 and STM32F4279 +''' + +DMA_Map = { + # format is [DMA_TABLE, StreamNum] + # extracted from tabula-STM324x7-306.csv and tabula-STM324x7-307.csv + "ADC1" : [(2,0),(2,4)], + "ADC2" : [(2,2),(2,3)], + "ADC3" : [(2,0),(2,1)], + "CRYP_IN" : [(2,6)], + "CRYP_OUT" : [(2,5)], + "DAC1" : [(1,5)], + "DAC2" : [(1,6)], + "DCMI" : [(2,1),(2,7)], + "HASH_IN" : [(2,7)], + "I2C1_RX" : [(1,0),(1,5)], + "I2C1_TX" : [(1,6),(1,7)], + "I2C2_RX" : [(1,2),(1,3)], + "I2C2_TX" : [(1,7)], + "I2C3_RX" : [(1,2)], + "I2C3_TX" : [(1,4)], + "I2S2_EXT_RX" : [(1,3)], + "I2S2_EXT_TX" : [(1,4)], + "I2S3_EXT_RX" : [(1,2),(1,0)], + "I2S3_EXT_TX" : [(1,5)], + "SAI1_A" : [(2,1),(2,3)], + "SAI1_B" : [(2,5),(2,4)], + "SDIO" : [(2,3),(2,6)], + "SPI1_RX" : [(2,0),(2,2)], + "SPI1_TX" : [(2,3),(2,5)], + "SPI2_RX" : [(1,3)], + "SPI2_TX" : [(1,4)], + "SPI3_RX" : [(1,0),(1,2)], + "SPI3_TX" : [(1,5),(1,7)], + "SPI4_RX" : [(2,0),(2,3)], + "SPI4_TX" : [(2,1),(2,4)], + "SPI5_RX" : [(2,3),(2,5)], + "SPI5_TX" : [(2,4),(2,6)], + "SPI6_RX" : [(2,6)], + "SPI6_TX" : [(2,5)], + "TIM1_CH1" : [(2,6),(2,1),(2,3)], + "TIM1_CH2" : [(2,6),(2,2)], + "TIM1_CH3" : [(2,6),(2,6)], + "TIM1_CH4" : [(2,4)], + "TIM1_COM" : [(2,4)], + "TIM1_TRIG" : [(2,0),(2,4)], + "TIM1_UP" : [(2,5)], + "TIM2_CH1" : [(1,5)], + "TIM2_CH2" : [(1,6)], + "TIM2_CH3" : [(1,1)], + "TIM2_CH4" : [(1,6),(1,7)], + "TIM2_UP" : [(1,1),(1,7)], + "TIM3_CH1" : [(1,4)], + "TIM3_CH2" : [(1,5)], + "TIM3_CH3" : [(1,7)], + "TIM3_CH4" : [(1,2)], + "TIM3_TRIG" : [(1,4)], + "TIM3_UP" : [(1,2)], + "TIM4_CH1" : [(1,0)], + "TIM4_CH2" : [(1,3)], + "TIM4_CH3" : [(1,7)], + "TIM4_UP" : [(1,6)], + "TIM5_CH1" : [(1,2)], + "TIM5_CH2" : [(1,4)], + "TIM5_CH3" : [(1,0)], + "TIM5_CH4" : [(1,1),(1,3)], + "TIM5_TRIG" : [(1,1),(1,3)], + "TIM5_UP" : [(1,0),(1,6)], + "TIM6_UP" : [(1,1)], + "TIM7_UP" : [(1,2),(1,4)], + "TIM8_CH1" : [(2,2),(2,2)], + "TIM8_CH2" : [(2,2),(2,3)], + "TIM8_CH3" : [(2,2),(2,4)], + "TIM8_CH4" : [(2,7)], + "TIM8_COM" : [(2,7)], + "TIM8_TRIG" : [(2,7)], + "TIM8_UP" : [(2,1)], + "UART4_RX" : [(1,2)], + "UART4_TX" : [(1,4)], + "UART5_RX" : [(1,0)], + "UART5_TX" : [(1,7)], + "UART7_RX" : [(1,3)], + "UART7_TX" : [(1,1)], + "UART8_RX" : [(1,6)], + "UART8_TX" : [(1,0)], + "USART1_RX" : [(2,2),(2,5)], + "USART1_TX" : [(2,7)], + "USART2_RX" : [(1,5)], + "USART2_TX" : [(1,6)], + "USART3_RX" : [(1,1)], + "USART3_TX" : [(1,3),(1,4)], + "USART6_RX" : [(2,1),(2,2)], + "USART6_TX" : [(2,6),(2,7)], +} + +AltFunction_map = { + # format is PIN:FUNCTION : AFNUM + # extracted from tabula-AF-F427.csv + "PA0:ETH_MII_CRS" : 11, + "PA0:EVENTOUT" : 15, + "PA0:TIM2_CH1" : 1, + "PA0:TIM2_ETR" : 1, + "PA0:TIM5_CH1" : 2, + "PA0:TIM8_ETR" : 3, + "PA0:UART4_TX" : 8, + "PA0:USART2_CTS" : 7, + "PA10:DCMI_D1" : 13, + "PA10:EVENTOUT" : 15, + "PA10:OTG_FS_ID" : 10, + "PA10:TIM1_CH3" : 1, + "PA10:USART1_RX" : 7, + "PA11:CAN1_RX" : 9, + "PA11:EVENTOUT" : 15, + "PA11:LCD_R4" : 14, + "PA11:OTG_FS_DM" : 10, + "PA11:TIM1_CH4" : 1, + "PA11:USART1_CTS" : 7, + "PA12:CAN1_TX" : 9, + "PA12:EVENTOUT" : 15, + "PA12:LCD_R5" : 14, + "PA12:OTG_FS_DP" : 10, + "PA12:TIM1_ETR" : 1, + "PA12:USART1_RTS" : 7, + "PA13:EVENTOUT" : 15, + "PA13:JTMS-SWDIO" : 0, + "PA14:EVENTOUT" : 15, + "PA14:JTCK-SWCLK" : 0, + "PA15:EVENTOUT" : 15, + "PA15:I2S3_WS" : 6, + "PA15:JTDI" : 0, + "PA15:SPI1_NSS" : 5, + "PA15:SPI3_NSS" : 6, + "PA15:TIM2_CH1" : 1, + "PA15:TIM2_ETR" : 1, + "PA1:ETH_MII_RX_CLK" : 11, + "PA1:ETH_RMII_REF_CLK" : 11, + "PA1:EVENTOUT" : 15, + "PA1:TIM2_CH2" : 1, + "PA1:TIM5_CH2" : 2, + "PA1:UART4_RX" : 8, + "PA1:USART2_RTS" : 7, + "PA2:ETH_MDIO" : 11, + "PA2:EVENTOUT" : 15, + "PA2:TIM2_CH3" : 1, + "PA2:TIM5_CH3" : 2, + "PA2:TIM9_CH1" : 3, + "PA2:USART2_TX" : 7, + "PA3:ETH_MII_COL" : 11, + "PA3:EVENTOUT" : 15, + "PA3:LCD_B5" : 14, + "PA3:OTG_HS_ULPI_D0" : 10, + "PA3:TIM2_CH4" : 1, + "PA3:TIM5_CH4" : 2, + "PA3:TIM9_CH2" : 3, + "PA3:USART2_RX" : 7, + "PA4:DCMI_HSYNC" : 13, + "PA4:EVENTOUT" : 15, + "PA4:I2S3_WS" : 6, + "PA4:LCD_VSYNC" : 14, + "PA4:OTG_HS_SOF" : 12, + "PA4:SPI1_NSS" : 5, + "PA4:SPI3_NSS" : 6, + "PA4:USART2_CK" : 7, + "PA5:EVENTOUT" : 15, + "PA5:OTG_HS_ULPI_CK" : 10, + "PA5:SPI1_SCK" : 5, + "PA5:TIM2_CH1" : 1, + "PA5:TIM2_ETR" : 1, + "PA5:TIM8_CH1N" : 3, + "PA6:DCMI_PIXCLK" : 13, + "PA6:EVENTOUT" : 15, + "PA6:LCD_G2" : 14, + "PA6:SPI1_MISO" : 5, + "PA6:TIM13_CH1" : 9, + "PA6:TIM1_BKIN" : 1, + "PA6:TIM3_CH1" : 2, + "PA6:TIM8_BKIN" : 3, + "PA7:ETH_MII_RX_DV" : 11, + "PA7:ETH_RMII_CRS_DV" : 11, + "PA7:EVENTOUT" : 15, + "PA7:SPI1_MOSI" : 5, + "PA7:TIM14_CH1" : 9, + "PA7:TIM1_CH1N" : 1, + "PA7:TIM3_CH2" : 2, + "PA7:TIM8_CH1N" : 3, + "PA8:EVENTOUT" : 15, + "PA8:I2C3_SCL" : 4, + "PA8:LCD_R6" : 14, + "PA8:MCO1" : 0, + "PA8:OTG_FS_SOF" : 10, + "PA8:TIM1_CH1" : 1, + "PA8:USART1_CK" : 7, + "PA9:DCMI_D0" : 13, + "PA9:EVENTOUT" : 15, + "PA9:I2C3_SMBA" : 4, + "PA9:TIM1_CH2" : 1, + "PA9:USART1_TX" : 7, + "PB0:ETH_MII_RXD2" : 11, + "PB0:EVENTOUT" : 15, + "PB0:LCD_R3" : 9, + "PB0:OTG_HS_ULPI_D1" : 10, + "PB0:TIM1_CH2N" : 1, + "PB0:TIM3_CH3" : 2, + "PB0:TIM8_CH2N" : 3, + "PB10:ETH_MII_RX_ER" : 11, + "PB10:EVENTOUT" : 15, + "PB10:I2C2_SCL" : 4, + "PB10:I2S2_CK" : 5, + "PB10:LCD_G4" : 14, + "PB10:OTG_HS_ULPI_D3" : 10, + "PB10:SPI2_SCK" : 5, + "PB10:TIM2_CH3" : 1, + "PB10:USART3_TX" : 7, + "PB11:ETH_MII_TX_EN" : 11, + "PB11:ETH_RMII_TX_EN" : 11, + "PB11:EVENTOUT" : 15, + "PB11:I2C2_SDA" : 4, + "PB11:LCD_G5" : 14, + "PB11:OTG_HS_ULPI_D4" : 10, + "PB11:TIM2_CH4" : 1, + "PB11:USART3_RX" : 7, + "PB12:CAN2_RX" : 9, + "PB12:ETH_MII_TXD0" : 11, + "PB12:ETH_RMII_TXD0" : 11, + "PB12:EVENTOUT" : 15, + "PB12:I2C2_SMBA" : 4, + "PB12:I2S2_WS" : 5, + "PB12:OTG_HS_ID" : 12, + "PB12:OTG_HS_ULPI_D5" : 10, + "PB12:SPI2_NSS" : 5, + "PB12:TIM1_BKIN" : 1, + "PB12:USART3_CK" : 7, + "PB13:CAN2_TX" : 9, + "PB13:ETH_MII_TXD1" : 11, + "PB13:ETH_RMII_TXD1" : 11, + "PB13:EVENTOUT" : 15, + "PB13:I2S2_CK" : 5, + "PB13:OTG_HS_ULPI_D6" : 10, + "PB13:SPI2_SCK" : 5, + "PB13:TIM1_CH1N" : 1, + "PB13:USART3_CTS" : 7, + "PB14:EVENTOUT" : 15, + "PB14:I2S2EXT_SD" : 6, + "PB14:OTG_HS_DM" : 12, + "PB14:SPI2_MISO" : 5, + "PB14:TIM12_CH1" : 9, + "PB14:TIM1_CH2N" : 1, + "PB14:TIM8_CH2N" : 3, + "PB14:USART3_RTS" : 7, + "PB15:EVENTOUT" : 15, + "PB15:I2S2_SD" : 5, + "PB15:OTG_HS_DP" : 12, + "PB15:RTC_REFIN" : 0, + "PB15:SPI2_MOSI" : 5, + "PB15:TIM12_CH2" : 9, + "PB15:TIM1_CH3N" : 1, + "PB15:TIM8_CH3N" : 3, + "PB1:ETH_MII_RXD3" : 11, + "PB1:EVENTOUT" : 15, + "PB1:LCD_R6" : 9, + "PB1:OTG_HS_ULPI_D2" : 10, + "PB1:TIM1_CH3N" : 1, + "PB1:TIM3_CH4" : 2, + "PB1:TIM8_CH3N" : 3, + "PB2:EVENTOUT" : 15, + "PB3:EVENTOUT" : 15, + "PB3:I2S3_CK" : 6, + "PB3:JTDO" : 0, + "PB3:SPI1_SCK" : 5, + "PB3:SPI3_SCK" : 6, + "PB3:TIM2_CH2" : 1, + "PB3:TRACESWO" : 0, + "PB4:EVENTOUT" : 15, + "PB4:I2S3EXT_SD" : 7, + "PB4:NJTRST" : 0, + "PB4:SPI1_MISO" : 5, + "PB4:SPI3_MISO" : 6, + "PB4:TIM3_CH1" : 2, + "PB5:CAN2_RX" : 9, + "PB5:DCMI_D10" : 13, + "PB5:ETH_PPS_OUT" : 11, + "PB5:EVENTOUT" : 15, + "PB5:FMC_SDCKE1" : 12, + "PB5:I2C1_SMBA" : 4, + "PB5:I2S3_SD" : 6, + "PB5:OTG_HS_ULPI_D7" : 10, + "PB5:SPI1_MOSI" : 5, + "PB5:SPI3_MOSI" : 6, + "PB5:TIM3_CH2" : 2, + "PB6:CAN2_TX" : 9, + "PB6:DCMI_D5" : 13, + "PB6:EVENTOUT" : 15, + "PB6:FMC_SDNE1" : 12, + "PB6:I2C1_SCL" : 4, + "PB6:TIM4_CH1" : 2, + "PB6:USART1_TX" : 7, + "PB7:DCMI_VSYNC" : 13, + "PB7:EVENTOUT" : 15, + "PB7:FMC_NL" : 12, + "PB7:I2C1_SDA" : 4, + "PB7:TIM4_CH2" : 2, + "PB7:USART1_RX" : 7, + "PB8:CAN1_RX" : 9, + "PB8:DCMI_D6" : 13, + "PB8:ETH_MII_TXD3" : 11, + "PB8:EVENTOUT" : 15, + "PB8:I2C1_SCL" : 4, + "PB8:LCD_B6" : 14, + "PB8:SDIO_D4" : 12, + "PB8:TIM10_CH1" : 3, + "PB8:TIM4_CH3" : 2, + "PB9:CAN1_TX" : 9, + "PB9:DCMI_D7" : 13, + "PB9:EVENTOUT" : 15, + "PB9:I2C1_SDA" : 4, + "PB9:I2S2_WS" : 5, + "PB9:LCD_B7" : 14, + "PB9:SDIO_D5" : 12, + "PB9:SPI2_NSS" : 5, + "PB9:TIM11_CH1" : 3, + "PB9:TIM4_CH4" : 2, + "PC0:EVENTOUT" : 15, + "PC0:FMC_SDNWE" : 12, + "PC0:OTG_HS_ULPI_STP" : 10, + "PC10:DCMI_D8" : 13, + "PC10:EVENTOUT" : 15, + "PC10:I2S3_CK" : 6, + "PC10:LCD_R2" : 14, + "PC10:SDIO_D2" : 12, + "PC10:SPI3_SCK" : 6, + "PC10:UART4_TX" : 8, + "PC10:USART3_TX" : 7, + "PC11:DCMI_D4" : 13, + "PC11:EVENTOUT" : 15, + "PC11:I2S3EXT_SD" : 5, + "PC11:SDIO_D3" : 12, + "PC11:SPI3_MISO" : 6, + "PC11:UART4_RX" : 8, + "PC11:USART3_RX" : 7, + "PC12:DCMI_D9" : 13, + "PC12:EVENTOUT" : 15, + "PC12:I2S3_SD" : 6, + "PC12:SDIO_CK" : 12, + "PC12:SPI3_MOSI" : 6, + "PC12:UART5_TX" : 8, + "PC12:USART3_CK" : 7, + "PC13:EVENTOUT" : 15, + "PC14:EVENTOUT" : 15, + "PC15:EVENTOUT" : 15, + "PC1:ETH_MDC" : 11, + "PC1:EVENTOUT" : 15, + "PC2:ETH_MII_TXD2" : 11, + "PC2:EVENTOUT" : 15, + "PC2:FMC_SDNE0" : 12, + "PC2:I2S2EXT_SD" : 6, + "PC2:OTG_HS_ULPI_DIR" : 10, + "PC2:SPI2_MISO" : 5, + "PC3:ETH_MII_TX_CLK" : 11, + "PC3:EVENTOUT" : 15, + "PC3:FMC_SDCKE0" : 12, + "PC3:I2S2_SD" : 5, + "PC3:OTG_HS_ULPI_NXT" : 10, + "PC3:SPI2_MOSI" : 5, + "PC4:ETH_MII_RXD0" : 11, + "PC4:ETH_RMII_RXD0" : 11, + "PC4:EVENTOUT" : 15, + "PC5:ETH_MII_RXD1" : 11, + "PC5:ETH_RMII_RXD1" : 11, + "PC5:EVENTOUT" : 15, + "PC6:DCMI_D0" : 13, + "PC6:EVENTOUT" : 15, + "PC6:I2S2_MCK" : 5, + "PC6:LCD_HSYNC" : 14, + "PC6:SDIO_D6" : 12, + "PC6:TIM3_CH1" : 2, + "PC6:TIM8_CH1" : 3, + "PC6:USART6_TX" : 8, + "PC7:DCMI_D1" : 13, + "PC7:EVENTOUT" : 15, + "PC7:I2S3_MCK" : 6, + "PC7:LCD_G6" : 14, + "PC7:SDIO_D7" : 12, + "PC7:TIM3_CH2" : 2, + "PC7:TIM8_CH2" : 3, + "PC7:USART6_RX" : 8, + "PC8:DCMI_D2" : 13, + "PC8:EVENTOUT" : 15, + "PC8:SDIO_D0" : 12, + "PC8:TIM3_CH3" : 2, + "PC8:TIM8_CH3" : 3, + "PC8:USART6_CK" : 8, + "PC9:DCMI_D3" : 13, + "PC9:EVENTOUT" : 15, + "PC9:I2C3_SDA" : 4, + "PC9:I2S_CKIN" : 5, + "PC9:MCO2" : 0, + "PC9:SDIO_D1" : 12, + "PC9:TIM3_CH4" : 2, + "PC9:TIM8_CH4" : 3, + "PD0:CAN1_RX" : 9, + "PD0:EVENTOUT" : 15, + "PD0:FMC_D2" : 12, + "PD10:EVENTOUT" : 15, + "PD10:FMC_D15" : 12, + "PD10:LCD_B3" : 14, + "PD10:USART3_CK" : 7, + "PD11:EVENTOUT" : 15, + "PD11:FMC_A16" : 12, + "PD11:USART3_CTS" : 7, + "PD12:EVENTOUT" : 15, + "PD12:FMC_A17" : 12, + "PD12:TIM4_CH1" : 2, + "PD12:USART3_RTS" : 7, + "PD13:EVENTOUT" : 15, + "PD13:FMC_A18" : 12, + "PD13:TIM4_CH2" : 2, + "PD14:EVENTOUT" : 15, + "PD14:FMC_D0" : 12, + "PD14:TIM4_CH3" : 2, + "PD15:EVENTOUT" : 15, + "PD15:FMC_D1" : 12, + "PD15:TIM4_CH4" : 2, + "PD1:CAN1_TX" : 9, + "PD1:EVENTOUT" : 15, + "PD1:FMC_D3" : 12, + "PD2:DCMI_D11" : 13, + "PD2:EVENTOUT" : 15, + "PD2:SDIO_CMD" : 12, + "PD2:TIM3_ETR" : 2, + "PD2:UART5_RX" : 8, + "PD3:DCMI_D5" : 13, + "PD3:EVENTOUT" : 15, + "PD3:FMC_CLK" : 12, + "PD3:I2S2_CK" : 5, + "PD3:LCD_G7" : 14, + "PD3:SPI2_SCK" : 5, + "PD3:USART2_CTS" : 7, + "PD4:EVENTOUT" : 15, + "PD4:FMC_NOE" : 12, + "PD4:USART2_RTS" : 7, + "PD5:EVENTOUT" : 15, + "PD5:FMC_NWE" : 12, + "PD5:USART2_TX" : 7, + "PD6:DCMI_D10" : 13, + "PD6:EVENTOUT" : 15, + "PD6:FMC_NWAIT" : 12, + "PD6:I2S3_SD" : 5, + "PD6:LCD_B2" : 14, + "PD6:SAI1_SD_A" : 6, + "PD6:SPI3_MOSI" : 5, + "PD6:USART2_RX" : 7, + "PD7:EVENTOUT" : 15, + "PD7:FMC_NCE2" : 12, + "PD7:FMC_NE1" : 12, + "PD7:USART2_CK" : 7, + "PD8:EVENTOUT" : 15, + "PD8:FMC_D13" : 12, + "PD8:USART3_TX" : 7, + "PD9:EVENTOUT" : 15, + "PD9:FMC_D14" : 12, + "PD9:USART3_RX" : 7, + "PE0:DCMI_D2" : 13, + "PE0:EVENTOUT" : 15, + "PE0:FMC_NBL0" : 12, + "PE0:TIM4_ETR" : 2, + "PE0:UART8_RX" : 8, + "PE10:EVENTOUT" : 15, + "PE10:FMC_D7" : 12, + "PE10:TIM1_CH2N" : 1, + "PE11:EVENTOUT" : 15, + "PE11:FMC_D8" : 12, + "PE11:LCD_G3" : 14, + "PE11:SPI4_NSS" : 5, + "PE11:TIM1_CH2" : 1, + "PE12:EVENTOUT" : 15, + "PE12:FMC_D9" : 12, + "PE12:LCD_B4" : 14, + "PE12:SPI4_SCK" : 5, + "PE12:TIM1_CH3N" : 1, + "PE13:EVENTOUT" : 15, + "PE13:FMC_D10" : 12, + "PE13:LCD_DE" : 14, + "PE13:SPI4_MISO" : 5, + "PE13:TIM1_CH3" : 1, + "PE14:EVENTOUT" : 15, + "PE14:FMC_D11" : 12, + "PE14:LCD_CLK" : 14, + "PE14:SPI4_MOSI" : 5, + "PE14:TIM1_CH4" : 1, + "PE15:" : 5, + "PE15:EVENTOUT" : 15, + "PE15:FMC_D12" : 12, + "PE15:LCD_R7" : 14, + "PE15:TIM1_BKIN" : 1, + "PE1:DCMI_D3" : 13, + "PE1:EVENTOUT" : 15, + "PE1:FMC_NBL1" : 12, + "PE1:UART8_TX" : 8, + "PE2:ETH_MII_TXD3" : 11, + "PE2:EVENTOUT" : 15, + "PE2:FMC_A23" : 12, + "PE2:SAI1_MCLK_A" : 6, + "PE2:SPI4_SCK" : 5, + "PE2:TRACECLK" : 0, + "PE3:EVENTOUT" : 15, + "PE3:FMC_A19" : 12, + "PE3:SAI1_SD_B" : 6, + "PE3:TRACED0" : 0, + "PE4:DCMI_D4" : 13, + "PE4:EVENTOUT" : 15, + "PE4:FMC_A20" : 12, + "PE4:LCD_B0" : 14, + "PE4:SAI1_FS_A" : 6, + "PE4:SPI4_NSS" : 5, + "PE4:TRACED1" : 0, + "PE5:DCMI_D6" : 13, + "PE5:EVENTOUT" : 15, + "PE5:FMC_A21" : 12, + "PE5:LCD_G0" : 14, + "PE5:SAI1_SCK_A" : 6, + "PE5:SPI4_MISO" : 5, + "PE5:TIM9_CH1" : 3, + "PE5:TRACED2" : 0, + "PE6:DCMI_D7" : 13, + "PE6:EVENTOUT" : 15, + "PE6:FMC_A22" : 12, + "PE6:LCD_G1" : 14, + "PE6:SAI1_SD_A" : 6, + "PE6:SPI4_MOSI" : 5, + "PE6:TIM9_CH2" : 3, + "PE6:TRACED3" : 0, + "PE7:EVENTOUT" : 15, + "PE7:FMC_D4" : 12, + "PE7:TIM1_ETR" : 1, + "PE7:UART7_RX" : 8, + "PE8:EVENTOUT" : 15, + "PE8:FMC_D5" : 12, + "PE8:TIM1_CH1N" : 1, + "PE8:UART7_TX" : 8, + "PE9:EVENTOUT" : 15, + "PE9:FMC_D6" : 12, + "PE9:TIM1_CH1" : 1, + "PF0:EVENTOUT" : 15, + "PF0:FMC_A0" : 12, + "PF0:I2C2_SDA" : 4, + "PF10:DCMI_D11" : 13, + "PF10:EVENTOUT" : 15, + "PF10:FMC_INTR" : 12, + "PF10:LCD_DE" : 14, + "PF11:DCMI_D12" : 13, + "PF11:EVENTOUT" : 15, + "PF11:FMC_SDNRAS" : 12, + "PF11:SPI5_MOSI" : 5, + "PF12:EVENTOUT" : 15, + "PF12:FMC_A6" : 12, + "PF13:EVENTOUT" : 15, + "PF13:FMC_A7" : 12, + "PF14:EVENTOUT" : 15, + "PF14:FMC_A8" : 12, + "PF15:EVENTOUT" : 15, + "PF15:FMC_A9" : 12, + "PF1:" : 3, + "PF1:EVENTOUT" : 15, + "PF1:FMC_A1" : 12, + "PF1:I2C2_SCL" : 4, + "PF2:EVENTOUT" : 15, + "PF2:FMC_A2" : 12, + "PF2:I2C2_SMBA" : 4, + "PF3:" : 4, + "PF3:EVENTOUT" : 15, + "PF3:FMC_A3" : 12, + "PF4:" : 4, + "PF4:EVENTOUT" : 15, + "PF4:FMC_A4" : 12, + "PF5:" : 4, + "PF5:EVENTOUT" : 15, + "PF5:FMC_A5" : 12, + "PF6:EVENTOUT" : 15, + "PF6:FMC_NIORD" : 12, + "PF6:SAI1_SD_B" : 6, + "PF6:SPI5_NSS" : 5, + "PF6:TIM10_CH1" : 3, + "PF6:UART7_RX" : 8, + "PF7:EVENTOUT" : 15, + "PF7:FMC_NREG" : 12, + "PF7:SAI1_MCLK_B" : 6, + "PF7:SPI5_SCK" : 5, + "PF7:TIM11_CH1" : 3, + "PF7:UART7_TX" : 8, + "PF8:EVENTOUT" : 15, + "PF8:FMC_NIOWR" : 12, + "PF8:SAI1_SCK_B" : 6, + "PF8:SPI5_MISO" : 5, + "PF8:TIM13_CH1" : 9, + "PF9:EVENTOUT" : 15, + "PF9:FMC_CD" : 12, + "PF9:SAI1_FS_B" : 6, + "PF9:SPI5_MOSI" : 5, + "PF9:TIM14_CH1" : 9, + "PG0:EVENTOUT" : 15, + "PG0:FMC_A10" : 12, + "PG10:DCMI_D2" : 13, + "PG10:EVENTOUT" : 15, + "PG10:FMC_NCE4_1" : 12, + "PG10:FMC_NE3" : 12, + "PG10:LCD_B2" : 14, + "PG10:LCD_G3" : 9, + "PG11:DCMI_D3" : 13, + "PG11:ETH_MII_TX_EN" : 11, + "PG11:ETH_RMII_TX_EN" : 11, + "PG11:EVENTOUT" : 15, + "PG11:FMC_NCE4_2" : 12, + "PG11:LCD_B3" : 14, + "PG12:EVENTOUT" : 15, + "PG12:FMC_NE4" : 12, + "PG12:LCD_B1" : 14, + "PG12:LCD_B4" : 9, + "PG12:SPI6_MISO" : 5, + "PG12:USART6_RTS" : 8, + "PG13:ETH_MII_TXD0" : 11, + "PG13:ETH_RMII_TXD0" : 11, + "PG13:EVENTOUT" : 15, + "PG13:FMC_A24" : 12, + "PG13:SPI6_SCK" : 5, + "PG13:USART6_CTS" : 8, + "PG14:ETH_MII_TXD1" : 11, + "PG14:ETH_RMII_TXD1" : 11, + "PG14:EVENTOUT" : 15, + "PG14:FMC_A25" : 12, + "PG14:SPI6_MOSI" : 5, + "PG14:USART6_TX" : 8, + "PG15:DCMI_D13" : 13, + "PG15:EVENTOUT" : 15, + "PG15:FMC_SDNCAS" : 12, + "PG15:USART6_CTS" : 8, + "PG1:EVENTOUT" : 15, + "PG1:FMC_A11" : 12, + "PG2:EVENTOUT" : 15, + "PG2:FMC_A12" : 12, + "PG3:EVENTOUT" : 15, + "PG3:FMC_A13" : 12, + "PG4:EVENTOUT" : 15, + "PG4:FMC_A14" : 12, + "PG4:FMC_BA0" : 12, + "PG5:EVENTOUT" : 15, + "PG5:FMC_A15" : 12, + "PG5:FMC_BA1" : 12, + "PG6:DCMI_D12" : 13, + "PG6:EVENTOUT" : 15, + "PG6:FMC_INT2" : 12, + "PG6:LCD_R7" : 14, + "PG7:DCMI_D13" : 13, + "PG7:EVENTOUT" : 15, + "PG7:FMC_INT3" : 12, + "PG7:LCD_CLK" : 14, + "PG7:USART6_CK" : 8, + "PG8:ETH_PPS_OUT" : 11, + "PG8:EVENTOUT" : 15, + "PG8:FMC_SDCLK" : 12, + "PG8:SPI6_NSS" : 5, + "PG8:USART6_RTS" : 8, + "PG9:DCMI_VSYNC(1)" : 13, + "PG9:EVENTOUT" : 15, + "PG9:FMC_NCE3" : 12, + "PG9:FMC_NE2" : 12, + "PG9:USART6_RX" : 8, + "PH0:EVENTOUT" : 15, + "PH10:DCMI_D1" : 13, + "PH10:EVENTOUT" : 15, + "PH10:FMC_D18" : 12, + "PH10:LCD_R4" : 14, + "PH10:TIM5_CH1" : 2, + "PH11:DCMI_D2" : 13, + "PH11:EVENTOUT" : 15, + "PH11:FMC_D19" : 12, + "PH11:LCD_R5" : 14, + "PH11:TIM5_CH2" : 2, + "PH12:DCMI_D3" : 13, + "PH12:EVENTOUT" : 15, + "PH12:FMC_D20" : 12, + "PH12:LCD_R6" : 14, + "PH12:TIM5_CH3" : 2, + "PH13:CAN1_TX" : 9, + "PH13:EVENTOUT" : 15, + "PH13:FMC_D21" : 12, + "PH13:LCD_G2" : 14, + "PH13:TIM8_CH1N" : 3, + "PH14:DCMI_D4" : 13, + "PH14:EVENTOUT" : 15, + "PH14:FMC_D22" : 12, + "PH14:LCD_G3" : 14, + "PH14:TIM8_CH2N" : 3, + "PH15:DCMI_D11" : 13, + "PH15:EVENTOUT" : 15, + "PH15:FMC_D23" : 12, + "PH15:LCD_G4" : 14, + "PH15:TIM8_CH3N" : 3, + "PH1:EVENTOUT" : 15, + "PH2:ETH_MII_CRS" : 11, + "PH2:EVENTOUT" : 15, + "PH2:FMC_SDCKE0" : 12, + "PH2:LCD_R0" : 14, + "PH3:ETH_MII_COL" : 11, + "PH3:EVENTOUT" : 15, + "PH3:FMC_SDNE0" : 12, + "PH3:LCD_R1" : 14, + "PH4:EVENTOUT" : 15, + "PH4:I2C2_SCL" : 4, + "PH4:OTG_HS_ULPI_NXT" : 10, + "PH5:EVENTOUT" : 15, + "PH5:FMC_SDNWE" : 12, + "PH5:I2C2_SDA" : 4, + "PH5:SPI5_NSS" : 5, + "PH6:DCMI_D8" : 13, + "PH6:FMC_SDNE1" : 12, + "PH6:I2C2_SMBA" : 4, + "PH6:SPI5_SCK" : 5, + "PH6:TIM12_CH1" : 9, + "PH7:DCMI_D9" : 13, + "PH7:ETH_MII_RXD3" : 11, + "PH7:FMC_SDCKE1" : 12, + "PH7:I2C3_SCL" : 4, + "PH7:SPI5_MISO" : 5, + "PH8:DCMI_HSYNC" : 13, + "PH8:EVENTOUT" : 15, + "PH8:FMC_D16" : 12, + "PH8:I2C3_SDA" : 4, + "PH8:LCD_R2" : 14, + "PH9:DCMI_D0" : 13, + "PH9:EVENTOUT" : 15, + "PH9:FMC_D17" : 12, + "PH9:I2C3_SMBA" : 4, + "PH9:LCD_R3" : 14, + "PH9:TIM12_CH2" : 9, +} + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/af_parse.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/af_parse.py new file mode 100755 index 0000000000..e64e519574 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/af_parse.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python +''' + create alternate function tables + +This assumes a csv file extracted from the datasheet using tablula: + + https://github.com/tabulapdf/tabula +''' + +import sys, csv, os + +def is_pin(str): + '''see if a string is a valid pin name''' + if len(str) < 3: + return False + if str[0] != 'P': + return False + if str[1] not in "ABCDEFGH": + return False + try: + p = int(str[2:]) + if p < 0 or p > 15: + return False + return True + except ValueError: + return False + +def parse_af_table(fname, table): + csvt = csv.reader(open(fname,'rb')) + i = 0 + aflist = [] + for row in csvt: + if len(row) > 2 and row[1] == 'AF0': + # it is a AF list + aflist = [] + for s in row[1:]: + if s: + aflist.append(int(s[2:])) + if not is_pin(row[0]): + continue + pin = row[0] + for i in range(len(aflist)): + af = aflist[i] + s = row[i+1] + s = s.replace('_\r', '_') + s = s.replace('\r_', '_') + s = s.replace('\r', '') + s = s.replace(' ', '') + if s == '-' or len(s) == 0: + continue + functions = s.split('/') + for f in functions: + table[pin+':'+f.upper()] = af + +table = {} + +if len(sys.argv) != 2: + print("Error: expected 1 CSV file") + sys.exit(1) + +parse_af_table(sys.argv[1], table) + +sys.stdout.write("AltFunction_map = {\n"); +sys.stdout.write('\t# format is PIN:FUNCTION : AFNUM\n') +sys.stdout.write('\t# extracted from %s\n' % os.path.basename(sys.argv[1])) +for k in sorted(table.keys()): + s = '"' + k + '"' + sys.stdout.write('\t%-20s\t:\t%s,\n' % (s, table[k])) +sys.stdout.write("}\n"); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py new file mode 100755 index 0000000000..e956bb9a4c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -0,0 +1,387 @@ +#!/usr/bin/env python +''' +setup board.h for chibios +''' + +import argparse, sys, fnmatch, os, dma_resolver, shlex + +parser = argparse.ArgumentParser("chibios_pins.py") +parser.add_argument('-D', '--outdir', type=str, default=None, help='Output directory') +parser.add_argument('hwdef', type=str, default=None, help='hardware definition file') + +args = parser.parse_args() + +# output variables for each pin +vtypes = ['MODER', 'OTYPER', 'OSPEEDR', 'PUPDR', 'ODR', 'AFRL', 'AFRH'] + +# number of pins in each port +pincount = { 'A': 16, 'B' : 16, 'C' : 16, 'D' : 16, 'E' : 16, 'F' : 16, 'G' : 16, 'H' : 2, 'I' : 0, 'J' : 0, 'K' : 0 } + +ports = pincount.keys() + +portmap = {} + +# dictionary of all config lines, indexed by first word +config = {} + +# list of all pins in config file order +allpins = [] + +# list of configs by type +bytype = {} + +mcu_type = None + +def get_alt_function(mcu, pin, function): + '''return alternative function number for a pin''' + import importlib + try: + lib = importlib.import_module(mcu) + alt_map = lib.AltFunction_map + except ImportError: + print("Unable to find module for MCU %s" % mcu) + sys.exit(1) + + af_labels = ['USART', 'UART', 'SPI', 'I2C', 'SDIO', 'OTG', 'JT', 'TIM'] + for l in af_labels: + if function.startswith(l): + s = pin+":"+function + if not s in alt_map: + print("Unknown pin function %s for MCU %s" % (s, mcu)) + sys.exit(1) + return alt_map[s] + return None + +class generic_pin(object): + '''class to hold pin definition''' + def __init__(self, port, pin, label, type, extra): + self.port = port + self.pin = pin + self.label = label + self.type = type + self.extra = extra + self.af = None + + def has_extra(self, v): + return v in self.extra + + def get_MODER(self): + '''return one of ALTERNATE, OUTPUT, ANALOG, INPUT''' + if self.af is not None: + v = "ALTERNATE" + elif self.type == 'OUTPUT': + v = "OUTPUT" + elif self.type.startswith('ADC'): + v = "ANALOG" + elif self.has_extra("CS"): + v = "OUTPUT" + else: + v = "INPUT" + return "PIN_MODE_%s(%uU)" % (v, self.pin) + + def get_OTYPER(self): + '''return one of PUSHPULL, OPENDRAIN''' + v = 'PUSHPULL' + if self.type.startswith('I2C'): + # default I2C to OPENDRAIN + v = 'OPENDRAIN' + values = ['PUSHPULL', 'OPENDRAIN'] + for e in self.extra: + if e in values: + v = e + return "PIN_OTYPE_%s(%uU)" % (v, self.pin) + + def get_OSPEEDR(self): + '''return one of SPEED_VERYLOW, SPEED_LOW, SPEED_MEDIUM, SPEED_HIGH''' + values = ['SPEED_VERYLOW', 'SPEED_LOW', 'SPEED_MEDIUM', 'SPEED_HIGH'] + v = 'SPEED_HIGH' + if self.has_extra("CS"): + v = "SPEED_MEDIUM" + if self.type.startswith("I2C"): + v = "SPEED_MEDIUM" + for e in self.extra: + if e in values: + v = e + return "PIN_O%s(%uU)" % (v, self.pin) + + def get_PUPDR(self): + '''return one of FLOATING, PULLUP, PULLDOWN''' + values = ['FLOATING', 'PULLUP', 'PULLDOWN'] + v = 'FLOATING' + if self.has_extra("CS"): + v = "PULLUP" + for e in self.extra: + if e in values: + v = e + return "PIN_PUPDR_%s(%uU)" % (v, self.pin) + + def get_ODR(self): + '''return one of LOW, HIGH''' + values = ['LOW', 'HIGH'] + v = 'HIGH' + for e in self.extra: + if e in values: + v = e + return "PIN_ODR_%s(%uU)" % (v, self.pin) + + def get_AFIO(self): + '''return AFIO''' + af = self.af + if af is None: + af = 0 + return "PIN_AFIO_AF(%uU, %uU)" % (self.pin, af) + + def get_AFRL(self): + '''return AFIO low 8''' + if self.pin >= 8: + return None + return self.get_AFIO() + + def get_AFRH(self): + '''return AFIO high 8''' + if self.pin < 8: + return None + return self.get_AFIO() + + def __str__(self): + afstr = '' + if self.af is not None: + afstr = " AF%u" % self.af + return "P%s%u %s %s%s" % (self.port, self.pin, self.label, self.type, afstr) + +# setup default as input pins +for port in ports: + portmap[port] = [] + for pin in range(pincount[port]): + portmap[port].append(generic_pin(port, pin, None, 'INPUT', [])) + +def get_config(name, column=0, required=True, default=None): + '''get a value from config dictionary''' + if not name in config: + if required and default is None: + print("Error: missing required value %s in hwdef.dat" % name) + sys.exit(1) + return default + if len(config[name]) < column+1: + print("Error: missing required value %s in hwdef.dat (column %u)" % (name, column)) + sys.exit(1) + return config[name][column] + +def process_line(line): + '''process one line of pin definition file''' + a = shlex.split(line) + # keep all config lines for later use + config[a[0]] = a[1:] + if a[0] == 'MCU': + global mcu_type + mcu_type = a[2] + if a[0].startswith('P') and a[0][1] in ports: + # it is a port/pin definition + try: + port = a[0][1] + pin = int(a[0][2:]) + label = a[1] + type = a[2] + extra = a[3:] + except Exception: + print("Bad pin line: %s" % a) + return + + p = generic_pin(port, pin, label, type, extra) + portmap[port][pin] = p + allpins.append(p) + if not type in bytype: + bytype[type] = [] + bytype[type].append(p) + af = get_alt_function(mcu_type, a[0], label) + if af is not None: + p.af = af + + +def write_mcu_config(f): + '''write MCU config defines''' + f.write('// MCU type (ChibiOS define)\n') + f.write('#define %s_MCUCONF\n' % get_config('MCU')) + f.write('#define %s\n\n' % get_config('MCU', 1)) + f.write('// Board voltage. Required for performance limits calculation\n') + f.write('#define STM32_VDD %s\n\n' % get_config('STM32_VDD')) + f.write('// crystal frequency\n') + f.write('#define STM32_HSECLK %sU\n\n' % get_config('OSCILLATOR_HZ')) + f.write('// UART used for stdout (printf)\n') + f.write('#define HAL_STDOUT_SERIAL %s\n\n' % get_config('STDOUT_SERIAL')) + f.write('// baudrate used for stdout (printf)\n') + f.write('#define HAL_STDOUT_BAUDRATE %s\n\n' % get_config('STDOUT_BAUDRATE')) + if 'SDIO' in bytype: + f.write('// SDIO available, enable POSIX filesystem support\n') + f.write('#define USE_POSIX\n\n') + + +def write_USB_config(f): + '''write USB config defines''' + if not 'USB_VENDOR' in config: + return + f.write('// USB configuration\n') + f.write('#define HAL_USB_VENDOR_ID %s\n' % get_config('USB_VENDOR')) + f.write('#define HAL_USB_PRODUCT_ID %s\n' % get_config('USB_PRODUCT')) + for s in ['USB_STRING_MANUFACTURER', 'USB_STRING_PRODUCT', 'USB_STRING_SERIAL']: + f.write('#define HAL_%s "%s"\n' % (s, get_config(s))) + + f.write('\n\n') + +def write_prototype_file(): + '''write the prototype file for apj generation''' + pf = open(os.path.join(outdir, "apj.prototype"), "w") + pf.write('''{ + "board_id": %s, + "magic": "PX4FWv1", + "description": "Firmware for the %s board", + "image": "", + "build_time": 0, + "summary": "PX4FMUv3", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} +''' % (get_config('APJ_BOARD_ID'), + get_config('APJ_BOARD_TYPE', default=mcu_type))) + +def write_peripheral_enable(f): + '''write peripheral enable lines''' + f.write('// peripherals enabled\n') + for type in sorted(bytype.keys()): + if type.startswith('USART') or type.startswith('UART'): + f.write('#define STM32_SERIAL_USE_%-6s TRUE\n' % type) + if type.startswith('SPI'): + f.write('#define STM32_SPI_USE_%s TRUE\n' % type) + if type.startswith('OTG'): + f.write('#define STM32_USB_USE_%s TRUE\n' % type) + if type.startswith('I2C'): + f.write('#define STM32_I2C_USE_%s TRUE\n' % type) + +def write_hwdef_header(outfilename): + '''write hwdef header file''' + print("Writing hwdef setup in %s" % outfilename) + f = open(outfilename, 'w') + + f.write('''/* + generated hardware definitions from hwdef.dat - DO NOT EDIT +*/ + +#pragma once + +'''); + + write_mcu_config(f) + write_USB_config(f) + + write_peripheral_enable(f) + write_prototype_file() + + dma_resolver.write_dma_header(f, periph_list, mcu_type) + + f.write(''' +/* + * I/O ports initial setup, this configuration is established soon after reset + * in the initialization code. + * Please refer to the STM32 Reference Manual for details. + */ +#define PIN_MODE_INPUT(n) (0U << ((n) * 2U)) +#define PIN_MODE_OUTPUT(n) (1U << ((n) * 2U)) +#define PIN_MODE_ALTERNATE(n) (2U << ((n) * 2U)) +#define PIN_MODE_ANALOG(n) (3U << ((n) * 2U)) +#define PIN_ODR_LOW(n) (0U << (n)) +#define PIN_ODR_HIGH(n) (1U << (n)) +#define PIN_OTYPE_PUSHPULL(n) (0U << (n)) +#define PIN_OTYPE_OPENDRAIN(n) (1U << (n)) +#define PIN_OSPEED_VERYLOW(n) (0U << ((n) * 2U)) +#define PIN_OSPEED_LOW(n) (1U << ((n) * 2U)) +#define PIN_OSPEED_MEDIUM(n) (2U << ((n) * 2U)) +#define PIN_OSPEED_HIGH(n) (3U << ((n) * 2U)) +#define PIN_PUPDR_FLOATING(n) (0U << ((n) * 2U)) +#define PIN_PUPDR_PULLUP(n) (1U << ((n) * 2U)) +#define PIN_PUPDR_PULLDOWN(n) (2U << ((n) * 2U)) +#define PIN_AFIO_AF(n, v) ((v) << (((n) % 8U) * 4U)) + +''') + + for port in sorted(ports): + f.write("/* PORT%s:\n" % port) + for pin in range(pincount[port]): + p = portmap[port][pin] + if p.label is not None: + f.write(" %s\n" % p) + f.write("*/\n\n") + + if pincount[port] == 0: + # handle blank ports + for vtype in vtypes: + f.write("#define VAL_GPIO%s_%-7s 0x0\n" % (port, vtype)) + f.write("\n\n\n") + continue + + for vtype in vtypes: + f.write("#define VAL_GPIO%s_%-7s (" % (p.port, vtype)) + first = True + for pin in range(pincount[port]): + p = portmap[port][pin] + modefunc = getattr(p, "get_" + vtype) + v = modefunc() + if v is None: + continue + if not first: + f.write(" | \\\n ") + f.write(v) + first = False + if first: + # there were no pin definitions, use 0 + f.write("0") + f.write(")\n\n") + +def build_peripheral_list(): + '''build a list of peripherals for DMA resolver to work on''' + peripherals = [] + done = set() + prefixes = ['SPI', 'USART', 'UART', 'I2C'] + for p in allpins: + type = p.type + if type in done: + continue + for prefix in prefixes: + if type.startswith(prefix): + peripherals.append(type + "_TX") + peripherals.append(type + "_RX") + if type.startswith('ADC'): + peripherals.append(type) + if type.startswith('SDIO'): + peripherals.append(type) + done.add(type) + return peripherals + +# process input file +hwdef_file = args.hwdef + +f = open(hwdef_file,"r") +for line in f.readlines(): + line = line.strip() + if len(line) == 0 or line[0] == '#': + continue + process_line(line) + +outdir = args.outdir +if outdir is None: + outdir = os.path.dirname(hwdef_file) + +if not "MCU" in config: + print("Missing MCU type in config") + sys.exit(1) + +mcu_type = get_config('MCU',1) +print("Setup for MCU %s" % mcu_type) + +# build a list for peripherals for DMA resolver +periph_list = build_peripheral_list() + +# write out hwdef.h +write_hwdef_header(os.path.join(outdir, "hwdef.h")) + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_parse.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_parse.py new file mode 100755 index 0000000000..312643e712 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_parse.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python +''' +extra DMA mapping tables from a stm32 datasheet + +This assumes a csv file extracted from the datasheet using tablula: + https://github.com/tabulapdf/tabula + +''' + +import sys, csv, os + +def parse_dma_table(fname, table): + dma_num = 1 + csvt = csv.reader(open(fname,'rb')) + i = 0 + last_channel = -1 + for row in csvt: + if len(row) > 1 and row[1].startswith('Channel '): + row = row[1:] + if not row[0].startswith('Channel '): + continue + channel = int(row[0].split(' ')[1]) + if channel < last_channel: + dma_num += 1 + last_channel = channel + for stream in range(8): + s = row[stream+1] + s = s.replace('_\r', '_') + s = s.replace('\r_', '_') + if s == '-': + continue + keys = s.split() + for k in keys: + brace = k.find('(') + if brace != -1: + k = k[:brace] + if k not in table: + table[k] = [] + table[k] += [(dma_num, stream)] + +table = {} + +if len(sys.argv) != 2: + print("Error: expected a CSV files and output file") + sys.exit(1) + +parse_dma_table(sys.argv[1], table) + +sys.stdout.write("DMA_map = {\n"); +sys.stdout.write('\t# format is [DMA_TABLE, StreamNum]\n') +sys.stdout.write('\t# extracted from %sn' % os.path.basename(sys.argv[1])) + +for k in sorted(table.iterkeys()): + s = '"%s"' % k + sys.stdout.write('\t%-10s\t:\t[' % s) + for i in range(len(table[k])): + sys.stdout.write("(%u,%u)" % (table[k][i][0], table[k][i][1])) + if i < len(table[k])-1: + sys.stdout.write(",") + sys.stdout.write("],\n") +sys.stdout.write("}\n"); diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py new file mode 100755 index 0000000000..d31c7be227 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python + +import sys, fnmatch +import importlib + +# peripheral types that can be shared, wildcard patterns +SHARED_MAP = [ "I2C*", "USART*_TX", "UART*_TX", "SPI*" ] + +ignore_list = [] +dma_map = None + +debug = False + +def check_possibility(periph, dma_stream, curr_dict, dma_map, check_list): + for other_periph in curr_dict: + if other_periph != periph: + if curr_dict[other_periph] == dma_stream: + ignore_list.append(periph) + check_str = "%s(%d,%d) %s(%d,%d)" % ( + other_periph, + curr_dict[other_periph][0], + curr_dict[other_periph][1], + periph, + dma_stream[0], + dma_stream[1]) + #check if we did this before + if check_str in check_list: + return False + check_list.append(check_str) + if debug: + print("Trying to Resolve Conflict: ", check_str) + #check if we can resolve by swapping with other periphs + for stream in dma_map[other_periph]: + if stream != curr_dict[other_periph] and \ + check_possibility(other_periph, stream, curr_dict, dma_map, check_list): + curr_dict[other_periph] = stream + return True + return False + return True + +def can_share(periph): + '''check if a peripheral is in the SHARED_MAP list''' + for f in SHARED_MAP: + if fnmatch.fnmatch(periph, f): + return True + if debug: + print("%s can't share" % periph) + return False + +def chibios_dma_define_name(key): + '''return define name needed for board.h for ChibiOS''' + if key.startswith('ADC'): + return 'STM32_ADC_%s_DMA_STREAM' % key + elif key.startswith('SPI'): + return 'STM32_SPI_%s_DMA_STREAM' % key + elif key.startswith('I2C'): + return 'STM32_I2C_%s_DMA_STREAM' % key + elif key.startswith('USART'): + return 'STM32_UART_%s_DMA_STREAM' % key + elif key.startswith('UART'): + return 'STM32_UART_%s_DMA_STREAM' % key + elif key.startswith('SDIO'): + return 'STM32_SDC_%s_DMA_STREAM' % key + else: + print("Error: Unknown key type %s" % key) + sys.exit(1) + +def write_dma_header(f, peripheral_list, mcu_type): + '''write out a DMA resolver header file''' + global dma_map + try: + lib = importlib.import_module(mcu_type) + dma_map = lib.DMA_Map + except ImportError: + print("Unable to find module for MCU %s" % mcu_type) + sys.exit(1) + + print("Writing DMA map") + unassigned = [] + curr_dict = {} + + for periph in peripheral_list: + assigned = False + check_list = [] + if not periph in dma_map: + print("Unknown peripheral function %s in DMA map for %s" % (periph, mcu_type)) + sys.exit(1) + for stream in dma_map[periph]: + if check_possibility(periph, stream, curr_dict, dma_map, check_list): + curr_dict[periph] = stream + assigned = True + break + if assigned == False: + unassigned.append(periph) + + # now look for shared DMA possibilities + stream_assign = {} + for k in curr_dict.iterkeys(): + stream_assign[curr_dict[k]] = [k] + + unassigned_new = unassigned[:] + for periph in unassigned: + for stream in dma_map[periph]: + share_ok = True + for periph2 in stream_assign[stream]: + if not can_share(periph) or not can_share(periph2): + share_ok = False + if share_ok: + if debug: + print("Sharing %s on %s with %s" % (periph, stream, stream_assign[stream])) + curr_dict[periph] = stream + stream_assign[stream].append(periph) + unassigned_new.remove(periph) + break + unassigned = unassigned_new + + + f.write("// auto-generated DMA mapping from dma_resolver.py\n") + f.write('\n#pragma once\n\n') + + + if unassigned: + f.write("\n// Note: The following peripherals can't be resolved for DMA: %s\n\n" % unassigned) + + for key in sorted(curr_dict.iterkeys()): + stream = curr_dict[key] + shared = '' + if len(stream_assign[stream]) > 1: + shared = ' // shared %s' % ','.join(stream_assign[stream]) + f.write("#define %-30s STM32_DMA_STREAM_ID(%u, %u)%s\n" % (chibios_dma_define_name(key), + curr_dict[key][0], + curr_dict[key][1], + shared)) + + # now generate UARTDriver.cpp config lines + f.write("\n\n// generated UART configuration lines\n") + for u in range(1,9): + key = None + if 'USART%u_TX' % u in peripheral_list: + key = 'USART%u' % u + if 'UART%u_TX' % u in peripheral_list: + key = 'UART%u' % u + if 'USART%u_RX' % u in peripheral_list: + key = 'USART%u' % u + if 'UART%u_RX' % u in peripheral_list: + key = 'UART%u' % u + if key is None: + continue + f.write("#define %s_CONFIG { (BaseSequentialStream*) &SD%u, false, " % (key, u)) + if key + "_RX" in curr_dict: + f.write("true, STM32_UART_%s_RX_DMA_STREAM, STM32_%s_RX_DMA_CHN, " % (key, key)) + else: + f.write("false, 0, 0, ") + if key + "_TX" in curr_dict: + f.write("true, STM32_UART_%s_TX_DMA_STREAM, STM32_%s_TX_DMA_CHN}\n" % (key, key)) + else: + f.write("false, 0, 0}\n") + + +if __name__ == '__main__': + import optparse + + parser = optparse.OptionParser("dma_resolver.py") + parser.add_option("-M", "--mcu", default=None, help='MCU type') + parser.add_option("-D", "--debug", action='store_true', help='enable debug') + parser.add_option("-P", "--peripherals", default=None, help='peripheral list (comma separated)') + + opts, args = parser.parse_args() + + if opts.peripherals is None: + print("Please provide a peripheral list with -P") + sys.exit(1) + + if opts.mcu is None: + print("Please provide a MCU type with -<") + sys.exit(1) + + debug = opts.debug + + plist = opts.peripherals.split(',') + mcu_type = opts.mcu + + f = open("dma.h", "w") + write_dma_header(f, plist, mcu_type) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/board.c b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/board.c new file mode 100644 index 0000000000..9aa7aa6cc0 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/board.c @@ -0,0 +1,154 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include "hal.h" +#include "hwdef.h" + +#if HAL_USE_PAL || defined(__DOXYGEN__) +/** + * @brief PAL setup. + * @details Digital I/O ports static configuration as defined in @p board.h. + * This variable is used by the HAL when initializing the PAL driver. + */ +const PALConfig pal_default_config = { +#if STM32_HAS_GPIOA + {VAL_GPIOA_MODER, VAL_GPIOA_OTYPER, VAL_GPIOA_OSPEEDR, VAL_GPIOA_PUPDR, + VAL_GPIOA_ODR, VAL_GPIOA_AFRL, VAL_GPIOA_AFRH}, +#endif +#if STM32_HAS_GPIOB + {VAL_GPIOB_MODER, VAL_GPIOB_OTYPER, VAL_GPIOB_OSPEEDR, VAL_GPIOB_PUPDR, + VAL_GPIOB_ODR, VAL_GPIOB_AFRL, VAL_GPIOB_AFRH}, +#endif +#if STM32_HAS_GPIOC + {VAL_GPIOC_MODER, VAL_GPIOC_OTYPER, VAL_GPIOC_OSPEEDR, VAL_GPIOC_PUPDR, + VAL_GPIOC_ODR, VAL_GPIOC_AFRL, VAL_GPIOC_AFRH}, +#endif +#if STM32_HAS_GPIOD + {VAL_GPIOD_MODER, VAL_GPIOD_OTYPER, VAL_GPIOD_OSPEEDR, VAL_GPIOD_PUPDR, + VAL_GPIOD_ODR, VAL_GPIOD_AFRL, VAL_GPIOD_AFRH}, +#endif +#if STM32_HAS_GPIOE + {VAL_GPIOE_MODER, VAL_GPIOE_OTYPER, VAL_GPIOE_OSPEEDR, VAL_GPIOE_PUPDR, + VAL_GPIOE_ODR, VAL_GPIOE_AFRL, VAL_GPIOE_AFRH}, +#endif +#if STM32_HAS_GPIOF + {VAL_GPIOF_MODER, VAL_GPIOF_OTYPER, VAL_GPIOF_OSPEEDR, VAL_GPIOF_PUPDR, + VAL_GPIOF_ODR, VAL_GPIOF_AFRL, VAL_GPIOF_AFRH}, +#endif +#if STM32_HAS_GPIOG + {VAL_GPIOG_MODER, VAL_GPIOG_OTYPER, VAL_GPIOG_OSPEEDR, VAL_GPIOG_PUPDR, + VAL_GPIOG_ODR, VAL_GPIOG_AFRL, VAL_GPIOG_AFRH}, +#endif +#if STM32_HAS_GPIOH + {VAL_GPIOH_MODER, VAL_GPIOH_OTYPER, VAL_GPIOH_OSPEEDR, VAL_GPIOH_PUPDR, + VAL_GPIOH_ODR, VAL_GPIOH_AFRL, VAL_GPIOH_AFRH}, +#endif +#if STM32_HAS_GPIOI + {VAL_GPIOI_MODER, VAL_GPIOI_OTYPER, VAL_GPIOI_OSPEEDR, VAL_GPIOI_PUPDR, + VAL_GPIOI_ODR, VAL_GPIOI_AFRL, VAL_GPIOI_AFRH} +#endif +#if STM32_HAS_GPIOJ + {VAL_GPIOJ_MODER, VAL_GPIOJ_OTYPER, VAL_GPIOJ_OSPEEDR, VAL_GPIOJ_PUPDR, + VAL_GPIOJ_ODR, VAL_GPIOJ_AFRL, VAL_GPIOJ_AFRH}, +#endif +#if STM32_HAS_GPIOK + {VAL_GPIOK_MODER, VAL_GPIOK_OTYPER, VAL_GPIOK_OSPEEDR, VAL_GPIOK_PUPDR, + VAL_GPIOK_ODR, VAL_GPIOK_AFRL, VAL_GPIOK_AFRH} +#endif +}; +#endif + +/** + * @brief Early initialization code. + * @details This initialization must be performed just after stack setup + * and before any other initialization. + */ +void __early_init(void) { + + stm32_clock_init(); +} + +void __late_init(void) { + halInit(); + chSysInit(); +} + +#if HAL_USE_SDC || defined(__DOXYGEN__) +/** + * @brief SDC card detection. + */ +bool sdc_lld_is_card_inserted(SDCDriver *sdcp) { + + (void)sdcp; + /* TODO: Fill the implementation.*/ + return true; +} + +/** + * @brief SDC card write protection detection. + */ +bool sdc_lld_is_write_protected(SDCDriver *sdcp) { + + (void)sdcp; + /* TODO: Fill the implementation.*/ + return false; +} +#endif /* HAL_USE_SDC */ + +#if HAL_USE_MMC_SPI || defined(__DOXYGEN__) +/** + * @brief MMC_SPI card detection. + */ +bool mmc_lld_is_card_inserted(MMCDriver *mmcp) { + + (void)mmcp; + /* TODO: Fill the implementation.*/ + return true; +} + +/** + * @brief MMC_SPI card write protection detection. + */ +bool mmc_lld_is_write_protected(MMCDriver *mmcp) { + + (void)mmcp; + /* TODO: Fill the implementation.*/ + return false; +} +#endif + +/** + * @brief Board-specific initialization code. + * @todo Add your board-specific code, if any. + */ +void boardInit(void) +{ +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/board.h b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/board.h new file mode 100644 index 0000000000..afa6ec68eb --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/board.h @@ -0,0 +1,60 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ +#ifndef _BOARD_H_ +#define _BOARD_H_ + +/* + * Setup for STMicroelectronics STM32 F412 SkyViper board. + */ + +#include "hwdef.h" + +/* + * APM HW Defines + */ + +#define HRT_TIMER GPTD5 + +#define PPM_ICU_TIMER ICUD1 +#define PPM_ICU_CHANNEL ICU_CHANNEL_1 + +#if !defined(_FROM_ASM_) +#ifdef __cplusplus +extern "C" { +#endif + void boardInit(void); +#ifdef __cplusplus +} +#endif +#endif /* _FROM_ASM_ */ + +#endif /* _BOARD_H_ */ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/chconf.h b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/chconf.h new file mode 100644 index 0000000000..2736788bb2 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/chconf.h @@ -0,0 +1,533 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ +/** + * @file templates/chconf.h + * @brief Configuration file template. + * @details A copy of this file must be placed in each project directory, it + * contains the application specific kernel settings. + * + * @addtogroup config + * @details Kernel related settings and hooks. + * @{ + */ + +#pragma once + +#define _CHIBIOS_RT_CONF_ + +/*===========================================================================*/ +/** + * @name System timers settings + * @{ + */ +/*===========================================================================*/ + +/** + * @brief System time counter resolution. + * @note Allowed values are 16 or 32 bits. + */ +#define CH_CFG_ST_RESOLUTION 32 + +/** + * @brief System tick frequency. + * @details Frequency of the system timer that drives the system ticks. This + * setting also defines the system tick time unit. + */ +#define CH_CFG_ST_FREQUENCY 10000 + +/** + * @brief Time delta constant for the tick-less mode. + * @note If this value is zero then the system uses the classic + * periodic tick. This value represents the minimum number + * of ticks that is safe to specify in a timeout directive. + * The value one is not valid, timeouts are rounded up to + * this value. + */ +#define CH_CFG_ST_TIMEDELTA 2 + +/** @} */ + +/*===========================================================================*/ +/** + * @name Kernel parameters and options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief Round robin interval. + * @details This constant is the number of system ticks allowed for the + * threads before preemption occurs. Setting this value to zero + * disables the preemption for threads with equal priority and the + * round robin becomes cooperative. Note that higher priority + * threads can still preempt, the kernel is always preemptive. + * @note Disabling the round robin preemption makes the kernel more compact + * and generally faster. + * @note The round robin preemption is not supported in tickless mode and + * must be set to zero in that case. + */ +#define CH_CFG_TIME_QUANTUM 0 + +/** + * @brief Managed RAM size. + * @details Size of the RAM area to be managed by the OS. If set to zero + * then the whole available RAM is used. The core memory is made + * available to the heap allocator and/or can be used directly through + * the simplified core memory allocator. + * + * @note In order to let the OS manage the whole RAM the linker script must + * provide the @p __heap_base__ and @p __heap_end__ symbols. + * @note Requires @p CH_CFG_USE_MEMCORE. + */ +#define CH_CFG_MEMCORE_SIZE 0 + +/** + * @brief Idle thread automatic spawn suppression. + * @details When this option is activated the function @p chSysInit() + * does not spawn the idle thread. The application @p main() + * function becomes the idle thread and must implement an + * infinite loop. + */ +#define CH_CFG_NO_IDLE_THREAD FALSE + +/** @} */ + +/*===========================================================================*/ +/** + * @name Performance options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief OS optimization. + * @details If enabled then time efficient rather than space efficient code + * is used when two possible implementations exist. + * + * @note This is not related to the compiler optimization options. + * @note The default is @p TRUE. + */ +#define CH_CFG_OPTIMIZE_SPEED TRUE + +/** @} */ + +/*===========================================================================*/ +/** + * @name Subsystem options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief Time Measurement APIs. + * @details If enabled then the time measurement APIs are included in + * the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_TM TRUE + +/** + * @brief Threads registry APIs. + * @details If enabled then the registry APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_REGISTRY TRUE + +/** + * @brief Threads synchronization APIs. + * @details If enabled then the @p chThdWait() function is included in + * the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_WAITEXIT TRUE + +/** + * @brief Semaphores APIs. + * @details If enabled then the Semaphores APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_SEMAPHORES TRUE + +/** + * @brief Semaphores queuing mode. + * @details If enabled then the threads are enqueued on semaphores by + * priority rather than in FIFO order. + * + * @note The default is @p FALSE. Enable this if you have special + * requirements. + * @note Requires @p CH_CFG_USE_SEMAPHORES. + */ +#define CH_CFG_USE_SEMAPHORES_PRIORITY FALSE + +/** + * @brief Mutexes APIs. + * @details If enabled then the mutexes APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_MUTEXES TRUE + +/** + * @brief Enables recursive behavior on mutexes. + * @note Recursive mutexes are heavier and have an increased + * memory footprint. + * + * @note The default is @p FALSE. + * @note Requires @p CH_CFG_USE_MUTEXES. + */ +#define CH_CFG_USE_MUTEXES_RECURSIVE FALSE + +/** + * @brief Conditional Variables APIs. + * @details If enabled then the conditional variables APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_CFG_USE_MUTEXES. + */ +#define CH_CFG_USE_CONDVARS TRUE + +/** + * @brief Conditional Variables APIs with timeout. + * @details If enabled then the conditional variables APIs with timeout + * specification are included in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_CFG_USE_CONDVARS. + */ +#define CH_CFG_USE_CONDVARS_TIMEOUT TRUE + +/** + * @brief Events Flags APIs. + * @details If enabled then the event flags APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_EVENTS TRUE + +/** + * @brief Events Flags APIs with timeout. + * @details If enabled then the events APIs with timeout specification + * are included in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_CFG_USE_EVENTS. + */ +#define CH_CFG_USE_EVENTS_TIMEOUT TRUE + +/** + * @brief Synchronous Messages APIs. + * @details If enabled then the synchronous messages APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_MESSAGES TRUE + +/** + * @brief Synchronous Messages queuing mode. + * @details If enabled then messages are served by priority rather than in + * FIFO order. + * + * @note The default is @p FALSE. Enable this if you have special + * requirements. + * @note Requires @p CH_CFG_USE_MESSAGES. + */ +#define CH_CFG_USE_MESSAGES_PRIORITY FALSE + +/** + * @brief Mailboxes APIs. + * @details If enabled then the asynchronous messages (mailboxes) APIs are + * included in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_CFG_USE_SEMAPHORES. + */ +#define CH_CFG_USE_MAILBOXES TRUE + +/** + * @brief Core Memory Manager APIs. + * @details If enabled then the core memory manager APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_MEMCORE TRUE + +/** + * @brief Heap Allocator APIs. + * @details If enabled then the memory heap allocator APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_CFG_USE_MEMCORE and either @p CH_CFG_USE_MUTEXES or + * @p CH_CFG_USE_SEMAPHORES. + * @note Mutexes are recommended. + */ +#define CH_CFG_USE_HEAP TRUE + +/** + * @brief Memory Pools Allocator APIs. + * @details If enabled then the memory pools allocator APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + */ +#define CH_CFG_USE_MEMPOOLS TRUE + +/** + * @brief Dynamic Threads APIs. + * @details If enabled then the dynamic threads creation APIs are included + * in the kernel. + * + * @note The default is @p TRUE. + * @note Requires @p CH_CFG_USE_WAITEXIT. + * @note Requires @p CH_CFG_USE_HEAP and/or @p CH_CFG_USE_MEMPOOLS. + */ +#define CH_CFG_USE_DYNAMIC TRUE + +/** @} */ + +/*===========================================================================*/ +/** + * @name Debug options + * @{ + */ +/*===========================================================================*/ + +/** + * @brief Debug option, kernel statistics. + * + * @note The default is @p FALSE. + */ +#define CH_DBG_STATISTICS TRUE + +/** + * @brief Debug option, system state check. + * @details If enabled the correct call protocol for system APIs is checked + * at runtime. + * + * @note The default is @p FALSE. + */ +#define CH_DBG_SYSTEM_STATE_CHECK TRUE + +/** + * @brief Debug option, parameters checks. + * @details If enabled then the checks on the API functions input + * parameters are activated. + * + * @note The default is @p FALSE. + */ +#define CH_DBG_ENABLE_CHECKS TRUE + +/** + * @brief Debug option, consistency checks. + * @details If enabled then all the assertions in the kernel code are + * activated. This includes consistency checks inside the kernel, + * runtime anomalies and port-defined checks. + * + * @note The default is @p FALSE. + */ +#define CH_DBG_ENABLE_ASSERTS TRUE + +/** + * @brief Debug option, trace buffer. + * @details If enabled then the trace buffer is activated. + * + * @note The default is @p CH_DBG_TRACE_MASK_DISABLED. + */ +#define CH_DBG_TRACE_MASK CH_DBG_TRACE_MASK_NONE + +/** + * @brief Trace buffer entries. + * @note The trace buffer is only allocated if @p CH_DBG_TRACE_MASK is + * different from @p CH_DBG_TRACE_MASK_DISABLED. + */ +#define CH_DBG_TRACE_BUFFER_SIZE 128 + +/** + * @brief Debug option, stack checks. + * @details If enabled then a runtime stack check is performed. + * + * @note The default is @p FALSE. + * @note The stack check is performed in a architecture/port dependent way. + * It may not be implemented or some ports. + * @note The default failure mode is to halt the system with the global + * @p panic_msg variable set to @p NULL. + */ +#define CH_DBG_ENABLE_STACK_CHECK TRUE + +/** + * @brief Debug option, stacks initialization. + * @details If enabled then the threads working area is filled with a byte + * value when a thread is created. This can be useful for the + * runtime measurement of the used stack. + * + * @note The default is @p FALSE. + */ +#define CH_DBG_FILL_THREADS TRUE + +/** + * @brief Debug option, threads profiling. + * @details If enabled then a field is added to the @p thread_t structure that + * counts the system ticks occurred while executing the thread. + * + * @note The default is @p FALSE. + * @note This debug option is not currently compatible with the + * tickless mode. + */ +#define CH_DBG_THREADS_PROFILING FALSE + +/** @} */ + +/*===========================================================================*/ +/** + * @name Kernel hooks + * @{ + */ +/*===========================================================================*/ + +/** + * @brief Threads descriptor structure extension. + * @details User fields added to the end of the @p thread_t structure. + */ +#define CH_CFG_THREAD_EXTRA_FIELDS \ + /* Add threads custom fields here.*/ + +/** + * @brief Threads initialization hook. + * @details User initialization code added to the @p chThdInit() API. + * + * @note It is invoked from within @p chThdInit() and implicitly from all + * the threads creation APIs. + */ +#define CH_CFG_THREAD_INIT_HOOK(tp) { \ + /* Add threads initialization code here.*/ \ +} + +/** + * @brief Threads finalization hook. + * @details User finalization code added to the @p chThdExit() API. + */ +#define CH_CFG_THREAD_EXIT_HOOK(tp) { \ + /* Add threads finalization code here.*/ \ +} + +/** + * @brief Context switch hook. + * @details This hook is invoked just before switching between threads. + */ +#define CH_CFG_CONTEXT_SWITCH_HOOK(ntp, otp) { \ + /* Context switch code here.*/ \ +} + +/** + * @brief ISR enter hook. + */ +#define CH_CFG_IRQ_PROLOGUE_HOOK() { \ + /* IRQ prologue code here.*/ \ +} + +/** + * @brief ISR exit hook. + */ +#define CH_CFG_IRQ_EPILOGUE_HOOK() { \ + /* IRQ epilogue code here.*/ \ +} + +/** + * @brief Idle thread enter hook. + * @note This hook is invoked within a critical zone, no OS functions + * should be invoked from here. + * @note This macro can be used to activate a power saving mode. + */ +#define CH_CFG_IDLE_ENTER_HOOK() { \ + /* Idle-enter code here.*/ \ +} + +/** + * @brief Idle thread leave hook. + * @note This hook is invoked within a critical zone, no OS functions + * should be invoked from here. + * @note This macro can be used to deactivate a power saving mode. + */ +#define CH_CFG_IDLE_LEAVE_HOOK() { \ + /* Idle-leave code here.*/ \ +} + +/** + * @brief Idle Loop hook. + * @details This hook is continuously invoked by the idle thread loop. + */ +#define CH_CFG_IDLE_LOOP_HOOK() { \ + /* Idle loop code here.*/ \ +} + +/** + * @brief System tick event hook. + * @details This hook is invoked in the system tick handler immediately + * after processing the virtual timers queue. + */ +#define CH_CFG_SYSTEM_TICK_HOOK() { \ + /* System tick event code here.*/ \ +} + +/** + * @brief System halt hook. + * @details This hook is invoked in case to a system halting error before + * the system is halted. + */ +#define CH_CFG_SYSTEM_HALT_HOOK(reason) { \ + /* System halt code here.*/ \ +} + +/** + * @brief Trace hook. + * @details This hook is invoked each time a new record is written in the + * trace buffer. + */ +#define CH_CFG_TRACE_HOOK(tep) { \ + /* Trace code here.*/ \ +} + +/** @} */ + +/*===========================================================================*/ +/* Port-specific settings (override port settings defaulted in chcore.h). */ +/*===========================================================================*/ + + +/** @} */ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/chibios_board.mk b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/chibios_board.mk new file mode 100644 index 0000000000..880cec105d --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/chibios_board.mk @@ -0,0 +1,225 @@ +############################################################################## +# Build global options +# NOTE: Can be overridden externally. +# + +# Compiler options here. +ifeq ($(USE_OPT),) + USE_OPT = -Os -g -fomit-frame-pointer -falign-functions=16 -DCHPRINTF_USE_FLOAT=1 +endif + +# C specific options here (added to USE_OPT). +ifeq ($(USE_COPT),) + USE_COPT = +endif + +# C++ specific options here (added to USE_OPT). +ifeq ($(USE_CPPOPT),) + USE_CPPOPT = -fno-rtti +endif + +# Enable this if you want the linker to remove unused code and data +ifeq ($(USE_LINK_GC),) + USE_LINK_GC = yes +endif + +# Linker extra options here. +ifeq ($(USE_LDOPT),) + USE_LDOPT = +endif + +# Enable this if you want link time optimizations (LTO) +ifeq ($(USE_LTO),) + USE_LTO = no +endif + +# If enabled, this option allows to compile the application in THUMB mode. +ifeq ($(USE_THUMB),) + USE_THUMB = yes +endif + +# Enable this if you want to see the full log while compiling. +ifeq ($(USE_VERBOSE_COMPILE),) + USE_VERBOSE_COMPILE = no +endif + +# If enabled, this option makes the build process faster by not compiling +# modules not used in the current configuration. +ifeq ($(USE_SMART_BUILD),) + USE_SMART_BUILD = no +endif + +# +# Build global options +############################################################################## + +############################################################################## +# Architecture or project specific options +# +HWDEF = $(AP_HAL)/hwdef +# Stack size to be allocated to the Cortex-M process stack. This stack is +# the stack used by the main() thread. +ifeq ($(USE_PROCESS_STACKSIZE),) + USE_PROCESS_STACKSIZE = 0x400 +endif + +# Stack size to the allocated to the Cortex-M main/exceptions stack. This +# stack is used for processing interrupts and exceptions. +ifeq ($(USE_EXCEPTIONS_STACKSIZE),) + USE_EXCEPTIONS_STACKSIZE = 0x400 +endif + +# Enables the use of FPU (no, softfp, hard). +ifeq ($(USE_FPU),) + USE_FPU = hard +endif + +# +# Architecture or project specific options +############################################################################## + +############################################################################## +# Project, sources and paths +# + +# Define project name here +PROJECT = ch + +# Imported source files and paths +# Startup files. +include $(CHIBIOS)/os/common/startup/ARMCMx/compilers/GCC/mk/startup_stm32f4xx.mk +# HAL-OSAL files (optional). +include $(CHIBIOS)/os/hal/hal.mk +include $(CHIBIOS)/os/hal/ports/STM32/STM32F4xx/platform.mk +include $(CHIBIOS)/os/hal/osal/rt/osal.mk +# RTOS files (optional). +include $(CHIBIOS)/os/rt/rt.mk +include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk +# Other files (optional). +#include $(CHIBIOS)/test/rt/test.mk +include $(CHIBIOS)/os/hal/lib/streams/streams.mk +#include $(CHIBIOS)/os/various/fatfs_bindings/fatfs.mk + +VARIOUSSRC = $(STREAMSSRC) + +VARIOUSINC = $(STREAMSINC) + +# C sources that can be compiled in ARM or THUMB mode depending on the global +# setting. +CSRC = $(STARTUPSRC) \ + $(KERNSRC) \ + $(PORTSRC) \ + $(OSALSRC) \ + $(HALSRC) \ + $(PLATFORMSRC) \ + $(VARIOUSSRC) \ + $(HWDEF)/common/stubs.c \ + $(HWDEF)/skyviper-f412/board.c \ + $(HWDEF)/common/ppm.c \ + $(HWDEF)/common/flash.c \ + $(HWDEF)/common/malloc.c \ + $(HWDEF)/common/stdio.c \ + $(HWDEF)/common/hrt.c +# $(HWDEF)/common/usbcfg.c \ +# $(TESTSRC) \ +# test.c + +# C++ sources that can be compiled in ARM or THUMB mode depending on the global +# setting. +CPPSRC = + +# C sources to be compiled in ARM mode regardless of the global setting. +# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler +# option that results in lower performance and larger code size. +ACSRC = + +# C++ sources to be compiled in ARM mode regardless of the global setting. +# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler +# option that results in lower performance and larger code size. +ACPPSRC = + +# C sources to be compiled in THUMB mode regardless of the global setting. +# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler +# option that results in lower performance and larger code size. +TCSRC = + +# C sources to be compiled in THUMB mode regardless of the global setting. +# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler +# option that results in lower performance and larger code size. +TCPPSRC = + +# List ASM source files here +ASMSRC = +ASMXSRC = $(STARTUPASM) $(PORTASM) $(OSALASM) + +INCDIR = $(CHIBIOS)/os/license \ + $(STARTUPINC) $(KERNINC) $(PORTINC) $(OSALINC) \ + $(HALINC) $(PLATFORMINC) $(BOARDINC) $(TESTINC) $(VARIOUSINC) \ + $(HWDEF)/common $(HWDEF)/skyviper-f412 + +# +# Project, sources and paths +############################################################################## + +############################################################################## +# Compiler settings +# + +MCU = cortex-m4 + +#TRGT = arm-elf- +TRGT = arm-none-eabi- +CC = $(TRGT)gcc +CPPC = $(TRGT)g++ +# Enable loading with g++ only if you need C++ runtime support. +# NOTE: You can use C++ even without C++ support if you are careful. C++ +# runtime support makes code size explode. +LD = $(TRGT)gcc +#LD = $(TRGT)g++ +CP = $(TRGT)objcopy +AS = $(TRGT)gcc -x assembler-with-cpp +AR = $(TRGT)ar +OD = $(TRGT)objdump +SZ = $(TRGT)size +HEX = $(CP) -O ihex +BIN = $(CP) -O binary + +# ARM-specific options here +AOPT = + +# THUMB-specific options here +TOPT = -mthumb -DTHUMB + +# Define C warning options here +CWARN = -Wall -Wextra -Wundef -Wstrict-prototypes + +# Define C++ warning options here +CPPWARN = -Wall -Wextra -Wundef + +# +# Compiler settings +############################################################################## + +############################################################################## +# Start of user section +# + +# List all user C define here, like -D_DEBUG=1 +UDEFS = -DBOARD_FLASH_SIZE=1024 + +# Define ASM defines here +UADEFS = + +# List all user directories here +UINCDIR = + +# List the user directory to look for the libraries here +ULIBDIR = + +# List all user libraries here +ULIBS = + +# +# End of user defines +############################################################################## +include $(HWDEF)/common/chibios_common.mk diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/halconf.h b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/halconf.h new file mode 100644 index 0000000000..dc0367cc31 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/halconf.h @@ -0,0 +1,402 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ +/** + * @file templates/halconf.h + * @brief HAL configuration header. + * @details HAL configuration file, this file allows to enable or disable the + * various device drivers from your application. You may also use + * this file in order to override the device drivers default settings. + * + * @addtogroup HAL_CONF + * @{ + */ + +#pragma once +#include "mcuconf.h" + +/** + * @brief Enables the PAL subsystem. + */ +#if !defined(HAL_USE_PAL) || defined(__DOXYGEN__) +#define HAL_USE_PAL TRUE +#endif + +/** + * @brief Enables the ADC subsystem. + */ +#if !defined(HAL_USE_ADC) || defined(__DOXYGEN__) +#define HAL_USE_ADC TRUE +#endif + +/** + * @brief Enables the CAN subsystem. + */ +#if !defined(HAL_USE_CAN) || defined(__DOXYGEN__) +#define HAL_USE_CAN FALSE +#endif + +/** + * @brief Enables the DAC subsystem. + */ +#if !defined(HAL_USE_DAC) || defined(__DOXYGEN__) +#define HAL_USE_DAC FALSE +#endif + +/** + * @brief Enables the EXT subsystem. + */ +#if !defined(HAL_USE_EXT) || defined(__DOXYGEN__) +#define HAL_USE_EXT TRUE +#endif + +/** + * @brief Enables the GPT subsystem. + */ +#if !defined(HAL_USE_GPT) || defined(__DOXYGEN__) +#define HAL_USE_GPT TRUE +#endif + +/** + * @brief Enables the I2C subsystem. + */ +#if !defined(HAL_USE_I2C) || defined(__DOXYGEN__) +#define HAL_USE_I2C TRUE +#endif + +/** + * @brief Enables the I2S subsystem. + */ +#if !defined(HAL_USE_I2S) || defined(__DOXYGEN__) +#define HAL_USE_I2S FALSE +#endif + +/** + * @brief Enables the ICU subsystem. + */ +#if !defined(HAL_USE_ICU) || defined(__DOXYGEN__) +#define HAL_USE_ICU TRUE +#endif + +/** + * @brief Enables the MAC subsystem. + */ +#if !defined(HAL_USE_MAC) || defined(__DOXYGEN__) +#define HAL_USE_MAC FALSE +#endif + +/** + * @brief Enables the MMC_SPI subsystem. + */ +#if !defined(HAL_USE_MMC_SPI) || defined(__DOXYGEN__) +#define HAL_USE_MMC_SPI FALSE +#endif + +/** + * @brief Enables the PWM subsystem. + */ +#if !defined(HAL_USE_PWM) || defined(__DOXYGEN__) +#define HAL_USE_PWM TRUE +#endif + +/** + * @brief Enables the QSPI subsystem. + */ +#if !defined(HAL_USE_QSPI) || defined(__DOXYGEN__) +#define HAL_USE_QSPI FALSE +#endif + +/** + * @brief Enables the RTC subsystem. + */ +#if !defined(HAL_USE_RTC) || defined(__DOXYGEN__) +#define HAL_USE_RTC FALSE +#endif + +/** + * @brief Enables the SDC subsystem. + */ +#if !defined(HAL_USE_SDC) || defined(__DOXYGEN__) +#define HAL_USE_SDC FALSE +#endif + +/** + * @brief Enables the SERIAL subsystem. + */ +#if !defined(HAL_USE_SERIAL) || defined(__DOXYGEN__) +#define HAL_USE_SERIAL TRUE +#endif + +/** + * @brief Enables the SERIAL over USB subsystem. + */ +#if !defined(HAL_USE_SERIAL_USB) || defined(__DOXYGEN__) +#define HAL_USE_SERIAL_USB FALSE +#endif + +/** + * @brief Enables the SPI subsystem. + */ +#if !defined(HAL_USE_SPI) || defined(__DOXYGEN__) +#define HAL_USE_SPI TRUE +#endif + +/** + * @brief Enables the UART subsystem. + */ +#if !defined(HAL_USE_UART) || defined(__DOXYGEN__) +#define HAL_USE_UART FALSE +#endif + +/** + * @brief Enables the USB subsystem. + */ +#if !defined(HAL_USE_USB) || defined(__DOXYGEN__) +#define HAL_USE_USB FALSE +#endif + +/** + * @brief Enables the WDG subsystem. + */ +#if !defined(HAL_USE_WDG) || defined(__DOXYGEN__) +#define HAL_USE_WDG FALSE +#endif + +/*===========================================================================*/ +/* ADC driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables synchronous APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(ADC_USE_WAIT) || defined(__DOXYGEN__) +#define ADC_USE_WAIT TRUE +#endif + +/** + * @brief Enables the @p adcAcquireBus() and @p adcReleaseBus() APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(ADC_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) +#define ADC_USE_MUTUAL_EXCLUSION TRUE +#endif + +/*===========================================================================*/ +/* CAN driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Sleep mode related APIs inclusion switch. + */ +#if !defined(CAN_USE_SLEEP_MODE) || defined(__DOXYGEN__) +#define CAN_USE_SLEEP_MODE TRUE +#endif + +/*===========================================================================*/ +/* I2C driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables the mutual exclusion APIs on the I2C bus. + */ +#if !defined(I2C_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) +#define I2C_USE_MUTUAL_EXCLUSION TRUE +#endif + +/*===========================================================================*/ +/* MAC driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables an event sources for incoming packets. + */ +#if !defined(MAC_USE_ZERO_COPY) || defined(__DOXYGEN__) +#define MAC_USE_ZERO_COPY FALSE +#endif + +/** + * @brief Enables an event sources for incoming packets. + */ +#if !defined(MAC_USE_EVENTS) || defined(__DOXYGEN__) +#define MAC_USE_EVENTS TRUE +#endif + +/*===========================================================================*/ +/* MMC_SPI driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Delays insertions. + * @details If enabled this options inserts delays into the MMC waiting + * routines releasing some extra CPU time for the threads with + * lower priority, this may slow down the driver a bit however. + * This option is recommended also if the SPI driver does not + * use a DMA channel and heavily loads the CPU. + */ +#if !defined(MMC_NICE_WAITING) || defined(__DOXYGEN__) +#define MMC_NICE_WAITING TRUE +#endif + +/*===========================================================================*/ +/* SDC driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Number of initialization attempts before rejecting the card. + * @note Attempts are performed at 10mS intervals. + */ +#if !defined(SDC_INIT_RETRY) || defined(__DOXYGEN__) +#define SDC_INIT_RETRY 100 +#endif + +/** + * @brief Include support for MMC cards. + * @note MMC support is not yet implemented so this option must be kept + * at @p FALSE. + */ +#if !defined(SDC_MMC_SUPPORT) || defined(__DOXYGEN__) +#define SDC_MMC_SUPPORT FALSE +#endif + +/** + * @brief Delays insertions. + * @details If enabled this options inserts delays into the MMC waiting + * routines releasing some extra CPU time for the threads with + * lower priority, this may slow down the driver a bit however. + */ +#if !defined(SDC_NICE_WAITING) || defined(__DOXYGEN__) +#define SDC_NICE_WAITING TRUE +#endif + +/*===========================================================================*/ +/* SERIAL driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Default bit rate. + * @details Configuration parameter, this is the baud rate selected for the + * default configuration. + */ +#if !defined(SERIAL_DEFAULT_BITRATE) || defined(__DOXYGEN__) +#define SERIAL_DEFAULT_BITRATE 38400 +#endif + +/** + * @brief Serial buffers size. + * @details Configuration parameter, you can change the depth of the queue + * buffers depending on the requirements of your application. + * @note The default is 16 bytes for both the transmission and receive + * buffers. + */ +#if !defined(SERIAL_BUFFERS_SIZE) || defined(__DOXYGEN__) +#define SERIAL_BUFFERS_SIZE 1024 +#endif + +/*===========================================================================*/ +/* SERIAL_USB driver related setting. */ +/*===========================================================================*/ + +/** + * @brief Serial over USB buffers size. + * @details Configuration parameter, the buffer size must be a multiple of + * the USB data endpoint maximum packet size. + * @note The default is 256 bytes for both the transmission and receive + * buffers. + */ +#if !defined(SERIAL_USB_BUFFERS_SIZE) || defined(__DOXYGEN__) +#define SERIAL_USB_BUFFERS_SIZE 256 +#endif + +/** + * @brief Serial over USB number of buffers. + * @note The default is 2 buffers. + */ +#if !defined(SERIAL_USB_BUFFERS_NUMBER) || defined(__DOXYGEN__) +#define SERIAL_USB_BUFFERS_NUMBER 2 +#endif + +/*===========================================================================*/ +/* SPI driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables synchronous APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(SPI_USE_WAIT) || defined(__DOXYGEN__) +#define SPI_USE_WAIT TRUE +#endif + +/** + * @brief Enables the @p spiAcquireBus() and @p spiReleaseBus() APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(SPI_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) +#define SPI_USE_MUTUAL_EXCLUSION TRUE +#endif + +/*===========================================================================*/ +/* UART driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables synchronous APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(UART_USE_WAIT) || defined(__DOXYGEN__) +#define UART_USE_WAIT FALSE +#endif + +/** + * @brief Enables the @p uartAcquireBus() and @p uartReleaseBus() APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(UART_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) +#define UART_USE_MUTUAL_EXCLUSION FALSE +#endif + +/*===========================================================================*/ +/* USB driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Enables synchronous APIs. + * @note Disabling this option saves both code and data space. + */ +#if !defined(USB_USE_WAIT) || defined(__DOXYGEN__) +#define USB_USE_WAIT FALSE +#endif + + +/** @} */ + + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/hwdef.dat new file mode 100644 index 0000000000..afd24c12a3 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/hwdef.dat @@ -0,0 +1,67 @@ +# hw definition file for processing by chibios_pins.py +# new F412 layout + +MCU STM32F4xx STM32F412Rx + +# board ID for firmware load +APJ_BOARD_ID 9 + +# crystal frequency +OSCILLATOR_HZ 24000000 + +# board voltage +STM32_VDD 330U + +# serial port for stdout +STDOUT_SERIAL SD1 +STDOUT_BAUDRATE 115200 + +PC0 MGND INPUT +PC1 PWM4_SENSE ADC1 +PC2 PWM2_SENSE ADC1 +PC3 PWM1_SENSE ADC1 +PA0 PWM3_SENSE ADC1 + +# USART2 is for sonix +PA2 USART2_TX USART2 +PA3 USART2_RX USART2 + +# SPI1 is radio +PA4 RADIO_CS SPI1 CS +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 +PC4 RADIO_CE OUTPUT +PC5 RADIO_PA_CTL OUTPUT +PB0 RADIO_IRQ INPUT +PB1 BATT_MON ADC1 +PB2 BOOT1 INPUT +PB10 I2C2_SCL I2C2 +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD +PB9 I2C1_SDA I2C1 +PB8 I2C1_SCL I2C1 +PB7 LEDF OUTPUT HIGH +PB6 LEDR OUTPUT HIGH +PB5 TIM3_CH2 TIM3 +PB3 I2C2_SDA I2C2 +PD2 OF_MOTION INPUT + +# USART6 is for GPS +PA11 USART6_TX USART6 +PC7 USART6_RX USART6 + +# USART1 is for debug console +PA10 USART1_RX USART1 +PA9 USART1_TX USART1 + +PA8 OF_MOTION INPUT +PC9 TIM3_CH4 TIM3 +PC8 TIM3_CH3 TIM3 +PC6 TIM3_CH1 TIM3 + +# SPI2 is optical flow +PB15 SPI2_MOSI SPI2 +PB14 SPI2_MISO SPI2 +PB13 SPI2_SCK SPI2 +PB12 OF_CS SPI2 CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/ldscript.ld b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/ldscript.ld new file mode 100644 index 0000000000..f458d2a79c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/ldscript.ld @@ -0,0 +1,86 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ + +/* + * STM32F412xG memory setup. + */ +MEMORY +{ + /* leave space for bootloader and two storage sectors */ + flash0 : org = 0x0800C000, len = 952k + flash1 : org = 0x00000000, len = 0 + flash2 : org = 0x00000000, len = 0 + flash3 : org = 0x00000000, len = 0 + flash4 : org = 0x00000000, len = 0 + flash5 : org = 0x00000000, len = 0 + flash6 : org = 0x00000000, len = 0 + flash7 : org = 0x00000000, len = 0 + ram0 : org = 0x20000000, len = 256k + ram1 : org = 0x00000000, len = 0 + ram2 : org = 0x00000000, len = 0 + ram3 : org = 0x00000000, len = 0 + ram4 : org = 0x00000000, len = 0 + ram5 : org = 0x00000000, len = 0 + ram6 : org = 0x00000000, len = 0 + ram7 : org = 0x00000000, len = 0 +} + +/* For each data/text section two region are defined, a virtual region + and a load region (_LMA suffix).*/ + +/* Flash region to be used for exception vectors.*/ +REGION_ALIAS("VECTORS_FLASH", flash0); +REGION_ALIAS("VECTORS_FLASH_LMA", flash0); + +/* Flash region to be used for constructors and destructors.*/ +REGION_ALIAS("XTORS_FLASH", flash0); +REGION_ALIAS("XTORS_FLASH_LMA", flash0); + +/* Flash region to be used for code text.*/ +REGION_ALIAS("TEXT_FLASH", flash0); +REGION_ALIAS("TEXT_FLASH_LMA", flash0); + +/* Flash region to be used for read only data.*/ +REGION_ALIAS("RODATA_FLASH", flash0); +REGION_ALIAS("RODATA_FLASH_LMA", flash0); + +/* Flash region to be used for various.*/ +REGION_ALIAS("VARIOUS_FLASH", flash0); +REGION_ALIAS("VARIOUS_FLASH_LMA", flash0); + +/* Flash region to be used for RAM(n) initialization data.*/ +REGION_ALIAS("RAM_INIT_FLASH_LMA", flash0); + +/* RAM region to be used for Main stack. This stack accommodates the processing + of all exceptions and interrupts.*/ +REGION_ALIAS("MAIN_STACK_RAM", ram0); + +/* RAM region to be used for the process stack. This is the stack used by + the main() function.*/ +REGION_ALIAS("PROCESS_STACK_RAM", ram0); + +/* RAM region to be used for data segment.*/ +REGION_ALIAS("DATA_RAM", ram0); +REGION_ALIAS("DATA_RAM_LMA", flash0); + +/* RAM region to be used for BSS segment.*/ +REGION_ALIAS("BSS_RAM", ram0); + +/* RAM region to be used for the default heap.*/ +REGION_ALIAS("HEAP_RAM", ram0); + +/* Generic rules inclusion.*/ +INCLUDE rules.ld \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/mcuconf.h new file mode 100644 index 0000000000..91624a7c54 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412/mcuconf.h @@ -0,0 +1,253 @@ +/* + ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for use in AP_HAL by Andrew Tridgell and Siddharth Bharat Purohit + */ +#pragma once +/* + * STM32F4xx drivers configuration. + * The following settings override the default settings present in + * the various device driver implementation headers. + * Note that the settings for each driver only have effect if the whole + * driver is enabled in halconf.h. + * + * IRQ priorities: + * 15...0 Lowest...Highest. + * + * DMA priorities: + * 0...3 Lowest...Highest. + */ + +#define STM32F4xx_MCUCONF + +/* + * HAL driver system settings. + */ +#define STM32_NO_INIT FALSE +#define STM32_HSI_ENABLED TRUE +#define STM32_LSI_ENABLED TRUE +#define STM32_HSE_ENABLED FALSE +#define STM32_LSE_ENABLED FALSE +#define STM32_CLOCK48_REQUIRED TRUE +#define STM32_SW STM32_SW_PLL +#define STM32_PLLSRC STM32_PLLSRC_HSI +#define STM32_PLLM_VALUE 16 +#define STM32_PLLN_VALUE 384 +#define STM32_PLLP_VALUE 4 +#define STM32_PLLQ_VALUE 8 +#define STM32_HPRE STM32_HPRE_DIV1 +#define STM32_PPRE1 STM32_PPRE1_DIV2 +#define STM32_PPRE2 STM32_PPRE2_DIV1 +#define STM32_RTCSEL STM32_RTCSEL_LSI +#define STM32_RTCPRE_VALUE 8 +#define STM32_MCO1SEL STM32_MCO1SEL_HSI +#define STM32_MCO1PRE STM32_MCO1PRE_DIV1 +#define STM32_MCO2SEL STM32_MCO2SEL_SYSCLK +#define STM32_MCO2PRE STM32_MCO2PRE_DIV5 +#define STM32_I2SSRC STM32_I2SSRC_CKIN +#define STM32_PLLI2SN_VALUE 192 +#define STM32_PLLI2SR_VALUE 5 +#define STM32_PVD_ENABLE FALSE +#define STM32_PLS STM32_PLS_LEV0 +#define STM32_BKPRAM_ENABLE FALSE + +/* + * ADC driver system settings. + */ +#define STM32_ADC_ADCPRE ADC_CCR_ADCPRE_DIV4 +#define STM32_ADC_USE_ADC1 TRUE +#define STM32_ADC_ADC1_DMA_PRIORITY 2 +#define STM32_ADC_IRQ_PRIORITY 6 +#define STM32_ADC_ADC1_DMA_IRQ_PRIORITY 6 + +/* + * EXT driver system settings. + */ +#define STM32_EXT_EXTI0_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI1_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI2_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI3_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI4_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI5_9_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI10_15_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI16_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI17_IRQ_PRIORITY 15 +#define STM32_EXT_EXTI18_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI19_IRQ_PRIORITY 6 +#define STM32_EXT_EXTI22_IRQ_PRIORITY 15 + +/* + * GPT driver system settings. + */ +#define STM32_GPT_USE_TIM1 FALSE +#define STM32_GPT_USE_TIM2 FALSE +#define STM32_GPT_USE_TIM3 FALSE +#define STM32_GPT_USE_TIM4 FALSE +#define STM32_GPT_USE_TIM5 TRUE +#define STM32_GPT_USE_TIM9 FALSE +#define STM32_GPT_USE_TIM11 FALSE +#define STM32_GPT_TIM1_IRQ_PRIORITY 7 +#define STM32_GPT_TIM2_IRQ_PRIORITY 7 +#define STM32_GPT_TIM3_IRQ_PRIORITY 7 +#define STM32_GPT_TIM4_IRQ_PRIORITY 7 +#define STM32_GPT_TIM5_IRQ_PRIORITY 7 +#define STM32_GPT_TIM9_IRQ_PRIORITY 7 +#define STM32_GPT_TIM11_IRQ_PRIORITY 7 + +/* + * I2C driver system settings. + */ +#define STM32_I2C_USE_I2C1 TRUE +#define STM32_I2C_USE_I2C2 TRUE +#define STM32_I2C_USE_I2C3 FALSE +#define STM32_I2C_BUSY_TIMEOUT 50 +#define STM32_I2C_I2C1_IRQ_PRIORITY 5 +#define STM32_I2C_I2C2_IRQ_PRIORITY 5 +#define STM32_I2C_I2C3_IRQ_PRIORITY 5 +#define STM32_I2C_I2C1_DMA_PRIORITY 3 +#define STM32_I2C_I2C2_DMA_PRIORITY 3 +#define STM32_I2C_I2C3_DMA_PRIORITY 3 +#define STM32_I2C_DMA_ERROR_HOOK(i2cp) osalSysHalt("DMA failure") + +/* + * I2S driver system settings. + */ +#define STM32_I2S_USE_SPI2 FALSE +#define STM32_I2S_USE_SPI3 FALSE +#define STM32_I2S_SPI2_IRQ_PRIORITY 10 +#define STM32_I2S_SPI3_IRQ_PRIORITY 10 +#define STM32_I2S_SPI2_DMA_PRIORITY 1 +#define STM32_I2S_SPI3_DMA_PRIORITY 1 +#define STM32_I2S_DMA_ERROR_HOOK(i2sp) osalSysHalt("DMA failure") + +/* + * ICU driver system settings. + */ +#define STM32_ICU_USE_TIM1 TRUE +#define STM32_ICU_USE_TIM2 FALSE +#define STM32_ICU_USE_TIM3 FALSE +#define STM32_ICU_USE_TIM4 FALSE +#define STM32_ICU_USE_TIM5 FALSE +#define STM32_ICU_USE_TIM9 FALSE +#define STM32_ICU_TIM1_IRQ_PRIORITY 7 +#define STM32_ICU_TIM2_IRQ_PRIORITY 7 +#define STM32_ICU_TIM3_IRQ_PRIORITY 7 +#define STM32_ICU_TIM4_IRQ_PRIORITY 7 +#define STM32_ICU_TIM5_IRQ_PRIORITY 7 +#define STM32_ICU_TIM9_IRQ_PRIORITY 7 + +/* + * PWM driver system settings. + */ +#define STM32_PWM_USE_ADVANCED FALSE +#define STM32_PWM_USE_TIM1 FALSE +#define STM32_PWM_USE_TIM2 FALSE +#define STM32_PWM_USE_TIM3 TRUE +#define STM32_PWM_USE_TIM4 FALSE +#define STM32_PWM_USE_TIM5 FALSE +#define STM32_PWM_USE_TIM9 FALSE +#define STM32_PWM_TIM1_IRQ_PRIORITY 7 +#define STM32_PWM_TIM2_IRQ_PRIORITY 7 +#define STM32_PWM_TIM3_IRQ_PRIORITY 7 +#define STM32_PWM_TIM4_IRQ_PRIORITY 7 +#define STM32_PWM_TIM5_IRQ_PRIORITY 7 +#define STM32_PWM_TIM9_IRQ_PRIORITY 7 + +/* + * SERIAL driver system settings. + */ +#define STM32_SERIAL_USE_USART1 TRUE +#define STM32_SERIAL_USE_USART2 TRUE +#define STM32_SERIAL_USE_USART3 FALSE +#define STM32_SERIAL_USE_USART6 TRUE +#define STM32_SERIAL_USART1_PRIORITY 12 +#define STM32_SERIAL_USART2_PRIORITY 12 +#define STM32_SERIAL_USART3_PRIORITY 12 +#define STM32_SERIAL_USART6_PRIORITY 12 + +/* + * SPI driver system settings. + */ +#define STM32_SPI_USE_SPI1 TRUE +#define STM32_SPI_USE_SPI2 TRUE +#define STM32_SPI_USE_SPI3 FALSE +#define STM32_SPI_USE_SPI4 FALSE +#define STM32_SPI_USE_SPI5 FALSE +#define STM32_SPI_SPI1_DMA_PRIORITY 1 +#define STM32_SPI_SPI2_DMA_PRIORITY 1 +#define STM32_SPI_SPI3_DMA_PRIORITY 1 +#define STM32_SPI_SPI4_DMA_PRIORITY 1 +#define STM32_SPI_SPI5_DMA_PRIORITY 1 +#define STM32_SPI_SPI1_IRQ_PRIORITY 10 +#define STM32_SPI_SPI2_IRQ_PRIORITY 10 +#define STM32_SPI_SPI3_IRQ_PRIORITY 10 +#define STM32_SPI_SPI4_IRQ_PRIORITY 10 +#define STM32_SPI_SPI5_IRQ_PRIORITY 10 +#define STM32_SPI_DMA_ERROR_HOOK(spip) osalSysHalt("DMA failure") + +/* + * ST driver system settings. + */ +#define STM32_ST_IRQ_PRIORITY 8 +#define STM32_ST_USE_TIMER 2 + +/* + * UART driver system settings. + */ +#define STM32_UART_USE_USART1 FALSE +#define STM32_UART_USE_USART2 FALSE +#define STM32_UART_USE_USART3 FALSE +#define STM32_UART_USE_USART6 FALSE +#define STM32_UART_USART1_IRQ_PRIORITY 12 +#define STM32_UART_USART2_IRQ_PRIORITY 12 +#define STM32_UART_USART3_IRQ_PRIORITY 12 +#define STM32_UART_USART6_IRQ_PRIORITY 12 +#define STM32_UART_USART1_DMA_PRIORITY 0 +#define STM32_UART_USART2_DMA_PRIORITY 0 +#define STM32_UART_USART3_DMA_PRIORITY 0 +#define STM32_UART_USART6_DMA_PRIORITY 0 +#define STM32_UART_DMA_ERROR_HOOK(uartp) osalSysHalt("DMA failure") + +/* + * USB driver system settings. + */ +#define STM32_USB_USE_OTG1 FALSE +#define STM32_USB_OTG1_IRQ_PRIORITY 14 +#define STM32_USB_OTG1_RX_FIFO_SIZE 512 +#define STM32_USB_OTG_THREAD_PRIO LOWPRIO +#define STM32_USB_OTG_THREAD_STACK_SIZE 128 +#define STM32_USB_OTGFIFO_FILL_BASEPRI 0 + +/* + * WDG driver system settings. + */ +#define STM32_WDG_USE_IWDG FALSE + +// include auto-generated DMA channel mapping +#include "hwdef.h" diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.cpp b/libraries/AP_HAL_ChibiOS/shared_dma.cpp new file mode 100644 index 0000000000..e7d405773d --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/shared_dma.cpp @@ -0,0 +1,117 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include "shared_dma.h" + +/* + code to handle sharing of DMA channels between peripherals + */ + +Shared_DMA::dma_lock Shared_DMA::locks[SHARED_DMA_MAX_STREAM_ID]; + +void Shared_DMA::init(void) +{ + for (uint8_t i=0; i. + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#pragma once + +#include "AP_HAL_ChibiOS.h" + +#define SHARED_DMA_MAX_STREAM_ID (8*2) + +// DMA stream ID for stream_id2 when only one is needed +#define SHARED_DMA_NONE 255 + +class Shared_DMA +{ +public: + FUNCTOR_TYPEDEF(dma_allocate_fn_t, void); + FUNCTOR_TYPEDEF(dma_deallocate_fn_t, void); + + // the use of two stream IDs is for support of peripherals that + // need both a RX and TX DMA channel + Shared_DMA(uint8_t stream_id1, uint8_t stream_id2, + dma_allocate_fn_t allocate, + dma_allocate_fn_t deallocate); + + // initialise the stream locks + static void init(void); + + // blocking lock call + void lock(void); + + // unlock call. The DMA channel will not be immediately + // deallocated. Instead it will be deallocated if another driver + // needs it + void unlock(void); + + // unlock call from an IRQ + void unlock_from_IRQ(void); + + //should be called inside the destructor of Shared DMA participants + void unregister(void); + +private: + dma_allocate_fn_t allocate; + dma_allocate_fn_t deallocate; + uint8_t stream_id1; + uint8_t stream_id2; + + static struct dma_lock { + // semaphore to ensure only one peripheral uses a DMA channel at a time + binary_semaphore_t semaphore; + + // a de-allocation function that is called to release an existing user + dma_deallocate_fn_t deallocate; + + // point to object that holds the allocation, if allocated + Shared_DMA *obj; + } locks[SHARED_DMA_MAX_STREAM_ID]; +}; diff --git a/libraries/AP_HAL_ChibiOS/system.cpp b/libraries/AP_HAL_ChibiOS/system.cpp new file mode 100644 index 0000000000..da381374ce --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/system.cpp @@ -0,0 +1,167 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Andrew Tridgell and Siddharth Bharat Purohit + */ +#include +#include +#include +#include + +#include +#include "hal.h" +#include + +extern const AP_HAL::HAL& hal; +extern "C" +{ +#define bkpt() __asm volatile("BKPT #0\n") +typedef enum { + Reset = 1, + NMI = 2, + HardFault = 3, + MemManage = 4, + BusFault = 5, + UsageFault = 6, +} FaultType; + +void *__dso_handle; + +void __cxa_pure_virtual() { while (1); } //TODO: Handle properly, maybe generate a traceback + +void NMI_Handler(void) { while (1); } + +void HardFault_Handler(void) { + //Copy to local variables (not pointers) to allow GDB "i loc" to directly show the info + //Get thread context. Contains main registers including PC and LR + struct port_extctx ctx; + memcpy(&ctx, (void*)__get_PSP(), sizeof(struct port_extctx)); + (void)ctx; + //Interrupt status register: Which interrupt have we encountered, e.g. HardFault? + FaultType faultType = (FaultType)__get_IPSR(); + (void)faultType; + //For HardFault/BusFault this is the address that was accessed causing the error + uint32_t faultAddress = SCB->BFAR; + (void)faultAddress; + //Flags about hardfault / busfault + //See http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dui0552a/Cihdjcfc.html for reference + bool isFaultPrecise = ((SCB->CFSR >> SCB_CFSR_BUSFAULTSR_Pos) & (1 << 1) ? true : false); + bool isFaultImprecise = ((SCB->CFSR >> SCB_CFSR_BUSFAULTSR_Pos) & (1 << 2) ? true : false); + bool isFaultOnUnstacking = ((SCB->CFSR >> SCB_CFSR_BUSFAULTSR_Pos) & (1 << 3) ? true : false); + bool isFaultOnStacking = ((SCB->CFSR >> SCB_CFSR_BUSFAULTSR_Pos) & (1 << 4) ? true : false); + bool isFaultAddressValid = ((SCB->CFSR >> SCB_CFSR_BUSFAULTSR_Pos) & (1 << 7) ? true : false); + (void)isFaultPrecise; + (void)isFaultImprecise; + (void)isFaultOnUnstacking; + (void)isFaultOnStacking; + (void)isFaultAddressValid; + //Cause debugger to stop. Ignored if no debugger is attached + while(1) {} +} + +void BusFault_Handler(void) __attribute__((alias("HardFault_Handler"))); + +void UsageFault_Handler(void) { + //Copy to local variables (not pointers) to allow GDB "i loc" to directly show the info + //Get thread context. Contains main registers including PC and LR + struct port_extctx ctx; + memcpy(&ctx, (void*)__get_PSP(), sizeof(struct port_extctx)); + (void)ctx; + //Interrupt status register: Which interrupt have we encountered, e.g. HardFault? + FaultType faultType = (FaultType)__get_IPSR(); + (void)faultType; + //Flags about hardfault / busfault + //See http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dui0552a/Cihdjcfc.html for reference + bool isUndefinedInstructionFault = ((SCB->CFSR >> SCB_CFSR_USGFAULTSR_Pos) & (1 << 0) ? true : false); + bool isEPSRUsageFault = ((SCB->CFSR >> SCB_CFSR_USGFAULTSR_Pos) & (1 << 1) ? true : false); + bool isInvalidPCFault = ((SCB->CFSR >> SCB_CFSR_USGFAULTSR_Pos) & (1 << 2) ? true : false); + bool isNoCoprocessorFault = ((SCB->CFSR >> SCB_CFSR_USGFAULTSR_Pos) & (1 << 3) ? true : false); + bool isUnalignedAccessFault = ((SCB->CFSR >> SCB_CFSR_USGFAULTSR_Pos) & (1 << 8) ? true : false); + bool isDivideByZeroFault = ((SCB->CFSR >> SCB_CFSR_USGFAULTSR_Pos) & (1 << 9) ? true : false); + (void)isUndefinedInstructionFault; + (void)isEPSRUsageFault; + (void)isInvalidPCFault; + (void)isNoCoprocessorFault; + (void)isUnalignedAccessFault; + (void)isDivideByZeroFault; + //Cause debugger to stop. Ignored if no debugger is attached + while(1) {} +} + +void MemManage_Handler(void) { + //Copy to local variables (not pointers) to allow GDB "i loc" to directly show the info + //Get thread context. Contains main registers including PC and LR + struct port_extctx ctx; + memcpy(&ctx, (void*)__get_PSP(), sizeof(struct port_extctx)); + (void)ctx; + //Interrupt status register: Which interrupt have we encountered, e.g. HardFault? + FaultType faultType = (FaultType)__get_IPSR(); + (void)faultType; + //For HardFault/BusFault this is the address that was accessed causing the error + uint32_t faultAddress = SCB->MMFAR; + (void)faultAddress; + //Flags about hardfault / busfault + //See http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dui0552a/Cihdjcfc.html for reference + bool isInstructionAccessViolation = ((SCB->CFSR >> SCB_CFSR_MEMFAULTSR_Pos) & (1 << 0) ? true : false); + bool isDataAccessViolation = ((SCB->CFSR >> SCB_CFSR_MEMFAULTSR_Pos) & (1 << 1) ? true : false); + bool isExceptionUnstackingFault = ((SCB->CFSR >> SCB_CFSR_MEMFAULTSR_Pos) & (1 << 3) ? true : false); + bool isExceptionStackingFault = ((SCB->CFSR >> SCB_CFSR_MEMFAULTSR_Pos) & (1 << 4) ? true : false); + bool isFaultAddressValid = ((SCB->CFSR >> SCB_CFSR_MEMFAULTSR_Pos) & (1 << 7) ? true : false); + (void)isInstructionAccessViolation; + (void)isDataAccessViolation; + (void)isExceptionUnstackingFault; + (void)isExceptionStackingFault; + (void)isFaultAddressValid; + while(1) {} +} +} +namespace AP_HAL { + +void init() +{ +} + +void panic(const char *errormsg, ...) +{ + va_list ap; + + va_start(ap, errormsg); + vprintf(errormsg, ap); + va_end(ap); + + hal.scheduler->delay_microseconds(10000); + while(1) {} +} + +uint32_t micros() +{ + return micros64() & 0xFFFFFFFF; +} + +uint32_t millis() +{ + return millis64() & 0xFFFFFFFF; +} + +uint64_t micros64() +{ + return hrt_micros(); +} + +uint64_t millis64() +{ + return micros64() / 1000; +} + +} // namespace AP_HAL