diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index bf7efbcf1..1bdc89da6 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -184,7 +184,7 @@ /* Begin PBXLegacyTarget section */ 6581071511DE809D0049FB12 /* OpenPilotOSX */ = { isa = PBXLegacyTarget; - buildArgumentsString = ef_discoveryf4bare; + buildArgumentsString = ef_coptercontrol; buildConfigurationList = 6581071A11DE80A30049FB12 /* Build configuration list for PBXLegacyTarget "OpenPilotOSX" */; buildPhases = ( ); diff --git a/flight/modules/ComUsbBridge/ComUsbBridge.c b/flight/modules/ComUsbBridge/ComUsbBridge.c index 15f9f5db9..fdc9a0144 100644 --- a/flight/modules/ComUsbBridge/ComUsbBridge.c +++ b/flight/modules/ComUsbBridge/ComUsbBridge.c @@ -36,6 +36,8 @@ #include +#include + // **************** // Private functions diff --git a/flight/modules/GPS/GPS.c b/flight/modules/GPS/GPS.c index dfc2ac2c6..b9ce967c4 100644 --- a/flight/modules/GPS/GPS.c +++ b/flight/modules/GPS/GPS.c @@ -45,6 +45,7 @@ #include "WorldMagModel.h" #include "CoordinateConversions.h" #include +#include #include "GPS.h" #include "NMEA.h" diff --git a/flight/modules/Osd/osdoutput/osdoutput.c b/flight/modules/Osd/osdoutput/osdoutput.c index 7f18b4720..f600aa148 100644 --- a/flight/modules/Osd/osdoutput/osdoutput.c +++ b/flight/modules/Osd/osdoutput/osdoutput.c @@ -43,6 +43,8 @@ #include "hwsettings.h" #include "flightstatus.h" +#include + static bool osdoutputEnabled; enum osd_hk_sync { diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 9796e8f59..a3bbf0a77 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -69,6 +69,8 @@ #include "hwsettings.h" #include "taskinfo.h" +#include + // Private constants #define MAX_QUEUE_SIZE TELEM_QUEUE_SIZE // Three different stack size parameter are accepted for Telemetry(RX PIOS_TELEM_RX_STACK_SIZE) diff --git a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c index 43bbbe433..6114a963c 100644 --- a/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c +++ b/flight/modules/UAVOMSPBridge/UAVOMSPBridge.c @@ -68,6 +68,8 @@ #include "pios_sensors.h" +#include + #define PIOS_INCLUDE_MSP_BRIDGE diff --git a/flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c b/flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c index bca1ab6fa..03f97252d 100644 --- a/flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c +++ b/flight/modules/UAVOMavlinkBridge/UAVOMavlinkBridge.c @@ -58,6 +58,9 @@ #include "custom_types.h" +#include + + #define OPLINK_LOW_RSSI -110 #define OPLINK_HIGH_RSSI -10 diff --git a/flight/pios/common/pios_board_io.c b/flight/pios/common/pios_board_io.c index f3b2ac92e..7b742b758 100644 --- a/flight/pios/common/pios_board_io.c +++ b/flight/pios/common/pios_board_io.c @@ -110,6 +110,7 @@ uint32_t pios_com_debug_id; /* DebugConsole */ uint32_t pios_com_telem_usb_id; /* USB telemetry */ #ifdef PIOS_INCLUDE_USB_RCTX +# include "pios_usb_rctx_priv.h" uint32_t pios_usb_rctx_id; #endif @@ -255,10 +256,8 @@ void PIOS_BOARD_IO_Configure_USB() break; case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: #if defined(PIOS_INCLUDE_USB_RCTX) - { - if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { /* this will reinstall endpoint callbacks */ - PIOS_Assert(0); - } + if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { /* this will reinstall endpoint callbacks */ + PIOS_Assert(0); } #endif /* PIOS_INCLUDE_USB_RCTX */ break; @@ -266,9 +265,11 @@ void PIOS_BOARD_IO_Configure_USB() #endif /* PIOS_INCLUDE_USB_HID */ +#ifndef STM32F10X if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } +#endif } #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/pios/common/pios_dsm.c b/flight/pios/common/pios_dsm.c index 23d3601e3..af52ce985 100644 --- a/flight/pios/common/pios_dsm.c +++ b/flight/pios/common/pios_dsm.c @@ -126,9 +126,13 @@ static void PIOS_DSM_Bind(struct stm32_gpio *rxpin, uint8_t bind) GPIO_InitTypeDef bindInit = { .GPIO_Pin = rxpin->init.GPIO_Pin, .GPIO_Speed = GPIO_Speed_2MHz, +#ifdef STM32F10X + .GPIO_Mode = GPIO_Mode_Out_PP, +#else .GPIO_Mode = GPIO_Mode_OUT, .GPIO_OType = GPIO_OType_PP, .GPIO_PuPd = GPIO_PuPd_NOPULL +#endif }; GPIO_Init(rxpin->gpio, &bindInit); diff --git a/flight/pios/common/pios_usb_desc_hid_cdc.c b/flight/pios/common/pios_usb_desc_hid_cdc.c index d14add637..14bb69f61 100644 --- a/flight/pios/common/pios_usb_desc_hid_cdc.c +++ b/flight/pios/common/pios_usb_desc_hid_cdc.c @@ -332,6 +332,10 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = { .data_tx_ep = 1, }; +const struct pios_usb_rctx_cfg pios_usb_rctx_cfg = { + .data_if = 2, + .data_tx_ep = 1, +}; int32_t PIOS_USB_DESC_HID_CDC_Init(void) { diff --git a/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h b/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h index 6a3145a8b..77d3b2347 100644 --- a/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h +++ b/flight/pios/inc/pios_usb_desc_hid_cdc_priv.h @@ -33,11 +33,13 @@ #include #include #include +#include extern int32_t PIOS_USB_DESC_HID_CDC_Init(void); extern const struct pios_usb_cdc_cfg pios_usb_cdc_cfg; extern const struct pios_usb_hid_cfg pios_usb_hid_cfg; +extern const struct pios_usb_rctx_cfg pios_usb_rctx_cfg; #endif /* PIOS_USB_DESC_HID_CDC_PRIV_H */ diff --git a/flight/pios/stm32f10x/inc/pios_servo_config.h b/flight/pios/stm32f10x/inc/pios_servo_config.h new file mode 100644 index 000000000..b5464ebae --- /dev/null +++ b/flight/pios/stm32f10x/inc/pios_servo_config.h @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * + * @file pios_servp_config.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @brief Architecture specific macros and definitions + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program 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 program 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, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef PIOS_SERVO_CONFIG_H_ +#define PIOS_SERVO_CONFIG_H_ + +/** + * Generic servo pin configuration structure for an STM32F10x + */ +#define TIM_SERVO_CHANNEL_CONFIG(_timer, _channel, _gpio, _pin, _remap) \ +{ \ + .timer = _timer, \ + .timer_chan = TIM_Channel_##_channel, \ + .pin = { \ + .gpio = GPIO##_gpio, \ + .init = { \ + .GPIO_Pin = GPIO_Pin_##_pin, \ + .GPIO_Mode = GPIO_Mode_IPD, \ + .GPIO_Speed = GPIO_Speed_2MHz,\ + }, \ + .pin_source = GPIO_PinSource##_pin, \ + }, \ + .remap = _remap, \ +} + + +#endif /* PIOS_SERVO_CONFIG_H_ */ diff --git a/flight/pios/stm32f10x/pios_sys.c b/flight/pios/stm32f10x/pios_sys.c index 63791fa22..19a6061b6 100644 --- a/flight/pios/stm32f10x/pios_sys.c +++ b/flight/pios/stm32f10x/pios_sys.c @@ -53,9 +53,63 @@ void PIOS_SYS_Init(void) /* Init the delay system */ PIOS_DELAY_Init(); - /* Enable GPIOA, GPIOB, GPIOC, GPIOD, GPIOE and AFIO clocks */ - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD | RCC_APB2Periph_GPIOE | - RCC_APB2Periph_AFIO, ENABLE); + /* + * Turn on all the peripheral clocks. + * Micromanaging clocks makes no sense given the power situation in the system, so + * light up everything we might reasonably use here and just leave it on. + */ + + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1 | + RCC_AHBPeriph_DMA2, + ENABLE); + + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2 | + RCC_APB1Periph_TIM3 | + RCC_APB1Periph_TIM4 | + RCC_APB1Periph_TIM5 | + RCC_APB1Periph_TIM6 | + RCC_APB1Periph_TIM7 | + RCC_APB1Periph_TIM12 | + RCC_APB1Periph_TIM13 | + RCC_APB1Periph_TIM14 | + RCC_APB1Periph_WWDG | + RCC_APB1Periph_SPI2 | + RCC_APB1Periph_SPI3 | + RCC_APB1Periph_USART2 | + RCC_APB1Periph_USART3 | + RCC_APB1Periph_UART4 | + RCC_APB1Periph_UART5 | + RCC_APB1Periph_I2C1 | + RCC_APB1Periph_I2C2 | + RCC_APB1Periph_USB | + RCC_APB1Periph_CAN1 | + RCC_APB1Periph_CAN2 | + RCC_APB1Periph_BKP | + RCC_APB1Periph_PWR | + RCC_APB1Periph_DAC, + ENABLE); + + + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | + RCC_APB2Periph_GPIOB | + RCC_APB2Periph_GPIOC | + RCC_APB2Periph_GPIOD | + RCC_APB2Periph_GPIOE | + RCC_APB2Periph_ADC1 | + RCC_APB2Periph_ADC2 | + RCC_APB2Periph_ADC3 | + RCC_APB2Periph_TIM1 | + RCC_APB2Periph_TIM8 | + RCC_APB2Periph_TIM9 | + RCC_APB2Periph_TIM10 | + RCC_APB2Periph_TIM11 | + RCC_APB2Periph_TIM15 | + RCC_APB2Periph_TIM16 | + RCC_APB2Periph_TIM17 | + RCC_APB2Periph_SPI1 | + RCC_APB2Periph_USART1 | + RCC_APB2Periph_AFIO, + ENABLE); #if (PIOS_USB_ENABLED) /* Ensure that pull-up is active on detect pin */ diff --git a/flight/pios/stm32f10x/pios_usart.c b/flight/pios/stm32f10x/pios_usart.c index 82cd577cd..bc165f38f 100644 --- a/flight/pios/stm32f10x/pios_usart.c +++ b/flight/pios/stm32f10x/pios_usart.c @@ -43,6 +43,7 @@ static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback r static void PIOS_USART_RegisterTxCallback(uint32_t usart_id, pios_com_callback tx_out_cb, uint32_t context); static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail); static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail); +static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); const struct pios_com_driver pios_usart_com_driver = { .set_baud = PIOS_USART_ChangeBaud, @@ -51,6 +52,7 @@ const struct pios_com_driver pios_usart_com_driver = { .rx_start = PIOS_USART_RxStart, .bind_tx_cb = PIOS_USART_RegisterTxCallback, .bind_rx_cb = PIOS_USART_RegisterRxCallback, + .ioctl = PIOS_USART_Ioctl, }; enum pios_usart_dev_magic { @@ -67,6 +69,7 @@ struct pios_usart_dev { uint32_t tx_out_context; uint32_t rx_dropped; + uint8_t irq_channel; }; static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) @@ -74,6 +77,30 @@ static bool PIOS_USART_validate(struct pios_usart_dev *usart_dev) return usart_dev->magic == PIOS_USART_DEV_MAGIC; } +const struct pios_usart_cfg *PIOS_USART_GetConfig(uint32_t usart_id) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + return usart_dev->cfg; +} + +static int32_t PIOS_USART_SetIrqPrio(struct pios_usart_dev *usart_dev, uint8_t irq_prio) +{ + NVIC_InitTypeDef init = { + .NVIC_IRQChannel = usart_dev->irq_channel, + .NVIC_IRQChannelPreemptionPriority = irq_prio, + .NVIC_IRQChannelCmd = ENABLE, + }; + + NVIC_Init(&init); + + return 0; +} + #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_usart_dev *PIOS_USART_alloc(void) { @@ -155,8 +182,8 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) /* Bind the configuration to the device instance */ usart_dev->cfg = cfg; - /* Copy the comm parameter structure */ - usart_dev->init = cfg->init; + /* Initialize the comm parameter structure */ + USART_StructInit(&usart_dev->init); // 9600 8n1 /* Enable the USART Pins Software Remapping */ if (usart_dev->cfg->remap) { @@ -167,19 +194,6 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) GPIO_Init(usart_dev->cfg->rx.gpio, &usart_dev->cfg->rx.init); GPIO_Init(usart_dev->cfg->tx.gpio, &usart_dev->cfg->tx.init); - /* Enable USART clock */ - switch ((uint32_t)usart_dev->cfg->regs) { - case (uint32_t)USART1: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); - break; - case (uint32_t)USART2: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); - break; - case (uint32_t)USART3: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); - break; - } - /* Configure the USART */ USART_Init(usart_dev->cfg->regs, &usart_dev->init); @@ -189,15 +203,20 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) switch ((uint32_t)usart_dev->cfg->regs) { case (uint32_t)USART1: PIOS_USART_1_id = (uint32_t)usart_dev; + usart_dev->irq_channel = USART1_IRQn; break; case (uint32_t)USART2: PIOS_USART_2_id = (uint32_t)usart_dev; + usart_dev->irq_channel = USART2_IRQn; break; case (uint32_t)USART3: PIOS_USART_3_id = (uint32_t)usart_dev; + usart_dev->irq_channel = USART3_IRQn; break; } - NVIC_Init(&usart_dev->cfg->irq.init); + + PIOS_USART_SetIrqPrio(usart_dev, PIOS_IRQ_PRIO_MID); + USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); @@ -430,6 +449,34 @@ static void PIOS_USART_generic_irq_handler(uint32_t usart_id) #endif /* PIOS_INCLUDE_FREERTOS */ } +static int32_t PIOS_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + switch (ctl) { + case PIOS_IOCTL_USART_SET_IRQ_PRIO: + return PIOS_USART_SetIrqPrio(usart_dev, *(uint8_t *)param); + + case PIOS_IOCTL_USART_GET_RXGPIO: + *(struct stm32_gpio *)param = usart_dev->cfg->rx; + break; + case PIOS_IOCTL_USART_GET_TXGPIO: + *(struct stm32_gpio *)param = usart_dev->cfg->tx; + break; + default: + if (usart_dev->cfg->ioctl) { + return usart_dev->cfg->ioctl(usart_id, ctl, param); + } + return -1; + } + + return 0; +} + #endif /* PIOS_INCLUDE_USART */ /** diff --git a/flight/pios/stm32f4xx/pios_usart.c b/flight/pios/stm32f4xx/pios_usart.c index 950cbac77..c6f72674d 100644 --- a/flight/pios/stm32f4xx/pios_usart.c +++ b/flight/pios/stm32f4xx/pios_usart.c @@ -208,7 +208,7 @@ int32_t PIOS_USART_Init(uint32_t *usart_id, const struct pios_usart_cfg *cfg) /* Bind the configuration to the device instance */ usart_dev->cfg = cfg; - /* Copy the comm parameter structure */ + /* Initialize the comm parameter structure */ USART_StructInit(&usart_dev->init); // 9600 8n1 /* Map pins to USART function */ diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index 4222540ef..f44e552f5 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -549,322 +549,59 @@ static const struct pios_tim_clock_cfg tim_4_cfg = { }, }; +#include "pios_servo_config.h" + static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM4, 1, B, 6, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 2, B, 5, GPIO_PartialRemap_TIM3), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 1, A, 0, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 2, A, 1, 0), }; static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM4, 4, B, 9, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM4, 3, B, 8, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM4, 2, B, 7, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM1, 1, A, 8, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 1, B, 4, GPIO_PartialRemap_TIM3), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2, 0), }; static const struct pios_tim_channel pios_tim_servoport_rcvrport_pins[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM4, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM4, 4, B, 9, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM4, 3, B, 8, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM4, 2, B, 7, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM1, 1, A, 8, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 1, B, 4, GPIO_PartialRemap_TIM3), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 3, A, 2, 0), // Receiver port pins // S3-S6 inputs are used as outputs in this case - { - .timer = TIM3, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM3, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_0, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM2, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_1, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, + TIM_SERVO_CHANNEL_CONFIG(TIM3, 3, B, 0, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM3, 4, B, 1, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 1, A, 0, 0), + TIM_SERVO_CHANNEL_CONFIG(TIM2, 2, A, 1, 0), }; -static const struct pios_tim_channel pios_tim_ppm_flexi_port = { - .timer = TIM2, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap2_TIM2, -}; +static const struct pios_tim_channel pios_tim_ppm_flexi_port = TIM_SERVO_CHANNEL_CONFIG(TIM2, 4, B, 11, GPIO_PartialRemap2_TIM2); #if defined(PIOS_INCLUDE_USART) #include "pios_usart_priv.h" -static const struct pios_usart_cfg pios_usart_generic_main_cfg = { +// Inverter for SBUS handling +#define MAIN_USART_INVERTER_GPIO GPIOB +#define MAIN_USART_INVERTER_PIN GPIO_Pin_2 +#define MAIN_USART_INVERTER_ENABLE Bit_SET +#define MAIN_USART_INVERTER_DISABLE Bit_RESET + +static int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param); + +static const struct pios_usart_cfg pios_usart_main_cfg = { .regs = USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { .gpio = GPIOA, .init = { @@ -881,26 +618,11 @@ static const struct pios_usart_cfg pios_usart_generic_main_cfg = { .GPIO_Mode = GPIO_Mode_AF_PP, }, }, + .ioctl = PIOS_BOARD_USART_Ioctl, }; -static const struct pios_usart_cfg pios_usart_generic_flexi_cfg = { +static const struct pios_usart_cfg pios_usart_flexi_cfg = { .regs = USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { .gpio = GPIOB, .init = { @@ -919,417 +641,6 @@ static const struct pios_usart_cfg pios_usart_generic_flexi_cfg = { }, }; -#if defined(PIOS_INCLUDE_DSM) -/* - * Spektrum/JR DSM USART - */ -#include - -static const struct pios_usart_cfg pios_usart_dsm_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - -static const struct pios_dsm_cfg pios_dsm_main_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_dsm_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - -static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { - .bind = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, -}; - -#endif /* PIOS_INCLUDE_DSM */ - -#if defined(PIOS_INCLUDE_HOTT) -/* - * HOTT USART - */ -#include - -static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; -#endif /* PIOS_INCLUDE_HOTT */ - -#if defined(PIOS_INCLUDE_EXBUS) -/* - * EXBUS USART - */ -#include - -static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 125000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; -#endif /* PIOS_INCLUDE_EXBUS */ - -#if defined(PIOS_INCLUDE_SRXL) -/* - * SRXL USART - */ -#include - -static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - -#endif /* PIOS_INCLUDE_SRXL */ - -#if defined(PIOS_INCLUDE_IBUS) -/* - * IBUS USART - */ -#include - -static const struct pios_usart_cfg pios_usart_ibus_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - -#endif /* PIOS_INCLUDE_IBUS */ - -#if defined(PIOS_INCLUDE_SBUS) -/* - * S.Bus USART - */ -#include - -static const struct pios_usart_cfg pios_usart_sbus_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 100000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_Even, - .USART_StopBits = USART_StopBits_2, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - -static const struct pios_sbus_cfg pios_sbus_cfg = { - /* Inverter configuration */ - .inv = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Mode = GPIO_Mode_Out_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .gpio_clk_func = RCC_APB2PeriphClockCmd, - .gpio_clk_periph = RCC_APB2Periph_GPIOB, - .gpio_inv_enable = Bit_SET, -}; - -#endif /* PIOS_INCLUDE_SBUS */ - -/* - * HK OSD - */ -static const struct pios_usart_cfg pios_usart_hkosd_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, -}; - -static const struct pios_usart_cfg pios_usart_hkosd_flexi_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 57600, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx | USART_Mode_Tx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, -}; - - #endif /* PIOS_INCLUDE_USART */ #if defined(PIOS_INCLUDE_COM) @@ -1653,6 +964,23 @@ static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = { .vsense_active_low = false }; +const struct pios_usb_cfg *PIOS_BOARD_HW_DEFS_GetUsbCfg(uint32_t board_revision) +{ + switch (board_revision) { + case BOARD_REVISION_CC: + return &pios_usb_main_cfg_cc; + + break; + case BOARD_REVISION_CC3D: + return &pios_usb_main_cfg_cc3d; + + break; + default: + PIOS_DEBUG_Assert(0); + } + return NULL; +} + #include "pios_usb_board_data_priv.h" #include "pios_usb_desc_hid_cdc_priv.h" #include "pios_usb_desc_hid_only_priv.h" @@ -1665,36 +993,60 @@ static const struct pios_usb_cfg pios_usb_main_cfg_cc3d = { #endif /* PIOS_INCLUDE_COM_MSG */ -#if defined(PIOS_INCLUDE_USB_HID) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 2, - .data_rx_ep = 1, - .data_tx_ep = 1, +/** + * Configuration for MPU6000 chip + */ +#if defined(PIOS_INCLUDE_MPU6000) +#include "pios_mpu6000.h" +#include "pios_mpu6000_config.h" +static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { + .vector = PIOS_MPU6000_IRQHandler, + .line = EXTI_Line3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line3, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, }; -#endif /* PIOS_INCLUDE_USB_HID */ - -#if defined(PIOS_INCLUDE_USB_RCTX) -#include - -const struct pios_usb_rctx_cfg pios_usb_rctx_cfg = { - .data_if = 2, - .data_tx_ep = 1, +static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { + .exti_cfg = &pios_exti_mpu6000_cfg, + .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, + // Clock at 8 khz, downsampled by 8 for 1000 Hz + .Smpl_rate_div_no_dlp = 7, + // Clock at 1 khz, downsampled by 1 for 1000 Hz + .Smpl_rate_div_dlp = 0, + .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, + .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, + .User_ctl = PIOS_MPU6000_USERCTL_DIS_I2C, + .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, + .accel_range = PIOS_MPU6000_ACCEL_8G, + .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, + .filter = PIOS_MPU6000_LOWPASS_256_HZ, + .orientation = PIOS_MPU6000_TOP_180DEG, + .fast_prescaler = PIOS_SPI_PRESCALER_4, + .std_prescaler = PIOS_SPI_PRESCALER_64, + .max_downsample = 2 }; +#endif /* PIOS_INCLUDE_MPU6000 */ -#endif /* PIOS_INCLUDE_USB_RCTX */ -#if defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 0, - .ctrl_tx_ep = 2, - - .data_if = 1, - .data_rx_ep = 3, - .data_tx_ep = 3, -}; -#endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/targets/boards/coptercontrol/bootloader/main.c b/flight/targets/boards/coptercontrol/bootloader/main.c index 20549d1fa..3495976df 100644 --- a/flight/targets/boards/coptercontrol/bootloader/main.c +++ b/flight/targets/boards/coptercontrol/bootloader/main.c @@ -34,6 +34,7 @@ #include #include #include +#include extern void FLASH_Download(); #define BSL_HOLD_STATE ((PIOS_USB_DETECT_GPIO_PORT->IDR & PIOS_USB_DETECT_GPIO_PIN) ? 0 : 1) diff --git a/flight/targets/boards/coptercontrol/bootloader/pios_board.c b/flight/targets/boards/coptercontrol/bootloader/pios_board.c index 4fd27d5ee..dda9fa5f5 100644 --- a/flight/targets/boards/coptercontrol/bootloader/pios_board.c +++ b/flight/targets/boards/coptercontrol/bootloader/pios_board.c @@ -44,6 +44,14 @@ uint32_t pios_com_telem_usb_id; * called from System/openpilot.c */ static bool board_init_complete = false; + +#if defined(PIOS_INCLUDE_USART) +static int32_t PIOS_BOARD_USART_Ioctl(__attribute__((unused)) uint32_t usart_id, __attribute__((unused)) uint32_t ctl, __attribute__((unused)) void *param) +{ + return -1; +} +#endif + void PIOS_Board_Init(void) { if (board_init_complete) { @@ -84,7 +92,7 @@ void PIOS_Board_Init(void) } #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { + if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_only_cfg, pios_usb_id)) { PIOS_Assert(0); } if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index 7556dff22..fdc6f517f 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -30,12 +30,11 @@ #include #include #include -#include -#include #include #include #include + #ifdef PIOS_INCLUDE_INSTRUMENTATION #include #endif @@ -43,6 +42,8 @@ #include #endif +#include + /* * Pull in the board-specific static HW definitions. * Including .c files is a bit ugly but this allows all of @@ -53,185 +54,59 @@ */ #include "../board_hw_defs.c" -/* One slot per selectable receiver group. - * eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS - * NOTE: No slot in this map for NONE. - */ -uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; - static SystemAlarmsExtendedAlarmStatusOptions CopterControlConfigHook(); static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); -#define PIOS_COM_TELEM_RF_RX_BUF_LEN 32 -#define PIOS_COM_TELEM_RF_TX_BUF_LEN 12 - -#define PIOS_COM_GPS_RX_BUF_LEN 32 - -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 - -#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 -#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 - -#define PIOS_COM_HKOSD_TX_BUF_LEN 22 - -#define PIOS_COM_MSP_TX_BUF_LEN 128 -#define PIOS_COM_MSP_RX_BUF_LEN 64 - -#define PIOS_COM_MAVLINK_TX_BUF_LEN 128 - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -#define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 -uint32_t pios_com_debug_id; -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -uint32_t pios_com_telem_rf_id; -uint32_t pios_com_telem_usb_id; -uint32_t pios_com_vcp_id; -uint32_t pios_com_gps_id; -uint32_t pios_com_bridge_id; -uint32_t pios_com_hkosd_id; -uint32_t pios_com_msp_id; -uint32_t pios_com_mavlink_id; -uint32_t pios_usb_rctx_id; - uintptr_t pios_uavo_settings_fs_id; uintptr_t pios_user_fs_id = 0; - -/* - * Setup a com port based on the passed cfg, driver and buffer sizes. - * tx size = 0 make the port rx only - * rx size = 0 make the port tx only - * having both tx and rx size = 0 is not valid and will fail further down in PIOS_COM_Init() - */ -static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, uint16_t rx_buf_len, uint16_t tx_buf_len, - const struct pios_com_driver *com_driver, uint32_t *pios_com_id) -{ - uint32_t pios_usart_id; - - if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) { - PIOS_Assert(0); - } - - uint8_t *rx_buffer = 0, *tx_buffer = 0; - - if (rx_buf_len > 0) { - rx_buffer = (uint8_t *)pios_malloc(rx_buf_len); - PIOS_Assert(rx_buffer); - } - - if (tx_buf_len > 0) { - tx_buffer = (uint8_t *)pios_malloc(tx_buf_len); - PIOS_Assert(tx_buffer); - } - - if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id, - rx_buffer, rx_buf_len, - tx_buffer, tx_buf_len)) { - PIOS_Assert(0); - } -} - -static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg, - const struct pios_com_driver *usart_com_driver, - ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind) -{ - uint32_t pios_usart_dsm_id; - - if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_id; - if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver, - pios_usart_dsm_id, *bind)) { - PIOS_Assert(0); - } - - uint32_t pios_dsm_rcvr_id; - if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id; -} - -static void PIOS_Board_configure_ibus(const struct pios_usart_cfg *usart_cfg) -{ - uint32_t pios_usart_ibus_id; - - if (PIOS_USART_Init(&pios_usart_ibus_id, usart_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_ibus_id; - if (PIOS_IBUS_Init(&pios_ibus_id, &pios_usart_com_driver, pios_usart_ibus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_ibus_rcvr_id; - if (PIOS_RCVR_Init(&pios_ibus_rcvr_id, &pios_ibus_rcvr_driver, pios_ibus_id)) { - PIOS_Assert(0); - } - - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_IBUS] = pios_ibus_rcvr_id; -} - -/** - * Configuration for MPU6000 chip - */ -#if defined(PIOS_INCLUDE_MPU6000) -#include "pios_mpu6000.h" -#include "pios_mpu6000_config.h" -static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = { - .vector = PIOS_MPU6000_IRQHandler, - .line = EXTI_Line3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = EXTI3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = EXTI_Line3, // matches above GPIO pin - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Rising, - .EXTI_LineCmd = ENABLE, - }, - }, +static const PIOS_BOARD_IO_UART_Function main_function_map[] = { + [HWSETTINGS_CC_MAINPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_CC_MAINPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_CC_MAINPORT_SBUS] = PIOS_BOARD_IO_UART_SBUS, + [HWSETTINGS_CC_MAINPORT_DSM] = PIOS_BOARD_IO_UART_DSM_MAIN, + [HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_CC_MAINPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_CC_MAINPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + [HWSETTINGS_CC_MAINPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_CC_MAINPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; -static const struct pios_mpu6000_cfg pios_mpu6000_cfg = { - .exti_cfg = &pios_exti_mpu6000_cfg, - .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT, - // Clock at 8 khz, downsampled by 8 for 1000 Hz - .Smpl_rate_div_no_dlp = 7, - // Clock at 1 khz, downsampled by 1 for 1000 Hz - .Smpl_rate_div_dlp = 0, - .interrupt_cfg = PIOS_MPU6000_INT_CLR_ANYRD, - .interrupt_en = PIOS_MPU6000_INTEN_DATA_RDY, - .User_ctl = PIOS_MPU6000_USERCTL_DIS_I2C, - .Pwr_mgmt_clk = PIOS_MPU6000_PWRMGMT_PLL_X_CLK, - .accel_range = PIOS_MPU6000_ACCEL_8G, - .gyro_range = PIOS_MPU6000_SCALE_2000_DEG, - .filter = PIOS_MPU6000_LOWPASS_256_HZ, - .orientation = PIOS_MPU6000_TOP_180DEG, - .fast_prescaler = PIOS_SPI_PRESCALER_4, - .std_prescaler = PIOS_SPI_PRESCALER_64, - .max_downsample = 2 +static const PIOS_BOARD_IO_UART_Function flexi_function_map[] = { + [HWSETTINGS_CC_FLEXIPORT_TELEMETRY] = PIOS_BOARD_IO_UART_TELEMETRY, + [HWSETTINGS_CC_FLEXIPORT_GPS] = PIOS_BOARD_IO_UART_GPS, + [HWSETTINGS_CC_FLEXIPORT_DSM] = PIOS_BOARD_IO_UART_DSM_FLEXI, + [HWSETTINGS_CC_FLEXIPORT_EXBUS] = PIOS_BOARD_IO_UART_EXBUS, + [HWSETTINGS_CC_FLEXIPORT_HOTTSUMD] = PIOS_BOARD_IO_UART_HOTT_SUMD, + [HWSETTINGS_CC_FLEXIPORT_HOTTSUMH] = PIOS_BOARD_IO_UART_HOTT_SUMH, + [HWSETTINGS_CC_FLEXIPORT_SRXL] = PIOS_BOARD_IO_UART_SRXL, + [HWSETTINGS_CC_FLEXIPORT_IBUS] = PIOS_BOARD_IO_UART_IBUS, + [HWSETTINGS_CC_FLEXIPORT_DEBUGCONSOLE] = PIOS_BOARD_IO_UART_DEBUGCONSOLE, + [HWSETTINGS_CC_FLEXIPORT_COMBRIDGE] = PIOS_BOARD_IO_UART_COMBRIDGE, + [HWSETTINGS_CC_FLEXIPORT_OSDHK] = PIOS_BOARD_IO_UART_OSDHK, + [HWSETTINGS_CC_FLEXIPORT_MSP] = PIOS_BOARD_IO_UART_MSP, + [HWSETTINGS_CC_FLEXIPORT_MAVLINK] = PIOS_BOARD_IO_UART_MAVLINK, }; -#endif /* PIOS_INCLUDE_MPU6000 */ + +int32_t PIOS_BOARD_USART_Ioctl(uint32_t usart_id, uint32_t ctl, void *param) +{ + const struct pios_usart_cfg *usart_cfg = PIOS_USART_GetConfig(usart_id); + + switch (ctl) { + case PIOS_IOCTL_USART_SET_INVERTED: + if (usart_cfg->regs == pios_usart_main_cfg.regs) { /* main port */ + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + (*(enum PIOS_USART_Inverted *)param & PIOS_USART_Inverted_Rx) ? MAIN_USART_INVERTER_ENABLE : MAIN_USART_INVERTER_DISABLE); + + return 0; + } + break; + } + + return -1; +} + /** * PIOS_Board_Init() @@ -352,368 +227,53 @@ void PIOS_Board_Init(void) PIOS_TIM_InitClock(&tim_4_cfg); #if defined(PIOS_INCLUDE_USB) - /* Initialize board specific USB data */ - PIOS_USB_BOARD_DATA_Init(); - - - /* Flags to determine if various USB interfaces are advertised */ - bool usb_hid_present = false; - bool usb_cdc_present = false; - -#if defined(PIOS_INCLUDE_USB_CDC) - if (PIOS_USB_DESC_HID_CDC_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; - usb_cdc_present = true; -#else - if (PIOS_USB_DESC_HID_ONLY_Init()) { - PIOS_Assert(0); - } - usb_hid_present = true; + PIOS_BOARD_IO_Configure_USB(); #endif - uint32_t pios_usb_id; - - switch (bdinfo->board_rev) { - case BOARD_REVISION_CC: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); - break; - case BOARD_REVISION_CC3D: - PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); - break; - default: - PIOS_Assert(0); + /* Configure FlexiPort */ + uint8_t hwsettings_flexiport; + HwSettingsCC_FlexiPortGet(&hwsettings_flexiport); + + if (hwsettings_flexiport < NELEMENTS(flexi_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_flexi_cfg, flexi_function_map[hwsettings_flexiport]); } -#if defined(PIOS_INCLUDE_USB_CDC) - - uint8_t hwsettings_usb_vcpport; - /* Configure the USB VCP port */ - HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); - - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; - } - - switch (hwsettings_usb_vcpport) { - case HWSETTINGS_USB_VCPPORT_DISABLED: - break; - case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_COMBRIDGE: -#if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, - tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - NULL, 0, - tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - } -#endif /* PIOS_INCLUDE_USB_CDC */ - -#if defined(PIOS_INCLUDE_USB_HID) - /* Configure the usb HID port */ - uint8_t hwsettings_usb_hidport; - HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); - - if (!usb_hid_present) { - /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ - hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; - } - - switch (hwsettings_usb_hidport) { - case HWSETTINGS_USB_HIDPORT_DISABLED: - break; - case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: -#if defined(PIOS_INCLUDE_COM) - { - uint32_t pios_usb_hid_id; - if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: -#if defined(PIOS_INCLUDE_USB_RCTX) - { - if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - } -#endif /* PIOS_INCLUDE_USB_RCTX */ - break; - } - -#endif /* PIOS_INCLUDE_USB_HID */ - -#endif /* PIOS_INCLUDE_USB */ - - /* Configure the main IO port */ - uint8_t hwsettings_DSMxBind; - HwSettingsDSMxBindGet(&hwsettings_DSMxBind); - uint8_t hwsettings_cc_mainport; - HwSettingsCC_MainPortGet(&hwsettings_cc_mainport); - - switch (hwsettings_cc_mainport) { - case HWSETTINGS_CC_MAINPORT_DISABLED: - break; - case HWSETTINGS_CC_MAINPORT_TELEMETRY: -#if defined(PIOS_INCLUDE_TELEMETRY_RF) - PIOS_Board_configure_com(&pios_usart_generic_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); -#endif /* PIOS_INCLUDE_TELEMETRY_RF */ - break; - case HWSETTINGS_CC_MAINPORT_SBUS: -#if defined(PIOS_INCLUDE_SBUS) - { - uint32_t pios_usart_sbus_id; - if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_id; - if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; - } -#endif /* PIOS_INCLUDE_SBUS */ - break; - case HWSETTINGS_CC_MAINPORT_GPS: -#if defined(PIOS_INCLUDE_GPS) - PIOS_Board_configure_com(&pios_usart_generic_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, 0, &pios_usart_com_driver, &pios_com_gps_id); -#endif /* PIOS_INCLUDE_GPS */ - break; - case HWSETTINGS_CC_MAINPORT_DSM: -#if defined(PIOS_INCLUDE_DSM) - PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); -#endif /* PIOS_INCLUDE_DSM */ - break; - case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - PIOS_Board_configure_com(&pios_usart_generic_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_CC_MAINPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_generic_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_CC_MAINPORT_MSP: - PIOS_Board_configure_com(&pios_usart_generic_main_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_CC_MAINPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_generic_main_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_CC_MAINPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, 0, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; - } - - /* Configure the flexi port */ - uint8_t hwsettings_cc_flexiport; - HwSettingsCC_FlexiPortGet(&hwsettings_cc_flexiport); - - switch (hwsettings_cc_flexiport) { - case HWSETTINGS_CC_FLEXIPORT_DISABLED: - break; - case HWSETTINGS_CC_FLEXIPORT_TELEMETRY: -#if defined(PIOS_INCLUDE_TELEMETRY_RF) - PIOS_Board_configure_com(&pios_usart_generic_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); -#endif /* PIOS_INCLUDE_TELEMETRY_RF */ - break; - case HWSETTINGS_CC_FLEXIPORT_COMBRIDGE: - PIOS_Board_configure_com(&pios_usart_generic_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); - break; - case HWSETTINGS_CC_FLEXIPORT_MSP: - PIOS_Board_configure_com(&pios_usart_generic_flexi_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id); - break; - case HWSETTINGS_CC_FLEXIPORT_MAVLINK: - PIOS_Board_configure_com(&pios_usart_generic_flexi_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); - break; - case HWSETTINGS_CC_FLEXIPORT_GPS: -#if defined(PIOS_INCLUDE_GPS) - PIOS_Board_configure_com(&pios_usart_generic_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, 0, &pios_usart_com_driver, &pios_com_gps_id); -#endif /* PIOS_INCLUDE_GPS */ - break; - case HWSETTINGS_CC_FLEXIPORT_PPM: -#if defined(PIOS_INCLUDE_PPM_FLEXI) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } -#endif /* PIOS_INCLUDE_PPM_FLEXI */ - break; - case HWSETTINGS_CC_FLEXIPORT_DSM: -#if defined(PIOS_INCLUDE_DSM) - PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, - &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT, &hwsettings_DSMxBind); -#endif /* PIOS_INCLUDE_DSM */ - break; - - case HWSETTINGS_CC_FLEXIPORT_HOTTSUMD: - case HWSETTINGS_CC_FLEXIPORT_HOTTSUMH: -#if defined(PIOS_INCLUDE_HOTT) - { - uint32_t pios_usart_hott_id; - if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_flexi_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_id; - if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, - hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_rcvr_id; - if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT] = pios_hott_rcvr_id; - } -#endif /* PIOS_INCLUDE_HOTT */ - break; - - case HWSETTINGS_CC_FLEXIPORT_EXBUS: -#if defined(PIOS_INCLUDE_EXBUS) - { - uint32_t pios_usart_exbus_id; - if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_flexi_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_id; - if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS] = pios_exbus_rcvr_id; - } -#endif /* PIOS_INCLUDE_EXBUS */ - break; - - case HWSETTINGS_CC_FLEXIPORT_SRXL: -#if defined(PIOS_INCLUDE_SRXL) - { - uint32_t pios_usart_srxl_id; - if (PIOS_USART_Init(&pios_usart_srxl_id, &pios_usart_srxl_flexi_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_srxl_id; - if (PIOS_SRXL_Init(&pios_srxl_id, &pios_usart_com_driver, pios_usart_srxl_id)) { - PIOS_Assert(0); - } - - uint32_t pios_srxl_rcvr_id; - if (PIOS_RCVR_Init(&pios_srxl_rcvr_id, &pios_srxl_rcvr_driver, pios_srxl_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL] = pios_srxl_rcvr_id; - } -#endif /* PIOS_INCLUDE_SRXL */ - break; - - case HWSETTINGS_CC_FLEXIPORT_IBUS: -#if defined(PIOS_INCLUDE_IBUS) - PIOS_Board_configure_ibus(&pios_usart_ibus_flexi_cfg); -#endif /* PIOS_INCLUDE_IBUS */ - break; - - case HWSETTINGS_CC_FLEXIPORT_DEBUGCONSOLE: -#if defined(PIOS_INCLUDE_COM) -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) - PIOS_Board_configure_com(&pios_usart_generic_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ -#endif /* PIOS_INCLUDE_COM */ - break; - case HWSETTINGS_CC_FLEXIPORT_I2C: + switch(hwsettings_flexiport) { + case HWSETTINGS_CC_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) - { if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { PIOS_Assert(0); } - } #endif /* PIOS_INCLUDE_I2C */ - break; - case HWSETTINGS_CC_FLEXIPORT_OSDHK: - PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, 0, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); - break; + break; + case HWSETTINGS_CC_FLEXIPORT_PPM: +#if defined(PIOS_INCLUDE_PPM_FLEXI) + PIOS_BOARD_IO_Configure_PPM(&pios_ppm_flexi_cfg); +#endif /* PIOS_INCLUDE_PPM_FLEXI */ + break; + } + + /* Configure main USART port */ + + /* Initialize inverter gpio and set it to off */ + { + GPIO_InitTypeDef inverterGPIOInit = { + .GPIO_Pin = MAIN_USART_INVERTER_PIN, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }; + + GPIO_Init(MAIN_USART_INVERTER_GPIO, &inverterGPIOInit); + GPIO_WriteBit(MAIN_USART_INVERTER_GPIO, + MAIN_USART_INVERTER_PIN, + MAIN_USART_INVERTER_DISABLE); + } + + uint8_t hwsettings_mainport; + HwSettingsCC_MainPortGet(&hwsettings_mainport); + + if (hwsettings_mainport < NELEMENTS(main_function_map)) { + PIOS_BOARD_IO_Configure_UART(&pios_usart_main_cfg, main_function_map[hwsettings_mainport]); } /* Configure the rcvr port */ @@ -731,79 +291,32 @@ void PIOS_Board_Init(void) break; case HWSETTINGS_CC_RCVRPORT_PWMNOONESHOT: #if defined(PIOS_INCLUDE_PWM) - { - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); - - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; - } + PIOS_BOARD_IO_Configure_PWM(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_CC_RCVRPORT_PPMNOONESHOT: case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTSNOONESHOT: case HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT: #if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - if (hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT) { - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_pin8_cfg); - } else { - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - } - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } + PIOS_BOARD_IO_Configure_PPM((hwsettings_rcvrport == HWSETTINGS_CC_RCVRPORT_PPM_PIN8ONESHOT) ? &pios_ppm_pin8_cfg : &pios_ppm_cfg); #endif /* PIOS_INCLUDE_PPM */ break; case HWSETTINGS_CC_RCVRPORT_PPMPWMNOONESHOT: /* This is a combination of PPM and PWM inputs */ #if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; - } + PIOS_BOARD_IO_Configure_PPM(&pios_ppm_cfg); #endif /* PIOS_INCLUDE_PPM */ #if defined(PIOS_INCLUDE_PWM) - { - uint32_t pios_pwm_id; - PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); - - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; - } + PIOS_BOARD_IO_Configure_PWM(&pios_pwm_with_ppm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_CC_RCVRPORT_OUTPUTSONESHOT: break; } -#if defined(PIOS_INCLUDE_GCSRCVR) - GCSReceiverInitialize(); - uint32_t pios_gcsrcvr_id; - PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; -#endif /* PIOS_INCLUDE_GCSRCVR */ +#ifdef PIOS_INCLUDE_GCSRCVR + PIOS_BOARD_IO_Configure_GCSRCVR(); +#endif /* Remap AFIO pin for PB4 (Servo 5 Out)*/ GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE); diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index 54e7d2fc8..f01fe9cc8 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -130,36 +130,25 @@ extern uint32_t pios_i2c_flexi_adapter_id; // ------------------------- #define PIOS_COM_MAX_DEVS 3 -extern uint32_t pios_com_telem_rf_id; -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 32 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 12 -#if defined(PIOS_INCLUDE_GPS) -extern uint32_t pios_com_gps_id; -#define PIOS_COM_GPS (pios_com_gps_id) -#endif /* PIOS_INCLUDE_GPS */ +#define PIOS_COM_GPS_RX_BUF_LEN 32 -extern uint32_t pios_com_bridge_id; -#define PIOS_COM_BRIDGE (pios_com_bridge_id) +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 65 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 65 -extern uint32_t pios_com_vcp_id; -#define PIOS_COM_VCP (pios_com_vcp_id) +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 -extern uint32_t pios_com_telem_usb_id; -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_HKOSD_TX_BUF_LEN 22 -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ +#define PIOS_COM_MSP_TX_BUF_LEN 128 +#define PIOS_COM_MSP_RX_BUF_LEN 64 -extern uint32_t pios_com_hkosd_id; -#define PIOS_COM_OSDHK (pios_com_hkosd_id) +#define PIOS_COM_MAVLINK_TX_BUF_LEN 128 -extern uint32_t pios_com_msp_id; -#define PIOS_COM_MSP (pios_com_msp_id) - -extern uint32_t pios_com_mavlink_id; -#define PIOS_COM_MAVLINK (pios_com_mavlink_id) +#define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 // ------------------------- // ADC diff --git a/flight/targets/boards/discoveryf4bare/pios_board.h b/flight/targets/boards/discoveryf4bare/pios_board.h index a6373ccd3..4b129899d 100644 --- a/flight/targets/boards/discoveryf4bare/pios_board.h +++ b/flight/targets/boards/discoveryf4bare/pios_board.h @@ -122,34 +122,6 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // See also pios_board.c // ------------------------- #define PIOS_COM_MAX_DEVS 4 -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_bridge_id; -extern uint32_t pios_com_vcp_id; -extern uint32_t pios_com_hkosd_id; -extern uint32_t pios_com_msp_id; -extern uint32_t pios_com_mavlink_id; - -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_OSDHK (pios_com_hkosd_id) -#define PIOS_COM_MSP (pios_com_msp_id) -#define PIOS_COM_MAVLINK (pios_com_mavlink_id) - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -#if defined(PIOS_INCLUDE_RFM22B) -extern uint32_t pios_rfm22b_id; -extern uint32_t pios_spi_telem_flash_id; -#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id) -#endif /* PIOS_INCLUDE_RFM22B */ // ------------------------- // Packet Handler diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index fb0f469e8..122cdb71f 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -141,38 +141,6 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // See also pios_board.c // ------------------------- #define PIOS_COM_MAX_DEVS 4 -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_rf_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_bridge_id; -extern uint32_t pios_com_hott_id; -extern uint32_t pios_com_vcp_id; -extern uint32_t pios_com_hkosd_id; -extern uint32_t pios_com_msp_id; -extern uint32_t pios_com_mavlink_id; - -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_RF (pios_com_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_HOTT (pios_com_hott_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_OSDHK (pios_com_hkosd_id) -#define PIOS_COM_MSP (pios_com_msp_id) -#define PIOS_COM_MAVLINK (pios_com_mavlink_id) - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -#if defined(PIOS_INCLUDE_RFM22B) -extern uint32_t pios_rfm22b_id; -extern uint32_t pios_spi_telem_flash_id; -#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id) -#endif /* PIOS_INCLUDE_RFM22B */ // ------------------------- // Packet Handler diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 43c55959c..5e1909dfd 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -141,37 +141,6 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // See also pios_board.c // ------------------------- #define PIOS_COM_MAX_DEVS 4 -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_rf_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_bridge_id; -extern uint32_t pios_com_vcp_id; -extern uint32_t pios_com_hkosd_id; -extern uint32_t pios_com_msp_id; -extern uint32_t pios_com_mavlink_id; - -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_RF (pios_com_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_OSDHK (pios_com_hkosd_id) -#define PIOS_COM_MSP (pios_com_msp_id) -#define PIOS_COM_MAVLINK (pios_com_mavlink_id) - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -#if defined(PIOS_INCLUDE_RFM22B) -extern uint32_t pios_rfm22b_id; -extern uint32_t pios_spi_telem_flash_id; -#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id) -#endif /* PIOS_INCLUDE_RFM22B */ - // ------------------------- // Packet Handler // ------------------------- diff --git a/flight/targets/boards/sparky2/pios_board.h b/flight/targets/boards/sparky2/pios_board.h index 63986b0fb..37fe24a12 100644 --- a/flight/targets/boards/sparky2/pios_board.h +++ b/flight/targets/boards/sparky2/pios_board.h @@ -143,36 +143,6 @@ extern uint32_t pios_i2c_flexiport_adapter_id; // See also pios_board.c // ------------------------- #define PIOS_COM_MAX_DEVS 4 -extern uint32_t pios_com_telem_rf_id; -extern uint32_t pios_com_rf_id; -extern uint32_t pios_com_gps_id; -extern uint32_t pios_com_telem_usb_id; -extern uint32_t pios_com_bridge_id; -extern uint32_t pios_com_vcp_id; -extern uint32_t pios_com_hkosd_id; -extern uint32_t pios_com_msp_id; -extern uint32_t pios_com_mavlink_id; - -#define PIOS_COM_GPS (pios_com_gps_id) -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) -#define PIOS_COM_RF (pios_com_rf_id) -#define PIOS_COM_BRIDGE (pios_com_bridge_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_OSDHK (pios_com_hkosd_id) -#define PIOS_COM_MSP (pios_com_msp_id) -#define PIOS_COM_MAVLINK (pios_com_mavlink_id) - -#if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -extern uint32_t pios_com_debug_id; -#define PIOS_COM_DEBUG (pios_com_debug_id) -#endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ - -#if defined(PIOS_INCLUDE_RFM22B) -extern uint32_t pios_rfm22b_id; -extern uint32_t pios_spi_telem_flash_id; -#define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id) -#endif /* PIOS_INCLUDE_RFM22B */ // ------------------------- // Packet Handler