diff --git a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h index a7a7aa4c01..09a04a9c6a 100644 --- a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h +++ b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h @@ -5,6 +5,7 @@ namespace ChibiOS { class AnalogSource; class DigitalSource; class GPIO; + class I2CBus; class I2CDevice; class I2CDeviceManager; class OpticalFlow; @@ -13,6 +14,8 @@ namespace ChibiOS { class RCOutput; class Scheduler; class Semaphore; + class SPIBus; + class SPIDesc; class SPIDevice; class SPIDeviceDriver; class SPIDeviceManager; diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index 7e73e07f49..751abd029e 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -32,8 +32,19 @@ static HAL_UARTC_DRIVER; static HAL_UARTD_DRIVER; static HAL_UARTE_DRIVER; static HAL_UARTF_DRIVER; + +#if HAL_USE_I2C == TRUE static ChibiOS::I2CDeviceManager i2cDeviceManager; +#else +static Empty::I2CDeviceManager i2cDeviceManager; +#endif + +#if HAL_USE_SPI == TRUE static ChibiOS::SPIDeviceManager spiDeviceManager; +#else +static Empty::SPIDeviceManager spiDeviceManager; +#endif + static ChibiOS::AnalogIn analogIn; #ifdef HAL_USE_EMPTY_STORAGE static Empty::Storage storageDriver; @@ -42,7 +53,13 @@ static ChibiOS::Storage storageDriver; #endif static ChibiOS::GPIO gpioDriver; static ChibiOS::RCInput rcinDriver; + +#if HAL_USE_PWM == TRUE static ChibiOS::RCOutput rcoutDriver; +#else +static Empty::RCOutput rcoutDriver; +#endif + static ChibiOS::Scheduler schedulerInstance; static ChibiOS::Util utilInstance; static Empty::OpticalFlow opticalFlowDriver; @@ -168,6 +185,8 @@ void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const * - Kernel initialization, the main() function becomes a thread and the * RTOS is active. */ + +#ifdef HAL_STDOUT_SERIAL //STDOUT Initialistion SerialConfig stdoutcfg = { @@ -177,6 +196,7 @@ void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const 0 }; sdStart((SerialDriver*)&HAL_STDOUT_SERIAL, &stdoutcfg); +#endif //Setup SD Card and Initialise FATFS bindings /* diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp index b063d1d6fa..b12d628dd5 100644 --- a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp @@ -22,6 +22,8 @@ #include "ch.h" #include "hal.h" +#if HAL_USE_I2C == TRUE + static const struct I2CInfo { struct I2CDriver *i2c; uint8_t dma_channel_rx; @@ -283,3 +285,5 @@ I2CDeviceManager::get_device(uint8_t bus, uint8_t address, auto dev = AP_HAL::OwnPtr(new I2CDevice(bus, address, bus_clock, use_smbus, timeout_ms)); return dev; } + +#endif // HAL_USE_I2C diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.h b/libraries/AP_HAL_ChibiOS/I2CDevice.h index 661e17ffea..51c5e172f1 100644 --- a/libraries/AP_HAL_ChibiOS/I2CDevice.h +++ b/libraries/AP_HAL_ChibiOS/I2CDevice.h @@ -28,9 +28,11 @@ #include "Device.h" #include "shared_dma.h" -namespace ChibiOS { +#if HAL_USE_I2C == TRUE -class I2CBus : public DeviceBus { +using namespace ChibiOS; + +class ChibiOS::I2CBus : public ChibiOS::DeviceBus { public: I2CConfig i2ccfg; uint8_t busnum; @@ -44,7 +46,7 @@ public: static void clear_bus(ioline_t scl_line, uint8_t scl_af); }; -class I2CDevice : public AP_HAL::I2CDevice { +class ChibiOS::I2CDevice : public AP_HAL::I2CDevice { public: static I2CDevice *from(AP_HAL::I2CDevice *dev) { @@ -100,7 +102,7 @@ private: uint32_t _timeout_ms; }; -class I2CDeviceManager : public AP_HAL::I2CDeviceManager { +class ChibiOS::I2CDeviceManager : public AP_HAL::I2CDeviceManager { public: friend class I2CDevice; @@ -120,4 +122,5 @@ public: uint32_t timeout_ms=4) override; }; -} +#endif // HAL_USE_I2C + diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index d57b640f36..3934aeec77 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -18,6 +18,8 @@ #include #include +#if HAL_USE_PWM == TRUE + using namespace ChibiOS; extern const AP_HAL::HAL& hal; @@ -494,3 +496,5 @@ void RCOutput::timer_tick(void) trigger_oneshot(); } } + +#endif // HAL_USE_PWM diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 393de41c34..5231c7b42b 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -20,6 +20,8 @@ #include "ch.h" #include "hal.h" +#if HAL_USE_PWM == TRUE + class ChibiOS::RCOutput : public AP_HAL::RCOutput { public: void init(); @@ -116,3 +118,5 @@ private: // trigger oneshot pulses void trigger_oneshot(void); }; + +#endif // HAL_USE_PWM diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp index 186230d3f7..198dff8b4d 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp @@ -21,6 +21,8 @@ #include "Semaphores.h" #include +#if HAL_USE_SPI == TRUE + using namespace ChibiOS; extern const AP_HAL::HAL& hal; @@ -45,7 +47,7 @@ static const struct SPIDriverInfo { #define MHZ (1000U*1000U) #define KHZ (1000U) // device list comes from hwdef.dat -SPIDesc SPIDeviceManager::device_table[] = { HAL_SPI_DEVICE_LIST }; +ChibiOS::SPIDesc SPIDeviceManager::device_table[] = { HAL_SPI_DEVICE_LIST }; SPIBus::SPIBus(uint8_t _bus) : DeviceBus(APM_SPI_PRIORITY), @@ -323,3 +325,5 @@ SPIDeviceManager::get_device(const char *name) return AP_HAL::OwnPtr(new SPIDevice(*busp, desc)); } + +#endif diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.h b/libraries/AP_HAL_ChibiOS/SPIDevice.h index 6486695bca..89703a3908 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.h +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.h @@ -21,11 +21,11 @@ #include "Scheduler.h" #include "Device.h" -namespace ChibiOS { +#if HAL_USE_SPI == TRUE -class SPIDesc; +using namespace ChibiOS; -class SPIBus : public DeviceBus { +class ChibiOS::SPIBus : public ChibiOS::DeviceBus { public: SPIBus(uint8_t bus); struct spi_dev_s *dev; @@ -36,7 +36,7 @@ public: bool spi_started; }; -struct SPIDesc { +struct ChibiOS::SPIDesc { SPIDesc(const char *_name, uint8_t _bus, uint8_t _device, ioline_t _pal_line, uint16_t _mode, uint32_t _lowspeed, uint32_t _highspeed) @@ -56,7 +56,7 @@ struct SPIDesc { }; -class SPIDevice : public AP_HAL::SPIDevice { +class ChibiOS::SPIDevice : public AP_HAL::SPIDevice { public: SPIDevice(SPIBus &_bus, SPIDesc &_device_desc); @@ -101,7 +101,7 @@ private: uint16_t derive_freq_flag(uint32_t _frequency); }; -class SPIDeviceManager : public AP_HAL::SPIDeviceManager { +class ChibiOS::SPIDeviceManager : public AP_HAL::SPIDeviceManager { public: friend class SPIDevice; @@ -117,4 +117,5 @@ private: SPIBus *buses; }; -} + +#endif // HAL_USE_SPI diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index 7dbf348f3a..f1a9d24a28 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -15,8 +15,6 @@ * Code by Andrew Tridgell and Siddharth Bharat Purohit */ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS - #include #include "Storage.h" @@ -24,6 +22,7 @@ using namespace ChibiOS; +#ifndef HAL_USE_EMPTY_STORAGE extern const AP_HAL::HAL& hal; @@ -198,4 +197,4 @@ bool Storage::_flash_erase_ok(void) return !hal.util->get_soft_armed(); } -#endif // CONFIG_HAL_BOARD +#endif // HAL_USE_EMPTY_STORAGE diff --git a/libraries/AP_HAL_ChibiOS/Storage.h b/libraries/AP_HAL_ChibiOS/Storage.h index f8f4a57ce6..fea40c2a2a 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.h +++ b/libraries/AP_HAL_ChibiOS/Storage.h @@ -25,6 +25,8 @@ #define CH_STORAGE_SIZE HAL_STORAGE_SIZE +#ifndef HAL_USE_EMPTY_STORAGE + // 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 @@ -71,3 +73,5 @@ private: bool using_fram; #endif }; + +#endif // HAL_USE_EMPTY_STORAGE diff --git a/libraries/AP_HAL_ChibiOS/ToneAlarm.h b/libraries/AP_HAL_ChibiOS/ToneAlarm.h index 4fc3d33e89..ee28b5e9c7 100644 --- a/libraries/AP_HAL_ChibiOS/ToneAlarm.h +++ b/libraries/AP_HAL_ChibiOS/ToneAlarm.h @@ -1,6 +1,7 @@ #pragma once #include "AP_HAL_ChibiOS.h" + #include "ch.h" #include "hal.h" @@ -110,6 +111,8 @@ #define TONE_NUMBER_OF_TUNES 11 +#ifdef HAL_PWM_ALARM + namespace ChibiOS { class ToneAlarm { @@ -149,3 +152,4 @@ private: }; } +#endif // HAL_PWM_ALARM diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index 31f103dfbd..7325b5c4cd 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -186,6 +186,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) } #endif } else { +#if HAL_USE_SERIAL == TRUE if (_baudrate != 0) { bool was_initialised = _device_initialised; //setup Rx DMA @@ -248,6 +249,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) } } } +#endif // HAL_USE_SERIAL } if (_writebuf.get_size() && _readbuf.get_size()) { @@ -261,6 +263,7 @@ void UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) void UARTDriver::dma_tx_allocate(void) { +#if HAL_USE_SERIAL == TRUE osalDbgAssert(txdma == nullptr, "double DMA allocation"); txdma = STM32_DMA_STREAM(sdef.dma_tx_stream_id); chSysLock(); @@ -271,6 +274,7 @@ void UARTDriver::dma_tx_allocate(void) osalDbgAssert(!dma_allocated, "stream already allocated"); chSysUnlock(); dmaStreamSetPeripheral(txdma, &((SerialDriver*)sdef.serial)->usart->DR); +#endif // HAL_USE_SERIAL } void UARTDriver::dma_tx_deallocate(void) @@ -303,6 +307,7 @@ void UARTDriver::tx_complete(void* self, uint32_t flags) void UARTDriver::rx_irq_cb(void* self) { +#if HAL_USE_SERIAL == TRUE UARTDriver* uart_drv = (UARTDriver*)self; if (!uart_drv->sdef.dma_rx) { return; @@ -314,10 +319,12 @@ void UARTDriver::rx_irq_cb(void* self) //disable dma, triggering DMA transfer complete interrupt uart_drv->rxdma->stream->CR &= ~STM32_DMA_CR_EN; } +#endif // HAL_USE_SERIAL } void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags) { +#if HAL_USE_SERIAL == TRUE UARTDriver* uart_drv = (UARTDriver*)self; if (uart_drv->_lock_rx_in_timer_tick) { return; @@ -342,6 +349,7 @@ void UARTDriver::rxbuff_full_irq(void* self, uint32_t flags) if (uart_drv->_rts_is_active) { uart_drv->update_rts_line(); } +#endif // HAL_USE_SERIAL } void UARTDriver::begin(uint32_t b) @@ -360,7 +368,9 @@ void UARTDriver::end() sduStop((SerialUSBDriver*)sdef.serial); #endif } else { +#if HAL_USE_SERIAL == TRUE sdStop((SerialDriver*)sdef.serial); +#endif } _readbuf.set_size(0); _writebuf.set_size(0); @@ -558,14 +568,16 @@ void UARTDriver::write_pending_bytes_NODMA(uint32_t n) ByteBuffer::IoVec vec[2]; const auto n_vec = _writebuf.peekiovec(vec, n); for (int i = 0; i < n_vec; i++) { - int ret; + int ret = -1; if (sdef.is_usb) { ret = 0; #ifdef HAVE_USB_SERIAL ret = chnWriteTimeout((SerialUSBDriver*)sdef.serial, vec[i].data, vec[i].len, TIME_IMMEDIATE); #endif } else { +#if HAL_USE_SERIAL == TRUE ret = chnWriteTimeout((SerialDriver*)sdef.serial, vec[i].data, vec[i].len, TIME_IMMEDIATE); +#endif } if (ret < 0) { break; @@ -682,7 +694,9 @@ void UARTDriver::_timer_tick(void) #endif } else { ret = 0; +#if HAL_USE_SERIAL == TRUE ret = chnReadTimeout((SerialDriver*)sdef.serial, vec[i].data, vec[i].len, TIME_IMMEDIATE); +#endif } if (ret < 0) { break; @@ -721,7 +735,7 @@ void UARTDriver::set_flow_control(enum flow_control flowcontrol) // no hw flow control available return; } - +#if HAL_USE_SERIAL == TRUE _flow_control = flowcontrol; if (!_initialised) { // not ready yet, we just set variable for when we call begin @@ -755,6 +769,7 @@ void UARTDriver::set_flow_control(enum flow_control flowcontrol) ((SerialDriver*)(sdef.serial))->usart->CR3 &= ~USART_CR3_RTSE; break; } +#endif // HAL_USE_SERIAL } /* @@ -798,6 +813,7 @@ void UARTDriver::configure_parity(uint8_t v) // not possible return; } +#if HAL_USE_SERIAL == TRUE // stop and start to take effect sdStop((SerialDriver*)sdef.serial); @@ -826,6 +842,7 @@ void UARTDriver::configure_parity(uint8_t v) //because we will handle them via DMA ((SerialDriver*)sdef.serial)->usart->CR1 &= ~USART_CR1_RXNEIE; } +#endif // HAL_USE_SERIAL } /* @@ -837,6 +854,7 @@ void UARTDriver::set_stop_bits(int n) // not possible return; } +#if HAL_USE_SERIAL // stop and start to take effect sdStop((SerialDriver*)sdef.serial); @@ -854,6 +872,7 @@ void UARTDriver::set_stop_bits(int n) //because we will handle them via DMA ((SerialDriver*)sdef.serial)->usart->CR1 &= ~USART_CR1_RXNEIE; } +#endif // HAL_USE_SERIAL } diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index ebcbbe8ffe..650dff277e 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -90,7 +90,9 @@ private: uint32_t _baudrate; uint16_t tx_len; +#if HAL_USE_SERIAL == TRUE SerialConfig sercfg; +#endif const thread_t* _uart_owner_thd; struct { diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c index 65528353a6..d47945c06c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.c @@ -99,7 +99,12 @@ int asprintf(char **strp, const char *fmt, ...) int vprintf(const char *fmt, va_list arg) { +#ifdef HAL_STDOUT_SERIAL return chvprintf ((BaseSequentialStream*)&HAL_STDOUT_SERIAL, fmt, arg); +#else + (void)arg; + return strlen(fmt); +#endif } int printf(const char *fmt, ...) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 88ee2a3900..d26295cff3 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -100,6 +100,12 @@ def get_alt_function(mcu, pin, function): return alt_map[s] return None +def have_type_prefix(ptype): + '''return True if we have a peripheral starting with the given peripheral type''' + for t in bytype.keys(): + if t.startswith(ptype): + return True + return False def get_ADC1_chan(mcu, pin): '''return ADC1 channel for an analog pin''' @@ -290,10 +296,10 @@ def write_mcu_config(f): 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 %u\n\n' % get_config( - 'STDOUT_BAUDRATE', type=int)) + if get_config('STDOUT_SERIAL', required=False): + 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 %u\n\n' % get_config('STDOUT_BAUDRATE', type=int)) if 'SDIO' in bytype: f.write('// SDIO available, enable POSIX filesystem support\n') f.write('#define USE_POSIX\n\n') @@ -308,7 +314,7 @@ def write_mcu_config(f): f.write('#define HAL_USE_SERIAL_USB TRUE\n') if 'OTG2' in bytype: f.write('#define STM32_USB_USE_OTG2 TRUE\n') - if 'CAN1' in bytype or 'CAN2' in bytype or 'CAN3' in bytype: + if have_type_prefix('CAN'): enable_can(f) # write any custom STM32 defines for d in alllines: @@ -362,7 +368,7 @@ INCLUDE common.ld def write_USB_config(f): '''write USB config defines''' - if not 'OTG1' in bytype: + if not have_type_prefix('OTG'): return; f.write('// USB configuration\n') f.write('#define HAL_USB_VENDOR_ID %s\n' % get_config('USB_VENDOR', default=0x0483)) # default to ST @@ -505,13 +511,16 @@ def write_UART_config(f): def write_I2C_config(f): '''write I2C config defines''' + if not have_type_prefix('I2C'): + print("No I2C peripherals") + f.write('#define HAL_USE_I2C FALSE\n') + return if not 'I2C_ORDER' in config: error("Missing I2C_ORDER config") i2c_list = config['I2C_ORDER'] f.write('// I2C configuration\n') if len(i2c_list) == 0: - f.write('#define HAL_USE_I2C FALSE\n') - return + error("I2C_ORDER invalid") devlist = [] for dev in i2c_list: if not dev.startswith('I2C') or dev[3] not in "1234": @@ -547,6 +556,11 @@ def write_PWM_config(f): pwm_out.append(p) if p.type not in pwm_timers: pwm_timers.append(p.type) + + if not pwm_out: + print("No PWM output defined") + f.write('#define HAL_USE_PWM FALSE\n') + if rc_in is not None: a = rc_in.label.split('_') chan_str = a[1][2:]