From e4ce2f532bfae073a6f1edf892cb0610ca03d9e7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 7 Mar 2018 09:41:03 +1100 Subject: [PATCH] HAL_ChibiOS: make smaller builds possible this allows for much smaller builds when you disable a lot of options. This is part of an effort to create a bootloader of less than 16k using ChibiOS --- libraries/AP_HAL_ChibiOS/AnalogIn.cpp | 6 ++++ libraries/AP_HAL_ChibiOS/AnalogIn.h | 4 +++ libraries/AP_HAL_ChibiOS/Device.cpp | 3 ++ libraries/AP_HAL_ChibiOS/GPIO.cpp | 5 +-- .../AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp | 15 ++++++++- libraries/AP_HAL_ChibiOS/RCInput.cpp | 32 +++++++++---------- libraries/AP_HAL_ChibiOS/RCInput.h | 3 +- libraries/AP_HAL_ChibiOS/Scheduler.cpp | 19 ++++++----- libraries/AP_HAL_ChibiOS/Semaphores.cpp | 8 ++--- libraries/AP_HAL_ChibiOS/Semaphores.h | 9 ++++-- libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 25 ++++++++------- libraries/AP_HAL_ChibiOS/UARTDriver.h | 3 +- libraries/AP_HAL_ChibiOS/Util.cpp | 7 ++-- .../AP_HAL_ChibiOS/hwdef/common/usbcfg.h | 8 +++++ 14 files changed, 97 insertions(+), 50 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp index f9202f1238..e441287ae7 100644 --- a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp @@ -15,6 +15,10 @@ * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include +#include "ch.h" +#include "hal.h" + +#if HAL_USE_ADC == TRUE #include "AnalogIn.h" @@ -357,3 +361,5 @@ void AnalogIn::update_power_flags(void) } _power_flags = flags; } +#endif // HAL_USE_ADC + diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.h b/libraries/AP_HAL_ChibiOS/AnalogIn.h index 86bca94e9c..1292c8112f 100644 --- a/libraries/AP_HAL_ChibiOS/AnalogIn.h +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.h @@ -24,6 +24,8 @@ // number of samples on each channel to gather on each DMA callback #define ADC_DMA_BUF_DEPTH 8 +#if HAL_USE_ADC == TRUE + class ChibiOS::AnalogSource : public AP_HAL::AnalogSource { public: friend class ChibiOS::AnalogIn; @@ -90,3 +92,5 @@ private: static uint32_t sample_sum[]; static uint32_t sample_count; }; + +#endif // HAL_USE_ADC diff --git a/libraries/AP_HAL_ChibiOS/Device.cpp b/libraries/AP_HAL_ChibiOS/Device.cpp index 085c860590..4e7d8a419f 100644 --- a/libraries/AP_HAL_ChibiOS/Device.cpp +++ b/libraries/AP_HAL_ChibiOS/Device.cpp @@ -21,6 +21,7 @@ #include "Semaphores.h" #include "Util.h" +#if HAL_USE_I2C == TRUE || HAL_USE_SPI == TRUE using namespace ChibiOS; @@ -188,3 +189,5 @@ void DeviceBus::bouncebuffer_rx_copy(uint8_t *buf_rx, uint16_t rx_len) { memcpy(buf_rx, bounce_buffer_rx, rx_len); } + +#endif // HAL_USE_I2C || HAL_USE_SPI diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp index 72d77e2dcc..11b1f0d9f7 100644 --- a/libraries/AP_HAL_ChibiOS/GPIO.cpp +++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp @@ -15,10 +15,11 @@ * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include "GPIO.h" -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include +#if HAL_USE_EXT == TRUE + using namespace ChibiOS; // GPIO pin table from hwdef.dat @@ -250,4 +251,4 @@ void ext_interrupt_cb(EXTDriver *extp, expchannel_t channel) } } -#endif //HAL_BOARD_ChibiOS +#endif // HAL_USE_EXT diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index bc52d2d2b0..108d0b5e0f 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -23,6 +23,7 @@ #include #include #include "shared_dma.h" +#include "hwdef/common/usbcfg.h" #include @@ -45,7 +46,12 @@ static ChibiOS::SPIDeviceManager spiDeviceManager; static Empty::SPIDeviceManager spiDeviceManager; #endif +#if HAL_USE_ADC == TRUE static ChibiOS::AnalogIn analogIn; +#else +static Empty::AnalogIn analogIn; +#endif + #ifdef HAL_USE_EMPTY_STORAGE static Empty::Storage storageDriver; #else @@ -108,10 +114,12 @@ extern const AP_HAL::HAL& hal; void hal_chibios_set_priority(uint8_t priority) { chSysLock(); +#if CH_CFG_USE_MUTEXES == TRUE if ((daemon_task->prio == daemon_task->realprio) || (priority > daemon_task->prio)) { daemon_task->prio = priority; } daemon_task->realprio = priority; +#endif chSchRescheduleS(); chSysUnlock(); } @@ -158,7 +166,8 @@ static THD_FUNCTION(main_loop,arg) hal.scheduler->system_initialized(); thread_running = true; - daemon_task->name = SKETCHNAME; + chRegSetThreadName(SKETCHNAME); + /* switch to high priority for main loop */ @@ -192,6 +201,10 @@ void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const * RTOS is active. */ +#ifdef HAL_USB_PRODUCT_ID + setup_usb_strings(); +#endif + #ifdef HAL_STDOUT_SERIAL //STDOUT Initialistion SerialConfig stdoutcfg = diff --git a/libraries/AP_HAL_ChibiOS/RCInput.cpp b/libraries/AP_HAL_ChibiOS/RCInput.cpp index 15825d71c1..e3d8a8b9a3 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCInput.cpp @@ -35,7 +35,6 @@ void RCInput::init() sig_reader.attach_capture_timer(&RCIN_ICU_TIMER, RCIN_ICU_CHANNEL, STM32_RCIN_DMA_STREAM, STM32_RCIN_DMA_CHANNEL); rcin_prot.init(); #endif - chMtxObjectInit(&rcin_mutex); _init = true; } @@ -44,7 +43,9 @@ bool RCInput::new_input() if (!_init) { return false; } - chMtxLock(&rcin_mutex); + if (!rcin_mutex.take_nonblocking()) { + return false; + } bool valid = _rcin_timestamp_last_signal != _last_read; if (_override_valid) { @@ -53,7 +54,7 @@ bool RCInput::new_input() } _last_read = _rcin_timestamp_last_signal; _override_valid = false; - chMtxUnlock(&rcin_mutex); + rcin_mutex.give(); #ifdef HAL_RCINPUT_WITH_AP_RADIO if (!_radio_init) { @@ -72,10 +73,7 @@ uint8_t RCInput::num_channels() if (!_init) { return 0; } - chMtxLock(&rcin_mutex); - uint8_t n = _num_channels; - chMtxUnlock(&rcin_mutex); - return n; + return _num_channels; } uint16_t RCInput::read(uint8_t channel) @@ -86,18 +84,18 @@ uint16_t RCInput::read(uint8_t channel) if (channel >= RC_INPUT_MAX_CHANNELS) { return 0; } - chMtxLock(&rcin_mutex); + rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER); if (_override[channel]) { uint16_t v = _override[channel]; - chMtxUnlock(&rcin_mutex); + rcin_mutex.give(); return v; } if (channel >= _num_channels) { - chMtxUnlock(&rcin_mutex); + rcin_mutex.give(); return 0; } uint16_t v = _rc_values[channel]; - chMtxUnlock(&rcin_mutex); + rcin_mutex.give(); #ifdef HAL_RCINPUT_WITH_AP_RADIO if (radio && channel == 0) { // hook to allow for update of radio on main thread, for mavlink sends @@ -176,36 +174,36 @@ void RCInput::_timer_tick(void) } if (rcin_prot.new_input()) { - chMtxLock(&rcin_mutex); + rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER); _rcin_timestamp_last_signal = AP_HAL::micros(); _num_channels = rcin_prot.num_channels(); for (uint8_t i=0; i<_num_channels; i++) { _rc_values[i] = rcin_prot.read(i); } - chMtxUnlock(&rcin_mutex); + rcin_mutex.give(); } #endif #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_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER); _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); + rcin_mutex.give(); } #endif #if HAL_WITH_IO_MCU - chMtxLock(&rcin_mutex); + rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER); if (AP_BoardConfig::io_enabled() && iomcu.check_rcinput(last_iomcu_us, _num_channels, _rc_values, RC_INPUT_MAX_CHANNELS)) { _rcin_timestamp_last_signal = last_iomcu_us; } - chMtxUnlock(&rcin_mutex); + rcin_mutex.give(); #endif diff --git a/libraries/AP_HAL_ChibiOS/RCInput.h b/libraries/AP_HAL_ChibiOS/RCInput.h index c80e008df5..36ce3188dc 100644 --- a/libraries/AP_HAL_ChibiOS/RCInput.h +++ b/libraries/AP_HAL_ChibiOS/RCInput.h @@ -18,6 +18,7 @@ #include "AP_HAL_ChibiOS.h" #include "SoftSigReader.h" +#include "Semaphores.h" #ifdef HAL_RCINPUT_WITH_AP_RADIO #include @@ -59,7 +60,7 @@ private: uint64_t _last_read; bool _override_valid; uint8_t _num_channels; - mutex_t rcin_mutex; + Semaphore rcin_mutex; int16_t _rssi = -1; uint32_t _rcin_timestamp_last_signal; bool _init; diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp index be89b58d02..f21d17a381 100644 --- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp +++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp @@ -15,7 +15,6 @@ * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include "AP_HAL_ChibiOS.h" #include "Scheduler.h" @@ -32,6 +31,8 @@ #include #include "shared_dma.h" +#if CH_CFG_USE_DYNAMIC == TRUE + using namespace ChibiOS; extern const AP_HAL::HAL& hal; @@ -285,8 +286,10 @@ void Scheduler::_run_timers(bool called_from_timer_thread) _failsafe(); } +#if HAL_USE_ADC == TRUE // process analog input ((AnalogIn *)hal.analogin)->_timer_tick(); +#endif _in_timer_proc = false; } @@ -294,7 +297,7 @@ void Scheduler::_run_timers(bool called_from_timer_thread) void Scheduler::_timer_thread(void *arg) { Scheduler *sched = (Scheduler *)arg; - sched->_timer_thread_ctx->name = "apm_timer"; + chRegSetThreadName("apm_timer"); while (!sched->_hal_initialized) { sched->delay_microseconds(1000); @@ -313,7 +316,7 @@ void Scheduler::_timer_thread(void *arg) void Scheduler::_uavcan_thread(void *arg) { Scheduler *sched = (Scheduler *)arg; - sched->_uavcan_thread_ctx->name = "apm_uavcan"; + chRegSetThreadName("apm_uavcan"); while (!sched->_hal_initialized) { sched->delay_microseconds(20000); } @@ -331,7 +334,7 @@ void Scheduler::_uavcan_thread(void *arg) void Scheduler::_rcin_thread(void *arg) { Scheduler *sched = (Scheduler *)arg; - sched->_rcin_thread_ctx->name = "apm_rcin"; + chRegSetThreadName("apm_rcin"); while (!sched->_hal_initialized) { sched->delay_microseconds(20000); } @@ -345,7 +348,7 @@ void Scheduler::_rcin_thread(void *arg) void Scheduler::_toneAlarm_thread(void *arg) { Scheduler *sched = (Scheduler *)arg; - sched->_toneAlarm_thread_ctx->name = "toneAlarm"; + chRegSetThreadName("toneAlarm"); while (!sched->_hal_initialized) { sched->delay_microseconds(20000); } @@ -379,7 +382,7 @@ void Scheduler::_run_io(void) void Scheduler::_io_thread(void* arg) { Scheduler *sched = (Scheduler *)arg; - sched->_io_thread_ctx->name = "apm_io"; + chRegSetThreadName("apm_io"); while (!sched->_hal_initialized) { sched->delay_microseconds(1000); } @@ -394,7 +397,7 @@ void Scheduler::_io_thread(void* arg) void Scheduler::_storage_thread(void* arg) { Scheduler *sched = (Scheduler *)arg; - sched->_storage_thread_ctx->name = "apm_storage"; + chRegSetThreadName("apm_storage"); while (!sched->_hal_initialized) { sched->delay_microseconds(10000); } @@ -438,4 +441,4 @@ void Scheduler::restore_interrupts(void *state) chSysRestoreStatusX((syssts_t)(uintptr_t)state); } -#endif +#endif // CH_CFG_USE_DYNAMIC diff --git a/libraries/AP_HAL_ChibiOS/Semaphores.cpp b/libraries/AP_HAL_ChibiOS/Semaphores.cpp index ab80184aee..dc6455427d 100644 --- a/libraries/AP_HAL_ChibiOS/Semaphores.cpp +++ b/libraries/AP_HAL_ChibiOS/Semaphores.cpp @@ -15,11 +15,10 @@ * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include - -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS - #include "Semaphores.h" +#if CH_CFG_USE_MUTEXES == TRUE + extern const AP_HAL::HAL& hal; using namespace ChibiOS; @@ -54,4 +53,5 @@ bool Semaphore::take_nonblocking() return chMtxTryLock(&_lock); } -#endif // CONFIG_HAL_BOARD +#endif // CH_CFG_USE_MUTEXES + diff --git a/libraries/AP_HAL_ChibiOS/Semaphores.h b/libraries/AP_HAL_ChibiOS/Semaphores.h index b79d888268..97183a7b01 100644 --- a/libraries/AP_HAL_ChibiOS/Semaphores.h +++ b/libraries/AP_HAL_ChibiOS/Semaphores.h @@ -18,19 +18,25 @@ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include "AP_HAL_ChibiOS.h" + class ChibiOS::Semaphore : public AP_HAL::Semaphore { public: Semaphore() { +#if CH_CFG_USE_MUTEXES == TRUE chMtxObjectInit(&_lock); +#endif } bool give(); bool take(uint32_t timeout_ms); bool take_nonblocking(); bool check_owner(void) { +#if CH_CFG_USE_MUTEXES == TRUE return _lock.owner == chThdGetSelfX(); +#else + return true; +#endif } void assert_owner(void) { osalDbgAssert(check_owner(), "owner"); @@ -38,4 +44,3 @@ public: private: mutex_t _lock; }; -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index af96986710..44475cd989 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -62,7 +62,6 @@ _initialised(false) { osalDbgAssert(serial_num < UART_MAX_DRIVERS, "too many UART drivers"); uart_drivers[serial_num] = this; - chMtxObjectInit(&_write_mutex); } /* @@ -104,12 +103,14 @@ void UARTDriver::thread_init(void) // already initialised return; } +#if CH_CFG_USE_HEAP == TRUE uart_thread_ctx = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(2048), "apm_uart", APM_UART_PRIORITY, uart_thread, this); +#endif } @@ -447,18 +448,18 @@ int16_t UARTDriver::read() /* Empty implementations of Print virtual methods */ size_t UARTDriver::write(uint8_t c) { - if (lock_key != 0 || !chMtxTryLock(&_write_mutex)) { + if (lock_key != 0 || !_write_mutex.take_nonblocking()) { return 0; } if (!_initialised) { - chMtxUnlock(&_write_mutex); + _write_mutex.give(); return 0; } while (_writebuf.space() == 0) { if (!_blocking_writes) { - chMtxUnlock(&_write_mutex); + _write_mutex.give(); return 0; } hal.scheduler->delay(1); @@ -467,7 +468,7 @@ size_t UARTDriver::write(uint8_t c) if (unbuffered_writes) { write_pending_bytes(); } - chMtxUnlock(&_write_mutex); + _write_mutex.give(); return ret; } @@ -477,7 +478,7 @@ size_t UARTDriver::write(const uint8_t *buffer, size_t size) return 0; } - if (!chMtxTryLock(&_write_mutex)) { + if (!_write_mutex.take_nonblocking()) { return 0; } @@ -485,7 +486,7 @@ size_t UARTDriver::write(const uint8_t *buffer, size_t size) /* use the per-byte delay loop in write() above for blocking writes */ - chMtxUnlock(&_write_mutex); + _write_mutex.give(); size_t ret = 0; while (size--) { if (write(*buffer++) != 1) break; @@ -498,7 +499,7 @@ size_t UARTDriver::write(const uint8_t *buffer, size_t size) if (unbuffered_writes) { write_pending_bytes(); } - chMtxUnlock(&_write_mutex); + _write_mutex.give(); return ret; } @@ -524,12 +525,12 @@ size_t UARTDriver::write_locked(const uint8_t *buffer, size_t size, uint32_t key if (lock_key != 0 && key != lock_key) { return 0; } - if (!chMtxTryLock(&_write_mutex)) { + if (!_write_mutex.take_nonblocking()) { return 0; } size_t ret = _writebuf.write(buffer, size); - chMtxUnlock(&_write_mutex); + _write_mutex.give(); return ret; } @@ -774,9 +775,9 @@ void UARTDriver::_timer_tick(void) // provided by the write() call, but if the write is larger // than the DMA buffer size then there can be extra bytes to // send here, and they must be sent with the write lock held - chMtxLock(&_write_mutex); + _write_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER); write_pending_bytes(); - chMtxUnlock(&_write_mutex); + _write_mutex.give(); } else { write_pending_bytes(); } diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index 727e706c88..7dd6e8bff0 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -20,6 +20,7 @@ #include "AP_HAL_ChibiOS.h" #include "shared_dma.h" +#include "Semaphores.h" #define RX_BOUNCE_BUFSIZE 128 #define TX_BOUNCE_BUFSIZE 64 @@ -119,7 +120,7 @@ private: uint8_t tx_bounce_buf[TX_BOUNCE_BUFSIZE]; ByteBuffer _readbuf{0}; ByteBuffer _writebuf{0}; - mutex_t _write_mutex; + Semaphore _write_mutex; const stm32_dma_stream_t* rxdma; const stm32_dma_stream_t* txdma; bool _in_timer; diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 90c6ebadb2..92c758fb6f 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -17,7 +17,6 @@ #include #include -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include "Util.h" #include #include "ToneAlarm.h" @@ -32,6 +31,8 @@ extern const AP_HAL::HAL& hal; using namespace ChibiOS; +#if CH_CFG_USE_HEAP == TRUE + extern "C" { size_t mem_available(void); void *malloc_ccm(size_t size); @@ -77,6 +78,8 @@ void* Util::try_alloc_from_ccm_ram(size_t size) return ret; } +#endif // CH_CFG_USE_HEAP + /* get safety switch state */ @@ -177,4 +180,4 @@ void Util::_toneAlarm_timer_tick() { } #endif // HAL_PWM_ALARM -#endif //CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h index 3deb5d599e..ea001566cb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/usbcfg.h @@ -28,6 +28,10 @@ */ #pragma once +#if defined(__cplusplus) +extern "C" { +#endif + #if HAL_USE_SERIAL_USB extern const USBConfig usbcfg; extern SerialUSBConfig serusbcfg; @@ -36,4 +40,8 @@ extern SerialUSBDriver SDU1; void setup_usb_strings(void); +#if defined(__cplusplus) +} +#endif + /** @} */