diff --git a/Makefile b/Makefile index 438f697a4..72adb05be 100644 --- a/Makefile +++ b/Makefile @@ -235,7 +235,7 @@ STM32FLASH_DIR := $(TOOLS_DIR)/stm32flash .PHONY: stm32flash_install stm32flash_install: STM32FLASH_URL := http://stm32flash.googlecode.com/svn/trunk -stm32flash_install: STM32FLASH_REV := 52 +stm32flash_install: STM32FLASH_REV := 61 stm32flash_install: stm32flash_clean # download the source $(V0) @echo " DOWNLOAD $(STM32FLASH_URL) @ r$(STM32FLASH_REV)" diff --git a/artwork/Dials/deluxe/lineardial-horizontal.svg b/artwork/Dials/deluxe/lineardial-horizontal.svg old mode 100644 new mode 100755 index 2f6f71b17..e37172ec0 --- a/artwork/Dials/deluxe/lineardial-horizontal.svg +++ b/artwork/Dials/deluxe/lineardial-horizontal.svg @@ -14,8 +14,8 @@ height="70.597504" id="svg10068" version="1.1" - inkscape:version="0.48.1 " - sodipodi:docname="lineardial-horizontal.svg" + inkscape:version="0.48.2 r9819" + sodipodi:docname="lineardial-horizontal-old2.svg" inkscape:export-filename="H:\Documents\Hobbies\W433\My Gauges\vbat-001.png" inkscape:export-xdpi="103.61" inkscape:export-ydpi="103.61" @@ -25,15 +25,19 @@ + offset="0.31684026" + style="stop-color:#a3a3a3;stop-opacity:1;" /> + @@ -57,15 +61,11 @@ + style="stop-color:#8a8a8a;stop-opacity:1;" /> - + id="stop4391" /> @@ -237,15 +237,15 @@ + style="stop-color:#676767;stop-opacity:1;" /> + style="stop-color:#7d7d7d;stop-opacity:1;" /> @@ -425,46 +425,34 @@ - + offset="0" + style="stop-color:#ffc001;stop-opacity:1;" /> - - + offset="0" + style="stop-color:#e60000;stop-opacity:1;" /> @@ -687,7 +675,7 @@ x2="87.074203" y2="168.83261" gradientUnits="userSpaceOnUse" - gradientTransform="matrix(0,0.99999995,-0.99999975,0,-11.23354,-270.8763)" /> + gradientTransform="matrix(0,1.0110991,-1.0062411,0,-10.360287,-272.19756)" /> + gradientTransform="matrix(0,1.0224134,-1.0048488,0,-10.554719,-273.54528)" /> + gradientTransform="matrix(0,1,-1.0055415,0,-10.459485,-270.8763)" /> + transform="translate(-368.2988,-507.08981)"> @@ -1225,17 +1212,16 @@ id="layer5" inkscape:label="Green Zone" style="display:inline" - transform="translate(-140.85549,-141.35611)" - sodipodi:insensitive="true"> + transform="translate(-140.85549,-141.35611)"> + transform="translate(-140.85549,-141.35611)"> + transform="translate(-140.85549,-141.35611)"> + transform="translate(-140.85549,-141.35611)"> + transform="matrix(0,1,-1,0,0,0)" + ry="2.5494981" + rx="1.9392394" /> + + + + + style="stop-color:#656565;stop-opacity:1;" /> + style="stop-color:#757575;stop-opacity:1;" /> @@ -83,15 +94,19 @@ + style="stop-color:#9c9c9c;stop-opacity:0;" /> + id="stop3921" + offset="0.359375" + style="stop-color:#6c6c6c;stop-opacity:0.88793105;" /> + + style="stop-color:#9c9c9c;stop-opacity:0;" /> @@ -215,46 +230,34 @@ - + offset="0" + style="stop-color:#ffc001;stop-opacity:1;" /> - - + offset="0" + style="stop-color:#e60000;stop-opacity:1;" /> @@ -475,7 +478,8 @@ y1="132.84332" x2="-58.661255" y2="169.46072" - gradientUnits="userSpaceOnUse" /> + gradientUnits="userSpaceOnUse" + gradientTransform="matrix(1.0045664,0,0,0.98072534,3.8285685,3.8328979)" /> + gradientUnits="userSpaceOnUse" + gradientTransform="matrix(1.0048708,0,0,1.0000284,3.816285,2.2385824)" /> + gradientUnits="userSpaceOnUse" + gradientTransform="matrix(1.0057658,0,0,0.99502095,3.8176724,2.8357789)" /> + y2="412.00528" + gradientTransform="matrix(1,0,0,0.98663274,0,3.8538758)" /> + x2="-136.75557" + y2="74.015358" /> + image/svg+xml - + Edouard Lafargue @@ -779,8 +796,7 @@ inkscape:label="Dark background" id="g2932" inkscape:groupmode="layer" - transform="translate(-357.06525,-236.21351)" - sodipodi:insensitive="true"> + transform="translate(-357.06525,-236.21351)"> @@ -789,39 +805,39 @@ style="fill:url(#linearGradient5344);fill-opacity:1;stroke:none" id="rect2936" width="318.58304" - height="46.756046" + height="46.131046" x="-556.79657" - y="358.44128" - ry="3.4504199" + y="357.50378" + ry="3.4042974" inkscape:export-filename="H:\Documents\Hobbies\W433\g9905.png" inkscape:export-xdpi="88.809998" inkscape:export-ydpi="88.809998" /> - + style="fill:url(#linearGradient3900);fill-opacity:1;stroke:none" /> + + transform="translate(-129.62194,129.52019)"> + style="display:inline" + transform="translate(-129.62194,129.52019)"> + transform="translate(-129.62194,129.52019)"> + width="34.173222" + height="5.9349518" + x="136.20108" + y="-114.68695" + inkscape:label="#rect5246" + ry="2.4981377" /> + transform="translate(-129.62194,129.52019)"> diff --git a/flight/AHRS/Makefile b/flight/AHRS/Makefile index 1943cc3a6..17559cf58 100644 --- a/flight/AHRS/Makefile +++ b/flight/AHRS/Makefile @@ -71,6 +71,7 @@ OPUAVOBJINC = $(OPUAVOBJ)/inc OPSYSINC = $(OPDIR)/System/inc BOOT = ../Bootloaders/AHRS BOOTINC = $(BOOT)/inc +HWDEFSINC = ../board_hw_defs/$(BOARD_NAME) OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight @@ -170,6 +171,7 @@ EXTRAINCDIRS += $(CMSISDIR) EXTRAINCDIRS += $(AHRSINC) EXTRAINCDIRS += $(OPUAVSYNTHDIR) EXTRAINCDIRS += $(BOOTINC) +EXTRAINCDIRS += $(HWDEFSINC) # List any extra directories to look for library files here. # Also add directories where the linker should search for diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 830245572..e8b5742e3 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -447,9 +447,9 @@ void print_ahrs_raw() valid_data_buffer = PIOS_ADC_GetRawBuffer(); if (total_conversion_blocks != previous_conversion + 1) - PIOS_LED_On(LED1); // not keeping up + PIOS_LED_On(PIOS_LED_HEARTBEAT); // not keeping up else - PIOS_LED_Off(LED1); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); previous_conversion = total_conversion_blocks; @@ -463,9 +463,9 @@ void print_ahrs_raw() PIOS_ADC_NUM_PINS * sizeof(valid_data_buffer[0])); if (result == 0) - PIOS_LED_Off(LED1); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); else { - PIOS_LED_On(LED1); + PIOS_LED_On(PIOS_LED_HEARTBEAT); } } #endif @@ -490,7 +490,7 @@ void print_ahrs_raw() // wait for new raw samples while (previous_conversion == total_conversion_blocks); if ((previous_conversion + 1) != total_conversion_blocks) - PIOS_LED_On(LED1); // we are not keeping up + PIOS_LED_On(PIOS_LED_HEARTBEAT); // we are not keeping up previous_conversion = total_conversion_blocks; // fetch the buffer address for the new samples @@ -534,9 +534,9 @@ void print_ahrs_raw() result += PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *)gyro_z, over_sampling * sizeof(gyro_z[0])); if (result != 0) - PIOS_LED_On(LED1); // all data not sent + PIOS_LED_On(PIOS_LED_HEARTBEAT); // all data not sent else - PIOS_LED_Off(LED1); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); } #endif @@ -616,7 +616,7 @@ for all data to be up to date before doing anything*/ // Alive signal if (((total_conversion_blocks % 100) & 0xFFFE) == 0) - PIOS_LED_Toggle(LED1); + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); // Delay for valid data diff --git a/flight/AHRS/inc/pios_config.h b/flight/AHRS/inc/pios_config.h index 64f3559a0..77ddf0f2f 100644 --- a/flight/AHRS/inc/pios_config.h +++ b/flight/AHRS/inc/pios_config.h @@ -41,5 +41,6 @@ #define PIOS_INCLUDE_GPIO #define PIOS_INCLUDE_EXTI #define PIOS_INCLUDE_BL_HELPER +#define PIOS_INCLUDE_IAP #endif /* PIOS_CONFIG_H */ diff --git a/flight/AHRS/pios_board.c b/flight/AHRS/pios_board.c index aa5b20855..2eb6ab668 100644 --- a/flight/AHRS/pios_board.c +++ b/flight/AHRS/pios_board.c @@ -23,336 +23,20 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +/* Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "board_hw_defs.c" + #include -#if defined(PIOS_INCLUDE_SPI) - -#include - -/* OP Interface - * - * NOTE: Leave this declared as const data so that it ends up in the - * .rodata section (ie. Flash) rather than in the .bss section (RAM). - */ -void PIOS_SPI_op_irq_handler(void); -void DMA1_Channel5_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler"))); -void DMA1_Channel4_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler"))); -static const struct pios_spi_cfg pios_spi_op_cfg = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Hard, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - }, - .use_crc = TRUE, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = - (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | - DMA1_FLAG_GL4), - .init = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel4, - .init = { - .DMA_PeripheralBaseAddr = - (uint32_t) & (SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = - DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = - DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = - DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel5, - .init = { - .DMA_PeripheralBaseAddr = - (uint32_t) & (SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = - DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = - DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = - DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .ssel = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - -uint32_t pios_spi_op_id; -void PIOS_SPI_op_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_op_id); -} - -#endif /* PIOS_INCLUDE_SPI */ - -/* - * ADC system - */ -#include "pios_adc_priv.h" -extern void PIOS_ADC_handler(void); -void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); -// Remap the ADC DMA handler to this one -static const struct pios_adc_cfg pios_adc_cfg = { - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - .irq = { - .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), - .init = { - .NVIC_IRQChannel = DMA1_Channel1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA1_Channel1, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR, - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - } - }, - .half_flag = DMA1_IT_HT1, - .full_flag = DMA1_IT_TC1, -}; - -struct pios_adc_dev pios_adc_devs[] = { - { - .cfg = &pios_adc_cfg, - .callback_function = NULL, - }, -}; - -uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs); - -void PIOS_ADC_handler() { - PIOS_ADC_DMA_Handler(); -} - - -#if defined(PIOS_INCLUDE_USART) -#include - -/* - * AUX USART - */ -static const struct pios_usart_cfg pios_usart_aux_cfg = { - .regs = USART3, - .init = { - .USART_BaudRate = 230400, - .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_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_AF_PP, - }, - }, -}; - -#endif /* PIOS_INCLUDE_USART */ - -#if defined(PIOS_INCLUDE_COM) - -#include - #define PIOS_COM_AUX_TX_BUF_LEN 192 static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN]; -#endif /* PIOS_INCLUDE_COM */ - -#if defined(PIOS_INCLUDE_I2C) - -#include - -/* - * I2C Adapters - */ - -void PIOS_I2C_main_adapter_ev_irq_handler(void); -void PIOS_I2C_main_adapter_er_irq_handler(void); -void I2C1_EV_IRQHandler() - __attribute__ ((alias("PIOS_I2C_main_adapter_ev_irq_handler"))); -void I2C1_ER_IRQHandler() - __attribute__ ((alias("PIOS_I2C_main_adapter_er_irq_handler"))); - -static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { - .regs = I2C1, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 200000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -uint32_t pios_i2c_main_adapter_id; -void PIOS_I2C_main_adapter_ev_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_main_adapter_id); -} - -void PIOS_I2C_main_adapter_er_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_main_adapter_id); -} - -#endif /* PIOS_INCLUDE_I2C */ - -#if defined(PIOS_ENABLE_DEBUG_PINS) - -static const struct stm32_gpio pios_debug_pins[] = { - { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, -}; - -#endif /* PIOS_ENABLE_DEBUG_PINS */ - -extern const struct pios_com_driver pios_usart_com_driver; - uint32_t pios_com_aux_id; uint8_t adc_fifo_buf[sizeof(float) * 6 * 4] __attribute__ ((aligned(4))); // align to 32-bit to try and provide speed improvement @@ -365,7 +49,13 @@ void PIOS_Board_Init(void) { /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); - PIOS_LED_On(LED1); +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_On(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ /* Delay system */ PIOS_DELAY_Init(); diff --git a/flight/Bootloaders/AHRS/Makefile b/flight/Bootloaders/AHRS/Makefile index 833dba72b..b4a060373 100644 --- a/flight/Bootloaders/AHRS/Makefile +++ b/flight/Bootloaders/AHRS/Makefile @@ -65,7 +65,7 @@ STMUSBDIR = $(STMLIBDIR)/STM32_USB-FS-Device_Driver STMSPDSRCDIR = $(STMSPDDIR)/src STMSPDINCDIR = $(STMSPDDIR)/inc CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3 - +HWDEFSINC = ../../board_hw_defs/$(BOARD_NAME) # List C source files here. (C dependencies are automatically generated.) # use file-extension c for "c-only"-files @@ -155,6 +155,7 @@ EXTRAINCDIRS += $(PIOSBOARDS) EXTRAINCDIRS += $(STMSPDINCDIR) EXTRAINCDIRS += $(CMSISDIR) EXTRAINCDIRS += $(AHRS_BLINC) +EXTRAINCDIRS += $(HWDEFSINC) # List any extra directories to look for library files here. # Also add directories where the linker should search for diff --git a/flight/Bootloaders/AHRS/ahrs_slave_test.c b/flight/Bootloaders/AHRS/ahrs_slave_test.c index 4ef55b975..d26957f1c 100644 --- a/flight/Bootloaders/AHRS/ahrs_slave_test.c +++ b/flight/Bootloaders/AHRS/ahrs_slave_test.c @@ -16,7 +16,7 @@ bool WriteData(uint32_t offset, uint8_t *buffer, uint32_t size) { PIOS_COM_SendFormattedString(PIOS_COM_AUX, "Wrote %d bytes to %d\r\n", size, offset); memcpy(buf, buffer, size); - PIOS_LED_Toggle(LED1); + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); return (true); } @@ -28,7 +28,7 @@ bool ReadData(uint32_t offset, uint8_t *buffer, uint32_t size) { PIOS_COM_SendFormattedString(PIOS_COM_AUX, "Read %d bytes from %d\r\n", size, offset); memcpy(buffer, buf, size); - PIOS_LED_Toggle(LED1); + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); return (true); } diff --git a/flight/Bootloaders/AHRS/inc/pios_config.h b/flight/Bootloaders/AHRS/inc/pios_config.h index b24e0fc5b..82344e2ad 100644 --- a/flight/Bootloaders/AHRS/inc/pios_config.h +++ b/flight/Bootloaders/AHRS/inc/pios_config.h @@ -36,5 +36,6 @@ #define PIOS_INCLUDE_BL_HELPER #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT #define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_IAP #endif /* PIOS_CONFIG_H */ diff --git a/flight/Bootloaders/AHRS/main.c b/flight/Bootloaders/AHRS/main.c index b284e6365..756d12566 100644 --- a/flight/Bootloaders/AHRS/main.c +++ b/flight/Bootloaders/AHRS/main.c @@ -79,12 +79,7 @@ int main() { break; } } - //while(TRUE) - // { - // PIOS_LED_Toggle(LED1); - // PIOS_DELAY_WaitmS(1000); - // } - //GO_dfu = TRUE; + PIOS_IAP_Init(); GO_dfu = GO_dfu | PIOS_IAP_CheckRequest();// OR with app boot request if (GO_dfu == FALSE) { @@ -97,7 +92,7 @@ int main() { PIOS_Board_Init(); boot_status = idle; Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc(); - PIOS_LED_On(LED1); + PIOS_LED_On(PIOS_LED_HEARTBEAT); while (1) { process_spi_request(); } @@ -150,7 +145,7 @@ void process_spi_request(void) { Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc(); lfsm_user_set_tx_v0(&user_tx_v0); boot_status = idle; - PIOS_LED_Off(LED1); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status; break; case OPAHRS_MSG_V0_REQ_RESET: @@ -158,7 +153,6 @@ void process_spi_request(void) { PIOS_SYS_Reset(); break; case OPAHRS_MSG_V0_REQ_VERSIONS: - //PIOS_LED_On(LED1); opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_VERSIONS); user_tx_v0.payload.user.v.rsp.versions.bl_version = BOOTLOADER_VERSION; user_tx_v0.payload.user.v.rsp.versions.hw_version = (BOARD_TYPE << 8) @@ -191,7 +185,7 @@ void process_spi_request(void) { lfsm_user_set_tx_v0(&user_tx_v0); break; case OPAHRS_MSG_V0_REQ_FWUP_DATA: - PIOS_LED_On(LED1); + PIOS_LED_On(PIOS_LED_HEARTBEAT); opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS); if (!(user_rx_v0.payload.user.v.req.fwup_data.adress < bdinfo->fw_base)) { @@ -209,7 +203,7 @@ void process_spi_request(void) { } else { boot_status = outside_dev_capabilities; } - PIOS_LED_Off(LED1); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status; lfsm_user_set_tx_v0(&user_tx_v0); break; @@ -227,10 +221,10 @@ void process_spi_request(void) { opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS); user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status; lfsm_user_set_tx_v0(&user_tx_v0); - PIOS_LED_On(LED1); + PIOS_LED_On(PIOS_LED_HEARTBEAT); if (PIOS_BL_HELPER_FLASH_Start() == TRUE) { boot_status = started; - PIOS_LED_Off(LED1); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); } else { boot_status = start_failed; break; @@ -254,15 +248,13 @@ void process_spi_request(void) { void jump_to_app() { const struct pios_board_info * bdinfo = &pios_board_info_blob; - PIOS_LED_On(LED1); + PIOS_LED_On(PIOS_LED_HEARTBEAT); if (((*(__IO uint32_t*) bdinfo->fw_base) & 0x2FFE0000) == 0x20000000) { /* Jump to user application */ FLASH_Lock(); RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); - //_SetCNTR(0); // clear interrupt mask - //_SetISTR(0); // clear all requests JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); Jump_To_Application = (pFunction) JumpAddress; diff --git a/flight/Bootloaders/AHRS/pios_board.c b/flight/Bootloaders/AHRS/pios_board.c index 756da9288..9b9eac875 100644 --- a/flight/Bootloaders/AHRS/pios_board.c +++ b/flight/Bootloaders/AHRS/pios_board.c @@ -23,112 +23,16 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include - -#if defined(PIOS_INCLUDE_SPI) - -#include - -/* OP Interface - * - * NOTE: Leave this declared as const data so that it ends up in the - * .rodata section (ie. Flash) rather than in the .bss section (RAM). +/* Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE */ -void PIOS_SPI_op_irq_handler(void); -void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_op_irq_handler"))); -void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_op_irq_handler"))); -static const struct pios_spi_cfg - pios_spi_op_cfg = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Hard, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - }, - .use_crc = TRUE, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, +#include "board_hw_defs.c" - .irq = { - .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), - .init = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel4, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel5, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, .ssel = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, }; - -uint32_t pios_spi_op_id; -void PIOS_SPI_op_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_op_id); -} - -#endif /* PIOS_INCLUDE_SPI */ +#include #include "bl_fsm.h" /* lfsm_* */ @@ -138,6 +42,10 @@ void PIOS_Board_Init() { return; } +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + /* Set up the SPI interface to the OP board */ if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) { PIOS_DEBUG_Assert(0); diff --git a/flight/Bootloaders/BootloaderUpdater/Makefile b/flight/Bootloaders/BootloaderUpdater/Makefile index aabfc88bf..62fc2c056 100644 --- a/flight/Bootloaders/BootloaderUpdater/Makefile +++ b/flight/Bootloaders/BootloaderUpdater/Makefile @@ -89,6 +89,7 @@ RTOSDIR = $(APPLIBDIR)/FreeRTOS RTOSSRCDIR = $(RTOSDIR)/Source RTOSINCDIR = $(RTOSSRCDIR)/include DOXYGENDIR = ../Doc/Doxygen +HWDEFSINC = $(TOP)/flight/board_hw_defs/$(BOARD_NAME) # List C source files here. (C dependencies are automatically generated.) # use file-extension c for "c-only"-files @@ -165,6 +166,7 @@ EXTRAINCDIRS += $(MSDDIR) EXTRAINCDIRS += $(RTOSINCDIR) EXTRAINCDIRS += $(APPLIBDIR) EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/ARM_CM3 +EXTRAINCDIRS += $(HWDEFSINC) diff --git a/flight/Bootloaders/BootloaderUpdater/main.c b/flight/Bootloaders/BootloaderUpdater/main.c index 9b1691dca..195a25b33 100644 --- a/flight/Bootloaders/BootloaderUpdater/main.c +++ b/flight/Bootloaders/BootloaderUpdater/main.c @@ -50,14 +50,14 @@ int main() { PIOS_SYS_Init(); PIOS_Board_Init(); - PIOS_LED_On(LED1); + PIOS_LED_On(PIOS_LED_HEARTBEAT); PIOS_DELAY_WaitmS(3000); - PIOS_LED_Off(LED1); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); /// Self overwrite check uint32_t base_address = SCB->VTOR; if ((0x08000000 + embedded_image_size) > base_address) - error(LED1); + error(PIOS_LED_HEARTBEAT); /// FLASH_Unlock(); @@ -82,7 +82,7 @@ int main() { } if (fail == true) - error(LED1); + error(PIOS_LED_HEARTBEAT); /// @@ -90,7 +90,7 @@ int main() { /// Bootloader programing for (uint32_t offset = 0; offset < embedded_image_size/sizeof(uint32_t); ++offset) { bool result = false; - PIOS_LED_Toggle(LED1); + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); for (uint8_t retry = 0; retry < MAX_WRI_RETRYS; ++retry) { if (result == false) { result = (FLASH_ProgramWord(0x08000000 + (offset * 4), embedded_image_start[offset]) @@ -98,13 +98,13 @@ int main() { } } if (result == false) - error(LED1); + error(PIOS_LED_HEARTBEAT); } /// for (uint8_t x = 0; x < 3; ++x) { - PIOS_LED_On(LED1); + PIOS_LED_On(PIOS_LED_HEARTBEAT); PIOS_DELAY_WaitmS(1000); - PIOS_LED_Off(LED1); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); PIOS_DELAY_WaitmS(1000); } diff --git a/flight/Bootloaders/BootloaderUpdater/pios_board.c b/flight/Bootloaders/BootloaderUpdater/pios_board.c index 5d25eb140..d19a2ba13 100644 --- a/flight/Bootloaders/BootloaderUpdater/pios_board.c +++ b/flight/Bootloaders/BootloaderUpdater/pios_board.c @@ -23,6 +23,15 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +/* Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "board_hw_defs.c" + #include void PIOS_Board_Init(void) { @@ -38,4 +47,8 @@ void PIOS_Board_Init(void) { /* Initialize the PiOS library */ PIOS_GPIO_Init(); +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + } diff --git a/flight/Bootloaders/CopterControl/Makefile b/flight/Bootloaders/CopterControl/Makefile index 1257ed579..be7e98190 100644 --- a/flight/Bootloaders/CopterControl/Makefile +++ b/flight/Bootloaders/CopterControl/Makefile @@ -86,6 +86,7 @@ RTOSDIR = $(APPLIBDIR)/FreeRTOS RTOSSRCDIR = $(RTOSDIR)/Source RTOSINCDIR = $(RTOSSRCDIR)/include DOXYGENDIR = ../Doc/Doxygen +HWDEFSINC = ../../board_hw_defs/$(BOARD_NAME) # List C source files here. (C dependencies are automatically generated.) # use file-extension c for "c-only"-files @@ -104,7 +105,6 @@ SRC += $(PIOSSTM32F10X)/pios_irq.c SRC += $(PIOSSTM32F10X)/pios_debug.c SRC += $(PIOSSTM32F10X)/pios_gpio.c - # PIOS USB related files (seperated to make code maintenance more easy) SRC += $(PIOSSTM32F10X)/pios_usb.c SRC += $(PIOSSTM32F10X)/pios_usbhook.c @@ -116,7 +116,7 @@ SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c ## PIOS Hardware (Common) SRC += $(PIOSCOMMON)/pios_board_info.c -SRC += $(PIOSCOMMON)/pios_com.c +SRC += $(PIOSCOMMON)/pios_com_msg.c SRC += $(PIOSCOMMON)/pios_bl_helper.c SRC += $(PIOSCOMMON)/pios_iap.c SRC += $(PIOSCOMMON)/printf-stdarg.c @@ -199,8 +199,7 @@ EXTRAINCDIRS += $(MSDDIR) EXTRAINCDIRS += $(RTOSINCDIR) EXTRAINCDIRS += $(APPLIBDIR) EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/ARM_CM3 - - +EXTRAINCDIRS += $(HWDEFSINC) # List any extra directories to look for library files here. # Also add directories where the linker should search for diff --git a/flight/Bootloaders/CopterControl/inc/pios_config.h b/flight/Bootloaders/CopterControl/inc/pios_config.h index 081ed1b0f..3eb2c281a 100644 --- a/flight/Bootloaders/CopterControl/inc/pios_config.h +++ b/flight/Bootloaders/CopterControl/inc/pios_config.h @@ -39,8 +39,9 @@ #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_USB #define PIOS_INCLUDE_USB_HID -#define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_COM_MSG #define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_IAP #define PIOS_NO_GPS #endif /* PIOS_CONFIG_H */ diff --git a/flight/Bootloaders/CopterControl/inc/pios_usb_board_data.h b/flight/Bootloaders/CopterControl/inc/pios_usb_board_data.h index 85f407a9e..f1accee17 100644 --- a/flight/Bootloaders/CopterControl/inc/pios_usb_board_data.h +++ b/flight/Bootloaders/CopterControl/inc/pios_usb_board_data.h @@ -37,33 +37,6 @@ #include "pios_usb_defs.h" /* struct usb_* */ -struct usb_board_config { - struct usb_configuration_desc config; - struct usb_interface_desc hid_if; - struct usb_hid_desc hid; - struct usb_endpoint_desc hid_in; - struct usb_endpoint_desc hid_out; -} __attribute__((packed)); - -extern const struct usb_device_desc PIOS_USB_BOARD_DeviceDescriptor; -extern const struct usb_board_config PIOS_USB_BOARD_Configuration; -extern const struct usb_string_langid PIOS_USB_BOARD_StringLangID; - -/* NOTE NOTE NOTE - * - * Care must be taken to ensure that the _actual_ contents of - * these arrays (in each board's pios_usb_board_data.c) is no - * smaller than the stated sizes here or these descriptors - * will end up with trailing zeros on the wire. - * - * The compiler will catch any time that these definitions are - * too small. - */ -extern const uint8_t PIOS_USB_BOARD_HidReportDescriptor[36]; -extern const uint8_t PIOS_USB_BOARD_StringVendorID[28]; -extern const uint8_t PIOS_USB_BOARD_StringProductID[28]; -extern uint8_t PIOS_USB_BOARD_StringSerial[52]; - #define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL #define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_BL) diff --git a/flight/Bootloaders/CopterControl/main.c b/flight/Bootloaders/CopterControl/main.c index c1def4a46..17733bbef 100644 --- a/flight/Bootloaders/CopterControl/main.c +++ b/flight/Bootloaders/CopterControl/main.c @@ -32,6 +32,7 @@ #include "usb_lib.h" #include "pios_iap.h" #include "fifo_buffer.h" +#include "pios_com_msg.h" /* Prototype of PIOS_Board_Init() function */ extern void PIOS_Board_Init(void); extern void FLASH_Download(); @@ -62,13 +63,12 @@ uint8_t JumpToApp = FALSE; uint8_t GO_dfu = FALSE; uint8_t USB_connected = FALSE; uint8_t User_DFU_request = FALSE; -static uint8_t mReceive_Buffer[64]; +static uint8_t mReceive_Buffer[63]; /* Private function prototypes -----------------------------------------------*/ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); uint8_t processRX(); void jump_to_app(); -#define BLUE LED1 int main() { PIOS_SYS_Init(); if (BSL_HOLD_STATE == 0) @@ -111,7 +111,7 @@ int main() { case DFUidle: period1 = 5000; sweep_steps1 = 100; - PIOS_LED_Off(BLUE); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); period2 = 0; break; case uploading: @@ -123,12 +123,12 @@ int main() { case downloading: period1 = 2500; sweep_steps1 = 50; - PIOS_LED_Off(BLUE); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); period2 = 0; break; case BLidle: period1 = 0; - PIOS_LED_On(BLUE); + PIOS_LED_On(PIOS_LED_HEARTBEAT); period2 = 0; break; default://error @@ -140,19 +140,19 @@ int main() { if (period1 != 0) { if (LedPWM(period1, sweep_steps1, stopwatch)) - PIOS_LED_On(BLUE); + PIOS_LED_On(PIOS_LED_HEARTBEAT); else - PIOS_LED_Off(BLUE); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); } else - PIOS_LED_On(BLUE); + PIOS_LED_On(PIOS_LED_HEARTBEAT); if (period2 != 0) { if (LedPWM(period2, sweep_steps2, stopwatch)) - PIOS_LED_On(BLUE); + PIOS_LED_On(PIOS_LED_HEARTBEAT); else - PIOS_LED_Off(BLUE); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); } else - PIOS_LED_Off(BLUE); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); if (stopwatch > 50 * 1000 * 1000) stopwatch = 0; @@ -198,7 +198,7 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { } uint8_t processRX() { - if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) { + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { processComand(mReceive_Buffer); } return TRUE; diff --git a/flight/Bootloaders/CopterControl/op_dfu.c b/flight/Bootloaders/CopterControl/op_dfu.c index ae85160c2..9f98d3394 100644 --- a/flight/Bootloaders/CopterControl/op_dfu.c +++ b/flight/Bootloaders/CopterControl/op_dfu.c @@ -30,6 +30,7 @@ #include "pios.h" #include "op_dfu.h" #include "pios_bl_helper.h" +#include "pios_com_msg.h" #include //programmable devices Device devicesTable[10]; @@ -446,9 +447,7 @@ uint32_t CalcFirmCRC() { } void sendData(uint8_t * buf, uint16_t size) { - PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, buf, size); - if (DeviceState == downloading) - PIOS_DELAY_WaitmS(20);//this is an hack, we should check wtf is wrong with hid + PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size); } bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type) { diff --git a/flight/Bootloaders/CopterControl/pios_board.c b/flight/Bootloaders/CopterControl/pios_board.c index 1b3c54af3..c5bd761ea 100644 --- a/flight/Bootloaders/CopterControl/pios_board.c +++ b/flight/Bootloaders/CopterControl/pios_board.c @@ -23,50 +23,17 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +/* Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "board_hw_defs.c" + #include -// *********************************************************************************** - -#if defined(PIOS_INCLUDE_COM) - -#include - -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 - -static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN]; -static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN]; - -#endif /* PIOS_INCLUDE_COM */ - -// *********************************************************************************** - -#if defined(PIOS_INCLUDE_USB) -#include "pios_usb_priv.h" - -static const struct pios_usb_cfg pios_usb_main_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; -#endif /* PIOS_INCLUDE_USB */ - -#if defined(PIOS_INCLUDE_USB_HID) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; - -#endif /* PIOS_INCLUDE_USB_HID */ - uint32_t pios_com_telem_usb_id; /** @@ -92,22 +59,30 @@ void PIOS_Board_Init(void) { /* Initialize the PiOS library */ PIOS_GPIO_Init(); +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + #if defined(PIOS_INCLUDE_USB) + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); + + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); + uint32_t pios_usb_id; if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) { PIOS_Assert(0); } -#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM) +#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)) { PIOS_Assert(0); } - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer), - pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) { + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { PIOS_Assert(0); } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM */ +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/Bootloaders/CopterControl/pios_usb_board_data.c b/flight/Bootloaders/CopterControl/pios_usb_board_data.c index b39fcbec3..4f02ce424 100644 --- a/flight/Bootloaders/CopterControl/pios_usb_board_data.c +++ b/flight/Bootloaders/CopterControl/pios_usb_board_data.c @@ -29,9 +29,11 @@ */ #include "pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include "pios_sys.h" /* PIOS_SYS_SerialNumberGet */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_* */ -const uint8_t PIOS_USB_BOARD_StringProductID[] = { - sizeof(PIOS_USB_BOARD_StringProductID), +static const uint8_t usb_product_id[28] = { + sizeof(usb_product_id), USB_DESC_TYPE_STRING, 'C', 0, 'o', 0, @@ -48,3 +50,74 @@ const uint8_t PIOS_USB_BOARD_StringProductID[] = { 'l', 0, }; +static uint8_t usb_serial_number[52] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0 +}; + +static const struct usb_string_langid usb_lang_id = { + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_UK), +}; + +static const uint8_t usb_vendor_id[28] = { + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 +}; + +int32_t PIOS_USB_BOARD_DATA_Init(void) +{ + /* Load device serial number into serial number string */ + uint8_t sn[25]; + PIOS_SYS_SerialNumberGet((char *)sn); + for (uint8_t i = 0; sn[i] != '\0' && (2 * i) < usb_serial_number[0]; i++) { + usb_serial_number[2 + 2 * i] = sn[i]; + } + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; +} diff --git a/flight/Bootloaders/OpenPilot/Makefile b/flight/Bootloaders/OpenPilot/Makefile index e9b55e134..071b20fec 100644 --- a/flight/Bootloaders/OpenPilot/Makefile +++ b/flight/Bootloaders/OpenPilot/Makefile @@ -86,6 +86,7 @@ RTOSDIR = $(APPLIBDIR)/FreeRTOS RTOSSRCDIR = $(RTOSDIR)/Source RTOSINCDIR = $(RTOSSRCDIR)/include DOXYGENDIR = ../Doc/Doxygen +HWDEFSINC = ../../board_hw_defs/$(BOARD_NAME) # List C source files here. (C dependencies are automatically generated.) # use file-extension c for "c-only"-files @@ -119,6 +120,7 @@ SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c ## PIOS Hardware (Common) SRC += $(PIOSCOMMON)/pios_board_info.c +SRC += $(PIOSCOMMON)/pios_com_msg.c SRC += $(PIOSCOMMON)/pios_com.c SRC += $(PIOSCOMMON)/pios_opahrs_v0.c SRC += $(PIOSCOMMON)/pios_bl_helper.c @@ -205,6 +207,7 @@ EXTRAINCDIRS += $(MSDDIR) EXTRAINCDIRS += $(RTOSINCDIR) EXTRAINCDIRS += $(APPLIBDIR) EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/ARM_CM3 +EXTRAINCDIRS += $(HWDEFSINC) @@ -292,7 +295,7 @@ CSTANDARD = -std=gnu99 # Flags for C and C++ (arm-elf-gcc/arm-elf-g++) ifeq ($(DEBUG),YES) -CFLAGS = -DDEBUG +CFLAGS += -DDEBUG endif CFLAGS += -g$(DEBUGF) diff --git a/flight/Bootloaders/OpenPilot/inc/pios_config.h b/flight/Bootloaders/OpenPilot/inc/pios_config.h index 024bcae9a..932c1a913 100644 --- a/flight/Bootloaders/OpenPilot/inc/pios_config.h +++ b/flight/Bootloaders/OpenPilot/inc/pios_config.h @@ -42,7 +42,9 @@ #define PIOS_INCLUDE_USB_HID #define PIOS_INCLUDE_OPAHRS #define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_COM_MSG #define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_IAP //#define DEBUG_SSP /* Defaults for Logging */ diff --git a/flight/Bootloaders/OpenPilot/inc/pios_usb_board_data.h b/flight/Bootloaders/OpenPilot/inc/pios_usb_board_data.h index d3e1079d6..ca9c948b0 100644 --- a/flight/Bootloaders/OpenPilot/inc/pios_usb_board_data.h +++ b/flight/Bootloaders/OpenPilot/inc/pios_usb_board_data.h @@ -37,33 +37,6 @@ #include "pios_usb_defs.h" /* struct usb_* */ -struct usb_board_config { - struct usb_configuration_desc config; - struct usb_interface_desc hid_if; - struct usb_hid_desc hid; - struct usb_endpoint_desc hid_in; - struct usb_endpoint_desc hid_out; -} __attribute__((packed)); - -extern const struct usb_device_desc PIOS_USB_BOARD_DeviceDescriptor; -extern const struct usb_board_config PIOS_USB_BOARD_Configuration; -extern const struct usb_string_langid PIOS_USB_BOARD_StringLangID; - -/* NOTE NOTE NOTE - * - * Care must be taken to ensure that the _actual_ contents of - * these arrays (in each board's pios_usb_board_data.c) is no - * smaller than the stated sizes here or these descriptors - * will end up with trailing zeros on the wire. - * - * The compiler will catch any time that these definitions are - * too small. - */ -extern const uint8_t PIOS_USB_BOARD_HidReportDescriptor[36]; -extern const uint8_t PIOS_USB_BOARD_StringVendorID[28]; -extern const uint8_t PIOS_USB_BOARD_StringProductID[20]; -extern uint8_t PIOS_USB_BOARD_StringSerial[52]; - #define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPENPILOT_MAIN #define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPENPILOT_MAIN, USB_OP_BOARD_MODE_BL) diff --git a/flight/Bootloaders/OpenPilot/main.c b/flight/Bootloaders/OpenPilot/main.c index 48056cff6..da39e2521 100644 --- a/flight/Bootloaders/OpenPilot/main.c +++ b/flight/Bootloaders/OpenPilot/main.c @@ -35,6 +35,7 @@ #include "pios_iap.h" #include "ssp.h" #include "fifo_buffer.h" +#include "pios_com_msg.h" /* Prototype of PIOS_Board_Init() function */ extern void PIOS_Board_Init(void); extern void FLASH_Download(); @@ -90,21 +91,15 @@ uint8_t JumpToApp = FALSE; uint8_t GO_dfu = FALSE; uint8_t USB_connected = FALSE; uint8_t User_DFU_request = FALSE; -static uint8_t mReceive_Buffer[64]; +static uint8_t mReceive_Buffer[63]; /* Private function prototypes -----------------------------------------------*/ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); uint8_t processRX(); void jump_to_app(); uint32_t sspTimeSource(); -#define BLUE LED1 -#define RED LED2 #define LED_PWM_TIMER TIM6 int main() { - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ - - /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); if (BSL_HOLD_STATE == 0) USB_connected = TRUE; @@ -162,7 +157,7 @@ int main() { case DFUidle: period1 = 50; sweep_steps1 = 100; - PIOS_LED_Off(RED); + PIOS_LED_Off(PIOS_LED_ALARM); period2 = 0; break; case uploading: @@ -174,12 +169,12 @@ int main() { case downloading: period1 = 25; sweep_steps1 = 50; - PIOS_LED_Off(RED); + PIOS_LED_Off(PIOS_LED_ALARM); period2 = 0; break; case BLidle: period1 = 0; - PIOS_LED_On(BLUE); + PIOS_LED_On(PIOS_LED_HEARTBEAT); period2 = 0; break; default://error @@ -191,19 +186,19 @@ int main() { if (period1 != 0) { if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER))) - PIOS_LED_On(BLUE); + PIOS_LED_On(PIOS_LED_HEARTBEAT); else - PIOS_LED_Off(BLUE); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); } else - PIOS_LED_On(BLUE); + PIOS_LED_On(PIOS_LED_HEARTBEAT); if (period2 != 0) { if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER))) - PIOS_LED_On(RED); + PIOS_LED_On(PIOS_LED_ALARM); else - PIOS_LED_Off(RED); + PIOS_LED_Off(PIOS_LED_ALARM); } else - PIOS_LED_Off(RED); + PIOS_LED_Off(PIOS_LED_ALARM); if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100) STOPWATCH_Reset(LED_PWM_TIMER); @@ -227,7 +222,6 @@ void jump_to_app() { RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); _SetCNTR(0); // clear interrupt mask _SetISTR(0); // clear all requests - JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); Jump_To_Application = (pFunction) JumpAddress; /* Initialize user application's Stack Pointer */ @@ -249,7 +243,7 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { uint8_t processRX() { if (ProgPort == Usb) { - if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) { + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { processComand(mReceive_Buffer); } } else if (ProgPort == Serial) { diff --git a/flight/Bootloaders/OpenPilot/op_dfu.c b/flight/Bootloaders/OpenPilot/op_dfu.c index bc62fff6c..ef30e4897 100644 --- a/flight/Bootloaders/OpenPilot/op_dfu.c +++ b/flight/Bootloaders/OpenPilot/op_dfu.c @@ -30,13 +30,10 @@ #include "pios.h" #include "op_dfu.h" #include "pios_bl_helper.h" +#include "pios_com_msg.h" #include #include "pios_opahrs.h" #include "ssp.h" -/* Private typedef -----------------------------------------------------------*/ -/* Private define ------------------------------------------------------------*/ -/* Private macro -------------------------------------------------------------*/ -/* Private variables ---------------------------------------------------------*/ //programmable devices Device devicesTable[10]; uint8_t numberOfDevices = 0; @@ -222,7 +219,7 @@ void processComand(uint8_t *xReceive_Buffer) { } } if (result != 1) { - DeviceState = Last_operation_failed;//ok + DeviceState = Last_operation_failed; Aditionals = (uint32_t) Command; } else { @@ -299,7 +296,6 @@ void processComand(uint8_t *xReceive_Buffer) { ++Next_Packet; } else { - DeviceState = wrong_packet_received; Aditionals = Count; } @@ -353,10 +349,18 @@ void processComand(uint8_t *xReceive_Buffer) { DeviceState = failed_jump; break; } else { + if (Data == 0x5AFE) { + /* Force board into safe mode */ + PIOS_IAP_WriteBootCount(0xFFFF); + } FLASH_Lock(); JumpToApp = 1; } } else { + if (Data == 0x5AFE) { + /* Force board into safe mode */ + PIOS_IAP_WriteBootCount(0xFFFF); + } FLASH_Lock(); JumpToApp = 1; } @@ -384,7 +388,6 @@ void processComand(uint8_t *xReceive_Buffer) { DeviceState = too_few_packets; } } - break; case Download_Req: #ifdef DEBUG_SSP @@ -508,6 +511,7 @@ uint32_t baseOfAdressType(DFUTransfer type) { return currentDevice.startOfUserCode + currentDevice.sizeOfCode; break; default: + return 0; } } @@ -550,11 +554,7 @@ uint32_t CalcFirmCRC() { } void sendData(uint8_t * buf, uint16_t size) { if (ProgPort == Usb) { - - PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, buf, size); - if (DeviceState == downloading) - PIOS_DELAY_WaitmS(10); - + PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size); } else if (ProgPort == Serial) { ssp_SendData(&ssp_port, buf, size); } diff --git a/flight/Bootloaders/OpenPilot/pios_board.c b/flight/Bootloaders/OpenPilot/pios_board.c index a2630815a..beee6ea7f 100644 --- a/flight/Bootloaders/OpenPilot/pios_board.c +++ b/flight/Bootloaders/OpenPilot/pios_board.c @@ -25,170 +25,16 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +/* Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "board_hw_defs.c" + #include -//#include -//#include - -#if defined(PIOS_INCLUDE_SPI) - -#include - -/* AHRS Interface - * - * NOTE: Leave this declared as const data so that it ends up in the - * .rodata section (ie. Flash) rather than in the .bss section (RAM). - */ -void PIOS_SPI_ahrs_irq_handler(void); -void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler"))); -void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler"))); -const struct pios_spi_cfg - pios_spi_ahrs_cfg = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, - }, - .use_crc = TRUE, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), - .init = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel4, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel5, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, .ssel = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, }; - -uint32_t pios_spi_ahrs_id; -void PIOS_SPI_ahrs_irq_handler(void) { - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_ahrs_id); -} - -#endif /* PIOS_INCLUDE_SPI */ - -#if defined(PIOS_INCLUDE_USART) - -#include "pios_usart_priv.h" - -/* - * Telemetry USART - */ -const struct pios_usart_cfg pios_usart_telem_cfg = { - .regs = USART2, - .init = { -#if defined (PIOS_COM_TELEM_BAUDRATE) - .USART_BaudRate = PIOS_COM_TELEM_BAUDRATE, -#else - .USART_BaudRate = 57600, -#endif - .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 = USART2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, }; - -#endif /* PIOS_INCLUDE_USART */ - -#if defined(PIOS_INCLUDE_COM) - -#include "pios_com_priv.h" - -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 - -static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN]; -static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN]; #define PIOS_COM_TELEM_RF_RX_BUF_LEN 192 #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 @@ -196,34 +42,6 @@ static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN]; static uint8_t pios_com_telem_rf_rx_buffer[PIOS_COM_TELEM_RF_RX_BUF_LEN]; static uint8_t pios_com_telem_rf_tx_buffer[PIOS_COM_TELEM_RF_TX_BUF_LEN]; -#endif /* PIOS_INCLUDE_COM */ - -#if defined(PIOS_INCLUDE_USB) -#include "pios_usb_priv.h" - -static const struct pios_usb_cfg pios_usb_main_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; -#endif /* PIOS_INCLUDE_USB_HID */ - -#if defined(PIOS_INCLUDE_USB_HID) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; - -#endif /* PIOS_INCLUDE_USB_HID */ - uint32_t pios_com_telem_rf_id; uint32_t pios_com_telem_usb_id; @@ -265,22 +83,30 @@ void PIOS_Board_Init(void) { PIOS_GPIO_Init(); +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + #if defined(PIOS_INCLUDE_USB) + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); + + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); + uint32_t pios_usb_id; if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) { PIOS_Assert(0); } -#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM) +#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)) { PIOS_Assert(0); } - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, - pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer), - pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) { + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { PIOS_Assert(0); } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM */ +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ #endif /* PIOS_INCLUDE_USB */ diff --git a/flight/Bootloaders/OpenPilot/pios_usb_board_data.c b/flight/Bootloaders/OpenPilot/pios_usb_board_data.c index 9492d316e..b73419ba4 100644 --- a/flight/Bootloaders/OpenPilot/pios_usb_board_data.c +++ b/flight/Bootloaders/OpenPilot/pios_usb_board_data.c @@ -29,9 +29,11 @@ */ #include "pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include "pios_sys.h" /* PIOS_SYS_SerialNumberGet */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_* */ -const uint8_t PIOS_USB_BOARD_StringProductID[] = { - sizeof(PIOS_USB_BOARD_StringProductID), +static const uint8_t usb_product_id[20] = { + sizeof(usb_product_id), USB_DESC_TYPE_STRING, 'O', 0, 'p', 0, @@ -43,3 +45,75 @@ const uint8_t PIOS_USB_BOARD_StringProductID[] = { 'o', 0, 't', 0, }; + +static uint8_t usb_serial_number[52] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0 +}; + +static const struct usb_string_langid usb_lang_id = { + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_UK), +}; + +static const uint8_t usb_vendor_id[28] = { + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 +}; + +int32_t PIOS_USB_BOARD_DATA_Init(void) +{ + /* Load device serial number into serial number string */ + uint8_t sn[25]; + PIOS_SYS_SerialNumberGet((char *)sn); + for (uint8_t i = 0; sn[i] != '\0' && (2 * i) < usb_serial_number[0]; i++) { + usb_serial_number[2 + 2 * i] = sn[i]; + } + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; +} diff --git a/flight/Bootloaders/PipXtreme/Makefile b/flight/Bootloaders/PipXtreme/Makefile index af6469ae0..a411b6403 100644 --- a/flight/Bootloaders/PipXtreme/Makefile +++ b/flight/Bootloaders/PipXtreme/Makefile @@ -115,7 +115,7 @@ SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c ## PIOS Hardware (Common) SRC += $(PIOSCOMMON)/pios_board_info.c -SRC += $(PIOSCOMMON)/pios_com.c +SRC += $(PIOSCOMMON)/pios_com_msg.c SRC += $(PIOSCOMMON)/pios_bl_helper.c SRC += $(PIOSCOMMON)/pios_iap.c SRC += $(PIOSCOMMON)/printf-stdarg.c @@ -287,7 +287,7 @@ CSTANDARD = -std=gnu99 # Flags for C and C++ (arm-elf-gcc/arm-elf-g++) ifeq ($(DEBUG),YES) -CFLAGS = -DDEBUG +CFLAGS += -DDEBUG endif CFLAGS += -g$(DEBUGF) diff --git a/flight/Bootloaders/PipXtreme/inc/pios_config.h b/flight/Bootloaders/PipXtreme/inc/pios_config.h index 0c2cc7877..1b1b9c2bc 100644 --- a/flight/Bootloaders/PipXtreme/inc/pios_config.h +++ b/flight/Bootloaders/PipXtreme/inc/pios_config.h @@ -38,9 +38,9 @@ #define PIOS_INCLUDE_SYS #define PIOS_INCLUDE_USB #define PIOS_INCLUDE_USB_HID -#define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_COM_MSG #define PIOS_INCLUDE_GPIO -//#define DEBUG_SSP +#define PIOS_INCLUDE_IAP /* Defaults for Logging */ #define LOG_FILENAME "PIOS.LOG" diff --git a/flight/Bootloaders/PipXtreme/inc/pios_usb_board_data.h b/flight/Bootloaders/PipXtreme/inc/pios_usb_board_data.h index fb6ced1f8..2e35f2fa4 100644 --- a/flight/Bootloaders/PipXtreme/inc/pios_usb_board_data.h +++ b/flight/Bootloaders/PipXtreme/inc/pios_usb_board_data.h @@ -37,33 +37,6 @@ #include "pios_usb_defs.h" /* struct usb_* */ -struct usb_board_config { - struct usb_configuration_desc config; - struct usb_interface_desc hid_if; - struct usb_hid_desc hid; - struct usb_endpoint_desc hid_in; - struct usb_endpoint_desc hid_out; -} __attribute__((packed)); - -extern const struct usb_device_desc PIOS_USB_BOARD_DeviceDescriptor; -extern const struct usb_board_config PIOS_USB_BOARD_Configuration; -extern const struct usb_string_langid PIOS_USB_BOARD_StringLangID; - -/* NOTE NOTE NOTE - * - * Care must be taken to ensure that the _actual_ contents of - * these arrays (in each board's pios_usb_board_data.c) is no - * smaller than the stated sizes here or these descriptors - * will end up with trailing zeros on the wire. - * - * The compiler will catch any time that these definitions are - * too small. - */ -extern const uint8_t PIOS_USB_BOARD_HidReportDescriptor[36]; -extern const uint8_t PIOS_USB_BOARD_StringVendorID[28]; -extern const uint8_t PIOS_USB_BOARD_StringProductID[20]; -extern uint8_t PIOS_USB_BOARD_StringSerial[52]; - #define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_PIPXTREME #define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_PIPXTREME, USB_OP_BOARD_MODE_BL) diff --git a/flight/Bootloaders/PipXtreme/main.c b/flight/Bootloaders/PipXtreme/main.c index f09881d42..908589178 100644 --- a/flight/Bootloaders/PipXtreme/main.c +++ b/flight/Bootloaders/PipXtreme/main.c @@ -32,6 +32,7 @@ #include "usb_lib.h" #include "pios_iap.h" #include "fifo_buffer.h" +#include "pios_com_msg.h" /* Prototype of PIOS_Board_Init() function */ extern void PIOS_Board_Init(void); extern void FLASH_Download(); @@ -62,19 +63,13 @@ uint8_t JumpToApp = FALSE; uint8_t GO_dfu = FALSE; uint8_t USB_connected = FALSE; uint8_t User_DFU_request = FALSE; -static uint8_t mReceive_Buffer[64]; +static uint8_t mReceive_Buffer[63]; /* Private function prototypes -----------------------------------------------*/ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count); uint8_t processRX(); void jump_to_app(); -#define BLUE LED1 -#define RED LED4 int main() { - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ - - /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); if (BSL_HOLD_STATE == 0) USB_connected = TRUE; @@ -116,7 +111,7 @@ int main() { case DFUidle: period1 = 5000; sweep_steps1 = 100; - PIOS_LED_Off(RED); + PIOS_LED_Off(PIOS_LED_ALARM); period2 = 0; break; case uploading: @@ -128,12 +123,12 @@ int main() { case downloading: period1 = 2500; sweep_steps1 = 50; - PIOS_LED_Off(RED); + PIOS_LED_Off(PIOS_LED_ALARM); period2 = 0; break; case BLidle: period1 = 0; - PIOS_LED_On(BLUE); + PIOS_LED_On(PIOS_LED_HEARTBEAT); period2 = 0; break; default://error @@ -145,19 +140,19 @@ int main() { if (period1 != 0) { if (LedPWM(period1, sweep_steps1, stopwatch)) - PIOS_LED_On(BLUE); + PIOS_LED_On(PIOS_LED_HEARTBEAT); else - PIOS_LED_Off(BLUE); + PIOS_LED_Off(PIOS_LED_HEARTBEAT); } else - PIOS_LED_On(BLUE); + PIOS_LED_On(PIOS_LED_HEARTBEAT); if (period2 != 0) { if (LedPWM(period2, sweep_steps2, stopwatch)) - PIOS_LED_On(RED); + PIOS_LED_On(PIOS_LED_ALARM); else - PIOS_LED_Off(RED); + PIOS_LED_Off(PIOS_LED_ALARM); } else - PIOS_LED_Off(RED); + PIOS_LED_Off(PIOS_LED_ALARM); if (stopwatch > 50 * 1000 * 1000) stopwatch = 0; @@ -181,7 +176,6 @@ void jump_to_app() { RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); _SetCNTR(0); // clear interrupt mask _SetISTR(0); // clear all requests - JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); Jump_To_Application = (pFunction) JumpAddress; /* Initialize user application's Stack Pointer */ @@ -204,7 +198,7 @@ uint32_t LedPWM(uint32_t pwm_period, uint32_t pwm_sweep_steps, uint32_t count) { } uint8_t processRX() { - if (PIOS_COM_ReceiveBuffer(PIOS_COM_TELEM_USB, mReceive_Buffer, 63, 0) == 63) { + if (PIOS_COM_MSG_Receive(PIOS_COM_TELEM_USB, mReceive_Buffer, sizeof(mReceive_Buffer))) { processComand(mReceive_Buffer); } return TRUE; diff --git a/flight/Bootloaders/PipXtreme/op_dfu.c b/flight/Bootloaders/PipXtreme/op_dfu.c index 770ce8237..249e3d320 100644 --- a/flight/Bootloaders/PipXtreme/op_dfu.c +++ b/flight/Bootloaders/PipXtreme/op_dfu.c @@ -30,11 +30,8 @@ #include "pios.h" #include "op_dfu.h" #include "pios_bl_helper.h" +#include "pios_com_msg.h" #include -/* Private typedef -----------------------------------------------------------*/ -/* Private define ------------------------------------------------------------*/ -/* Private macro -------------------------------------------------------------*/ -/* Private variables ---------------------------------------------------------*/ //programmable devices Device devicesTable[10]; uint8_t numberOfDevices = 0; @@ -102,33 +99,9 @@ void DataDownload(DownloadAction action) { currentProgrammingDestination)) { DeviceState = Last_operation_failed; } - /* - switch (currentProgrammingDestination) { - case Remote_flash_via_spi: - if (downType == Descript) { - SendBuffer[6 + (x * 4)] - = spi_dev_desc[(uint8_t) partoffset]; - SendBuffer[7 + (x * 4)] = spi_dev_desc[(uint8_t) partoffset - + 1]; - SendBuffer[8 + (x * 4)] = spi_dev_desc[(uint8_t) partoffset - + 2]; - SendBuffer[9 + (x * 4)] = spi_dev_desc[(uint8_t) partoffset - + 3]; - } - break; - case Self_flash: - SendBuffer[6 + (x * 4)] = *PIOS_BL_HELPER_FLASH_If_Read(offset); - SendBuffer[7 + (x * 4)] = *PIOS_BL_HELPER_FLASH_If_Read(offset + 1); - SendBuffer[8 + (x * 4)] = *PIOS_BL_HELPER_FLASH_If_Read(offset + 2); - SendBuffer[9 + (x * 4)] = *PIOS_BL_HELPER_FLASH_If_Read(offset + 3); - break; - } - */ } - //PIOS USB_SIL_Write(EP1_IN, (uint8_t*) SendBuffer, 64); downPacketCurrent = downPacketCurrent + 1; if (downPacketCurrent > downPacketTotal - 1) { - // STM_EVAL_LEDOn(LED2); DeviceState = Last_operation_Success; Aditionals = (uint32_t) Download; } @@ -168,7 +141,7 @@ void processComand(uint8_t *xReceive_Buffer) { case EnterDFU: if (((DeviceState == BLidle) && (Data0 < numberOfDevices)) || (DeviceState == DFUidle)) { - if (Data0 > 0)//PORQUE??? + if (Data0 > 0) OPDfuIni(TRUE); DeviceState = DFUidle; currentProgrammingDestination = devicesTable[Data0].programmingType; @@ -225,7 +198,7 @@ void processComand(uint8_t *xReceive_Buffer) { } } if (result != 1) { - DeviceState = Last_operation_failed;//ok + DeviceState = Last_operation_failed; Aditionals = (uint32_t) Command; } else { @@ -276,11 +249,9 @@ void processComand(uint8_t *xReceive_Buffer) { ++Next_Packet; } else { - DeviceState = wrong_packet_received; Aditionals = Count; } - // FLASH_ProgramWord(MemLocations[TransferType]+4,++Next_Packet);//+Count,Data); } else { DeviceState = Last_operation_failed; Aditionals = (uint32_t) Command; @@ -322,9 +293,12 @@ void processComand(uint8_t *xReceive_Buffer) { Buffer[15] = devicesTable[Data0 - 1].devID; } sendData(Buffer + 1, 63); - //PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, Buffer + 1, 63);//FIX+1 break; case JumpFW: + if (Data == 0x5AFE) { + /* Force board into safe mode */ + PIOS_IAP_WriteBootCount(0xFFFF); + } FLASH_Lock(); JumpToApp = 1; break; @@ -351,7 +325,6 @@ void processComand(uint8_t *xReceive_Buffer) { DeviceState = too_few_packets; } } - break; case Download_Req: #ifdef DEBUG_SSP @@ -474,9 +447,7 @@ uint32_t CalcFirmCRC() { } void sendData(uint8_t * buf, uint16_t size) { - PIOS_COM_SendBuffer(PIOS_COM_TELEM_USB, buf, size); - if (DeviceState == downloading) - PIOS_DELAY_WaitmS(10); + PIOS_COM_MSG_Send(PIOS_COM_TELEM_USB, buf, size); } bool flash_read(uint8_t * buffer, uint32_t adr, DFUProgType type) { diff --git a/flight/Bootloaders/PipXtreme/pios_board.c b/flight/Bootloaders/PipXtreme/pios_board.c index abf079340..60ec1db76 100644 --- a/flight/Bootloaders/PipXtreme/pios_board.c +++ b/flight/Bootloaders/PipXtreme/pios_board.c @@ -25,21 +25,64 @@ #include -// *********************************************************************************** +#if defined(PIOS_INCLUDE_LED) -#if defined(PIOS_INCLUDE_COM) +#include +static const struct pios_led pios_leds[] = { + [PIOS_LED_USB] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_LINK] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_RX] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_TX] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, +}; -#include +static const struct pios_led_cfg pios_led_cfg = { + .leds = pios_leds, + .num_leds = NELEMENTS(pios_leds), +}; -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 +#endif /* PIOS_INCLUDE_LED */ -static uint8_t pios_com_telem_usb_rx_buffer[PIOS_COM_TELEM_USB_RX_BUF_LEN]; -static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN]; +#if defined(PIOS_INCLUDE_COM_MSG) -#endif /* PIOS_INCLUDE_COM */ +#include -// *********************************************************************************** +#endif /* PIOS_INCLUDE_COM_MSG */ #if defined(PIOS_INCLUDE_USB) #include "pios_usb_priv.h" @@ -55,6 +98,9 @@ static const struct pios_usb_cfg pios_usb_main_cfg = { }, }; +#include "pios_usb_board_data_priv.h" +#include "pios_usb_desc_hid_only_priv.h" + #endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_USB_HID) @@ -94,21 +140,26 @@ void PIOS_Board_Init(void) { PIOS_GPIO_Init(); #if defined(PIOS_INCLUDE_USB) + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); + + /* Activate the HID-only USB configuration */ + PIOS_USB_DESC_HID_ONLY_Init(); + uint32_t pios_usb_id; if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) { PIOS_Assert(0); } -#if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM) - uint32_t pios_usb_com_id; - if (PIOS_USB_HID_Init(&pios_usb_com_id, &pios_usb_hid_cfg, pios_usb_id)) { +#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)) { PIOS_Assert(0); } - if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_com_id, - pios_com_telem_usb_rx_buffer, sizeof(pios_com_telem_usb_rx_buffer), - pios_com_telem_usb_tx_buffer, sizeof(pios_com_telem_usb_tx_buffer))) { + if (PIOS_COM_MSG_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id)) { PIOS_Assert(0); } -#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM */ +#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_COM_MSG */ + #endif /* PIOS_INCLUDE_USB */ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar diff --git a/flight/Bootloaders/PipXtreme/pios_usb_board_data.c b/flight/Bootloaders/PipXtreme/pios_usb_board_data.c index d54ca6565..3e41e5ac9 100644 --- a/flight/Bootloaders/PipXtreme/pios_usb_board_data.c +++ b/flight/Bootloaders/PipXtreme/pios_usb_board_data.c @@ -29,9 +29,11 @@ */ #include "pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include "pios_sys.h" /* PIOS_SYS_SerialNumberGet */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_* */ -const uint8_t PIOS_USB_BOARD_StringProductID[] = { - sizeof(PIOS_USB_BOARD_StringProductID), +static const uint8_t usb_product_id[20] = { + sizeof(usb_product_id), USB_DESC_TYPE_STRING, 'P', 0, 'i', 0, @@ -44,3 +46,74 @@ const uint8_t PIOS_USB_BOARD_StringProductID[] = { 'e', 0, }; +static uint8_t usb_serial_number[52] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0 +}; + +static const struct usb_string_langid usb_lang_id = { + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_UK), +}; + +static const uint8_t usb_vendor_id[28] = { + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 +}; + +int32_t PIOS_USB_BOARD_DATA_Init(void) +{ + /* Load device serial number into serial number string */ + uint8_t sn[25]; + PIOS_SYS_SerialNumberGet((char *)sn); + for (uint8_t i = 0; sn[i] != '\0' && (2 * i) < usb_serial_number[0]; i++) { + usb_serial_number[2 + 2 * i] = sn[i]; + } + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; +} diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 589251a4a..39afb2cee 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -50,7 +50,7 @@ ENABLE_AUX_UART ?= NO USE_GPS ?= YES -USE_I2C ?= NO +USE_I2C ?= YES # Set to YES when using Code Sourcery toolchain CODE_SOURCERY ?= YES @@ -65,7 +65,7 @@ endif FLASH_TOOL = OPENOCD # List of modules to include -OPTMODULES = CameraStab ComUsbBridge +OPTMODULES = CameraStab ComUsbBridge Altitude ifeq ($(USE_GPS), YES) OPTMODULES += GPS endif @@ -121,6 +121,7 @@ PYMITEINC += $(PYMITEPLAT) PYMITEINC += $(OUTDIR) FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans +HWDEFSINC = ../board_hw_defs/$(BOARD_NAME) OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight @@ -136,7 +137,6 @@ SRC += ${OPMODULEDIR}/System/systemmod.c SRC += $(OPSYSTEM)/coptercontrol.c SRC += $(OPSYSTEM)/pios_board.c SRC += $(OPSYSTEM)/alarms.c -SRC += $(OPSYSTEM)/taskmonitor.c SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c @@ -182,6 +182,7 @@ SRC += $(OPUAVSYNTHDIR)/receiveractivity.c SRC += $(OPUAVSYNTHDIR)/taskinfo.c SRC += $(OPUAVSYNTHDIR)/mixerstatus.c SRC += $(OPUAVSYNTHDIR)/ratedesired.c +SRC += $(OPUAVSYNTHDIR)/baroaltitude.c endif @@ -216,6 +217,7 @@ SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c SRC += $(OPSYSTEM)/pios_usb_board_data.c SRC += $(PIOSCOMMON)/pios_usb_desc_hid_cdc.c +SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c ## PIOS Hardware (Common) SRC += $(PIOSCOMMON)/pios_crc.c @@ -224,6 +226,7 @@ SRC += $(PIOSCOMMON)/pios_flash_w25x.c SRC += $(PIOSCOMMON)/pios_adxl345.c SRC += $(PIOSCOMMON)/pios_com.c SRC += $(PIOSCOMMON)/pios_i2c_esc.c +SRC += $(PIOSCOMMON)/pios_bmp085.c SRC += $(PIOSCOMMON)/pios_iap.c SRC += $(PIOSCOMMON)/pios_bl_helper.c SRC += $(PIOSCOMMON)/pios_rcvr.c @@ -232,6 +235,7 @@ SRC += $(PIOSCOMMON)/printf-stdarg.c ## Libraries for flight calculations SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/CoordinateConversions.c +SRC += $(FLIGHTLIB)/taskmonitor.c ## CMSIS for STM32 SRC += $(CMSISDIR)/core_cm3.c @@ -345,6 +349,7 @@ EXTRAINCDIRS += $(APPLIBDIR) EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/ARM_CM3 EXTRAINCDIRS += $(AHRSBOOTLOADERINC) EXTRAINCDIRS += $(PYMITEINC) +EXTRAINCDIRS += $(HWDEFSINC) EXTRAINCDIRS += ${foreach MOD, ${OPTMODULES} ${MODULES}, ${OPMODULEDIR}/${MOD}/inc} ${OPMODULEDIR}/System/inc @@ -409,6 +414,9 @@ ifeq ($(USE_I2C), YES) CDEFS += -DUSE_I2C endif +# Declare all non-optional modules as built-in to force inclusion +CDEFS += ${foreach MOD, ${MODULES}, -DMODULE_$(MOD)_BUILTIN } + # Place project-specific -D and/or -U options for # Assembler with preprocessor here. #ADEFS = -DUSE_IRQ_ASM_WRAPPER @@ -435,15 +443,15 @@ CSTANDARD = -std=gnu99 # Flags for C and C++ (arm-elf-gcc/arm-elf-g++) ifeq ($(DEBUG),YES) -CFLAGS = -DDEBUG +CFLAGS += -DDEBUG endif ifeq ($(DIAGNOSTICS),YES) -CFLAGS = -DDIAGNOSTICS +CFLAGS += -DDIAGNOSTICS endif ifeq ($(DIAG_TASKS),YES) -CFLAGS = -DDIAG_TASKS +CFLAGS += -DDIAG_TASKS endif CFLAGS += -g$(DEBUGF) diff --git a/flight/CopterControl/System/coptercontrol.c b/flight/CopterControl/System/coptercontrol.c index a7cd909d9..3c64141ec 100644 --- a/flight/CopterControl/System/coptercontrol.c +++ b/flight/CopterControl/System/coptercontrol.c @@ -72,7 +72,9 @@ int main() #ifdef ERASE_FLASH PIOS_Flash_W25X_EraseChip(); - PIOS_LED_Off(LED1); +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Off(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ while (1) ; #endif @@ -88,13 +90,14 @@ int main() /* If all is well we will never reach here as the scheduler will now be running. */ /* Do some indication to user that something bad just happened */ - PIOS_LED_Off(LED1); while (1) { - PIOS_LED_Toggle(LED1); +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ PIOS_DELAY_WaitmS(100); } - return 0; + return 0; } /** diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index 54c5b3b22..1d701a1af 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -42,6 +42,8 @@ #endif #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_TIM #define PIOS_INCLUDE_RCVR @@ -76,13 +78,11 @@ #define PIOS_INCLUDE_ADXL345 #define PIOS_INCLUDE_FLASH +#define PIOS_INCLUDE_BMP085 + /* A really shitty setting saving implementation */ #define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS -/* Defaults for Logging */ -#define LOG_FILENAME "PIOS.LOG" -#define STARTUP_LOG_ENABLED 1 - /* Alarm Thresholds */ #define HEAP_LIMIT_WARNING 220 #define HEAP_LIMIT_CRITICAL 40 diff --git a/flight/CopterControl/System/inc/pios_usb_board_data.h b/flight/CopterControl/System/inc/pios_usb_board_data.h index ee53c7455..9907a4934 100644 --- a/flight/CopterControl/System/inc/pios_usb_board_data.h +++ b/flight/CopterControl/System/inc/pios_usb_board_data.h @@ -37,44 +37,7 @@ #define PIOS_USB_BOARD_EP_NUM 4 -#include "pios_usb_defs.h" /* struct usb_* */ - -struct usb_board_config { - struct usb_configuration_desc config; - struct usb_interface_association_desc iad; - struct usb_interface_desc hid_if; - struct usb_hid_desc hid; - struct usb_endpoint_desc hid_in; - struct usb_endpoint_desc hid_out; - struct usb_interface_desc cdc_control_if; - struct usb_cdc_header_func_desc cdc_header; - struct usb_cdc_callmgmt_func_desc cdc_callmgmt; - struct usb_cdc_acm_func_desc cdc_acm; - struct usb_cdc_union_func_desc cdc_union; - struct usb_endpoint_desc cdc_mgmt_in; - struct usb_interface_desc cdc_data_if; - struct usb_endpoint_desc cdc_in; - struct usb_endpoint_desc cdc_out; -} __attribute__((packed)); - -extern const struct usb_device_desc PIOS_USB_BOARD_DeviceDescriptor; -extern const struct usb_board_config PIOS_USB_BOARD_Configuration; -extern const struct usb_string_langid PIOS_USB_BOARD_StringLangID; - -/* NOTE NOTE NOTE - * - * Care must be taken to ensure that the _actual_ contents of - * these arrays (in each board's pios_usb_board_data.c) is no - * smaller than the stated sizes here or these descriptors - * will end up with trailing zeros on the wire. - * - * The compiler will catch any time that these definitions are - * too small. - */ -extern const uint8_t PIOS_USB_BOARD_HidReportDescriptor[36]; -extern const uint8_t PIOS_USB_BOARD_StringVendorID[28]; -extern const uint8_t PIOS_USB_BOARD_StringProductID[28]; -extern uint8_t PIOS_USB_BOARD_StringSerial[52]; +#include "pios_usb_defs.h" /* USB_* macros */ #define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_COPTERCONTROL #define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_COPTERCONTROL, USB_OP_BOARD_MODE_FW) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 3a4c3e89f..a103c3d17 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -27,6 +27,15 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +/* Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "board_hw_defs.c" + #include #include #include @@ -34,751 +43,11 @@ #include #include -#if defined(PIOS_INCLUDE_SPI) - -#include - -/* Flash/Accel Interface - * - * NOTE: Leave this declared as const data so that it ends up in the - * .rodata section (ie. Flash) rather than in the .bss section (RAM). +/* One slot per selectable receiver group. + * eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS + * NOTE: No slot in this map for NONE. */ -void PIOS_SPI_flash_accel_irq_handler(void); -void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler"))); -void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler"))); -static const struct pios_spi_cfg pios_spi_flash_accel_cfg = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = FALSE, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), - .init = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel4, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel5, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .ssel = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, -}; - -static uint32_t pios_spi_flash_accel_id; -void PIOS_SPI_flash_accel_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_flash_accel_id); -} - -#endif /* PIOS_INCLUDE_SPI */ - -/* - * ADC system - */ -#include "pios_adc_priv.h" -extern void PIOS_ADC_handler(void); -void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); -// Remap the ADC DMA handler to this one -static const struct pios_adc_cfg pios_adc_cfg = { - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - .irq = { - .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), - .init = { - .NVIC_IRQChannel = DMA1_Channel1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA1_Channel1, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR, - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - } - }, - .half_flag = DMA1_IT_HT1, - .full_flag = DMA1_IT_TC1, -}; - -struct pios_adc_dev pios_adc_devs[] = { - { - .cfg = &pios_adc_cfg, - .callback_function = NULL, - }, -}; - -uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs); - -void PIOS_ADC_handler() { - PIOS_ADC_DMA_Handler(); -} - -#include "pios_tim_priv.h" - -static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, -}; - -static const struct pios_tim_clock_cfg tim_1_cfg = { - .timer = TIM1, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_2_cfg = { - .timer = TIM2, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_3_cfg = { - .timer = TIM3, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_4_cfg = { - .timer = TIM4, - .time_base_init = &tim_1_2_3_4_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -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, - }, - }, - }, -}; - -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, - }, - }, - }, -}; - - -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, - }, - }, - }, - - // 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, - }, - }, - }, -}; -#if defined(PIOS_INCLUDE_USART) - -#include "pios_usart_priv.h" - -static const struct pios_usart_cfg pios_usart_generic_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_generic_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, - }, - }, -}; - -#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_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 */ - -#endif /* PIOS_INCLUDE_USART */ - -#if defined(PIOS_INCLUDE_COM) - -#include "pios_com_priv.h" +uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; #define PIOS_COM_TELEM_RF_RX_BUF_LEN 32 #define PIOS_COM_TELEM_RF_TX_BUF_LEN 12 @@ -791,238 +60,6 @@ static const struct pios_sbus_cfg pios_sbus_cfg = { #define PIOS_COM_BRIDGE_RX_BUF_LEN 65 #define PIOS_COM_BRIDGE_TX_BUF_LEN 12 -#endif /* PIOS_INCLUDE_COM */ - -#if defined(PIOS_INCLUDE_RTC) -/* - * Realtime Clock (RTC) - */ -#include - -void PIOS_RTC_IRQ_Handler (void); -void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); -static const struct pios_rtc_cfg pios_rtc_main_cfg = { - .clksrc = RCC_RTCCLKSource_HSE_Div128, - .prescaler = 100, - .irq = { - .init = { - .NVIC_IRQChannel = RTC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -void PIOS_RTC_IRQ_Handler (void) -{ - PIOS_RTC_irq_handler (); -} - -#endif - -/* - * Servo outputs - */ -#include - -const struct pios_servo_cfg pios_servo_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = NELEMENTS(pios_tim_servoport_all_pins), -}; - -const struct pios_servo_cfg pios_servo_rcvr_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_rcvrport_pins, - .num_channels = NELEMENTS(pios_tim_servoport_rcvrport_pins), -}; - - -/* - * PPM Inputs - */ -#if defined(PIOS_INCLUDE_PPM) -#include - -const struct pios_ppm_cfg pios_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - /* Use only the first channel for ppm */ - .channels = &pios_tim_rcvrport_all_channels[0], - .num_channels = 1, -}; - -#endif /* PIOS_INCLUDE_PPM */ - -/* - * PWM Inputs - */ -#if defined(PIOS_INCLUDE_PWM) -#include - -const struct pios_pwm_cfg pios_pwm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = pios_tim_rcvrport_all_channels, - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), -}; -#endif - -#if defined(PIOS_INCLUDE_I2C) - -#include - -/* - * I2C Adapters - */ - -void PIOS_I2C_main_adapter_ev_irq_handler(void); -void PIOS_I2C_main_adapter_er_irq_handler(void); -void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_ev_irq_handler"))); -void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_er_irq_handler"))); - -static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { - .regs = I2C2, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -uint32_t pios_i2c_main_adapter_id; -void PIOS_I2C_main_adapter_ev_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_main_adapter_id); -} - -void PIOS_I2C_main_adapter_er_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_main_adapter_id); -} - -#endif /* PIOS_INCLUDE_I2C */ - -#if defined(PIOS_INCLUDE_GCSRCVR) -#include "pios_gcsrcvr_priv.h" -#endif /* PIOS_INCLUDE_GCSRCVR */ - -#if defined(PIOS_INCLUDE_RCVR) -#include "pios_rcvr_priv.h" - -/* 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]; - -#endif /* PIOS_INCLUDE_RCVR */ - -#if defined(PIOS_INCLUDE_USB) -#include "pios_usb_priv.h" - -static const struct pios_usb_cfg pios_usb_main_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; -#endif /* PIOS_INCLUDE_USB */ - -#if defined(PIOS_INCLUDE_USB_HID) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; -#endif /* PIOS_INCLUDE_USB_HID */ - -#if defined(PIOS_INCLUDE_USB_CDC) -#include - -const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { - .ctrl_if = 1, - .ctrl_tx_ep = 2, - - .data_if = 2, - .data_rx_ep = 3, - .data_tx_ep = 3, -}; -#endif /* PIOS_INCLUDE_USB_CDC */ - uint32_t pios_com_telem_rf_id; uint32_t pios_com_telem_usb_id; uint32_t pios_com_vcp_id; @@ -1053,6 +90,15 @@ void PIOS_Board_Init(void) { EventDispatcherInitialize(); UAVObjInitialize(); +#if defined(PIOS_INCLUDE_RTC) + /* Initialize the real-time clock and its associated tick */ + PIOS_RTC_Init(&pios_rtc_main_cfg); +#endif + +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + HwSettingsInitialize(); #ifndef ERASE_FLASH @@ -1075,11 +121,6 @@ void PIOS_Board_Init(void) { AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } -#if defined(PIOS_INCLUDE_RTC) - /* Initialize the real-time clock and its associated tick */ - PIOS_RTC_Init(&pios_rtc_main_cfg); -#endif - /* Initialize the task monitor library */ TaskMonitorInitialize(); @@ -1090,14 +131,50 @@ 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; + + uint8_t hwsettings_usb_devicetype; + HwSettingsUSB_DeviceTypeGet(&hwsettings_usb_devicetype); + + switch (hwsettings_usb_devicetype) { + case HWSETTINGS_USB_DEVICETYPE_HIDONLY: + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + break; + case HWSETTINGS_USB_DEVICETYPE_HIDVCP: + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + usb_cdc_present = true; + break; + case HWSETTINGS_USB_DEVICETYPE_VCPONLY: + break; + default: + PIOS_Assert(0); + } + uint32_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_CDC) - /* Configure the USB VCP port */ + 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; @@ -1136,7 +213,6 @@ void PIOS_Board_Init(void) { tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } - } #endif /* PIOS_INCLUDE_COM */ break; @@ -1148,6 +224,11 @@ void PIOS_Board_Init(void) { 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; @@ -1419,7 +500,7 @@ void PIOS_Board_Init(void) { case HWSETTINGS_CC_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) { - if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) { + if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { PIOS_Assert(0); } } diff --git a/flight/CopterControl/System/pios_usb_board_data.c b/flight/CopterControl/System/pios_usb_board_data.c index b39fcbec3..4f02ce424 100644 --- a/flight/CopterControl/System/pios_usb_board_data.c +++ b/flight/CopterControl/System/pios_usb_board_data.c @@ -29,9 +29,11 @@ */ #include "pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include "pios_sys.h" /* PIOS_SYS_SerialNumberGet */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_* */ -const uint8_t PIOS_USB_BOARD_StringProductID[] = { - sizeof(PIOS_USB_BOARD_StringProductID), +static const uint8_t usb_product_id[28] = { + sizeof(usb_product_id), USB_DESC_TYPE_STRING, 'C', 0, 'o', 0, @@ -48,3 +50,74 @@ const uint8_t PIOS_USB_BOARD_StringProductID[] = { 'l', 0, }; +static uint8_t usb_serial_number[52] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0 +}; + +static const struct usb_string_langid usb_lang_id = { + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_UK), +}; + +static const uint8_t usb_vendor_id[28] = { + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 +}; + +int32_t PIOS_USB_BOARD_DATA_Init(void) +{ + /* Load device serial number into serial number string */ + uint8_t sn[25]; + PIOS_SYS_SerialNumberGet((char *)sn); + for (uint8_t i = 0; sn[i] != '\0' && (2 * i) < usb_serial_number[0]; i++) { + usb_serial_number[2 + 2 * i] = sn[i]; + } + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; +} diff --git a/flight/CopterControl/System/taskmonitor.c b/flight/CopterControl/System/taskmonitor.c deleted file mode 100644 index 627dedaab..000000000 --- a/flight/CopterControl/System/taskmonitor.c +++ /dev/null @@ -1,131 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotSystem OpenPilot System - * @{ - * @addtogroup OpenPilotLibraries OpenPilot System Libraries - * @{ - * @file taskmonitor.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Task monitoring library - * @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 - */ -#include "openpilot.h" -//#include "taskmonitor.h" - -// Private constants - -// Private types - -// Private variables -static xSemaphoreHandle lock; -static xTaskHandle handles[TASKINFO_RUNNING_NUMELEM]; -static uint32_t lastMonitorTime; - -// Private functions - -/** - * Initialize library - */ -int32_t TaskMonitorInitialize(void) -{ - lock = xSemaphoreCreateRecursiveMutex(); - memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM); - lastMonitorTime = 0; -#if defined(DIAG_TASKS) - lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE(); -#endif - return 0; -} - -/** - * Register a task handle with the library - */ -int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle) -{ - if (task < TASKINFO_RUNNING_NUMELEM) - { - xSemaphoreTakeRecursive(lock, portMAX_DELAY); - handles[task] = handle; - xSemaphoreGiveRecursive(lock); - return 0; - } - else - { - return -1; - } -} - -/** - * Update the status of all tasks - */ -void TaskMonitorUpdateAll(void) -{ -#if defined(DIAG_TASKS) - TaskInfoData data; - int n; - - // Lock - xSemaphoreTakeRecursive(lock, portMAX_DELAY); - -#if ( configGENERATE_RUN_TIME_STATS == 1 ) - uint32_t currentTime; - uint32_t deltaTime; - - /* - * Calculate the amount of elapsed run time between the last time we - * measured and now. Scale so that we can convert task run times - * directly to percentages. - */ - currentTime = portGET_RUN_TIME_COUNTER_VALUE(); - deltaTime = ((currentTime - lastMonitorTime) / 100) ? : 1; /* avoid divide-by-zero if the interval is too small */ - lastMonitorTime = currentTime; -#endif - - // Update all task information - for (n = 0; n < TASKINFO_RUNNING_NUMELEM; ++n) - { - if (handles[n] != 0) - { - data.Running[n] = TASKINFO_RUNNING_TRUE; -#if defined(ARCH_POSIX) || defined(ARCH_WIN32) - data.StackRemaining[n] = 10000; -#else - data.StackRemaining[n] = uxTaskGetStackHighWaterMark(handles[n]) * 4; -#if ( configGENERATE_RUN_TIME_STATS == 1 ) - /* Generate run time stats */ - data.RunningTime[n] = uxTaskGetRunTime(handles[n]) / deltaTime; -#endif -#endif - - } - else - { - data.Running[n] = TASKINFO_RUNNING_FALSE; - data.StackRemaining[n] = 0; - data.RunningTime[n] = 0; - } - } - - // Update object - TaskInfoSet(&data); - - // Done - xSemaphoreGiveRecursive(lock); -#endif -} diff --git a/flight/INS/Makefile b/flight/INS/Makefile index 4b39267f7..a58f71b9b 100644 --- a/flight/INS/Makefile +++ b/flight/INS/Makefile @@ -71,6 +71,7 @@ OPUAVOBJINC = $(OPUAVOBJ)/inc OPSYSINC = $(OPDIR)/System/inc BOOT = ../Bootloaders/INS BOOTINC = $(BOOT)/inc +HWDEFSINC = ../board_hw_defs/$(BOARD_NAME) OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight @@ -172,6 +173,7 @@ EXTRAINCDIRS += $(CMSISDIR) EXTRAINCDIRS += $(INSINC) EXTRAINCDIRS += $(OPUAVSYNTHDIR) EXTRAINCDIRS += $(BOOTINC) +EXTRAINCDIRS += $(HWDEFSINC) # List any extra directories to look for library files here. # Also add directories where the linker should search for diff --git a/flight/INS/inc/pios_config.h b/flight/INS/inc/pios_config.h index ae6561236..a13394179 100644 --- a/flight/INS/inc/pios_config.h +++ b/flight/INS/inc/pios_config.h @@ -51,6 +51,7 @@ #define PIOS_INCLUDE_IMU3000 #define PIOS_INCLUDE_GPIO #define PIOS_INCLUDE_EXTI +#define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_BMA180 diff --git a/flight/INS/ins.c b/flight/INS/ins.c index 92224a239..2fe538acc 100644 --- a/flight/INS/ins.c +++ b/flight/INS/ins.c @@ -94,18 +94,18 @@ void blink(int led, int times) void test_accel() { if(PIOS_BMA180_Test()) - blink(LED1, 1); + blink(PIOS_LED_HEARTBEAT, 1); else - blink(LED2, 1); + blink(PIOS_LED_ALARM, 1); } #if defined (PIOS_INCLUDE_HMC5883) void test_mag() { if(PIOS_HMC5883_Test()) - blink(LED1, 2); + blink(PIOS_LED_HEARTBEAT, 2); else - blink(LED2, 2); + blink(PIOS_LED_ALARM, 2); } #endif @@ -113,9 +113,9 @@ void test_mag() void test_pressure() { if(PIOS_BMP085_Test()) - blink(LED1, 3); + blink(PIOS_LED_HEARTBEAT, 3); else - blink(LED2, 3); + blink(PIOS_LED_ALARM, 3); } #endif @@ -123,9 +123,9 @@ void test_pressure() void test_imu() { if(PIOS_IMU3000_Test()) - blink(LED1, 4); + blink(PIOS_LED_HEARTBEAT, 4); else - blink(LED2, 4); + blink(PIOS_LED_ALARM, 4); } #endif diff --git a/flight/INS/pios_board.c b/flight/INS/pios_board.c index ffbf27c45..d1c215e0e 100644 --- a/flight/INS/pios_board.c +++ b/flight/INS/pios_board.c @@ -29,326 +29,16 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include - -#if defined(PIOS_INCLUDE_SPI) - -#include - -/* SPI2 Interface - * - Used for mainboard communications and magnetometer +/* Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. * - * NOTE: Leave this declared as const data so that it ends up in the - * .rodata section (ie. Flash) rather than in the .bss section (RAM). + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE */ -void PIOS_SPI_op_mag_irq_handler(void); -void DMA1_Channel5_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_mag_irq_handler"))); -void DMA1_Channel4_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_mag_irq_handler"))); -static const struct pios_spi_cfg pios_spi_op_mag_cfg = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Slave, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Hard, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - }, - .use_crc = TRUE, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, +#include "board_hw_defs.c" - .irq = { - .flags = - (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | - DMA1_FLAG_GL4), - .init = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel4, - .init = { - .DMA_PeripheralBaseAddr = - (uint32_t) & (SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = - DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = - DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = - DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel5, - .init = { - .DMA_PeripheralBaseAddr = - (uint32_t) & (SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = - DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = - DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = - DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .ssel = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - -uint32_t pios_spi_op_mag_id; -void PIOS_SPI_op_mag_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_op_mag_id); -} - -/* SPI1 Interface - * - Used for BMA180 accelerometer - */ -void PIOS_SPI_accel_irq_handler(void); -void DMA1_Channel2_IRQHandler() __attribute__ ((alias("PIOS_SPI_accel_irq_handler"))); -void DMA1_Channel3_IRQHandler() __attribute__ ((alias("PIOS_SPI_accel_irq_handler"))); -static const struct pios_spi_cfg pios_spi_accel_cfg = { - .regs = SPI1, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_1Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, - }, - .use_crc = FALSE, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), - .init = { - .NVIC_IRQChannel = DMA1_Channel2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel2, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel3, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .ssel = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, - .sclk = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .mosi = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, -}; - -static uint32_t pios_spi_accel_id; -void PIOS_SPI_accel_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_accel_id); -} - -#endif /* PIOS_INCLUDE_SPI */ - - - -#if defined(PIOS_INCLUDE_GPS) -#include - -/* - * GPS USART - */ -static const struct pios_usart_cfg pios_usart_gps_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, - }, - }, -}; - -#endif /* PIOS_INCLUDE_GPS */ - -#ifdef PIOS_COM_AUX -/* - * AUX USART - */ -static const struct pios_usart_cfg pios_usart_aux_cfg = { - .regs = USART4, - .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 = USART4_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_AF_PP, - }, - }, -}; - -#endif /* PIOS_COM_AUX */ - - -#if defined(PIOS_INCLUDE_COM) - -#include +#include #if 0 #define PIOS_COM_AUX_TX_BUF_LEN 192 @@ -358,156 +48,6 @@ static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN]; #define PIOS_COM_GPS_RX_BUF_LEN 96 static uint8_t pios_com_gps_rx_buffer[PIOS_COM_GPS_RX_BUF_LEN]; - -#endif /* PIOS_INCLUDE_COM */ - -#if defined(PIOS_INCLUDE_I2C) - -#include - -/* - * I2C Adapters - */ - -void PIOS_I2C_pres_mag_adapter_ev_irq_handler(void); -void PIOS_I2C_pres_mag_adapter_er_irq_handler(void); -void I2C1_EV_IRQHandler() - __attribute__ ((alias("PIOS_I2C_pres_mag_adapter_ev_irq_handler"))); -void I2C1_ER_IRQHandler() - __attribute__ ((alias("PIOS_I2C_pres_mag_adapter_er_irq_handler"))); - -static const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = { - .regs = I2C1, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 200000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C1_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -uint32_t pios_i2c_pres_mag_adapter_id; -void PIOS_I2C_pres_mag_adapter_ev_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_pres_mag_adapter_id); -} - -void PIOS_I2C_pres_mag_adapter_er_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_pres_mag_adapter_id); -} - - -void PIOS_I2C_gyro_adapter_ev_irq_handler(void); -void PIOS_I2C_gyro_adapter_er_irq_handler(void); -void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_ev_irq_handler"))); -void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_er_irq_handler"))); - -static const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = { - .regs = I2C2, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -uint32_t pios_i2c_gyro_adapter_id; -void PIOS_I2C_gyro_adapter_ev_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_gyro_adapter_id); -} - -void PIOS_I2C_gyro_adapter_er_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_gyro_adapter_id); -} - -#endif /* PIOS_INCLUDE_I2C */ - - - -extern const struct pios_com_driver pios_usart_com_driver; - uint32_t pios_com_aux_id; uint32_t pios_com_gps_id; @@ -520,6 +60,10 @@ void PIOS_Board_Init(void) { /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + /* Delay system */ PIOS_DELAY_Init(); diff --git a/flight/OpenPilot/System/inc/taskmonitor.h b/flight/Libraries/inc/taskmonitor.h similarity index 100% rename from flight/OpenPilot/System/inc/taskmonitor.h rename to flight/Libraries/inc/taskmonitor.h diff --git a/flight/OpenPilot/System/taskmonitor.c b/flight/Libraries/taskmonitor.c similarity index 94% rename from flight/OpenPilot/System/taskmonitor.c rename to flight/Libraries/taskmonitor.c index dcd08945c..fffc6435c 100644 --- a/flight/OpenPilot/System/taskmonitor.c +++ b/flight/Libraries/taskmonitor.c @@ -47,7 +47,7 @@ int32_t TaskMonitorInitialize(void) lock = xSemaphoreCreateRecursiveMutex(); memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM); lastMonitorTime = 0; -#if defined(DIAGNOSTICS) +#if defined(DIAG_TASKS) lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE(); #endif return 0; @@ -94,7 +94,7 @@ int32_t TaskMonitorRemove(TaskInfoRunningElem task) */ void TaskMonitorUpdateAll(void) { -#if defined(DIAGNOSTICS) +#if defined(DIAG_TASKS) TaskInfoData data; int n; @@ -128,7 +128,6 @@ void TaskMonitorUpdateAll(void) #if ( configGENERATE_RUN_TIME_STATS == 1 ) /* Generate run time stats */ data.RunningTime[n] = uxTaskGetRunTime(handles[n]) / deltaTime; - #endif #endif diff --git a/flight/Modules/Altitude/altitude.c b/flight/Modules/Altitude/altitude.c index d1655aaa8..87a4240b6 100644 --- a/flight/Modules/Altitude/altitude.c +++ b/flight/Modules/Altitude/altitude.c @@ -37,6 +37,7 @@ */ #include "openpilot.h" +#include "hwsettings.h" #include "altitude.h" #include "baroaltitude.h" // object that will be updated by the module #if defined(PIOS_INCLUDE_HCSR04) @@ -46,8 +47,7 @@ // Private constants #define STACK_SIZE_BYTES 500 #define TASK_PRIORITY (tskIDLE_PRIORITY+1) -//#define UPDATE_PERIOD 100 -#define UPDATE_PERIOD 25 +#define UPDATE_PERIOD 50 // Private types @@ -60,6 +60,8 @@ static int32_t alt_ds_temp = 0; static int32_t alt_ds_pres = 0; static int alt_ds_count = 0; +static bool altitudeEnabled; + // Private functions static void altitudeTask(void *parameters); @@ -69,17 +71,19 @@ static void altitudeTask(void *parameters); */ int32_t AltitudeStart() { - - BaroAltitudeInitialize(); -#if defined(PIOS_INCLUDE_HCSR04) - SonarAltitudeInitialze(); -#endif - - // Start main task - xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDE, taskHandle); - return 0; + if (altitudeEnabled) { + BaroAltitudeInitialize(); +#if defined(PIOS_INCLUDE_HCSR04) + SonarAltitudeInitialze(); +#endif + + // Start main task + xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); + TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDE, taskHandle); + return 0; + } + return -1; } /** @@ -88,11 +92,23 @@ int32_t AltitudeStart() */ int32_t AltitudeInitialize() { +#ifdef MODULE_Altitude_BUILTIN + altitudeEnabled = 1; +#else + HwSettingsInitialize(); + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + if (optionalModules[HWSETTINGS_OPTIONALMODULES_ALTITUDE] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + altitudeEnabled = 1; + } else { + altitudeEnabled = 0; + } +#endif // init down-sampling data - alt_ds_temp = 0; - alt_ds_pres = 0; - alt_ds_count = 0; + alt_ds_temp = 0; + alt_ds_pres = 0; + alt_ds_count = 0; return 0; } @@ -144,34 +160,42 @@ static void altitudeTask(void *parameters) #endif // Update the temperature data PIOS_BMP085_StartADC(TemperatureConv); +#ifdef PIOS_BMP085_HAS_GPIOS xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); +#else + vTaskDelay(5 / portTICK_RATE_MS); +#endif PIOS_BMP085_ReadADC(); alt_ds_temp += PIOS_BMP085_GetTemperature(); // Update the pressure data PIOS_BMP085_StartADC(PressureConv); +#ifdef PIOS_BMP085_HAS_GPIOS xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY); +#else + vTaskDelay(26 / portTICK_RATE_MS); +#endif PIOS_BMP085_ReadADC(); alt_ds_pres += PIOS_BMP085_GetPressure(); if (++alt_ds_count >= alt_ds_size) - { - alt_ds_count = 0; + { + alt_ds_count = 0; - // Convert from 1/10ths of degC to degC - data.Temperature = alt_ds_temp / (10.0 * alt_ds_size); - alt_ds_temp = 0; + // Convert from 1/10ths of degC to degC + data.Temperature = alt_ds_temp / (10.0 * alt_ds_size); + alt_ds_temp = 0; - // Convert from Pa to kPa - data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size); - alt_ds_pres = 0; + // Convert from Pa to kPa + data.Pressure = alt_ds_pres / (1000.0f * alt_ds_size); + alt_ds_pres = 0; - // Compute the current altitude (all pressures in kPa) - data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255))); + // Compute the current altitude (all pressures in kPa) + data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255))); - // Update the AltitudeActual UAVObject - BaroAltitudeSet(&data); - } + // Update the AltitudeActual UAVObject + BaroAltitudeSet(&data); + } // Delay until it is time to read the next sample vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD / portTICK_RATE_MS); diff --git a/flight/Modules/CameraStab/camerastab.c b/flight/Modules/CameraStab/camerastab.c index b0625693f..4e83ae33b 100644 --- a/flight/Modules/CameraStab/camerastab.c +++ b/flight/Modules/CameraStab/camerastab.c @@ -78,6 +78,10 @@ static float bound(float val, float limit); int32_t CameraStabInitialize(void) { bool cameraStabEnabled; + +#ifdef MODULE_CameraStab_BUILTIN + cameraStabEnabled = true; +#else uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; HwSettingsInitialize(); @@ -87,6 +91,7 @@ int32_t CameraStabInitialize(void) cameraStabEnabled = true; else cameraStabEnabled = false; +#endif if (cameraStabEnabled) { diff --git a/flight/Modules/ComUsbBridge/ComUsbBridge.c b/flight/Modules/ComUsbBridge/ComUsbBridge.c index 984a53944..678e7039d 100644 --- a/flight/Modules/ComUsbBridge/ComUsbBridge.c +++ b/flight/Modules/ComUsbBridge/ComUsbBridge.c @@ -94,6 +94,9 @@ static int32_t comUsbBridgeInitialize(void) usart_port = PIOS_COM_BRIDGE; vcp_port = PIOS_COM_VCP; +#ifdef MODULE_ComUsbBridge_BUILTIN + bridge_enabled = true; +#else HwSettingsInitialize(); uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; @@ -104,6 +107,7 @@ static int32_t comUsbBridgeInitialize(void) bridge_enabled = true; else bridge_enabled = false; +#endif if (bridge_enabled) { com2usb_buf = pvPortMalloc(BRIDGE_BUF_LEN); diff --git a/flight/Modules/Fault/Fault.c b/flight/Modules/Fault/Fault.c index 0ac3748a6..868330739 100644 --- a/flight/Modules/Fault/Fault.c +++ b/flight/Modules/Fault/Fault.c @@ -40,6 +40,9 @@ static uint8_t active_fault; static int32_t fault_initialize(void) { +#ifdef MODULE_Fault_BUILTIN + module_enabled = true; +#else HwSettingsInitialize(); uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; @@ -50,6 +53,7 @@ static int32_t fault_initialize(void) } else { module_enabled = false; } +#endif /* Do this outside the module_enabled test so that it * can be changed even when the module has been disabled. diff --git a/flight/Modules/FirmwareIAP/firmwareiap.c b/flight/Modules/FirmwareIAP/firmwareiap.c index 21678397e..aaf2622bd 100644 --- a/flight/Modules/FirmwareIAP/firmwareiap.c +++ b/flight/Modules/FirmwareIAP/firmwareiap.c @@ -230,11 +230,14 @@ static uint32_t get_time(void) * Executed by event dispatcher callback to reset INS before resetting OP */ static void resetTask(UAVObjEvent * ev) -{ - PIOS_LED_Toggle(LED1); -#if (PIOS_LED_NUM > 1) - PIOS_LED_Toggle(LED2); -#endif +{ +#if defined (PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + +#if defined (PIOS_LED_ALARM) + PIOS_LED_Toggle(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ if((portTickType) (xTaskGetTickCount() - lastResetSysTime) > RESET_DELAY / portTICK_RATE_MS) { lastResetSysTime = xTaskGetTickCount(); diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index bc8f92c17..1f93d6dba 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -121,6 +121,9 @@ int32_t GPSInitialize(void) { gpsPort = PIOS_COM_GPS; +#ifdef MODULE_GPS_BUILTIN + gpsEnabled = true; +#else HwSettingsInitialize(); uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; @@ -130,6 +133,7 @@ int32_t GPSInitialize(void) gpsEnabled = true; else gpsEnabled = false; +#endif if (gpsPort && gpsEnabled) { GPSPositionInitialize(); diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c index bb3496b0f..cae6af590 100644 --- a/flight/Modules/System/systemmod.c +++ b/flight/Modules/System/systemmod.c @@ -48,8 +48,6 @@ #include "taskinfo.h" #include "watchdogstatus.h" #include "taskmonitor.h" -#include "pios_iap.h" - // Private constants #define SYSTEM_UPDATE_PERIOD_MS 1000 @@ -148,8 +146,10 @@ static void systemTask(void *parameters) PIOS_SYS_Reset(); } +#if defined(PIOS_INCLUDE_IAP) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); +#endif // Initialize vars idleCounter = 0; @@ -170,20 +170,25 @@ static void systemTask(void *parameters) updateI2Cstats(); updateWDGstats(); #endif + +#if defined(DIAG_TASKS) // Update the task status object TaskMonitorUpdateAll(); +#endif // Flash the heartbeat LED - PIOS_LED_Toggle(LED1); +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set -#if (PIOS_LED_NUM > 1) +#if defined (PIOS_LED_ALARM) if (AlarmsHasWarnings()) { - PIOS_LED_On(LED2); + PIOS_LED_On(PIOS_LED_ALARM); } else { - PIOS_LED_Off(LED2); + PIOS_LED_Off(PIOS_LED_ALARM); } -#endif +#endif /* PIOS_LED_ALARM */ FlightStatusData flightStatus; FlightStatusGet(&flightStatus); diff --git a/flight/Modules/Telemetry/telemetry.c b/flight/Modules/Telemetry/telemetry.c index d01085591..12818529f 100644 --- a/flight/Modules/Telemetry/telemetry.c +++ b/flight/Modules/Telemetry/telemetry.c @@ -35,7 +35,6 @@ #include "flighttelemetrystats.h" #include "gcstelemetrystats.h" #include "hwsettings.h" -#include "pios_usb.h" /* PIOS_USB_* */ // Private constants #define MAX_QUEUE_SIZE TELEM_QUEUE_SIZE diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index 8f9d9bef9..96be8ff31 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -39,6 +39,7 @@ DEBUG ?= YES # Include objects that are just nice information to show DIAGNOSTICS ?= YES +DIAG_TASKS ?= YES # Set to YES to use the Servo output pins for debugging via scope or logic analyser ENABLE_DEBUG_PINS ?= NO @@ -112,6 +113,7 @@ PYMITEINC += $(PYMITEPLAT) PYMITEINC += $(OUTDIR) FLIGHTPLANLIB = $(OPMODULEDIR)/FlightPlan/lib FLIGHTPLANS = $(OPMODULEDIR)/FlightPlan/flightplans +HWDEFSINC = ../board_hw_defs/$(BOARD_NAME) UAVOBJSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight @@ -138,7 +140,6 @@ SRC += ${OPMODULEDIR}/System/systemmod.c SRC += $(OPSYSTEM)/openpilot.c SRC += $(OPSYSTEM)/pios_board.c SRC += $(OPSYSTEM)/alarms.c -SRC += $(OPSYSTEM)/taskmonitor.c SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c @@ -183,9 +184,11 @@ SRC += $(PIOSSTM32F10X)/pios_wdg.c SRC += $(PIOSSTM32F10X)/pios_usb.c SRC += $(PIOSSTM32F10X)/pios_usbhook.c SRC += $(PIOSSTM32F10X)/pios_usb_hid.c +SRC += $(PIOSSTM32F10X)/pios_usb_cdc.c SRC += $(PIOSSTM32F10X)/pios_usb_hid_istr.c SRC += $(PIOSSTM32F10X)/pios_usb_hid_pwr.c SRC += $(OPSYSTEM)/pios_usb_board_data.c +SRC += $(PIOSCOMMON)/pios_usb_desc_hid_cdc.c SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c ## PIOS Hardware (Common) @@ -205,6 +208,7 @@ SRC += $(FLIGHTLIB)/ahrs_comm_objects.c SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/CoordinateConversions.c +SRC += $(FLIGHTLIB)/taskmonitor.c ## CMSIS for STM32 SRC += $(CMSISDIR)/core_cm3.c @@ -314,6 +318,7 @@ EXTRAINCDIRS += $(APPLIBDIR) EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/ARM_CM3 EXTRAINCDIRS += $(AHRSBOOTLOADERINC) EXTRAINCDIRS += $(PYMITEINC) +EXTRAINCDIRS += $(HWDEFSINC) EXTRAINCDIRS += ${foreach MOD, ${OPTMODULES} ${MODULES} ${PYMODULES}, $(OPMODULEDIR)/${MOD}/inc} ${OPMODULEDIR}/System/inc @@ -367,6 +372,9 @@ ifeq ($(ENABLE_AUX_UART), YES) CDEFS += -DPIOS_ENABLE_AUX_UART endif +# Declare all non-optional modules as built-in to force inclusion +CDEFS += ${foreach MOD, ${MODULES}, -DMODULE_$(MOD)_BUILTIN } + # Place project-specific -D and/or -U options for # Assembler with preprocessor here. #ADEFS = -DUSE_IRQ_ASM_WRAPPER @@ -393,11 +401,15 @@ CSTANDARD = -std=gnu99 # Flags for C and C++ (arm-elf-gcc/arm-elf-g++) ifeq ($(DEBUG),YES) -CFLAGS = -DDEBUG +CFLAGS += -DDEBUG endif ifeq ($(DIAGNOSTICS),YES) -CFLAGS = -DDIAGNOSTICS +CFLAGS += -DDIAGNOSTICS +endif + +ifeq ($(DIAG_TASKS),YES) +CFLAGS += -DDIAG_TASKS endif CFLAGS += -g$(DEBUGF) diff --git a/flight/OpenPilot/Makefile.posix b/flight/OpenPilot/Makefile.posix index 4a947ae8d..5fbc66b4b 100644 --- a/flight/OpenPilot/Makefile.posix +++ b/flight/OpenPilot/Makefile.posix @@ -149,7 +149,6 @@ SRC += ${OPMODULEDIR}/System/systemmod.c SRC += $(OPSYSTEM)/openpilot.c SRC += $(OPSYSTEM)/pios_board_posix.c SRC += $(OPSYSTEM)/alarms.c -SRC += $(OPSYSTEM)/taskmonitor.c SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c @@ -189,6 +188,7 @@ SRC += $(PIOSPOSIX)/pios_rcvr.c SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/CoordinateConversions.c +SRC += $(FLIGHTLIB)/taskmonitor.c ## RTOS and RTOS Portable SRC += $(RTOSSRCDIR)/list.c SRC += $(RTOSSRCDIR)/queue.c diff --git a/flight/OpenPilot/System/inc/pios_config.h b/flight/OpenPilot/System/inc/pios_config.h index 3ec902e8e..b676ec291 100644 --- a/flight/OpenPilot/System/inc/pios_config.h +++ b/flight/OpenPilot/System/inc/pios_config.h @@ -41,6 +41,8 @@ #define PIOS_INCLUDE_I2C #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_IAP +#define PIOS_INCLUDE_TIM #define PIOS_INCLUDE_RCVR @@ -57,6 +59,7 @@ #define PIOS_INCLUDE_USART #define PIOS_INCLUDE_USB #define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_USB_CDC #define PIOS_INCLUDE_BMP085 //#define PIOS_INCLUDE_HCSR04 #define PIOS_INCLUDE_OPAHRS diff --git a/flight/OpenPilot/System/inc/pios_usb_board_data.h b/flight/OpenPilot/System/inc/pios_usb_board_data.h index e2f079bc2..08fc3423b 100644 --- a/flight/OpenPilot/System/inc/pios_usb_board_data.h +++ b/flight/OpenPilot/System/inc/pios_usb_board_data.h @@ -39,43 +39,6 @@ #include "pios_usb_defs.h" /* struct usb_* */ -struct usb_board_config { - struct usb_configuration_desc config; - struct usb_interface_association_desc iad; - struct usb_interface_desc hid_if; - struct usb_hid_desc hid; - struct usb_endpoint_desc hid_in; - struct usb_endpoint_desc hid_out; - struct usb_interface_desc cdc_control_if; - struct usb_cdc_header_func_desc cdc_header; - struct usb_cdc_callmgmt_func_desc cdc_callmgmt; - struct usb_cdc_acm_func_desc cdc_acm; - struct usb_cdc_union_func_desc cdc_union; - struct usb_endpoint_desc cdc_mgmt_in; - struct usb_interface_desc cdc_data_if; - struct usb_endpoint_desc cdc_in; - struct usb_endpoint_desc cdc_out; -} __attribute__((packed)); - -extern const struct usb_device_desc PIOS_USB_BOARD_DeviceDescriptor; -extern const struct usb_board_config PIOS_USB_BOARD_Configuration; -extern const struct usb_string_langid PIOS_USB_BOARD_StringLangID; - -/* NOTE NOTE NOTE - * - * Care must be taken to ensure that the _actual_ contents of - * these arrays (in each board's pios_usb_board_data.c) is no - * smaller than the stated sizes here or these descriptors - * will end up with trailing zeros on the wire. - * - * The compiler will catch any time that these definitions are - * too small. - */ -extern const uint8_t PIOS_USB_BOARD_HidReportDescriptor[36]; -extern const uint8_t PIOS_USB_BOARD_StringVendorID[28]; -extern const uint8_t PIOS_USB_BOARD_StringProductID[20]; -extern uint8_t PIOS_USB_BOARD_StringSerial[52]; - #define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_OPENPILOT_MAIN #define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_OPENPILOT_MAIN, USB_OP_BOARD_MODE_FW) diff --git a/flight/OpenPilot/System/openpilot.c b/flight/OpenPilot/System/openpilot.c index c90f5fd81..affe65926 100644 --- a/flight/OpenPilot/System/openpilot.c +++ b/flight/OpenPilot/System/openpilot.c @@ -110,13 +110,12 @@ int main() /* If all is well we will never reach here as the scheduler will now be running. */ /* Do some indication to user that something bad just happened */ - PIOS_LED_Off(LED1); \ - for(;;) { \ - PIOS_LED_Toggle(LED1); \ - PIOS_DELAY_WaitmS(100); \ - }; - - return 0; + while (1) { +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ + PIOS_DELAY_WaitmS(100); + } } diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index 314a8e24d..ad9c13083 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -27,6 +27,16 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ + +/* Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. + * + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE + */ +#include "board_hw_defs.c" + #include #include #include @@ -36,572 +46,11 @@ //#define I2C_DEBUG_PIN 0 //#define USART_GPS_DEBUG_PIN 1 -#if defined(PIOS_INCLUDE_SPI) - -#include - -/* MicroSD Interface - * - * NOTE: Leave this declared as const data so that it ends up in the - * .rodata section (ie. Flash) rather than in the .bss section (RAM). +/* One slot per selectable receiver group. + * eg. PWM, PPM, GCS, DSMMAINPORT, DSMFLEXIPORT, SBUS + * NOTE: No slot in this map for NONE. */ -void PIOS_SPI_sdcard_irq_handler(void); -void DMA1_Channel2_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler"))); -void DMA1_Channel3_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler"))); -static const struct pios_spi_cfg pios_spi_sdcard_cfg = { - .regs = SPI1, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256, /* Maximum divider (ie. slowest clock rate) */ - }, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), - .init = { - .NVIC_IRQChannel = DMA1_Channel2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel2, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel3, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .ssel = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, - .sclk = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .mosi = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, -}; - -/* AHRS Interface - * - * NOTE: Leave this declared as const data so that it ends up in the - * .rodata section (ie. Flash) rather than in the .bss section (RAM). - */ -void PIOS_SPI_ahrs_irq_handler(void); -void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler"))); -void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler"))); -static const struct pios_spi_cfg pios_spi_ahrs_cfg = { - .regs = SPI2, - .init = { - .SPI_Mode = SPI_Mode_Master, - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, - .SPI_DataSize = SPI_DataSize_8b, - .SPI_NSS = SPI_NSS_Soft, - .SPI_FirstBit = SPI_FirstBit_MSB, - .SPI_CRCPolynomial = 7, - .SPI_CPOL = SPI_CPOL_High, - .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, - }, - .use_crc = TRUE, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), - .init = { - .NVIC_IRQChannel = DMA1_Channel4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel4, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel5, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - .ssel = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_12, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, - .sclk = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_13, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_14, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .mosi = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_15, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, -}; - -static uint32_t pios_spi_sdcard_id; -void PIOS_SPI_sdcard_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_sdcard_id); -} - -uint32_t pios_spi_ahrs_id; -void PIOS_SPI_ahrs_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_ahrs_id); -} - -#endif /* PIOS_INCLUDE_SPI */ - -/* - * ADC system - */ -#include "pios_adc_priv.h" -extern void PIOS_ADC_handler(void); -void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); -// Remap the ADC DMA handler to this one -static const struct pios_adc_cfg pios_adc_cfg = { - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - .irq = { - .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), - .init = { - .NVIC_IRQChannel = DMA1_Channel1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA1_Channel1, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR, - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_Low, - .DMA_M2M = DMA_M2M_Disable, - }, - } - }, - .half_flag = DMA1_IT_HT1, - .full_flag = DMA1_IT_TC1, -}; - -struct pios_adc_dev pios_adc_devs[] = { - { - .cfg = &pios_adc_cfg, - .callback_function = NULL, - }, -}; - -uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs); - -void PIOS_ADC_handler() { - PIOS_ADC_DMA_Handler(); -} - -#include "pios_tim_priv.h" - -static const TIM_TimeBaseInitTypeDef tim_4_8_time_base = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, -}; - -static const struct pios_tim_clock_cfg tim_4_cfg = { - .timer = TIM4, - .time_base_init = &tim_4_8_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM4_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_8_cfg = { - .timer = TIM8, - .time_base_init = &tim_4_8_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM8_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const TIM_TimeBaseInitTypeDef tim_1_3_5_time_base = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, -}; - -static const struct pios_tim_clock_cfg tim_1_cfg = { - .timer = TIM1, - .time_base_init = &tim_1_3_5_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM1_CC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_3_cfg = { - .timer = TIM3, - .time_base_init = &tim_1_3_5_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM3_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -static const struct pios_tim_clock_cfg tim_5_cfg = { - .timer = TIM5, - .time_base_init = &tim_1_3_5_time_base, - .irq = { - .init = { - .NVIC_IRQChannel = TIM5_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -#if defined(PIOS_INCLUDE_USART) - -#include "pios_usart_priv.h" - -/* - * Telemetry USART - */ -static const struct pios_usart_cfg pios_usart_telem_cfg = { - .regs = USART2, - .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 = USART2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_3, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_2, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, -}; - -/* - * GPS USART - */ -static const struct pios_usart_cfg pios_usart_gps_cfg = { - .regs = USART3, - .remap = GPIO_PartialRemap_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 = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, -}; - -#ifdef PIOS_COM_AUX -/* - * AUX USART - */ -static const struct pios_usart_cfg pios_usart_aux_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, - }, - }, - .remap = GPIO_Remap_USART1, - .rx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, -}; -#endif - -#if defined(PIOS_INCLUDE_RTC) -/* - * Realtime Clock (RTC) - */ -#include - -void PIOS_RTC_IRQ_Handler (void); -void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); -static const struct pios_rtc_cfg pios_rtc_main_cfg = { - .clksrc = RCC_RTCCLKSource_HSE_Div128, - .prescaler = 100, - .irq = { - .init = { - .NVIC_IRQChannel = RTC_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -void PIOS_RTC_IRQ_Handler (void) -{ - PIOS_RTC_irq_handler (); -} - -#endif - -#if defined(PIOS_INCLUDE_DSM) -/* - * Spektrum/JR DSM USART - */ -#include - -static const struct pios_usart_cfg pios_usart_dsm_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_cfg = { - .bind = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, -}; - -#endif /* PIOS_COM_DSM */ - -#if defined(PIOS_INCLUDE_SBUS) -#error PIOS_INCLUDE_SBUS not implemented -#endif /* PIOS_INCLUDE_SBUS */ - -#endif /* PIOS_INCLUDE_USART */ - -#if defined(PIOS_INCLUDE_COM) - -#include "pios_com_priv.h" +uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; #define PIOS_COM_TELEM_RF_RX_BUF_LEN 192 #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 @@ -611,464 +60,12 @@ static const struct pios_dsm_cfg pios_dsm_cfg = { #define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 -#endif /* PIOS_INCLUDE_COM */ - -/** - * Pios servo configuration structures - */ -#include -static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { - { - .timer = TIM4, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .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 = 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_4, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM8, - .timer_chan = TIM_Channel_4, - .pin = { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, -}; - -const struct pios_servo_cfg pios_servo_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_High, - .TIM_OCNPolarity = TIM_OCPolarity_High, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channels = pios_tim_servoport_all_pins, - .num_channels = NELEMENTS(pios_tim_servoport_all_pins), -}; - - -/* - * PWM Inputs - */ -#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) -#include -static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { - { - .timer = TIM1, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM1, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - }, - { - .timer = TIM5, - .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 = TIM1, - .timer_chan = TIM_Channel_1, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .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 = 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_1, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_PartialRemap_TIM3, - }, - { - .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, - }, -}; - -const struct pios_pwm_cfg pios_pwm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = pios_tim_rcvrport_all_channels, - .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), -}; -#endif - -/* - * PPM Input - */ -#if defined(PIOS_INCLUDE_PPM) -#include -static const struct pios_ppm_cfg pios_ppm_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - .TIM_Channel = TIM_Channel_2, - }, - /* Use only the first channel for ppm */ - .channels = &pios_tim_rcvrport_all_channels[0], - .num_channels = 1, -}; - -#endif //PPM - -#if defined(PIOS_INCLUDE_I2C) - -#include - -/* - * I2C Adapters - */ - -void PIOS_I2C_main_adapter_ev_irq_handler(void); -void PIOS_I2C_main_adapter_er_irq_handler(void); -void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_ev_irq_handler"))); -void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_er_irq_handler"))); - -static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { - .regs = I2C2, - .init = { - .I2C_Mode = I2C_Mode_I2C, - .I2C_OwnAddress1 = 0, - .I2C_Ack = I2C_Ack_Enable, - .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, - .I2C_DutyCycle = I2C_DutyCycle_2, - .I2C_ClockSpeed = 400000, /* bits/s */ - }, - .transfer_timeout_ms = 50, - .scl = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .sda = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_11, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_OD, - }, - }, - .event = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -uint32_t pios_i2c_main_adapter_id; -void PIOS_I2C_main_adapter_ev_irq_handler(void) -{ -#ifdef I2C_DEBUG_PIN - PIOS_DEBUG_PinHigh(I2C_DEBUG_PIN); -#endif - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_EV_IRQ_Handler(pios_i2c_main_adapter_id); -#ifdef I2C_DEBUG_PIN - PIOS_DEBUG_PinLow(I2C_DEBUG_PIN); -#endif -} - -void PIOS_I2C_main_adapter_er_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_I2C_ER_IRQ_Handler(pios_i2c_main_adapter_id); -} - -#endif /* PIOS_INCLUDE_I2C */ - -#if defined(PIOS_ENABLE_DEBUG_PINS) - -static const struct stm32_gpio pios_debug_pins[] = { - #define PIOS_DEBUG_PIN_SERVO_1 0 - { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, - #define PIOS_DEBUG_PIN_SERVO_2 1 - { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, - #define PIOS_DEBUG_PIN_SERVO_3 2 - { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, - #define PIOS_DEBUG_PIN_SERVO_4 3 - { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, - #define PIOS_DEBUG_PIN_SERVO_5 4 - { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, - #define PIOS_DEBUG_PIN_SERVO_6 5 - { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, - #define PIOS_DEBUG_PIN_SERVO_7 6 - { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_8, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, - #define PIOS_DEBUG_PIN_SERVO_8 7 - { - .gpio = GPIOC, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_50MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, -}; - -#endif /* PIOS_ENABLE_DEBUG_PINS */ - -#if defined(PIOS_INCLUDE_RCVR) -#include "pios_rcvr_priv.h" - -/* 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]; - -#endif /* PIOS_INCLUDE_RCVR */ - -#if defined(PIOS_INCLUDE_USB) -#include "pios_usb_priv.h" - -static const struct pios_usb_cfg pios_usb_main_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; -#endif /* PIOS_INCLUDE_USB */ - -#if defined(PIOS_INCLUDE_USB_HID) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; - -#endif /* PIOS_INCLUDE_USB_HID */ +#define PIOS_COM_BRIDGE_RX_BUF_LEN 65 +#define PIOS_COM_BRIDGE_TX_BUF_LEN 12 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_aux_id; uint32_t pios_com_dsm_id; @@ -1107,16 +104,33 @@ void PIOS_Board_Init(void) { EventDispatcherInitialize(); UAVObjInitialize(); - HwSettingsInitialize(); - #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ PIOS_RTC_Init(&pios_rtc_main_cfg); #endif +#if defined(PIOS_INCLUDE_LED) + PIOS_LED_Init(&pios_led_cfg); +#endif /* PIOS_INCLUDE_LED */ + + HwSettingsInitialize(); + + PIOS_WDG_Init(); + /* Initialize the alarms library */ AlarmsInitialize(); + PIOS_IAP_Init(); + uint16_t boot_count = PIOS_IAP_ReadBootCount(); + if (boot_count < 3) { + PIOS_IAP_WriteBootCount(++boot_count); + AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); + } else { + /* Too many failed boot attempts, force hwsettings to defaults */ + HwSettingsSetDefaults(HwSettingsHandle(), 0); + AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); + } + /* Initialize the task monitor library */ TaskMonitorInitialize(); @@ -1124,7 +138,6 @@ void PIOS_Board_Init(void) { PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_3_cfg); PIOS_TIM_InitClock(&tim_5_cfg); - PIOS_TIM_InitClock(&tim_4_cfg); PIOS_TIM_InitClock(&tim_8_cfg); @@ -1139,6 +152,133 @@ void PIOS_Board_Init(void) { /* Bind the AHRS comms layer to the AHRS SPI link */ AhrsConnect(pios_spi_ahrs_id); +#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; + + uint8_t hwsettings_usb_devicetype; + HwSettingsUSB_DeviceTypeGet(&hwsettings_usb_devicetype); + + switch (hwsettings_usb_devicetype) { + case HWSETTINGS_USB_DEVICETYPE_HIDONLY: + if (PIOS_USB_DESC_HID_ONLY_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + break; + case HWSETTINGS_USB_DEVICETYPE_HIDVCP: + if (PIOS_USB_DESC_HID_CDC_Init()) { + PIOS_Assert(0); + } + usb_hid_present = true; + usb_cdc_present = true; + break; + case HWSETTINGS_USB_DEVICETYPE_VCPONLY: + break; + default: + PIOS_Assert(0); + } + + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + +#if defined(PIOS_INCLUDE_USB_CDC) + /* Configure the USB VCP port */ + uint8_t hwsettings_usb_vcpport; + 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 *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(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 *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(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; + } +#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 *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(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; + } + +#endif /* PIOS_INCLUDE_USB_HID */ + +#endif /* PIOS_INCLUDE_USB */ + /* Configure the main IO port */ uint8_t hwsettings_op_mainport; HwSettingsOP_MainPortGet(&hwsettings_op_mainport); @@ -1285,36 +425,14 @@ void PIOS_Board_Init(void) { break; } -#if defined(PIOS_INCLUDE_USB) - uint32_t pios_usb_id; - if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) { - PIOS_Assert(0); - } -#if defined(PIOS_INCLUDE_USB_HID) && 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 *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(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_USB_HID && PIOS_INCLUDE_COM */ - -#endif /* PIOS_INCLUDE_USB */ - #if defined(PIOS_INCLUDE_I2C) if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) { PIOS_Assert(0); } #endif /* PIOS_INCLUDE_I2C */ - PIOS_IAP_Init(); - PIOS_WDG_Init(); + + /* Make sure we have at least one telemetry link configured or else fail initialization */ + PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); } /** diff --git a/flight/OpenPilot/System/pios_usb_board_data.c b/flight/OpenPilot/System/pios_usb_board_data.c index 9492d316e..a6fa67abe 100644 --- a/flight/OpenPilot/System/pios_usb_board_data.c +++ b/flight/OpenPilot/System/pios_usb_board_data.c @@ -29,9 +29,11 @@ */ #include "pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include "pios_sys.h" /* PIOS_SYS_SerialNumberGet */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_* */ -const uint8_t PIOS_USB_BOARD_StringProductID[] = { - sizeof(PIOS_USB_BOARD_StringProductID), +static const uint8_t usb_product_id[20] = { + sizeof(usb_product_id), USB_DESC_TYPE_STRING, 'O', 0, 'p', 0, @@ -43,3 +45,76 @@ const uint8_t PIOS_USB_BOARD_StringProductID[] = { 'o', 0, 't', 0, }; + +static uint8_t usb_serial_number[52] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0 +}; + +static const struct usb_string_langid usb_lang_id = { + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_UK), +}; + +static const uint8_t usb_vendor_id[28] = { + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 +}; + +int32_t PIOS_USB_BOARD_DATA_Init(void) +{ + /* Load device serial number into serial number string */ + uint8_t sn[25]; + PIOS_SYS_SerialNumberGet((char *)sn); + for (uint8_t i = 0; sn[i] != '\0' && (2 * i) < usb_serial_number[0]; i++) { + usb_serial_number[2 + 2 * i] = sn[i]; + } + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; +} + diff --git a/flight/PiOS.posix/posix/pios_debug.c b/flight/PiOS.posix/posix/pios_debug.c index 5956a9d04..7f3dc99da 100644 --- a/flight/PiOS.posix/posix/pios_debug.c +++ b/flight/PiOS.posix/posix/pios_debug.c @@ -72,7 +72,7 @@ void PIOS_DEBUG_PinValue4BitL(uint8_t value) /** * Report a serious error and halt */ -void PIOS_DEBUG_Panic(const char *msg) +void PIOS_DEBUG_Panic(const char *msg) __attribute__ ((noreturn)) { #ifdef PIOS_COM_DEBUG register int *lr asm("lr"); // Link-register holds the PC of the caller diff --git a/flight/PiOS/Boards/STM32103CB_AHRS.h b/flight/PiOS/Boards/STM32103CB_AHRS.h index 26d88fce1..1852d60fb 100644 --- a/flight/PiOS/Boards/STM32103CB_AHRS.h +++ b/flight/PiOS/Boards/STM32103CB_AHRS.h @@ -71,13 +71,7 @@ TIM8 | | | | //------------------------ // PIOS_LED //------------------------ -#define PIOS_LED_LED1_GPIO_PORT GPIOA -#define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_3 -#define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOA -#define PIOS_LED_NUM 1 -#define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT } -#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN } -#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK } +#define PIOS_LED_HEARTBEAT 0 //------------------------- // System Settings diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h index 437a49109..0b1884a42 100644 --- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h @@ -83,13 +83,7 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 //------------------------ // PIOS_LED //------------------------ -#define PIOS_LED_LED1_GPIO_PORT GPIOA -#define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_6 -#define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOA -#define PIOS_LED_NUM 1 -#define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT } -#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN } -#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK } +#define PIOS_LED_HEARTBEAT 0 //------------------------- // System Settings @@ -110,8 +104,15 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 // See also pios_board.c //------------------------ #define PIOS_I2C_MAX_DEVS 1 -extern uint32_t pios_i2c_main_adapter_id; -#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_main_adapter_id) +extern uint32_t pios_i2c_flexi_adapter_id; +#define PIOS_I2C_MAIN_ADAPTER (pios_i2c_flexi_adapter_id) +#define PIOS_I2C_ESC_ADAPTER (pios_i2c_flexi_adapter_id) +#define PIOS_I2C_BMP085_ADAPTER (pios_i2c_flexi_adapter_id) + +//------------------------ +// PIOS_BMP085 +//------------------------ +#define PIOS_BMP085_OVERSAMPLING 3 //------------------------- // SPI @@ -277,6 +278,4 @@ extern uint32_t pios_com_telem_usb_id; #define PIOS_USB_DETECT_GPIO_PORT GPIOC #define PIOS_USB_MAX_DEVS 1 #define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_15 -#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line15 -#define PIOS_IRQ_USB_PRIORITY PIOS_IRQ_PRIO_MID #endif /* STM32103CB_AHRS_H_ */ diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index b918ead67..8d04e05a4 100644 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -80,42 +80,29 @@ TIM4 | STOPWATCH | // ***************************************************************** // PIOS_LED -#define PIOS_LED_LED1_GPIO_PORT GPIOA // USB Activity LED -#define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_3 -#define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOA +#define PIOS_LED_USB 0 +#define PIOS_LED_LINK 1 +#define PIOS_LED_RX 2 +#define PIOS_LED_TX 3 -#define PIOS_LED_LED2_GPIO_PORT GPIOB // LINK LED -#define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_5 -#define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOB +#define PIOS_LED_HEARTBEAT PIOS_LED_USB +#define PIOS_LED_ALARM PIOS_LED_TX -#define PIOS_LED_LED3_GPIO_PORT GPIOB // RX LED -#define PIOS_LED_LED3_GPIO_PIN GPIO_Pin_6 -#define PIOS_LED_LED3_GPIO_CLK RCC_APB2Periph_GPIOB +#define USB_LED_ON PIOS_LED_On(PIOS_LED_USB) +#define USB_LED_OFF PIOS_LED_Off(PIOS_LED_USB) +#define USB_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_USB) -#define PIOS_LED_LED4_GPIO_PORT GPIOB // TX LED -#define PIOS_LED_LED4_GPIO_PIN GPIO_Pin_7 -#define PIOS_LED_LED4_GPIO_CLK RCC_APB2Periph_GPIOB +#define LINK_LED_ON PIOS_LED_On(PIOS_LED_LINK) +#define LINK_LED_OFF PIOS_LED_Off(PIOS_LED_LINK) +#define LINK_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_LINK) -#define PIOS_LED_NUM 4 -#define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT, PIOS_LED_LED3_GPIO_PORT, PIOS_LED_LED4_GPIO_PORT } -#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN, PIOS_LED_LED3_GPIO_PIN, PIOS_LED_LED4_GPIO_PIN } -#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK, PIOS_LED_LED3_GPIO_CLK, PIOS_LED_LED4_GPIO_CLK } +#define RX_LED_ON PIOS_LED_On(PIOS_LED_RX) +#define RX_LED_OFF PIOS_LED_Off(PIOS_LED_RX) +#define RX_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_RX) -#define USB_LED_ON PIOS_LED_On(LED1) -#define USB_LED_OFF PIOS_LED_Off(LED1) -#define USB_LED_TOGGLE PIOS_LED_Toggle(LED1) - -#define LINK_LED_ON PIOS_LED_On(LED2) -#define LINK_LED_OFF PIOS_LED_Off(LED2) -#define LINK_LED_TOGGLE PIOS_LED_Toggle(LED2) - -#define RX_LED_ON PIOS_LED_On(LED3) -#define RX_LED_OFF PIOS_LED_Off(LED3) -#define RX_LED_TOGGLE PIOS_LED_Toggle(LED3) - -#define TX_LED_ON PIOS_LED_On(LED4) -#define TX_LED_OFF PIOS_LED_Off(LED4) -#define TX_LED_TOGGLE PIOS_LED_Toggle(LED4) +#define TX_LED_ON PIOS_LED_On(PIOS_LED_TX) +#define TX_LED_OFF PIOS_LED_Off(PIOS_LED_TX) +#define TX_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_TX) // ***************************************************************** // Timer interrupt diff --git a/flight/PiOS/Boards/STM3210E_INS.h b/flight/PiOS/Boards/STM3210E_INS.h index d661e8b96..77283a5e4 100644 --- a/flight/PiOS/Boards/STM3210E_INS.h +++ b/flight/PiOS/Boards/STM3210E_INS.h @@ -76,16 +76,8 @@ TIM8 | | | | //------------------------ // PIOS_LED //------------------------ -#define PIOS_LED_LED1_GPIO_PORT GPIOA -#define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_3 -#define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOA -#define PIOS_LED_LED2_GPIO_PORT GPIOA -#define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_2 -#define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOA -#define PIOS_LED_NUM 2 -#define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } -#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } -#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 //------------------------ // PIOS_SPI @@ -100,12 +92,14 @@ TIM8 | | | | #define PIOS_I2C_MAX_DEVS 3 extern uint32_t pios_i2c_pres_mag_adapter_id; #define PIOS_I2C_MAIN_ADAPTER (pios_i2c_pres_mag_adapter_id) +#define PIOS_I2C_BMP085_ADAPTER (pios_i2c_pres_mag_adapter_id) extern uint32_t pios_i2c_gyro_adapter_id; #define PIOS_I2C_GYRO_ADAPTER (pios_i2c_gyro_adapter_id) //------------------------ // PIOS_BMP085 //------------------------ +#define PIOS_BMP085_HAS_GPIOS #define PIOS_BMP085_EOC_GPIO_PORT GPIOC #define PIOS_BMP085_EOC_GPIO_PIN GPIO_Pin_2 #define PIOS_BMP085_EOC_PORT_SOURCE GPIO_PortSourceGPIOC diff --git a/flight/PiOS/Boards/STM3210E_OP.h b/flight/PiOS/Boards/STM3210E_OP.h index a7754c2ed..5cb6ce591 100644 --- a/flight/PiOS/Boards/STM3210E_OP.h +++ b/flight/PiOS/Boards/STM3210E_OP.h @@ -91,16 +91,8 @@ TIM8 | Servo 5 | Servo 6 | Servo 7 | Servo 8 //------------------------ // PIOS_LED //------------------------ -#define PIOS_LED_LED1_GPIO_PORT GPIOC -#define PIOS_LED_LED1_GPIO_PIN GPIO_Pin_12 -#define PIOS_LED_LED1_GPIO_CLK RCC_APB2Periph_GPIOC -#define PIOS_LED_LED2_GPIO_PORT GPIOC -#define PIOS_LED_LED2_GPIO_PIN GPIO_Pin_13 -#define PIOS_LED_LED2_GPIO_CLK RCC_APB2Periph_GPIOC -#define PIOS_LED_NUM 2 -#define PIOS_LED_PORTS { PIOS_LED_LED1_GPIO_PORT, PIOS_LED_LED2_GPIO_PORT } -#define PIOS_LED_PINS { PIOS_LED_LED1_GPIO_PIN, PIOS_LED_LED2_GPIO_PIN } -#define PIOS_LED_CLKS { PIOS_LED_LED1_GPIO_CLK, PIOS_LED_LED2_GPIO_CLK } +#define PIOS_LED_HEARTBEAT 0 +#define PIOS_LED_ALARM 1 //------------------------ // PIOS_SPI @@ -115,10 +107,13 @@ TIM8 | Servo 5 | Servo 6 | Servo 7 | Servo 8 #define PIOS_I2C_MAX_DEVS 1 extern uint32_t pios_i2c_main_adapter_id; #define PIOS_I2C_MAIN_ADAPTER (pios_i2c_main_adapter_id) +#define PIOS_I2C_ESC_ADAPTER (pios_i2c_main_adapter_id) +#define PIOS_I2C_BMP085_ADAPTER (pios_i2c_main_adapter_id) //------------------------ // PIOS_BMP085 //------------------------ +#define PIOS_BMP085_HAS_GPIOS #define PIOS_BMP085_EOC_GPIO_PORT GPIOC #define PIOS_BMP085_EOC_GPIO_PIN GPIO_Pin_15 #define PIOS_BMP085_EOC_PORT_SOURCE GPIO_PortSourceGPIOC @@ -313,8 +308,6 @@ extern uint32_t pios_com_aux_id; #define PIOS_USB_MAX_DEVS 1 #define PIOS_USB_HID_MAX_DEVS 1 #define PIOS_USB_DETECT_GPIO_PIN GPIO_Pin_4 -#define PIOS_USB_DETECT_EXTI_LINE EXTI_Line4 -#define PIOS_IRQ_USB_PRIORITY PIOS_IRQ_PRIO_MID /** * glue macros for file IO diff --git a/flight/PiOS/Common/pios_bmp085.c b/flight/PiOS/Common/pios_bmp085.c index 9a6a2310f..cd294230b 100644 --- a/flight/PiOS/Common/pios_bmp085.c +++ b/flight/PiOS/Common/pios_bmp085.c @@ -36,13 +36,10 @@ #error PIOS_EXTI Must be included in the project! #endif /* PIOS_INCLUDE_EXTI */ +#include + /* Glocal Variables */ ConversionTypeTypeDef CurrentRead; -#if defined(PIOS_INCLUDE_FREERTOS) -xSemaphoreHandle PIOS_BMP085_EOC; -#else -int32_t PIOS_BMP085_EOC; -#endif /* Local Variables */ static BMP085CalibDataTypeDef CalibData; @@ -55,14 +52,68 @@ static volatile uint32_t RawPressure; static volatile uint32_t Pressure; static volatile uint16_t Temperature; +#ifdef PIOS_BMP085_HAS_GPIOS + +#if defined(PIOS_INCLUDE_FREERTOS) +xSemaphoreHandle PIOS_BMP085_EOC; +#else +int32_t PIOS_BMP085_EOC; +#endif + +void PIOS_BMP085_EndOfConversion (void) +{ +#if defined(PIOS_INCLUDE_FREERTOS) + portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; +#endif + + /* Read the ADC Value */ +#if defined(PIOS_INCLUDE_FREERTOS) + xSemaphoreGiveFromISR(PIOS_BMP085_EOC, &xHigherPriorityTaskWoken); +#else + PIOS_BMP085_EOC=1; +#endif + +#if defined(PIOS_INCLUDE_FREERTOS) + /* Yield From ISR if needed */ + portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); +#endif +} + +static const struct pios_exti_cfg pios_exti_bmp085_cfg __exti_config = { + .vector = PIOS_BMP085_EndOfConversion, + .line = PIOS_BMP085_EOC_EXTI_LINE, + .pin = { + .gpio = PIOS_BMP085_EOC_GPIO_PORT, + .init = { + .GPIO_Pin = PIOS_BMP085_EOC_GPIO_PIN, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = PIOS_BMP085_EOC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_BMP085_EOC_PRIO, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = PIOS_BMP085_EOC_EXTI_LINE, + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, +}; + +#endif /* PIOS_BMP085_HAS_GPIOS */ /** * Initialise the BMP085 sensor */ void PIOS_BMP085_Init(void) { - GPIO_InitTypeDef GPIO_InitStructure; - EXTI_InitTypeDef EXTI_InitStructure; - NVIC_InitTypeDef NVIC_InitStructure; +#ifdef PIOS_BMP085_HAS_GPIOS #if defined(PIOS_INCLUDE_FREERTOS) /* Semaphore used by ISR to signal End-Of-Conversion */ @@ -76,31 +127,18 @@ void PIOS_BMP085_Init(void) /* Enable EOC GPIO clock */ RCC_APB2PeriphClockCmd(PIOS_BMP085_EOC_CLK | RCC_APB2Periph_AFIO, ENABLE); - /* Configure EOC pin as input floating */ - GPIO_InitStructure.GPIO_Pin = PIOS_BMP085_EOC_GPIO_PIN; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; - GPIO_Init(PIOS_BMP085_EOC_GPIO_PORT, &GPIO_InitStructure); - - /* Configure the End Of Conversion (EOC) interrupt */ - GPIO_EXTILineConfig(PIOS_BMP085_EOC_PORT_SOURCE, PIOS_BMP085_EOC_PIN_SOURCE); - EXTI_InitStructure.EXTI_Line = PIOS_BMP085_EOC_EXTI_LINE; - EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; - EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; - EXTI_InitStructure.EXTI_LineCmd = ENABLE; - EXTI_Init(&EXTI_InitStructure); - - /* Enable and set EOC EXTI Interrupt to the lowest priority */ - NVIC_InitStructure.NVIC_IRQChannel = PIOS_BMP085_EOC_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_BMP085_EOC_PRIO; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); + if (PIOS_EXTI_Init(&pios_exti_bmp085_cfg)) { + PIOS_Assert(0); + } /* Configure XCLR pin as push/pull alternate funtion output */ + GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Pin = PIOS_BMP085_XCLR_GPIO_PIN; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; GPIO_Init(PIOS_BMP085_XCLR_GPIO_PORT, &GPIO_InitStructure); +#endif /* PIOS_BMP085_HAS_GPIOS */ + /* Read all 22 bytes of calibration data in one transfer, this is a very optimized way of doing things */ uint8_t Data[BMP085_CALIB_LEN]; while (!PIOS_BMP085_Read(BMP085_CALIB_ADDR, Data, BMP085_CALIB_LEN)) @@ -235,7 +273,7 @@ bool PIOS_BMP085_Read(uint8_t address, uint8_t * buffer, uint8_t len) } }; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_BMP085_ADAPTER, txn_list, NELEMENTS(txn_list)); } /** @@ -264,7 +302,7 @@ bool PIOS_BMP085_Write(uint8_t address, uint8_t buffer) , }; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_BMP085_ADAPTER, txn_list, NELEMENTS(txn_list)); } /** diff --git a/flight/PiOS/Common/pios_com_msg.c b/flight/PiOS/Common/pios_com_msg.c new file mode 100644 index 000000000..c1cebe1d9 --- /dev/null +++ b/flight/PiOS/Common/pios_com_msg.c @@ -0,0 +1,185 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_COM COM MSG layer functions + * @brief Hardware communication layer + * @{ + * + * @file pios_com_msg.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief COM MSG layer functions + * @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 + */ + +/* Project Includes */ +#include "pios.h" + +#if defined(PIOS_INCLUDE_COM_MSG) + +#include "pios_com.h" + +#define PIOS_COM_MSG_MAX_LEN 63 + +struct pios_com_msg_dev { + uint32_t lower_id; + const struct pios_com_driver * driver; + + uint8_t rx_msg_buffer[PIOS_COM_MSG_MAX_LEN]; + volatile bool rx_msg_full; + + uint8_t tx_msg_buffer[PIOS_COM_MSG_MAX_LEN]; + volatile bool tx_msg_full; +}; + +static struct pios_com_msg_dev com_msg_dev; + +static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); +static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); + +int32_t PIOS_COM_MSG_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id) +{ + PIOS_Assert(com_id); + PIOS_Assert(driver); + + PIOS_Assert(driver->bind_tx_cb); + PIOS_Assert(driver->bind_rx_cb); + + struct pios_com_msg_dev * com_dev = &com_msg_dev; + + com_dev->driver = driver; + com_dev->lower_id = lower_id; + + com_dev->rx_msg_full = false; + (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_MSG_RxInCallback, (uint32_t)com_dev); + (com_dev->driver->rx_start)(com_dev->lower_id, sizeof(com_dev->rx_msg_buffer)); + + com_dev->tx_msg_full = false; + (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_MSG_TxOutCallback, (uint32_t)com_dev); + + *com_id = (uint32_t)com_dev; + return(0); +} + +static uint16_t PIOS_COM_MSG_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +{ + struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)context; + + PIOS_Assert(buf); + PIOS_Assert(buf_len); + + uint16_t bytes_from_fifo = 0; + + if (com_dev->tx_msg_full && (buf_len >= sizeof(com_dev->tx_msg_buffer))) { + /* Room for an entire message, send it */ + memcpy(buf, com_dev->tx_msg_buffer, sizeof(com_dev->tx_msg_buffer)); + bytes_from_fifo = sizeof(com_dev->tx_msg_buffer); + com_dev->tx_msg_full = false; + } + + if (headroom) { + if (com_dev->tx_msg_full) { + *headroom = sizeof(com_dev->tx_msg_buffer); + } else { + *headroom = 0; + } + } + + return (bytes_from_fifo); +} + +static uint16_t PIOS_COM_MSG_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +{ + struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)context; + + uint16_t bytes_into_fifo = 0; + + if (!com_dev->rx_msg_full && (buf_len >= sizeof(com_dev->rx_msg_buffer))) { + memcpy(com_dev->rx_msg_buffer, buf, sizeof(com_dev->rx_msg_buffer)); + bytes_into_fifo = sizeof(com_dev->rx_msg_buffer); + com_dev->rx_msg_full = true; + } + + if (headroom) { + if (!com_dev->rx_msg_full) { + *headroom = sizeof(com_dev->rx_msg_buffer); + } else { + *headroom = 0; + } + } + + return (bytes_into_fifo); +} + +int32_t PIOS_COM_MSG_Send(uint32_t com_id, const uint8_t *msg, uint16_t msg_len) +{ + PIOS_Assert(msg); + PIOS_Assert(msg_len); + + struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)com_id; + + PIOS_Assert(msg_len == sizeof(com_dev->tx_msg_buffer)); + + /* Wait forever for room in the tx buffer */ + while (com_dev->tx_msg_full) { + /* Kick the transmitter while we wait */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, sizeof(com_dev->tx_msg_buffer)); + } + } + + memcpy((void *) com_dev->tx_msg_buffer, msg, msg_len); + com_dev->tx_msg_full = true; + + /* Kick the transmitter now that we've queued our message */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, sizeof(com_dev->tx_msg_buffer)); + } + + return 0; +} + +uint16_t PIOS_COM_MSG_Receive(uint32_t com_id, uint8_t * msg, uint16_t msg_len) +{ + PIOS_Assert(msg); + PIOS_Assert(msg_len); + + struct pios_com_msg_dev * com_dev = (struct pios_com_msg_dev *)com_id; + + PIOS_Assert(msg_len == sizeof(com_dev->rx_msg_buffer)); + + if (!com_dev->rx_msg_full) { + /* There's room in our buffer, kick the receiver */ + (com_dev->driver->rx_start)(com_dev->lower_id, sizeof(com_dev->rx_msg_buffer)); + } else { + memcpy(msg, com_dev->rx_msg_buffer, msg_len); + com_dev->rx_msg_full = false; + + return msg_len; + } + + return 0; +} + +#endif /* PIOS_INCLUDE_COM_MSG */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/Common/pios_flashfs_objlist.c b/flight/PiOS/Common/pios_flashfs_objlist.c index e02683657..b4322ab78 100644 --- a/flight/PiOS/Common/pios_flashfs_objlist.c +++ b/flight/PiOS/Common/pios_flashfs_objlist.c @@ -81,7 +81,9 @@ int32_t PIOS_FLASHFS_Init() if(object_table_magic != OBJECT_TABLE_MAGIC) { if(magic_fail_count++ > MAX_BADMAGIC) { PIOS_FLASHFS_ClearObjectTableHeader(); - PIOS_LED_Toggle(LED1); +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ magic_fail_count = 0; magic_good = true; } else { diff --git a/flight/PiOS/Common/pios_hmc5843.c b/flight/PiOS/Common/pios_hmc5843.c index b62499d71..688d2e90f 100644 --- a/flight/PiOS/Common/pios_hmc5843.c +++ b/flight/PiOS/Common/pios_hmc5843.c @@ -34,6 +34,8 @@ #if defined(PIOS_INCLUDE_HMC5843) +#include + /* HMC5843 Addresses */ #define PIOS_HMC5843_I2C_ADDR 0x1E #define PIOS_HMC5843_CONFIG_REG_A (uint8_t)0x00 @@ -107,37 +109,48 @@ static void PIOS_HMC5843_Config(PIOS_HMC5843_ConfigTypeDef * HMC5843_Config_Stru static bool PIOS_HMC5843_Read(uint8_t address, uint8_t * buffer, uint8_t len); static bool PIOS_HMC5843_Write(uint8_t address, uint8_t buffer); +void PIOS_HMC5843_EndOfConversion (void) +{ + pios_hmc5843_data_ready = true; +} + +static const struct pios_exti_cfg pios_exti_hmc5843_cfg __exti_config = { + .vector = PIOS_HMC5843_EndOfConversion, + .line = PIOS_HMC5843_DRDY_EXTI_LINE, + .pin = { + .gpio = PIOS_HMC5843_DRDY_GPIO_PORT, + .init = { + .GPIO_Pin = PIOS_HMC5843_DRDY_GPIO_PIN, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = PIOS_HMC5843_DRDY_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_HMC5843_DRDY_PRIO, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = PIOS_HMC5843_DRDY_EXTI_LINE, + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Rising, + .EXTI_LineCmd = ENABLE, + }, + }, +}; + /** - * @brieft Initialise the HMC5843 sensor + * @brief Initialise the HMC5843 sensor */ void PIOS_HMC5843_Init(void) { - GPIO_InitTypeDef GPIO_InitStructure; - EXTI_InitTypeDef EXTI_InitStructure; - NVIC_InitTypeDef NVIC_InitStructure; - /* Enable DRDY GPIO clock */ RCC_APB2PeriphClockCmd(PIOS_HMC5843_DRDY_CLK | RCC_APB2Periph_AFIO, ENABLE); - /* Configure EOC pin as input floating */ - GPIO_InitStructure.GPIO_Pin = PIOS_HMC5843_DRDY_GPIO_PIN; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; - GPIO_Init(PIOS_HMC5843_DRDY_GPIO_PORT, &GPIO_InitStructure); - - /* Configure the End Of Conversion (EOC) interrupt */ - GPIO_EXTILineConfig(PIOS_HMC5843_DRDY_PORT_SOURCE, PIOS_HMC5843_DRDY_PIN_SOURCE); - EXTI_InitStructure.EXTI_Line = PIOS_HMC5843_DRDY_EXTI_LINE; - EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; - EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; - EXTI_InitStructure.EXTI_LineCmd = ENABLE; - EXTI_Init(&EXTI_InitStructure); - - /* Enable and set EOC EXTI Interrupt to the lowest priority */ - NVIC_InitStructure.NVIC_IRQChannel = PIOS_HMC5843_DRDY_IRQn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_HMC5843_DRDY_PRIO; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); + PIOS_EXTI_Init(&pios_exti_hmc5843_cfg); /* Configure the HMC5843 Sensor */ PIOS_HMC5843_ConfigTypeDef HMC5843_InitStructure; @@ -362,11 +375,6 @@ static bool PIOS_HMC5843_Write(uint8_t address, uint8_t buffer) return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); } -void PIOS_HMC5843_IRQHandler(void) -{ - pios_hmc5843_data_ready = true; -} - #endif /** diff --git a/flight/PiOS/Common/pios_i2c_esc.c b/flight/PiOS/Common/pios_i2c_esc.c index fdb58eba0..d440bfa48 100644 --- a/flight/PiOS/Common/pios_i2c_esc.c +++ b/flight/PiOS/Common/pios_i2c_esc.c @@ -93,7 +93,7 @@ bool PIOS_I2C_ESC_SetSpeed(uint8_t speed[4]) } }; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); } bool PIOS_SetMKSpeed(uint8_t motornum, uint8_t speed) { @@ -115,7 +115,7 @@ bool PIOS_SetMKSpeed(uint8_t motornum, uint8_t speed) { } }; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); } bool PIOS_SetAstec4Address(uint8_t new_address) { @@ -134,7 +134,7 @@ bool PIOS_SetAstec4Address(uint8_t new_address) { } }; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); } bool PIOS_SetAstec4Speed(uint8_t motornum, uint8_t speed) { @@ -161,7 +161,7 @@ bool PIOS_SetAstec4Speed(uint8_t motornum, uint8_t speed) { } }; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(PIOS_I2C_ESC_ADAPTER, txn_list, NELEMENTS(txn_list)); } #endif diff --git a/flight/PiOS/Common/pios_usb_desc_hid_cdc.c b/flight/PiOS/Common/pios_usb_desc_hid_cdc.c index 3d5748163..cd4e8a1da 100644 --- a/flight/PiOS/Common/pios_usb_desc_hid_cdc.c +++ b/flight/PiOS/Common/pios_usb_desc_hid_cdc.c @@ -28,9 +28,12 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include "pios_usb_desc_hid_cdc_priv.h" /* exported API */ +#include "pios_usb_defs.h" /* struct usb_*, USB_* */ +#include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_Register* */ -const struct usb_device_desc PIOS_USB_BOARD_DeviceDescriptor = { +static const struct usb_device_desc device_desc = { .bLength = sizeof(struct usb_device_desc), .bDescriptorType = USB_DESC_TYPE_DEVICE, .bcdUSB = htousbs(0x0200), @@ -41,17 +44,79 @@ const struct usb_device_desc PIOS_USB_BOARD_DeviceDescriptor = { .idVendor = htousbs(USB_VENDOR_ID_OPENPILOT), .idProduct = htousbs(PIOS_USB_BOARD_PRODUCT_ID), .bcdDevice = htousbs(PIOS_USB_BOARD_DEVICE_VER), - .iManufacturer = 1, - .iProduct = 2, - .iSerialNumber = 3, + .iManufacturer = USB_STRING_DESC_VENDOR, + .iProduct = USB_STRING_DESC_PRODUCT, + .iSerialNumber = USB_STRING_DESC_SERIAL, .bNumConfigurations = 1, }; -const struct usb_board_config PIOS_USB_BOARD_Configuration = { +static const uint8_t hid_report_desc[36] = { + HID_GLOBAL_ITEM_2 (HID_TAG_GLOBAL_USAGE_PAGE), + 0x9C, 0xFF, /* Usage Page 0xFF9C (Vendor Defined) */ + HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), + 0x01, /* Usage ID 0x0001 (0x01-0x1F uaually for top-level collections) */ + + HID_MAIN_ITEM_1 (HID_TAG_MAIN_COLLECTION), + 0x01, /* Application */ + + /* Device -> Host emulated serial channel */ + HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), + 0x01, /* OpenPilot emulated serial channel (Device -> Host) */ + HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), + 0x02, + HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), + 0x00, /* Values range from min = 0x00 */ + HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MAX), + 0xFF, /* Values range to max = 0xFF */ + HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), + 0x08, /* 8 bits wide */ + HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), + PIOS_USB_BOARD_HID_DATA_LENGTH-1, /* Need to leave room for a report ID */ + HID_MAIN_ITEM_1 (HID_TAG_MAIN_INPUT), + 0x03, /* Variable, Constant (read-only) */ + + /* Host -> Host emulated serial channel */ + HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), + 0x02, /* OpenPilot emulated Serial Channel (Host -> Device) */ + HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), + 0x02, + HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), + 0x00, /* Values range from min = 0x00 */ + HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MAX), + 0xFF, /* Values range to max = 0xFF */ + HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), + 0x08, /* 8 bits wide */ + HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), + PIOS_USB_BOARD_HID_DATA_LENGTH-1, /* Need to leave room for a report ID */ + HID_MAIN_ITEM_1 (HID_TAG_MAIN_OUTPUT), + 0x82, /* Volatile, Variable */ + + HID_MAIN_ITEM_0 (HID_TAG_MAIN_ENDCOLLECTION), +}; + +struct usb_config_hid_cdc { + struct usb_configuration_desc config; + struct usb_interface_association_desc iad; + struct usb_interface_desc hid_if; + struct usb_hid_desc hid; + struct usb_endpoint_desc hid_in; + struct usb_endpoint_desc hid_out; + struct usb_interface_desc cdc_control_if; + struct usb_cdc_header_func_desc cdc_header; + struct usb_cdc_callmgmt_func_desc cdc_callmgmt; + struct usb_cdc_acm_func_desc cdc_acm; + struct usb_cdc_union_func_desc cdc_union; + struct usb_endpoint_desc cdc_mgmt_in; + struct usb_interface_desc cdc_data_if; + struct usb_endpoint_desc cdc_in; + struct usb_endpoint_desc cdc_out; +} __attribute__((packed)); + +static const struct usb_config_hid_cdc config_hid_cdc = { .config = { .bLength = sizeof(struct usb_configuration_desc), .bDescriptorType = USB_DESC_TYPE_CONFIGURATION, - .wTotalLength = htousbs(sizeof(struct usb_board_config)), + .wTotalLength = htousbs(sizeof(struct usb_config_hid_cdc)), .bNumInterfaces = 3, .bConfigurationValue = 1, .iConfiguration = 0, @@ -86,7 +151,7 @@ const struct usb_board_config PIOS_USB_BOARD_Configuration = { .bCountryCode = 0, .bNumDescriptors = 1, .bClassDescriptorType = USB_DESC_TYPE_REPORT, - .wItemLength = htousbs(sizeof(PIOS_USB_BOARD_HidReportDescriptor)), + .wItemLength = htousbs(sizeof(hid_report_desc)), }, .hid_in = { .bLength = sizeof(struct usb_endpoint_desc), @@ -178,100 +243,14 @@ const struct usb_board_config PIOS_USB_BOARD_Configuration = { }, }; -const uint8_t PIOS_USB_BOARD_HidReportDescriptor[] = { - HID_GLOBAL_ITEM_2 (HID_TAG_GLOBAL_USAGE_PAGE), - 0x9C, 0xFF, /* Usage Page 0xFF9C (Vendor Defined) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x01, /* Usage ID 0x0001 (0x01-0x1F uaually for top-level collections) */ +int32_t PIOS_USB_DESC_HID_CDC_Init(void) +{ + PIOS_USBHOOK_RegisterConfig(1, (uint8_t *)&config_hid_cdc, sizeof(config_hid_cdc)); - HID_MAIN_ITEM_1 (HID_TAG_MAIN_COLLECTION), - 0x01, /* Application */ + PIOS_USBHOOK_RegisterDevice((uint8_t *)&device_desc, sizeof(device_desc)); - /* Device -> Host emulated serial channel */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), - 0x01, /* OpenPilot emulated serial channel (Device -> Host) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x02, - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), - 0x00, /* Values range from min = 0x00 */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MAX), - 0xFF, /* Values range to max = 0xFF */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x08, /* 8 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - PIOS_USB_BOARD_HID_DATA_LENGTH-1, /* Need to leave room for a report ID */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_INPUT), - 0x03, /* Variable, Constant (read-only) */ + PIOS_USBHOOK_RegisterHidInterface((uint8_t *)&(config_hid_cdc.hid_if), sizeof(config_hid_cdc.hid_if)); + PIOS_USBHOOK_RegisterHidReport((uint8_t *)hid_report_desc, sizeof(hid_report_desc)); - /* Host -> Host emulated serial channel */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_ID), - 0x02, /* OpenPilot emulated Serial Channel (Host -> Device) */ - HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), - 0x02, - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MIN), - 0x00, /* Values range from min = 0x00 */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_LOGICAL_MAX), - 0xFF, /* Values range to max = 0xFF */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_SIZE), - 0x08, /* 8 bits wide */ - HID_GLOBAL_ITEM_1 (HID_TAG_GLOBAL_REPORT_CNT), - PIOS_USB_BOARD_HID_DATA_LENGTH-1, /* Need to leave room for a report ID */ - HID_MAIN_ITEM_1 (HID_TAG_MAIN_OUTPUT), - 0x82, /* Volatile, Variable */ - - HID_MAIN_ITEM_0 (HID_TAG_MAIN_ENDCOLLECTION), -}; - -const struct usb_string_langid PIOS_USB_BOARD_StringLangID = { - .bLength = sizeof(PIOS_USB_BOARD_StringLangID), - .bDescriptorType = USB_DESC_TYPE_STRING, - .bLangID = htousbs(USB_LANGID_ENGLISH_UK), -}; - -const uint8_t PIOS_USB_BOARD_StringVendorID[] = { - sizeof(PIOS_USB_BOARD_StringVendorID), - USB_DESC_TYPE_STRING, - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 -}; - -uint8_t PIOS_USB_BOARD_StringSerial[] = { - sizeof(PIOS_USB_BOARD_StringSerial), - USB_DESC_TYPE_STRING, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0 -}; + return 0; +} diff --git a/flight/PiOS/Common/pios_usb_desc_hid_only.c b/flight/PiOS/Common/pios_usb_desc_hid_only.c index 913c18b16..168efe841 100644 --- a/flight/PiOS/Common/pios_usb_desc_hid_only.c +++ b/flight/PiOS/Common/pios_usb_desc_hid_only.c @@ -28,9 +28,12 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include "pios_usb_desc_hid_only_priv.h" /* exported API */ +#include "pios_usb_defs.h" /* struct usb_*, USB_* */ +#include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_Register* */ -const struct usb_device_desc PIOS_USB_BOARD_DeviceDescriptor = { +static const struct usb_device_desc device_desc = { .bLength = sizeof(struct usb_device_desc), .bDescriptorType = USB_DESC_TYPE_DEVICE, .bcdUSB = htousbs(0x0200), @@ -47,56 +50,7 @@ const struct usb_device_desc PIOS_USB_BOARD_DeviceDescriptor = { .bNumConfigurations = 1, }; -const struct usb_board_config PIOS_USB_BOARD_Configuration = { - .config = { - .bLength = sizeof(struct usb_configuration_desc), - .bDescriptorType = USB_DESC_TYPE_CONFIGURATION, - .wTotalLength = htousbs(sizeof(struct usb_board_config)), - .bNumInterfaces = 1, - .bConfigurationValue = 1, - .iConfiguration = 0, - .bmAttributes = 0xC0, - .bMaxPower = 250/2, /* in units of 2ma */ - }, - .hid_if = { - .bLength = sizeof(struct usb_interface_desc), - .bDescriptorType = USB_DESC_TYPE_INTERFACE, - .bInterfaceNumber = 0, - .bAlternateSetting = 0, - .bNumEndpoints = 2, - .bInterfaceClass = USB_INTERFACE_CLASS_HID, - .bInterfaceSubClass = 0, /* no boot */ - .nInterfaceProtocol = 0, /* none */ - .iInterface = 0, - }, - .hid = { - .bLength = sizeof(struct usb_hid_desc), - .bDescriptorType = USB_DESC_TYPE_HID, - .bcdHID = htousbs(0x0110), - .bCountryCode = 0, - .bNumDescriptors = 1, - .bClassDescriptorType = USB_DESC_TYPE_REPORT, - .wItemLength = htousbs(sizeof(PIOS_USB_BOARD_HidReportDescriptor)), - }, - .hid_in = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_IN(1), - .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), - .bInterval = 4, /* ms */ - }, - .hid_out = { - .bLength = sizeof(struct usb_endpoint_desc), - .bDescriptorType = USB_DESC_TYPE_ENDPOINT, - .bEndpointAddress = USB_EP_OUT(1), - .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, - .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), - .bInterval = 4, /* ms */ - }, -}; - -const uint8_t PIOS_USB_BOARD_HidReportDescriptor[] = { +static const uint8_t hid_report_desc[36] = { HID_GLOBAL_ITEM_2 (HID_TAG_GLOBAL_USAGE_PAGE), 0x9C, 0xFF, /* Usage Page 0xFF9C (Vendor Defined) */ HID_LOCAL_ITEM_1 (HID_TAG_LOCAL_USAGE), @@ -140,56 +94,71 @@ const uint8_t PIOS_USB_BOARD_HidReportDescriptor[] = { HID_MAIN_ITEM_0 (HID_TAG_MAIN_ENDCOLLECTION), }; -const struct usb_string_langid PIOS_USB_BOARD_StringLangID = { - .bLength = sizeof(PIOS_USB_BOARD_StringLangID), - .bDescriptorType = USB_DESC_TYPE_STRING, - .bLangID = htousbs(USB_LANGID_ENGLISH_UK), +struct usb_config_hid_only { + struct usb_configuration_desc config; + struct usb_interface_desc hid_if; + struct usb_hid_desc hid; + struct usb_endpoint_desc hid_in; + struct usb_endpoint_desc hid_out; +} __attribute__((packed)); + +const struct usb_config_hid_only config_hid_only = { + .config = { + .bLength = sizeof(struct usb_configuration_desc), + .bDescriptorType = USB_DESC_TYPE_CONFIGURATION, + .wTotalLength = htousbs(sizeof(struct usb_config_hid_only)), + .bNumInterfaces = 1, + .bConfigurationValue = 1, + .iConfiguration = 0, + .bmAttributes = 0xC0, + .bMaxPower = 250/2, /* in units of 2ma */ + }, + .hid_if = { + .bLength = sizeof(struct usb_interface_desc), + .bDescriptorType = USB_DESC_TYPE_INTERFACE, + .bInterfaceNumber = 0, + .bAlternateSetting = 0, + .bNumEndpoints = 2, + .bInterfaceClass = USB_INTERFACE_CLASS_HID, + .bInterfaceSubClass = 0, /* no boot */ + .nInterfaceProtocol = 0, /* none */ + .iInterface = 0, + }, + .hid = { + .bLength = sizeof(struct usb_hid_desc), + .bDescriptorType = USB_DESC_TYPE_HID, + .bcdHID = htousbs(0x0110), + .bCountryCode = 0, + .bNumDescriptors = 1, + .bClassDescriptorType = USB_DESC_TYPE_REPORT, + .wItemLength = htousbs(sizeof(hid_report_desc)), + }, + .hid_in = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_IN(1), + .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), + .bInterval = 4, /* ms */ + }, + .hid_out = { + .bLength = sizeof(struct usb_endpoint_desc), + .bDescriptorType = USB_DESC_TYPE_ENDPOINT, + .bEndpointAddress = USB_EP_OUT(1), + .bmAttributes = USB_EP_ATTR_TT_INTERRUPT, + .wMaxPacketSize = htousbs(PIOS_USB_BOARD_HID_DATA_LENGTH), + .bInterval = 4, /* ms */ + }, }; -const uint8_t PIOS_USB_BOARD_StringVendorID[] = { - sizeof(PIOS_USB_BOARD_StringVendorID), - USB_DESC_TYPE_STRING, - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 -}; +int32_t PIOS_USB_DESC_HID_ONLY_Init(void) +{ + PIOS_USBHOOK_RegisterConfig(1, (uint8_t *)&config_hid_only, sizeof(config_hid_only)); -uint8_t PIOS_USB_BOARD_StringSerial[] = { - sizeof(PIOS_USB_BOARD_StringSerial), - USB_DESC_TYPE_STRING, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0 -}; + PIOS_USBHOOK_RegisterDevice((uint8_t *)&device_desc, sizeof(device_desc)); + + PIOS_USBHOOK_RegisterHidInterface((uint8_t *)&(config_hid_only.hid_if), sizeof(config_hid_only.hid_if)); + PIOS_USBHOOK_RegisterHidReport((uint8_t *)hid_report_desc, sizeof(hid_report_desc)); + + return 0; +} diff --git a/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_exti.h b/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_exti.h index 29bed11cf..99084e3f3 100644 --- a/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_exti.h +++ b/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_exti.h @@ -155,7 +155,7 @@ typedef struct */ void EXTI_DeInit(void); -void EXTI_Init(EXTI_InitTypeDef* EXTI_InitStruct); +void EXTI_Init(const EXTI_InitTypeDef* EXTI_InitStruct); void EXTI_StructInit(EXTI_InitTypeDef* EXTI_InitStruct); void EXTI_GenerateSWInterrupt(uint32_t EXTI_Line); FlagStatus EXTI_GetFlagStatus(uint32_t EXTI_Line); diff --git a/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_exti.c b/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_exti.c index f9467d0dc..73de195cb 100755 --- a/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_exti.c +++ b/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_exti.c @@ -97,7 +97,7 @@ void EXTI_DeInit(void) * that contains the configuration information for the EXTI peripheral. * @retval None */ -void EXTI_Init(EXTI_InitTypeDef* EXTI_InitStruct) +void EXTI_Init(const EXTI_InitTypeDef* EXTI_InitStruct) { uint32_t tmp = 0; diff --git a/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/inc/usb_core.h b/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/inc/usb_core.h index 84f975da6..7e606e44b 100644 --- a/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/inc/usb_core.h +++ b/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/inc/usb_core.h @@ -35,7 +35,7 @@ typedef enum _CONTROL_STATE typedef struct OneDescriptor { - uint8_t *Descriptor; + const uint8_t *Descriptor; uint16_t Descriptor_Size; } ONE_DESCRIPTOR, *PONE_DESCRIPTOR; @@ -80,7 +80,8 @@ typedef struct _ENDPOINT_INFO uint16_t Usb_wLength; uint16_t Usb_wOffset; uint16_t PacketSize; - uint8_t *(*CopyData)(uint16_t Length); + const uint8_t *(*CopyDataIn)(uint16_t Length); + uint8_t *(*CopyDataOut)(uint16_t Length); }ENDPOINT_INFO; /*-*-*-*-*-*-*-*-*-*-*-* Definitions for device level -*-*-*-*-*-*-*-*-*-*-*-*/ @@ -169,9 +170,9 @@ typedef struct _DEVICE_PROP RESULT (*Class_Get_Interface_Setting)(uint8_t Interface, uint8_t AlternateSetting); - uint8_t* (*GetDeviceDescriptor)(uint16_t Length); - uint8_t* (*GetConfigDescriptor)(uint16_t Length); - uint8_t* (*GetStringDescriptor)(uint16_t Length); + const uint8_t* (*GetDeviceDescriptor)(uint16_t Length); + const uint8_t* (*GetConfigDescriptor)(uint16_t Length); + const uint8_t* (*GetStringDescriptor)(uint16_t Length); /* This field is not used in current library version. It is kept only for compatibility with previous versions */ @@ -221,13 +222,13 @@ uint8_t In0_Process(void); RESULT Standard_SetEndPointFeature(void); RESULT Standard_SetDeviceFeature(void); -uint8_t *Standard_GetConfiguration(uint16_t Length); +const uint8_t *Standard_GetConfiguration(uint16_t Length); RESULT Standard_SetConfiguration(void); -uint8_t *Standard_GetInterface(uint16_t Length); +const uint8_t *Standard_GetInterface(uint16_t Length); RESULT Standard_SetInterface(void); -uint8_t *Standard_GetDescriptorData(uint16_t Length, const ONE_DESCRIPTOR * pDesc); +const uint8_t *Standard_GetDescriptorData(uint16_t Length, ONE_DESCRIPTOR *pDesc); -uint8_t *Standard_GetStatus(uint16_t Length); +const uint8_t *Standard_GetStatus(uint16_t Length); RESULT Standard_ClearFeature(void); void SetDeviceAddress(uint8_t); void NOP_Process(void); diff --git a/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/inc/usb_mem.h b/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/inc/usb_mem.h index 4d43fbe8c..f7f0ace12 100644 --- a/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/inc/usb_mem.h +++ b/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/inc/usb_mem.h @@ -22,7 +22,7 @@ /* Exported constants --------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ /* Exported functions ------------------------------------------------------- */ -void UserToPMABufferCopy(uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes); +void UserToPMABufferCopy(const uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes); void PMAToUserBufferCopy(uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes); /* External variables --------------------------------------------------------*/ diff --git a/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/src/usb_core.c b/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/src/usb_core.c index fb5ca3e40..75ddaa42e 100644 --- a/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/src/usb_core.c +++ b/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/src/usb_core.c @@ -58,7 +58,7 @@ static void Data_Setup0(void); * Return : Return 1 , if the request is invalid when "Length" is 0. * Return "Buffer" if the "Length" is not 0. *******************************************************************************/ -uint8_t *Standard_GetConfiguration(uint16_t Length) +const uint8_t *Standard_GetConfiguration(uint16_t Length) { if (Length == 0) { @@ -104,7 +104,7 @@ RESULT Standard_SetConfiguration(void) * Return : Return 0, if the request is invalid when "Length" is 0. * Return "Buffer" if the "Length" is not 0. *******************************************************************************/ -uint8_t *Standard_GetInterface(uint16_t Length) +const uint8_t *Standard_GetInterface(uint16_t Length) { if (Length == 0) { @@ -160,7 +160,7 @@ RESULT Standard_SetInterface(void) * Return : Return 0, if the request is at end of data block, * or is invalid when "Length" is 0. *******************************************************************************/ -uint8_t *Standard_GetStatus(uint16_t Length) +const uint8_t *Standard_GetStatus(uint16_t Length) { if (Length == 0) { @@ -415,7 +415,7 @@ RESULT Standard_SetDeviceFeature(void) * wOffset The buffer pointed by this address contains at least * Length bytes. *******************************************************************************/ -uint8_t *Standard_GetDescriptorData(uint16_t Length, const ONE_DESCRIPTOR * pDesc) +const uint8_t *Standard_GetDescriptorData(uint16_t Length, PONE_DESCRIPTOR pDesc) { uint32_t wOffset; @@ -443,7 +443,7 @@ void DataStageOut(void) save_rLength = pEPinfo->Usb_rLength; - if (pEPinfo->CopyData && save_rLength) + if (pEPinfo->CopyDataOut && save_rLength) { uint8_t *Buffer; uint32_t Length; @@ -454,7 +454,7 @@ void DataStageOut(void) Length = save_rLength; } - Buffer = (*pEPinfo->CopyData)(Length); + Buffer = (*pEPinfo->CopyDataOut)(Length); pEPinfo->Usb_rLength -= Length; pEPinfo->Usb_rOffset += Length; @@ -503,7 +503,7 @@ void DataStageIn(void) uint32_t save_wLength = pEPinfo->Usb_wLength; uint32_t ControlState = pInformation->ControlState; - uint8_t *DataBuffer; + const uint8_t *DataBuffer; uint32_t Length; if ((save_wLength == 0) && (ControlState == LAST_IN_DATA)) @@ -540,7 +540,7 @@ void DataStageIn(void) Length = save_wLength; } - DataBuffer = (*pEPinfo->CopyData)(Length); + DataBuffer = (*pEPinfo->CopyDataIn)(Length); #ifdef STM32F10X_CL PCD_EP_Write (ENDP0, DataBuffer, Length); @@ -697,7 +697,7 @@ exit_NoData_Setup0: *******************************************************************************/ void Data_Setup0(void) { - uint8_t *(*CopyRoutine)(uint16_t); + const uint8_t *(*CopyRoutine)(uint16_t); RESULT Result; uint32_t Request_No = pInformation->USBbRequest; @@ -802,7 +802,7 @@ void Data_Setup0(void) if (CopyRoutine) { pInformation->Ctrl_Info.Usb_wOffset = wOffset; - pInformation->Ctrl_Info.CopyData = CopyRoutine; + pInformation->Ctrl_Info.CopyDataIn = CopyRoutine; /* sb in the original the cast to word was directly */ /* now the cast is made step by step */ (*CopyRoutine)(0); diff --git a/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/src/usb_mem.c b/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/src/usb_mem.c index e729b844c..ca50a23c4 100644 --- a/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/src/usb_mem.c +++ b/flight/PiOS/STM32F10x/Libraries/STM32_USB-FS-Device_Driver/src/usb_mem.c @@ -33,7 +33,7 @@ * Output : None. * Return : None . *******************************************************************************/ -void UserToPMABufferCopy(uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes) +void UserToPMABufferCopy(const uint8_t *pbUsrBuf, uint16_t wPMABufAddr, uint16_t wNBytes) { uint32_t n = (wNBytes + 1) >> 1; /* n = (wNBytes + 1) / 2 */ uint32_t i, temp1, temp2; diff --git a/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld b/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld index d3e9f797e..ac579364b 100644 --- a/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld @@ -98,16 +98,9 @@ SECTIONS _init_stack_top = . - 4 ; } > SRAM + _eram = ORIGIN(SRAM) + LENGTH(SRAM) ; + _ebss = _eram ; - _free_ram = . ; - .free_ram (NOLOAD) : - { - . = ORIGIN(SRAM) + LENGTH(SRAM) - _free_ram ; - /* This is used by the startup in order to initialize the .bss section */ - _ebss = . ; - _eram = . ; - } > SRAM - /* keep the heap section at the end of the SRAM * this will allow to claim the remaining bytes not used * at run time! (done by the reset vector). diff --git a/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld b/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld index 066fa01ee..f23ed327f 100644 --- a/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld @@ -275,14 +275,8 @@ SECTIONS _init_stack_top = . - 4 ; } > RAM - _free_ram = . ; - .free_ram (NOLOAD) : - { - . = ORIGIN(RAM) + LENGTH(RAM) - _free_ram ; - /* This is used by the startup in order to initialize the .bss section */ - _ebss = . ; - _eram = . ; - } > RAM + _eram = ORIGIN(SRAM) + LENGTH(SRAM) ; + _ebss = _eram ; /* keep the heap section at the end of the SRAM * this will allow to claim the remaining bytes not used diff --git a/flight/PiOS/STM32F10x/pios_exti.c b/flight/PiOS/STM32F10x/pios_exti.c index 0b886666a..8b1d2dc32 100644 --- a/flight/PiOS/STM32F10x/pios_exti.c +++ b/flight/PiOS/STM32F10x/pios_exti.c @@ -5,7 +5,6 @@ * @{ * @addtogroup PIOS_EXTI External Interrupt Handlers * @brief External interrupt handler functions - * @note Currently deals with BMP085 readings * @{ * * @file pios_exti.c @@ -35,61 +34,196 @@ #if defined(PIOS_INCLUDE_EXTI) -/** -* Handle external lines 15 to 10 interrupt requests -*/ -void EXTI15_10_IRQHandler(void) +/* Map EXTI line to full config */ +#define EXTI_MAX_LINES 16 +#define PIOS_EXTI_INVALID 0xFF +static uint8_t pios_exti_line_to_cfg_map[EXTI_MAX_LINES] = { + [0 ... EXTI_MAX_LINES-1] = PIOS_EXTI_INVALID, +}; + +/* Table of exti configs registered at compile time */ +extern struct pios_exti_cfg __start__exti __attribute__((weak)); +extern struct pios_exti_cfg __stop__exti __attribute__((weak)); + +static uint8_t PIOS_EXTI_line_to_index (uint32_t line) { -#if defined(PIOS_INCLUDE_FREERTOS) - portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE; -#endif - -#if defined(PIOS_INCLUDE_BMP085) - if (EXTI_GetITStatus(PIOS_BMP085_EOC_EXTI_LINE) != RESET) { - /* Read the ADC Value */ -#if defined(PIOS_INCLUDE_FREERTOS) - xSemaphoreGiveFromISR(PIOS_BMP085_EOC, &xHigherPriorityTaskWoken); -#else - PIOS_BMP085_EOC=1; -#endif - - /* Clear the EXTI line pending bit */ - EXTI_ClearITPendingBit(PIOS_BMP085_EOC_EXTI_LINE); + switch (line) { + case EXTI_Line0: return 0; + case EXTI_Line1: return 1; + case EXTI_Line2: return 2; + case EXTI_Line3: return 3; + case EXTI_Line4: return 4; + case EXTI_Line5: return 5; + case EXTI_Line6: return 6; + case EXTI_Line7: return 7; + case EXTI_Line8: return 8; + case EXTI_Line9: return 9; + case EXTI_Line10: return 10; + case EXTI_Line11: return 11; + case EXTI_Line12: return 12; + case EXTI_Line13: return 13; + case EXTI_Line14: return 14; + case EXTI_Line15: return 15; } -#endif -#if defined(PIOS_INCLUDE_FREERTOS) - /* Yield From ISR if needed */ - portEND_SWITCHING_ISR(xHigherPriorityTaskWoken); -#endif + PIOS_Assert(0); + return 0xFF; } -/** -* Handle external lines 9 to 5 interrupt requests -*/ -extern void PIOS_HMC5843_IRQHandler(void); -void EXTI9_5_IRQHandler(void) +uint8_t PIOS_EXTI_gpio_port_to_exti_source_port(GPIO_TypeDef * gpio_port) { -#if defined(PIOS_INCLUDE_HMC5843) - if (EXTI_GetITStatus(PIOS_HMC5843_DRDY_EXTI_LINE) != RESET) { - PIOS_HMC5843_IRQHandler(); - EXTI_ClearITPendingBit(PIOS_HMC5843_DRDY_EXTI_LINE); + switch((uint32_t)gpio_port) { + case (uint32_t)GPIOA: return (GPIO_PortSourceGPIOA); + case (uint32_t)GPIOB: return (GPIO_PortSourceGPIOB); + case (uint32_t)GPIOC: return (GPIO_PortSourceGPIOC); + case (uint32_t)GPIOD: return (GPIO_PortSourceGPIOD); + case (uint32_t)GPIOE: return (GPIO_PortSourceGPIOE); + case (uint32_t)GPIOF: return (GPIO_PortSourceGPIOF); + case (uint32_t)GPIOG: return (GPIO_PortSourceGPIOG); } -#endif + + PIOS_Assert(0); + return 0xFF; } -/** -* Handle external line 4 interrupt requests -*/ -#if defined(PIOS_INCLUDE_USB) -void EXTI4_IRQHandler(void) +uint8_t PIOS_EXTI_gpio_pin_to_exti_source_pin(uint32_t gpio_pin) { - if (EXTI_GetITStatus(PIOS_USB_DETECT_EXTI_LINE) != RESET) { - /* Clear the EXTI line pending bit */ - EXTI_ClearITPendingBit(PIOS_USB_DETECT_EXTI_LINE); + switch((uint32_t)gpio_pin) { + case GPIO_Pin_0: return (GPIO_PinSource0); + case GPIO_Pin_1: return (GPIO_PinSource1); + case GPIO_Pin_2: return (GPIO_PinSource2); + case GPIO_Pin_3: return (GPIO_PinSource3); + case GPIO_Pin_4: return (GPIO_PinSource4); + case GPIO_Pin_5: return (GPIO_PinSource5); + case GPIO_Pin_6: return (GPIO_PinSource6); + case GPIO_Pin_7: return (GPIO_PinSource7); + case GPIO_Pin_8: return (GPIO_PinSource8); + case GPIO_Pin_9: return (GPIO_PinSource9); + case GPIO_Pin_10: return (GPIO_PinSource10); + case GPIO_Pin_11: return (GPIO_PinSource11); + case GPIO_Pin_12: return (GPIO_PinSource12); + case GPIO_Pin_13: return (GPIO_PinSource13); + case GPIO_Pin_14: return (GPIO_PinSource14); + case GPIO_Pin_15: return (GPIO_PinSource15); } + + PIOS_Assert(0); + return 0xFF; } -#endif + +int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg) +{ + PIOS_Assert(cfg); + PIOS_Assert(&__start__exti); + PIOS_Assert(cfg >= &__start__exti); + PIOS_Assert(cfg < &__stop__exti); + + uint8_t cfg_index = cfg - &__start__exti; + + /* Connect this config to the requested vector */ + uint8_t line_index = PIOS_EXTI_line_to_index(cfg->line); + + if (pios_exti_line_to_cfg_map[line_index] != PIOS_EXTI_INVALID) { + /* Someone else already has this mapped */ + goto out_fail; + } + + /* Bind the config to the exti line */ + pios_exti_line_to_cfg_map[line_index] = cfg_index; + + /* Initialize the GPIO pin */ + GPIO_Init(cfg->pin.gpio, &cfg->pin.init); + + /* Set up the EXTI interrupt source */ + uint8_t exti_source_port = PIOS_EXTI_gpio_port_to_exti_source_port(cfg->pin.gpio); + uint8_t exti_source_pin = PIOS_EXTI_gpio_pin_to_exti_source_pin(cfg->pin.init.GPIO_Pin); + GPIO_EXTILineConfig(exti_source_port, exti_source_pin); + EXTI_Init(&cfg->exti.init); + + /* Enable the interrupt channel */ + NVIC_Init(&cfg->irq.init); + + return 0; + +out_fail: + return -1; +} + +static void PIOS_EXTI_generic_irq_handler(uint8_t line_index) +{ + uint8_t cfg_index = pios_exti_line_to_cfg_map[line_index]; + + PIOS_Assert(&__start__exti); + + if (cfg_index > NELEMENTS(pios_exti_line_to_cfg_map) || + cfg_index == PIOS_EXTI_INVALID) { + /* Unconfigured interrupt just fired! */ + return; + } + + struct pios_exti_cfg * cfg = &__start__exti + cfg_index; + cfg->vector(); +} + +/* Bind Interrupt Handlers */ + +#define PIOS_EXTI_HANDLE_LINE(line) \ + if (EXTI_GetITStatus(EXTI_Line##line) != RESET) { \ + EXTI_ClearITPendingBit(EXTI_Line##line); \ + PIOS_EXTI_generic_irq_handler(line); \ + } + +static void PIOS_EXTI_0_irq_handler (void) +{ + PIOS_EXTI_HANDLE_LINE(0); +} +void EXTI0_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_0_irq_handler"))); + +static void PIOS_EXTI_1_irq_handler (void) +{ + PIOS_EXTI_HANDLE_LINE(1); +} +void EXTI1_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_1_irq_handler"))); + +static void PIOS_EXTI_2_irq_handler (void) +{ + PIOS_EXTI_HANDLE_LINE(2); +} +void EXTI2_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_2_irq_handler"))); + +static void PIOS_EXTI_3_irq_handler (void) +{ + PIOS_EXTI_HANDLE_LINE(3); +} +void EXTI3_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_3_irq_handler"))); + +static void PIOS_EXTI_4_irq_handler (void) +{ + PIOS_EXTI_HANDLE_LINE(4); +} +void EXTI4_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_4_irq_handler"))); + +static void PIOS_EXTI_9_5_irq_handler (void) +{ + PIOS_EXTI_HANDLE_LINE(5); + PIOS_EXTI_HANDLE_LINE(6); + PIOS_EXTI_HANDLE_LINE(7); + PIOS_EXTI_HANDLE_LINE(8); + PIOS_EXTI_HANDLE_LINE(9); +} +void EXTI9_5_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_9_5_irq_handler"))); + +static void PIOS_EXTI_15_10_irq_handler (void) +{ + PIOS_EXTI_HANDLE_LINE(10); + PIOS_EXTI_HANDLE_LINE(11); + PIOS_EXTI_HANDLE_LINE(12); + PIOS_EXTI_HANDLE_LINE(13); + PIOS_EXTI_HANDLE_LINE(14); + PIOS_EXTI_HANDLE_LINE(15); +} +void EXTI15_10_IRQHandler(void) __attribute__ ((alias ("PIOS_EXTI_15_10_irq_handler"))); + #endif /** diff --git a/flight/PiOS/STM32F10x/pios_i2c.c b/flight/PiOS/STM32F10x/pios_i2c.c index 90c208133..e1bdb44f8 100644 --- a/flight/PiOS/STM32F10x/pios_i2c.c +++ b/flight/PiOS/STM32F10x/pios_i2c.c @@ -891,6 +891,10 @@ int32_t PIOS_I2C_Init(uint32_t * i2c_id, const struct pios_i2c_adapter_cfg * cfg break; } + if (i2c_adapter->cfg->remap) { + GPIO_PinRemapConfig(i2c_adapter->cfg->remap, ENABLE); + } + /* Initialize the state machine */ i2c_adapter_fsm_init(i2c_adapter); diff --git a/flight/PiOS/STM32F10x/pios_led.c b/flight/PiOS/STM32F10x/pios_led.c index 76b5bcccf..5db5cc884 100644 --- a/flight/PiOS/STM32F10x/pios_led.c +++ b/flight/PiOS/STM32F10x/pios_led.c @@ -33,57 +33,107 @@ #if defined(PIOS_INCLUDE_LED) -/* Private Function Prototypes */ +#include -/* Local Variables */ -static GPIO_TypeDef *LED_GPIO_PORT[PIOS_LED_NUM] = PIOS_LED_PORTS; -static const uint32_t LED_GPIO_PIN[PIOS_LED_NUM] = PIOS_LED_PINS; -static const uint32_t LED_GPIO_CLK[PIOS_LED_NUM] = PIOS_LED_CLKS; +const static struct pios_led_cfg * led_cfg; /** * Initialises all the LED's */ -void PIOS_LED_Init(void) +int32_t PIOS_LED_Init(const struct pios_led_cfg * cfg) { - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + PIOS_Assert(cfg); - for (int LEDNum = 0; LEDNum < PIOS_LED_NUM; LEDNum++) { - RCC_APB2PeriphClockCmd(LED_GPIO_CLK[LEDNum], ENABLE); - GPIO_InitStructure.GPIO_Pin = LED_GPIO_PIN[LEDNum]; - GPIO_Init(LED_GPIO_PORT[LEDNum], &GPIO_InitStructure); + /* Store away the config in a global used by API functions */ + led_cfg = cfg; - /* LED's Off */ - LED_GPIO_PORT[LEDNum]->BSRR = LED_GPIO_PIN[LEDNum]; + for (uint8_t i = 0; i < cfg->num_leds; i++) { + const struct pios_led * led = &(cfg->leds[i]); + + /* Enable the peripheral clock for the GPIO */ + switch ((uint32_t)led->pin.gpio) { + case (uint32_t) GPIOA: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); + break; + case (uint32_t) GPIOB: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + case (uint32_t) GPIOC: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + default: + PIOS_Assert(0); + break; + } + + if (led->remap) { + GPIO_PinRemapConfig(led->remap, ENABLE); + } + + GPIO_Init(led->pin.gpio, &led->pin.init); + + PIOS_LED_Off(i); } + + return 0; } /** * Turn on LED -* \param[in] LED LED Name (LED1, LED2) +* \param[in] LED LED id */ -void PIOS_LED_On(LedTypeDef LED) +void PIOS_LED_On(uint32_t led_id) { - LED_GPIO_PORT[LED]->BRR = LED_GPIO_PIN[LED]; + PIOS_Assert(led_cfg); + + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } + + const struct pios_led * led = &(led_cfg->leds[led_id]); + + GPIO_ResetBits(led->pin.gpio, led->pin.init.GPIO_Pin); } /** * Turn off LED -* \param[in] LED LED Name (LED1, LED2) +* \param[in] LED LED id */ -void PIOS_LED_Off(LedTypeDef LED) +void PIOS_LED_Off(uint32_t led_id) { - LED_GPIO_PORT[LED]->BSRR = LED_GPIO_PIN[LED]; + PIOS_Assert(led_cfg); + + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } + + const struct pios_led * led = &(led_cfg->leds[led_id]); + + GPIO_SetBits(led->pin.gpio, led->pin.init.GPIO_Pin); } /** * Toggle LED on/off -* \param[in] LED LED Name (LED1, LED2) +* \param[in] LED LED id */ -void PIOS_LED_Toggle(LedTypeDef LED) +void PIOS_LED_Toggle(uint32_t led_id) { - LED_GPIO_PORT[LED]->ODR ^= LED_GPIO_PIN[LED]; + PIOS_Assert(led_cfg); + + if (led_id >= led_cfg->num_leds) { + /* LED index out of range */ + return; + } + + const struct pios_led * led = &(led_cfg->leds[led_id]); + + if (GPIO_ReadOutputDataBit(led->pin.gpio, led->pin.init.GPIO_Pin) == Bit_SET) { + PIOS_LED_On(led_id); + } else { + PIOS_LED_Off(led_id); + } } #endif diff --git a/flight/PiOS/STM32F10x/pios_sys.c b/flight/PiOS/STM32F10x/pios_sys.c index cc35194e7..632be2426 100644 --- a/flight/PiOS/STM32F10x/pios_sys.c +++ b/flight/PiOS/STM32F10x/pios_sys.c @@ -77,10 +77,6 @@ void PIOS_SYS_Init(void) /* Initialise Basic NVIC */ NVIC_Configuration(); -#if defined(PIOS_INCLUDE_LED) - /* Initialise LEDs */ - PIOS_LED_Init(); -#endif } /** @@ -105,16 +101,12 @@ int32_t PIOS_SYS_Reset(void) PIOS_IRQ_Disable(); // turn off all board LEDs -#if (PIOS_LED_NUM == 1) - PIOS_LED_Off(LED1); -#elif (PIOS_LED_NUM == 2) - PIOS_LED_Off(LED1); - PIOS_LED_Off(LED2); -#endif - - /* Reset STM32 */ - //RCC_APB2PeriphResetCmd(0xfffffff8, ENABLE); /* MBHP_CORE_STM32: don't reset GPIOA/AF due to USB pins */ - //RCC_APB1PeriphResetCmd(0xff7fffff, ENABLE); /* don't reset USB, so that the connection can survive! */ +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Off(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ +#if defined(PIOS_LED_ALARM) + PIOS_LED_Off(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); @@ -210,13 +202,21 @@ void assert_failed(uint8_t * file, uint32_t line) /* printf("Wrong parameters value: file %s on line %d\r\n", file, line); */ /* Setup the LEDs to Alternate */ - PIOS_LED_On(LED1); - PIOS_LED_Off(LED2); +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_On(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ +#if defined(PIOS_LED_ALARM) + PIOS_LED_Off(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ /* Infinite loop */ while (1) { - PIOS_LED_Toggle(LED1); - PIOS_LED_Toggle(LED2); +#if defined(PIOS_LED_HEARTBEAT) + PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); +#endif /* PIOS_LED_HEARTBEAT */ +#if defined(PIOS_LED_ALARM) + PIOS_LED_Toggle(PIOS_LED_ALARM); +#endif /* PIOS_LED_ALARM */ for (int i = 0; i < 1000000; i++) ; } } diff --git a/flight/PiOS/STM32F10x/pios_usb.c b/flight/PiOS/STM32F10x/pios_usb.c index df17777c7..15decfca1 100644 --- a/flight/PiOS/STM32F10x/pios_usb.c +++ b/flight/PiOS/STM32F10x/pios_usb.c @@ -123,13 +123,6 @@ int32_t PIOS_USB_Init(uint32_t * usb_id, const struct pios_usb_cfg * cfg) /* Enable the USB clock */ RCC_APB1PeriphClockCmd(RCC_APB1Periph_USB, ENABLE); - /* Update the USB serial number from the chip */ - uint8_t sn[25]; - PIOS_SYS_SerialNumberGet((char *)sn); - for (uint8_t i = 0; sn[i] != '\0' && (2 * i) < PIOS_USB_BOARD_StringSerial[0]; i++) { - PIOS_USB_BOARD_StringSerial[2 + 2 * i] = sn[i]; - } - USB_Init(); USB_SIL_Init(); diff --git a/flight/PiOS/STM32F10x/pios_usb_cdc.c b/flight/PiOS/STM32F10x/pios_usb_cdc.c index 994c276ea..4bb1303d9 100644 --- a/flight/PiOS/STM32F10x/pios_usb_cdc.c +++ b/flight/PiOS/STM32F10x/pios_usb_cdc.c @@ -346,7 +346,7 @@ RESULT PIOS_USB_CDC_SetLineCoding(void) return USB_SUCCESS; } -uint8_t *PIOS_USB_CDC_GetLineCoding(uint16_t Length) +const uint8_t *PIOS_USB_CDC_GetLineCoding(uint16_t Length) { struct pios_usb_cdc_dev * usb_cdc_dev = (struct pios_usb_cdc_dev *)pios_usb_cdc_id; diff --git a/flight/PiOS/STM32F10x/pios_usb_hid.c b/flight/PiOS/STM32F10x/pios_usb_hid.c index 11eb988c1..e70c93046 100644 --- a/flight/PiOS/STM32F10x/pios_usb_hid.c +++ b/flight/PiOS/STM32F10x/pios_usb_hid.c @@ -207,9 +207,15 @@ static void PIOS_USB_HID_RxStart(uint32_t usbhid_id, uint16_t rx_bytes_avail) { } // If endpoint was stalled and there is now space make it valid +#ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; +#else + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; +#endif + PIOS_IRQ_Disable(); if ((GetEPRxStatus(usb_hid_dev->cfg->data_rx_ep) != EP_RX_VALID) && - (rx_bytes_avail >= PIOS_USB_BOARD_HID_DATA_LENGTH)) { + (rx_bytes_avail >= max_payload_length)) { SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_VALID); } PIOS_IRQ_Enable(); @@ -328,7 +334,15 @@ static void PIOS_USB_HID_EP_OUT_Callback(void) &headroom, &need_yield); #endif - if (headroom >= PIOS_USB_BOARD_HID_DATA_LENGTH) { + +#ifdef PIOS_USB_BOARD_BL_HID_HAS_NO_LENGTH_BYTE + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 1; +#else + uint16_t max_payload_length = PIOS_USB_BOARD_HID_DATA_LENGTH - 2; +#endif + + if (headroom >= max_payload_length) { + /* We have room for a maximum length message */ SetEPRxStatus(usb_hid_dev->cfg->data_rx_ep, EP_RX_VALID); } else { diff --git a/flight/PiOS/STM32F10x/pios_usbhook.c b/flight/PiOS/STM32F10x/pios_usbhook.c index ade1f8ac1..7800faed1 100644 --- a/flight/PiOS/STM32F10x/pios_usbhook.c +++ b/flight/PiOS/STM32F10x/pios_usbhook.c @@ -36,6 +36,48 @@ #include "pios_usb_cdc_priv.h" /* PIOS_USB_CDC_* */ #include "pios_usb_board_data.h" /* PIOS_USB_BOARD_* */ +static ONE_DESCRIPTOR Device_Descriptor; + +void PIOS_USBHOOK_RegisterDevice(const uint8_t * desc, uint16_t desc_size) +{ + Device_Descriptor.Descriptor = desc; + Device_Descriptor.Descriptor_Size = desc_size; +} + +static ONE_DESCRIPTOR Config_Descriptor; + +void PIOS_USBHOOK_RegisterConfig(uint8_t config_id, const uint8_t * desc, uint16_t desc_size) +{ + Config_Descriptor.Descriptor = desc; + Config_Descriptor.Descriptor_Size = desc_size; +} + +static ONE_DESCRIPTOR String_Descriptor[4]; + +void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t * desc, uint16_t desc_size) +{ + if (string_id < NELEMENTS(String_Descriptor)) { + String_Descriptor[string_id].Descriptor = desc; + String_Descriptor[string_id].Descriptor_Size = desc_size; + } +} + +static ONE_DESCRIPTOR Hid_Interface_Descriptor; + +void PIOS_USBHOOK_RegisterHidInterface(const uint8_t * desc, uint16_t desc_size) +{ + Hid_Interface_Descriptor.Descriptor = desc; + Hid_Interface_Descriptor.Descriptor_Size = desc_size; +} + +static ONE_DESCRIPTOR Hid_Report_Descriptor; + +void PIOS_USBHOOK_RegisterHidReport(const uint8_t * desc, uint16_t desc_size) +{ + Hid_Report_Descriptor.Descriptor = desc; + Hid_Report_Descriptor.Descriptor_Size = desc_size; +} + #include "stm32f10x.h" /* __IO */ __IO uint8_t EXTI_Enable; @@ -53,9 +95,9 @@ static void PIOS_USBHOOK_Status_Out(void); static RESULT PIOS_USBHOOK_Data_Setup(uint8_t RequestNo); static RESULT PIOS_USBHOOK_NoData_Setup(uint8_t RequestNo); static RESULT PIOS_USBHOOK_Get_Interface_Setting(uint8_t Interface, uint8_t AlternateSetting); -static uint8_t *PIOS_USBHOOK_GetDeviceDescriptor(uint16_t Length); -static uint8_t *PIOS_USBHOOK_GetConfigDescriptor(uint16_t Length); -static uint8_t *PIOS_USBHOOK_GetStringDescriptor(uint16_t Length); +static const uint8_t *PIOS_USBHOOK_GetDeviceDescriptor(uint16_t Length); +static const uint8_t *PIOS_USBHOOK_GetConfigDescriptor(uint16_t Length); +static const uint8_t *PIOS_USBHOOK_GetStringDescriptor(uint16_t Length); DEVICE_PROP Device_Property = { .Init = PIOS_USBHOOK_Init, @@ -87,49 +129,10 @@ USER_STANDARD_REQUESTS User_Standard_Requests = { .User_SetDeviceAddress = PIOS_USBHOOK_SetDeviceAddress }; -const static ONE_DESCRIPTOR Device_Descriptor = { - (uint8_t *) &PIOS_USB_BOARD_DeviceDescriptor, - sizeof(PIOS_USB_BOARD_DeviceDescriptor), -}; - -static ONE_DESCRIPTOR Config_Descriptor = { - (uint8_t *) &PIOS_USB_BOARD_Configuration, - sizeof(PIOS_USB_BOARD_Configuration), -}; - -const static ONE_DESCRIPTOR Hid_Report_Descriptor = { - (uint8_t *) &PIOS_USB_BOARD_HidReportDescriptor, - sizeof(PIOS_USB_BOARD_HidReportDescriptor), -}; - -const static ONE_DESCRIPTOR Hid_Interface_Descriptor = { - (uint8_t *) &PIOS_USB_BOARD_Configuration.hid_if, - sizeof(PIOS_USB_BOARD_Configuration.hid_if), -}; - -const static ONE_DESCRIPTOR String_Descriptor[] = { - { - (uint8_t *) &PIOS_USB_BOARD_StringLangID, - sizeof(PIOS_USB_BOARD_StringLangID), - }, - { - (uint8_t *) PIOS_USB_BOARD_StringVendorID, - sizeof(PIOS_USB_BOARD_StringVendorID), - }, - { - (uint8_t *) PIOS_USB_BOARD_StringProductID, - sizeof(PIOS_USB_BOARD_StringProductID), - }, - { - (uint8_t *) PIOS_USB_BOARD_StringSerial, - sizeof(PIOS_USB_BOARD_StringSerial), - }, -}; - static RESULT PIOS_USBHOOK_SetProtocol(void); -static uint8_t *PIOS_USBHOOK_GetProtocolValue(uint16_t Length); -static uint8_t *PIOS_USBHOOK_GetReportDescriptor(uint16_t Length); -static uint8_t *PIOS_USBHOOK_GetHIDDescriptor(uint16_t Length); +static const uint8_t *PIOS_USBHOOK_GetProtocolValue(uint16_t Length); +static const uint8_t *PIOS_USBHOOK_GetReportDescriptor(uint16_t Length); +static const uint8_t *PIOS_USBHOOK_GetHIDDescriptor(uint16_t Length); /******************************************************************************* * Function Name : PIOS_USBHOOK_Init. @@ -140,11 +143,8 @@ static uint8_t *PIOS_USBHOOK_GetHIDDescriptor(uint16_t Length); *******************************************************************************/ static void PIOS_USBHOOK_Init(void) { - /* Update the serial number string descriptor with the data from the unique - ID */ - //Get_SerialNum(); - pInformation->Current_Configuration = 0; + /* Connect the device */ PowerOn(); @@ -292,7 +292,7 @@ static void PIOS_USBHOOK_Status_Out(void) *******************************************************************************/ static RESULT PIOS_USBHOOK_Data_Setup(uint8_t RequestNo) { - uint8_t *(*CopyRoutine) (uint16_t); + const uint8_t *(*CopyRoutine) (uint16_t); CopyRoutine = NULL; @@ -350,7 +350,7 @@ static RESULT PIOS_USBHOOK_Data_Setup(uint8_t RequestNo) return USB_UNSUPPORT; } - pInformation->Ctrl_Info.CopyData = CopyRoutine; + pInformation->Ctrl_Info.CopyDataIn = CopyRoutine; pInformation->Ctrl_Info.Usb_wOffset = 0; (*CopyRoutine) (0); return USB_SUCCESS; @@ -405,7 +405,7 @@ static RESULT PIOS_USBHOOK_NoData_Setup(uint8_t RequestNo) * Output : None. * Return : The address of the device descriptor. *******************************************************************************/ -static uint8_t *PIOS_USBHOOK_GetDeviceDescriptor(uint16_t Length) +static const uint8_t *PIOS_USBHOOK_GetDeviceDescriptor(uint16_t Length) { return Standard_GetDescriptorData(Length, &Device_Descriptor); } @@ -417,7 +417,7 @@ static uint8_t *PIOS_USBHOOK_GetDeviceDescriptor(uint16_t Length) * Output : None. * Return : The address of the configuration descriptor. *******************************************************************************/ -static uint8_t *PIOS_USBHOOK_GetConfigDescriptor(uint16_t Length) +static const uint8_t *PIOS_USBHOOK_GetConfigDescriptor(uint16_t Length) { return Standard_GetDescriptorData(Length, &Config_Descriptor); } @@ -429,7 +429,7 @@ static uint8_t *PIOS_USBHOOK_GetConfigDescriptor(uint16_t Length) * Output : None. * Return : The address of the string descriptors. *******************************************************************************/ -static uint8_t *PIOS_USBHOOK_GetStringDescriptor(uint16_t Length) +static const uint8_t *PIOS_USBHOOK_GetStringDescriptor(uint16_t Length) { uint8_t wValue0 = pInformation->USBwValue0; if (wValue0 > 4) { @@ -446,7 +446,7 @@ static uint8_t *PIOS_USBHOOK_GetStringDescriptor(uint16_t Length) * Output : None. * Return : The address of the configuration descriptor. *******************************************************************************/ -static uint8_t *PIOS_USBHOOK_GetReportDescriptor(uint16_t Length) +static const uint8_t *PIOS_USBHOOK_GetReportDescriptor(uint16_t Length) { return Standard_GetDescriptorData(Length, &Hid_Report_Descriptor); } @@ -458,7 +458,7 @@ static uint8_t *PIOS_USBHOOK_GetReportDescriptor(uint16_t Length) * Output : None. * Return : The address of the configuration descriptor. *******************************************************************************/ -static uint8_t *PIOS_USBHOOK_GetHIDDescriptor(uint16_t Length) +static const uint8_t *PIOS_USBHOOK_GetHIDDescriptor(uint16_t Length) { return Standard_GetDescriptorData(Length, &Hid_Interface_Descriptor); } @@ -503,7 +503,7 @@ static RESULT PIOS_USBHOOK_SetProtocol(void) * Output : None. * Return : address of the protcol value. *******************************************************************************/ -static uint8_t *PIOS_USBHOOK_GetProtocolValue(uint16_t Length) +static const uint8_t *PIOS_USBHOOK_GetProtocolValue(uint16_t Length) { if (Length == 0) { pInformation->Ctrl_Info.Usb_wLength = 1; diff --git a/flight/PiOS/inc/pios_com.h b/flight/PiOS/inc/pios_com.h index 19abb2f27..10591f116 100644 --- a/flight/PiOS/inc/pios_com.h +++ b/flight/PiOS/inc/pios_com.h @@ -31,6 +31,9 @@ #ifndef PIOS_COM_H #define PIOS_COM_H +#include /* uint*_t */ +#include /* bool */ + typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * task_woken); struct pios_com_driver { @@ -43,7 +46,6 @@ struct pios_com_driver { }; /* Public Functions */ -extern int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len); extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud); extern int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c); extern int32_t PIOS_COM_SendChar(uint32_t com_id, char c); diff --git a/flight/PiOS/inc/pios_com_msg.h b/flight/PiOS/inc/pios_com_msg.h new file mode 100644 index 000000000..1caaeabeb --- /dev/null +++ b/flight/PiOS/inc/pios_com_msg.h @@ -0,0 +1,45 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_COM COM MSG layer functions + * @brief Hardware communication layer + * @{ + * + * @file pios_com_msg.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief COM MSG layer functions header + * @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_COM_MSG_H +#define PIOS_COM_MSG_H + +#include /* uint*_t */ + +/* Public Functions */ +extern int32_t PIOS_COM_MSG_Send(uint32_t com_id, const uint8_t *msg, uint16_t msg_len); +extern uint16_t PIOS_COM_MSG_Receive(uint32_t com_id, uint8_t * buf, uint16_t buf_len); + +#endif /* PIOS_COM_MSG_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_com_msg_priv.h b/flight/PiOS/inc/pios_com_msg_priv.h new file mode 100644 index 000000000..20992de47 --- /dev/null +++ b/flight/PiOS/inc/pios_com_msg_priv.h @@ -0,0 +1,44 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_COM COM MSG layer functions + * @brief Hardware communication layer + * @{ + * + * @file pios_com_msg_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief COM MSG private 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_COM_MSG_PRIV_H +#define PIOS_COM_MSG_PRIV_H + +#include /* uint*_t */ +#include "pios_com_priv.h" /* struct pios_com_driver */ + +extern int32_t PIOS_COM_MSG_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id); + +#endif /* PIOS_COM_MSG_PRIV_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_com_priv.h b/flight/PiOS/inc/pios_com_priv.h index 7bd9a9be0..c39522f79 100644 --- a/flight/PiOS/inc/pios_com_priv.h +++ b/flight/PiOS/inc/pios_com_priv.h @@ -31,6 +31,8 @@ #ifndef PIOS_COM_PRIV_H #define PIOS_COM_PRIV_H +extern int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len); + #endif /* PIOS_COM_PRIV_H */ /** diff --git a/flight/PiOS/inc/pios_exti.h b/flight/PiOS/inc/pios_exti.h index fdccc6bf2..73a3bb556 100644 --- a/flight/PiOS/inc/pios_exti.h +++ b/flight/PiOS/inc/pios_exti.h @@ -33,6 +33,21 @@ /* Public Functions */ +#include + +struct pios_exti_cfg { + void (* vector)(void); + uint32_t line; /* use EXTI_LineN macros */ + struct stm32_gpio pin; + struct stm32_irq irq; + struct stm32_exti exti; +}; + +/* must be added to any pios_exti_cfg definition for it to be valid */ +#define __exti_config __attribute__((section("_exti"))) + +extern int32_t PIOS_EXTI_Init(const struct pios_exti_cfg * cfg); + #endif /* PIOS_EXTI_H */ /** diff --git a/flight/PiOS/inc/pios_i2c_priv.h b/flight/PiOS/inc/pios_i2c_priv.h index 5834fef34..a10fe0603 100644 --- a/flight/PiOS/inc/pios_i2c_priv.h +++ b/flight/PiOS/inc/pios_i2c_priv.h @@ -37,6 +37,7 @@ struct pios_i2c_adapter_cfg { uint32_t transfer_timeout_ms; struct stm32_gpio scl; struct stm32_gpio sda; + uint32_t remap; struct stm32_irq event; struct stm32_irq error; }; diff --git a/flight/PiOS/inc/pios_led.h b/flight/PiOS/inc/pios_led.h index c18ea3524..2506b75cf 100644 --- a/flight/PiOS/inc/pios_led.h +++ b/flight/PiOS/inc/pios_led.h @@ -30,23 +30,9 @@ #ifndef PIOS_LED_H #define PIOS_LED_H -/* Type Definitions */ -#if (PIOS_LED_NUM == 1) -typedef enum { LED1 = 0 } LedTypeDef; -#elif (PIOS_LED_NUM == 2) -typedef enum { LED1 = 0, LED2 = 1 } LedTypeDef; -#elif (PIOS_LED_NUM == 3) -typedef enum { LED1 = 0, LED2 = 1, LED3 = 2 } LedTypeDef; -#elif (PIOS_LED_NUM == 4) -typedef enum { LED1 = 0, LED2 = 1, LED3 = 2, LED4 = 3 } LedTypeDef; -#else -#error PIOS_LED_NUM not defined -#endif - /* Public Functions */ -extern void PIOS_LED_Init(void); -extern void PIOS_LED_On(LedTypeDef LED); -extern void PIOS_LED_Off(LedTypeDef LED); -extern void PIOS_LED_Toggle(LedTypeDef LED); +extern void PIOS_LED_On(uint32_t led_id); +extern void PIOS_LED_Off(uint32_t led_id); +extern void PIOS_LED_Toggle(uint32_t led_id); #endif /* PIOS_LED_H */ diff --git a/flight/PiOS/inc/pios_led_priv.h b/flight/PiOS/inc/pios_led_priv.h new file mode 100644 index 000000000..20f09eafc --- /dev/null +++ b/flight/PiOS/inc/pios_led_priv.h @@ -0,0 +1,54 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_LED LED Functions + * @brief PIOS interface for LEDs + * @{ + * + * @file pios_led_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief LED private 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_LED_PRIV_H +#define PIOS_LED_PRIV_H + +#include +#include + +struct pios_led { + struct stm32_gpio pin; + uint32_t remap; +}; + +struct pios_led_cfg { + const struct pios_led * leds; + uint8_t num_leds; +}; + +extern int32_t PIOS_LED_Init(const struct pios_led_cfg * cfg); + +#endif /* PIOS_LED_PRIV_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_stm32.h b/flight/PiOS/inc/pios_stm32.h index fb9163208..4e63d6937 100644 --- a/flight/PiOS/inc/pios_stm32.h +++ b/flight/PiOS/inc/pios_stm32.h @@ -36,6 +36,10 @@ struct stm32_irq { NVIC_InitTypeDef init; }; +struct stm32_exti { + EXTI_InitTypeDef init; +}; + struct stm32_dma_chan { DMA_Channel_TypeDef *channel; DMA_InitTypeDef init; diff --git a/flight/PiOS/inc/pios_usb_board_data_priv.h b/flight/PiOS/inc/pios_usb_board_data_priv.h new file mode 100644 index 000000000..f4e7f7c05 --- /dev/null +++ b/flight/PiOS/inc/pios_usb_board_data_priv.h @@ -0,0 +1,40 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_BOARD USB board layer functions + * @{ + * + * @file pios_usb_board_data_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines the API to the board-specific USB data setup code + * @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_USB_BOARD_DATA_PRIV_H +#define PIOS_USB_BOARD_DATA_PRIV_H + +extern int32_t PIOS_USB_BOARD_DATA_Init(void); + +#endif /* PIOS_USB_BOARD_DATA_PRIV_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_usb_cdc_priv.h b/flight/PiOS/inc/pios_usb_cdc_priv.h index e6b94bad8..74d6e347d 100644 --- a/flight/PiOS/inc/pios_usb_cdc_priv.h +++ b/flight/PiOS/inc/pios_usb_cdc_priv.h @@ -46,7 +46,7 @@ extern const struct pios_com_driver pios_usb_cdc_com_driver; extern int32_t PIOS_USB_CDC_Init(uint32_t * usbcdc_id, const struct pios_usb_cdc_cfg * cfg, uint32_t lower_id); -extern uint8_t *PIOS_USB_CDC_GetLineCoding(uint16_t Length); +extern const uint8_t *PIOS_USB_CDC_GetLineCoding(uint16_t Length); extern RESULT PIOS_USB_CDC_SetControlLineState(void); extern RESULT PIOS_USB_CDC_SetLineCoding(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 new file mode 100644 index 000000000..2ae94c74e --- /dev/null +++ b/flight/PiOS/inc/pios_usb_desc_hid_cdc_priv.h @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_DESC USB descriptor layer functions + * @{ + * + * @file pios_usb_desc_hid_cdc_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines the API to set up the HID + CDC USB descriptor config + * @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_USB_DESC_HID_CDC_PRIV_H +#define PIOS_USB_DESC_HID_CDC_PRIV_H + +#include + +extern int32_t PIOS_USB_DESC_HID_CDC_Init(void); + +#endif /* PIOS_USB_DESC_HID_CDC_PRIV_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_usb_desc_hid_only_priv.h b/flight/PiOS/inc/pios_usb_desc_hid_only_priv.h new file mode 100644 index 000000000..321d15efa --- /dev/null +++ b/flight/PiOS/inc/pios_usb_desc_hid_only_priv.h @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_USB_DESC USB descriptor layer functions + * @{ + * + * @file pios_usb_desc_hid_only_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Defines the API to set up the HID-only USB descriptor config + * @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_USB_DESC_HID_ONLY_PRIV_H +#define PIOS_USB_DESC_HID_ONLY_PRIV_H + +#include + +extern int32_t PIOS_USB_DESC_HID_ONLY_Init(void); + +#endif /* PIOS_USB_DESC_HID_ONLY_PRIV_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_usbhook.h b/flight/PiOS/inc/pios_usbhook.h index d5f032450..f005a6fa1 100644 --- a/flight/PiOS/inc/pios_usbhook.h +++ b/flight/PiOS/inc/pios_usbhook.h @@ -47,5 +47,18 @@ typedef enum CDC_REQUESTS { SET_CONTROL_LINE_STATE = 0x23, } CDC_REQUESTS; +enum usb_string_desc { + USB_STRING_DESC_LANG = 0, + USB_STRING_DESC_VENDOR = 1, + USB_STRING_DESC_PRODUCT = 2, + USB_STRING_DESC_SERIAL = 3, +} __attribute__((packed)); + +extern void PIOS_USBHOOK_RegisterDevice(const uint8_t * desc, uint16_t desc_size); +extern void PIOS_USBHOOK_RegisterConfig(uint8_t config_id, const uint8_t * desc, uint16_t desc_size); +extern void PIOS_USBHOOK_RegisterString(enum usb_string_desc string_id, const uint8_t * desc, uint16_t desc_size); +extern void PIOS_USBHOOK_RegisterHidInterface(const uint8_t * desc, uint16_t desc_size); +extern void PIOS_USBHOOK_RegisterHidReport(const uint8_t * desc, uint16_t desc_size); + #endif /* PIOS_USBHOOK_H */ diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h index 58d869517..e7d02dfd2 100644 --- a/flight/PiOS/pios.h +++ b/flight/PiOS/pios.h @@ -112,7 +112,10 @@ #if defined(PIOS_INCLUDE_IMU3000) #include #endif + +#if defined(PIOS_INCLUDE_IAP) #include +#endif #if defined(PIOS_INCLUDE_ADXL345) #include @@ -134,6 +137,7 @@ #if defined(PIOS_INCLUDE_USB) /* USB Libs */ #include +#include #endif #include diff --git a/flight/PipXtreme/Makefile b/flight/PipXtreme/Makefile index c4bc900ca..a21fdb74a 100644 --- a/flight/PipXtreme/Makefile +++ b/flight/PipXtreme/Makefile @@ -75,7 +75,7 @@ endif CMSISDIR = $(STMLIBDIR)/CMSIS/Core/CM3 BOOT = ../Bootloaders/AHRS BOOTINC = $(BOOT)/inc - +HWDEFSINC = ../board_hw_defs/$(BOARD_NAME) # List C source files here. (C dependencies are automatically generated.) # use file-extension c for "c-only"-files @@ -200,6 +200,7 @@ ifeq ($(USE_USB), YES) endif EXTRAINCDIRS += $(CMSISDIR) EXTRAINCDIRS += $(BOOTINC) +EXTRAINCDIRS += $(HWDEFSINC) # List any extra directories to look for library files here. # Also add directories where the linker should search for diff --git a/flight/PipXtreme/inc/pios_usb_board_data.h b/flight/PipXtreme/inc/pios_usb_board_data.h index 95dc03d73..d4f4daf0a 100644 --- a/flight/PipXtreme/inc/pios_usb_board_data.h +++ b/flight/PipXtreme/inc/pios_usb_board_data.h @@ -37,33 +37,6 @@ #include "pios_usb_defs.h" /* struct usb_* */ -struct usb_board_config { - struct usb_configuration_desc config; - struct usb_interface_desc hid_if; - struct usb_hid_desc hid; - struct usb_endpoint_desc hid_in; - struct usb_endpoint_desc hid_out; -} __attribute__((packed)); - -extern const struct usb_device_desc PIOS_USB_BOARD_DeviceDescriptor; -extern const struct usb_board_config PIOS_USB_BOARD_Configuration; -extern const struct usb_string_langid PIOS_USB_BOARD_StringLangID; - -/* NOTE NOTE NOTE - * - * Care must be taken to ensure that the _actual_ contents of - * these arrays (in each board's pios_usb_board_data.c) is no - * smaller than the stated sizes here or these descriptors - * will end up with trailing zeros on the wire. - * - * The compiler will catch any time that these definitions are - * too small. - */ -extern const uint8_t PIOS_USB_BOARD_HidReportDescriptor[36]; -extern const uint8_t PIOS_USB_BOARD_StringVendorID[28]; -extern const uint8_t PIOS_USB_BOARD_StringProductID[20]; -extern uint8_t PIOS_USB_BOARD_StringSerial[52]; - #define PIOS_USB_BOARD_PRODUCT_ID USB_PRODUCT_ID_PIPXTREME #define PIOS_USB_BOARD_DEVICE_VER USB_OP_DEVICE_VER(USB_OP_BOARD_ID_PIPXTREME, USB_OP_BOARD_MODE_FW) diff --git a/flight/PipXtreme/inc/pios_usb_hid_desc.h b/flight/PipXtreme/inc/pios_usb_hid_desc.h deleted file mode 100644 index 4810c1274..000000000 --- a/flight/PipXtreme/inc/pios_usb_hid_desc.h +++ /dev/null @@ -1,56 +0,0 @@ -/******************** (C) COPYRIGHT 2010 STMicroelectronics ******************** -* File Name : usb_desc.h -* Author : MCD Application Team -* Version : V3.2.1 -* Date : 07/05/2010 -* Description : Descriptor Header for Custom HID Demo -******************************************************************************** -* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS -* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. -* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, -* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE -* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING -* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -*******************************************************************************/ - -/* Define to prevent recursive inclusion -------------------------------------*/ -#ifndef __USB_DESC_H -#define __USB_DESC_H - -/* Includes ------------------------------------------------------------------*/ -/* Exported types ------------------------------------------------------------*/ -/* Exported constants --------------------------------------------------------*/ -/* Exported macro ------------------------------------------------------------*/ -/* Exported define -----------------------------------------------------------*/ -#define USB_DEVICE_DESCRIPTOR_TYPE 0x01 -#define USB_CONFIGURATION_DESCRIPTOR_TYPE 0x02 -#define USB_STRING_DESCRIPTOR_TYPE 0x03 -#define USB_INTERFACE_DESCRIPTOR_TYPE 0x04 -#define USB_ENDPOINT_DESCRIPTOR_TYPE 0x05 - -#define HID_DESCRIPTOR_TYPE 0x21 -#define PIOS_HID_SIZ_HID_DESC 0x09 -#define PIOS_HID_OFF_HID_DESC 0x12 - -#define PIOS_HID_SIZ_DEVICE_DESC 18 -#define PIOS_HID_SIZ_CONFIG_DESC 41 -#define PIOS_HID_SIZ_REPORT_DESC 36 -#define PIOS_HID_SIZ_STRING_LANGID 4 -#define PIOS_HID_SIZ_STRING_VENDOR 28 -#define PIOS_HID_SIZ_STRING_PRODUCT 20 -#define PIOS_HID_SIZ_STRING_SERIAL 52 /* 96 bits, 12 bytes, 24 characters, 48 in unicode */ - -#define STANDARD_ENDPOINT_DESC_SIZE 0x09 - -/* Exported functions ------------------------------------------------------- */ -extern const uint8_t PIOS_HID_DeviceDescriptor[PIOS_HID_SIZ_DEVICE_DESC]; -extern const uint8_t PIOS_HID_ConfigDescriptor[PIOS_HID_SIZ_CONFIG_DESC]; -extern const uint8_t PIOS_HID_ReportDescriptor[PIOS_HID_SIZ_REPORT_DESC]; -extern const uint8_t PIOS_HID_StringLangID[PIOS_HID_SIZ_STRING_LANGID]; -extern const uint8_t PIOS_HID_StringVendor[PIOS_HID_SIZ_STRING_VENDOR]; -extern const uint8_t PIOS_HID_StringProduct[PIOS_HID_SIZ_STRING_PRODUCT]; -extern uint8_t PIOS_HID_StringSerial[PIOS_HID_SIZ_STRING_SERIAL]; - -#endif /* __USB_DESC_H */ - -/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/ diff --git a/flight/PipXtreme/pios_board.c b/flight/PipXtreme/pios_board.c index 7da3705a8..a5536db2b 100644 --- a/flight/PipXtreme/pios_board.c +++ b/flight/PipXtreme/pios_board.c @@ -23,277 +23,16 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include - -// *********************************************************************************** -// SPI - -#if defined(PIOS_INCLUDE_SPI) - -#include - -/* OP Interface +/* Pull in the board-specific static HW definitions. + * Including .c files is a bit ugly but this allows all of + * the HW definitions to be const and static to limit their + * scope. * - * NOTE: Leave this declared as const data so that it ends up in the - * .rodata section (ie. Flash) rather than in the .bss section (RAM). + * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE */ -void PIOS_SPI_port_irq_handler(void); -void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_port_irq_handler"))); -void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_port_irq_handler"))); +#include "board_hw_defs.c" -static const struct pios_spi_cfg pios_spi_port_cfg = -{ - .regs = SPI1, -// .regs = SPI2, -// .regs = SPI3, - - .init = - { - .SPI_Mode = SPI_Mode_Master, -// .SPI_Mode = SPI_Mode_Slave, - - .SPI_Direction = SPI_Direction_2Lines_FullDuplex, -// .SPI_Direction = SPI_Direction_2Lines_RxOnly, -// .SPI_Direction = SPI_Direction_1Line_Rx, -// .SPI_Direction = SPI_Direction_1Line_Tx, - -// .SPI_DataSize = SPI_DataSize_16b, - .SPI_DataSize = SPI_DataSize_8b, - - .SPI_NSS = SPI_NSS_Soft, -// .SPI_NSS = SPI_NSS_Hard, - - .SPI_FirstBit = SPI_FirstBit_MSB, -// .SPI_FirstBit = SPI_FirstBit_LSB, - - .SPI_CRCPolynomial = 0, - - .SPI_CPOL = SPI_CPOL_Low, -// .SPI_CPOL = SPI_CPOL_High, - - .SPI_CPHA = SPI_CPHA_1Edge, -// .SPI_CPHA = SPI_CPHA_2Edge, - -// .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, // fastest SCLK -// .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4, -// .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, -// .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, -// .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_32, -// .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_64, -// .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_128, - .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256, // slowest SCLK - }, - .use_crc = FALSE, - - .dma = - { - .ahb_clk = RCC_AHBPeriph_DMA1, - .irq = - { - .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), - .init = { - .NVIC_IRQChannel = DMA1_Channel2_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - - .rx = { - .channel = DMA1_Channel2, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - .tx = { - .channel = DMA1_Channel3, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), - .DMA_DIR = DMA_DIR_PeripheralDST, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, - .DMA_Mode = DMA_Mode_Normal, - .DMA_Priority = DMA_Priority_Medium, - .DMA_M2M = DMA_M2M_Disable, - }, - }, - }, - - .ssel = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_4, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_Out_PP, - }, - }, - .sclk = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_5, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, - .miso = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, -// .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .mosi = - { - .gpio = GPIOA, - .init = - { - .GPIO_Pin = GPIO_Pin_7, - .GPIO_Speed = GPIO_Speed_10MHz, - .GPIO_Mode = GPIO_Mode_AF_PP, - }, - }, -}; - -uint32_t pios_spi_port_id; -void PIOS_SPI_port_irq_handler(void) -{ - /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_port_id); -} - -#endif /* PIOS_INCLUDE_SPI */ - -/* - * ADC system - */ -#include "pios_adc_priv.h" -extern void PIOS_ADC_handler(void); -void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); -// Remap the ADC DMA handler to this one -static const struct pios_adc_cfg pios_adc_cfg = { - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - .irq = { - .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), - .init = { - .NVIC_IRQChannel = DMA1_Channel1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .channel = DMA1_Channel1, - .init = { - .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR, - .DMA_DIR = DMA_DIR_PeripheralSRC, - .DMA_PeripheralInc = DMA_PeripheralInc_Disable, - .DMA_MemoryInc = DMA_MemoryInc_Enable, - .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, - .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, - .DMA_Mode = DMA_Mode_Circular, - .DMA_Priority = DMA_Priority_High, - .DMA_M2M = DMA_M2M_Disable, - }, - } - }, - .half_flag = DMA1_IT_HT1, - .full_flag = DMA1_IT_TC1, -}; - -struct pios_adc_dev pios_adc_devs[] = { - { - .cfg = &pios_adc_cfg, - .callback_function = NULL, - }, -}; - -uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs); - -void PIOS_ADC_handler() { - PIOS_ADC_DMA_Handler(); -} - - -// *********************************************************************************** -// USART - -#if defined(PIOS_INCLUDE_USART) - -#include - -/* - * SERIAL USART - */ -static const struct pios_usart_cfg pios_usart_serial_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_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_AF_PP, - }, - }, -}; - -#endif /* PIOS_INCLUDE_USART */ - -// *********************************************************************************** - -#if defined(PIOS_INCLUDE_COM) - -#include +#include #define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 @@ -307,36 +46,6 @@ static uint8_t pios_com_telem_usb_tx_buffer[PIOS_COM_TELEM_USB_TX_BUF_LEN]; static uint8_t pios_com_serial_rx_buffer[PIOS_COM_SERIAL_RX_BUF_LEN]; static uint8_t pios_com_serial_tx_buffer[PIOS_COM_SERIAL_TX_BUF_LEN]; -#endif /* PIOS_INCLUDE_COM */ - -// *********************************************************************************** - -#if defined(PIOS_INCLUDE_USB) -#include "pios_usb_priv.h" - -static const struct pios_usb_cfg pios_usb_main_cfg = { - .irq = { - .init = { - .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; -#endif /* PIOS_INCLUDE_USB */ - -#if defined(PIOS_INCLUDE_USB_HID) -#include - -const struct pios_usb_hid_cfg pios_usb_hid_cfg = { - .data_if = 0, - .data_rx_ep = 1, - .data_tx_ep = 1, -}; - -#endif /* PIOS_INCLUDE_USB_HID */ - uint32_t pios_com_serial_id; uint32_t pios_com_telem_usb_id; @@ -369,10 +78,16 @@ void PIOS_Board_Init(void) { } #if defined(PIOS_INCLUDE_USB) - uint32_t pios_usb_id; - if (PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg)) { + /* Initialize board specific USB data */ + PIOS_USB_BOARD_DATA_Init(); + + if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } + + uint32_t pios_usb_id; + PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); + #if defined(PIOS_INCLUDE_USB_HID) && 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)) { diff --git a/flight/PipXtreme/pios_usb_board_data.c b/flight/PipXtreme/pios_usb_board_data.c index d54ca6565..3e41e5ac9 100644 --- a/flight/PipXtreme/pios_usb_board_data.c +++ b/flight/PipXtreme/pios_usb_board_data.c @@ -29,9 +29,11 @@ */ #include "pios_usb_board_data.h" /* struct usb_*, USB_* */ +#include "pios_sys.h" /* PIOS_SYS_SerialNumberGet */ +#include "pios_usbhook.h" /* PIOS_USBHOOK_* */ -const uint8_t PIOS_USB_BOARD_StringProductID[] = { - sizeof(PIOS_USB_BOARD_StringProductID), +static const uint8_t usb_product_id[20] = { + sizeof(usb_product_id), USB_DESC_TYPE_STRING, 'P', 0, 'i', 0, @@ -44,3 +46,74 @@ const uint8_t PIOS_USB_BOARD_StringProductID[] = { 'e', 0, }; +static uint8_t usb_serial_number[52] = { + sizeof(usb_serial_number), + USB_DESC_TYPE_STRING, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0 +}; + +static const struct usb_string_langid usb_lang_id = { + .bLength = sizeof(usb_lang_id), + .bDescriptorType = USB_DESC_TYPE_STRING, + .bLangID = htousbs(USB_LANGID_ENGLISH_UK), +}; + +static const uint8_t usb_vendor_id[28] = { + sizeof(usb_vendor_id), + USB_DESC_TYPE_STRING, + 'o', 0, + 'p', 0, + 'e', 0, + 'n', 0, + 'p', 0, + 'i', 0, + 'l', 0, + 'o', 0, + 't', 0, + '.', 0, + 'o', 0, + 'r', 0, + 'g', 0 +}; + +int32_t PIOS_USB_BOARD_DATA_Init(void) +{ + /* Load device serial number into serial number string */ + uint8_t sn[25]; + PIOS_SYS_SerialNumberGet((char *)sn); + for (uint8_t i = 0; sn[i] != '\0' && (2 * i) < usb_serial_number[0]; i++) { + usb_serial_number[2 + 2 * i] = sn[i]; + } + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_PRODUCT, (uint8_t *)&usb_product_id, sizeof(usb_product_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_SERIAL, (uint8_t *)&usb_serial_number, sizeof(usb_serial_number)); + + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_LANG, (uint8_t *)&usb_lang_id, sizeof(usb_lang_id)); + PIOS_USBHOOK_RegisterString(USB_STRING_DESC_VENDOR, (uint8_t *)&usb_vendor_id, sizeof(usb_vendor_id)); + + return 0; +} diff --git a/flight/PipXtreme/pios_usb_hid_desc.c b/flight/PipXtreme/pios_usb_hid_desc.c deleted file mode 100644 index c15a80551..000000000 --- a/flight/PipXtreme/pios_usb_hid_desc.c +++ /dev/null @@ -1,220 +0,0 @@ -/******************** (C) COPYRIGHT 2010 STMicroelectronics ******************** -* File Name : usb_desc.c -* Author : MCD Application Team -* Version : V3.2.1 -* Date : 07/05/2010 -* Description : Descriptors for Custom HID Demo -******************************************************************************** -* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS -* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME. -* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT, -* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE -* CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING -* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. -*******************************************************************************/ - -#include "usb_lib.h" -#include "pios_usb.h" -#include "pios_usb_hid.h" -#include "pios_usb_hid_desc.h" - -// ************************************************* -// USB Standard Device Descriptor - -const uint8_t PIOS_HID_DeviceDescriptor[PIOS_HID_SIZ_DEVICE_DESC] = - { - 0x12, // bLength - USB_DEVICE_DESCRIPTOR_TYPE, // bDescriptorType - 0x00, // bcdUSB - 0x02, - 0x00, // bDeviceClass - 0x00, // bDeviceSubClass - 0x00, // bDeviceProtocol - 0x40, // bMaxPacketSize40 - (uint8_t)((PIOS_USB_VENDOR_ID) & 0xff), // idVendor - (uint8_t)((PIOS_USB_VENDOR_ID) >> 8), - (uint8_t)((PIOS_USB_PRODUCT_ID) & 0xff), // idProduct - (uint8_t)((PIOS_USB_PRODUCT_ID) >> 8), - (uint8_t)((PIOS_USB_VERSION_ID) & 0xff), // bcdDevice - (uint8_t)((PIOS_USB_VERSION_ID) >> 8), - 0x01, // Index of string descriptor describing manufacturer - 0x02, // Index of string descriptor describing product - 0x03, // Index of string descriptor describing the device serial number - 0x01 // bNumConfigurations - }; - -// ************************************************* -// USB Configuration Descriptor -// All Descriptors (Configuration, Interface, Endpoint, Class, Vendor - -const uint8_t PIOS_HID_ConfigDescriptor[PIOS_HID_SIZ_CONFIG_DESC] = - { - 0x09, // bLength: Configuation Descriptor size - USB_CONFIGURATION_DESCRIPTOR_TYPE, // bDescriptorType: Configuration - PIOS_HID_SIZ_CONFIG_DESC, - // wTotalLength: Bytes returned - 0x00, - 0x01, // bNumInterfaces: 1 interface - 0x01, // bConfigurationValue: Configuration value - 0x00, // iConfiguration: Index of string descriptor describing the configuration - 0xC0, // bmAttributes: Bus powered - 0x7D, // MaxPower 250 mA - needed to power the RF transmitter - - // ************** Descriptor of Custom HID interface **************** - // 09 - 0x09, // bLength: Interface Descriptor size - USB_INTERFACE_DESCRIPTOR_TYPE, // bDescriptorType: Interface descriptor type - 0x00, // bInterfaceNumber: Number of Interface - 0x00, // bAlternateSetting: Alternate setting - 0x02, // bNumEndpoints - 0x03, // bInterfaceClass: HID - 0x00, // bInterfaceSubClass : 1=BOOT, 0=no boot - 0x00, // nInterfaceProtocol : 0=none, 1=keyboard, 2=mouse - 0, // iInterface: Index of string descriptor - - // ******************** Descriptor of Custom HID HID ******************** - // 18 - 0x09, // bLength: HID Descriptor size - HID_DESCRIPTOR_TYPE, // bDescriptorType: HID - 0x10, // bcdHID: HID Class Spec release number - 0x01, - 0x00, // bCountryCode: Hardware target country - 0x01, // bNumDescriptors: Number of HID class descriptors to follow - 0x22, // bDescriptorType - PIOS_HID_SIZ_REPORT_DESC, // wItemLength: Total length of Report descriptor - 0x00, - - // ******************** Descriptor of Custom HID endpoints ****************** - // 27 - 0x07, // bLength: Endpoint Descriptor size - USB_ENDPOINT_DESCRIPTOR_TYPE, // bDescriptorType: - - 0x81, // bEndpointAddress: Endpoint Address (IN) - 0x03, // bmAttributes: Interrupt endpoint - 0x40, // wMaxPacketSize: 2 Bytes max - 0x00, - 0x04, // bInterval: Polling Interval in ms - // 34 - - 0x07, // bLength: Endpoint Descriptor size - USB_ENDPOINT_DESCRIPTOR_TYPE, // bDescriptorType: - // Endpoint descriptor type - 0x01, // bEndpointAddress: - // Endpoint Address (OUT) - 0x03, // bmAttributes: Interrupt endpoint - 0x40, // wMaxPacketSize: 2 Bytes max - 0x00, - 0x04, // bInterval: Polling Interval in ms - // 41 - }; - -// ************************************************* - - const uint8_t PIOS_HID_ReportDescriptor[PIOS_HID_SIZ_REPORT_DESC] = - { - 0x06, 0x9c, 0xff, // USAGE_PAGE (Vendor Page: 0xFF00) - 0x09, 0x01, // USAGE (Demo Kit) - 0xa1, 0x01, // COLLECTION (Application) - // 6 - - // Data 1 - 0x85, 0x01, // REPORT_ID (1) - 0x09, 0x02, // USAGE (LED 1) - 0x15, 0x00, // LOGICAL_MINIMUM (0) - 0x25, 0xff, // LOGICAL_MAXIMUM (255) - 0x75, 0x08, // REPORT_SIZE (8) - 0x95, PIOS_USB_COM_DATA_LENGTH+1, // REPORT_COUNT (1) - 0x81, 0x83, // INPUT (Const,Var,Array) - // 20 - - // Data 1 - 0x85, 0x02, // REPORT_ID (2) - 0x09, 0x03, // USAGE (LED 1) - 0x15, 0x00, // LOGICAL_MINIMUM (0) - 0x25, 0xff, // LOGICAL_MAXIMUM (255) - 0x75, 0x08, // REPORT_SIZE (8) - 0x95, PIOS_USB_COM_DATA_LENGTH+1, // REPORT_COUNT (1) - 0x91, 0x82, // OUTPUT (Data,Var,Abs,Vol) - // 34 - - 0xc0 // END_COLLECTION - }; - -// ************************************************* -// USB String Descriptors (optional) - -const uint8_t PIOS_HID_StringLangID[PIOS_HID_SIZ_STRING_LANGID] = - { - PIOS_HID_SIZ_STRING_LANGID, - USB_STRING_DESCRIPTOR_TYPE, - 0x09, 0x08 // LangID = 0x0809: UK. English -// 0x09, 0x04 // LangID = 0x0409: U.S. English - }; - -const uint8_t PIOS_HID_StringVendor[PIOS_HID_SIZ_STRING_VENDOR] = - { - PIOS_HID_SIZ_STRING_VENDOR, // Size of Vendor string - USB_STRING_DESCRIPTOR_TYPE, // bDescriptorType - // Manufacturer: "STMicroelectronics" - 'o', 0, - 'p', 0, - 'e', 0, - 'n', 0, - 'p', 0, - 'i', 0, - 'l', 0, - 'o', 0, - 't', 0, - '.', 0, - 'o', 0, - 'r', 0, - 'g', 0 - }; - -const uint8_t PIOS_HID_StringProduct[PIOS_HID_SIZ_STRING_PRODUCT] = - { - PIOS_HID_SIZ_STRING_PRODUCT, // bLength - USB_STRING_DESCRIPTOR_TYPE, // bDescriptorType - 'P', 0, - 'i', 0, - 'p', 0, - 'X', 0, - 't', 0, - 'r', 0, - 'e', 0, - 'm', 0, - 'e', 0 - }; - -uint8_t PIOS_HID_StringSerial[PIOS_HID_SIZ_STRING_SERIAL] = - { - PIOS_HID_SIZ_STRING_SERIAL, // bLength - USB_STRING_DESCRIPTOR_TYPE, // bDescriptorType - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0 - }; - -// ************************************************* diff --git a/flight/board_hw_defs/ahrs/board_hw_defs.c b/flight/board_hw_defs/ahrs/board_hw_defs.c new file mode 100644 index 000000000..d774ce799 --- /dev/null +++ b/flight/board_hw_defs/ahrs/board_hw_defs.c @@ -0,0 +1,351 @@ +#include + +#if defined(PIOS_INCLUDE_LED) + +#include +static const struct pios_led pios_leds[] = { + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, +}; + +static const struct pios_led_cfg pios_led_cfg = { + .leds = pios_leds, + .num_leds = NELEMENTS(pios_leds), +}; + +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_SPI) + +#include + +/* OP Interface + * + * NOTE: Leave this declared as const data so that it ends up in the + * .rodata section (ie. Flash) rather than in the .bss section (RAM). + */ +void PIOS_SPI_op_irq_handler(void); +void DMA1_Channel5_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler"))); +void DMA1_Channel4_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_irq_handler"))); +static const struct pios_spi_cfg pios_spi_op_cfg = { + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Slave, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Hard, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + }, + .use_crc = TRUE, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = + (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | + DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = + (uint32_t) & (SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = + DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = + DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = + DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = + (uint32_t) & (SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = + DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = + DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = + DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .ssel = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, +}; + +uint32_t pios_spi_op_id; +void PIOS_SPI_op_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_op_id); +} + +#endif /* PIOS_INCLUDE_SPI */ + +#if defined(PIOS_INCLUDE_ADC) + +/* + * ADC system + */ +#include "pios_adc_priv.h" +extern void PIOS_ADC_handler(void); +void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); +// Remap the ADC DMA handler to this one +static const struct pios_adc_cfg pios_adc_cfg = { + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + .irq = { + .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), + .init = { + .NVIC_IRQChannel = DMA1_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA1_Channel1, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR, + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + } + }, + .half_flag = DMA1_IT_HT1, + .full_flag = DMA1_IT_TC1, +}; + +struct pios_adc_dev pios_adc_devs[] = { + { + .cfg = &pios_adc_cfg, + .callback_function = NULL, + }, +}; + +uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs); + +void PIOS_ADC_handler() { + PIOS_ADC_DMA_Handler(); +} + +#endif /* PIOS_INCLUDE_ADC */ + +#if defined(PIOS_INCLUDE_USART) + +#include + +/* + * AUX USART + */ +static const struct pios_usart_cfg pios_usart_aux_cfg = { + .regs = USART3, + .init = { + .USART_BaudRate = 230400, + .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_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_AF_PP, + }, + }, +}; + +#endif /* PIOS_INCLUDE_USART */ + +#if defined(PIOS_INCLUDE_COM) + +#include + +#endif /* PIOS_INCLUDE_COM */ + +#if defined(PIOS_INCLUDE_I2C) + +#include + +/* + * I2C Adapters + */ + +void PIOS_I2C_main_adapter_ev_irq_handler(void); +void PIOS_I2C_main_adapter_er_irq_handler(void); +void I2C1_EV_IRQHandler() + __attribute__ ((alias("PIOS_I2C_main_adapter_ev_irq_handler"))); +void I2C1_ER_IRQHandler() + __attribute__ ((alias("PIOS_I2C_main_adapter_er_irq_handler"))); + +static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { + .regs = I2C1, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 200000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +uint32_t pios_i2c_main_adapter_id; +void PIOS_I2C_main_adapter_ev_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_main_adapter_id); +} + +void PIOS_I2C_main_adapter_er_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_main_adapter_id); +} + +#endif /* PIOS_INCLUDE_I2C */ + +#if defined(PIOS_ENABLE_DEBUG_PINS) + +static const struct stm32_gpio pios_debug_pins[] = { + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, +}; + +#endif /* PIOS_ENABLE_DEBUG_PINS */ diff --git a/flight/board_hw_defs/coptercontrol/board_hw_defs.c b/flight/board_hw_defs/coptercontrol/board_hw_defs.c new file mode 100644 index 000000000..b5f81027f --- /dev/null +++ b/flight/board_hw_defs/coptercontrol/board_hw_defs.c @@ -0,0 +1,1018 @@ +#include + +#if defined(PIOS_INCLUDE_LED) + +#include +static const struct pios_led pios_leds[] = { + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, +}; + +static const struct pios_led_cfg pios_led_cfg = { + .leds = pios_leds, + .num_leds = NELEMENTS(pios_leds), +}; + +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_SPI) + +#include + +/* Flash/Accel Interface + * + * NOTE: Leave this declared as const data so that it ends up in the + * .rodata section (ie. Flash) rather than in the .bss section (RAM). + */ +void PIOS_SPI_flash_accel_irq_handler(void); +void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler"))); +void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_flash_accel_irq_handler"))); +static const struct pios_spi_cfg pios_spi_flash_accel_cfg = { + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = FALSE, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .ssel = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, +}; + +static uint32_t pios_spi_flash_accel_id; +void PIOS_SPI_flash_accel_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_flash_accel_id); +} + +#endif /* PIOS_INCLUDE_SPI */ + +#if defined(PIOS_INCLUDE_ADC) +/* + * ADC system + */ +#include "pios_adc_priv.h" +extern void PIOS_ADC_handler(void); +void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); +// Remap the ADC DMA handler to this one +static const struct pios_adc_cfg pios_adc_cfg = { + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + .irq = { + .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), + .init = { + .NVIC_IRQChannel = DMA1_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA1_Channel1, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR, + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + } + }, + .half_flag = DMA1_IT_HT1, + .full_flag = DMA1_IT_TC1, +}; + +struct pios_adc_dev pios_adc_devs[] = { + { + .cfg = &pios_adc_cfg, + .callback_function = NULL, + }, +}; + +uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs); + +void PIOS_ADC_handler() { + PIOS_ADC_DMA_Handler(); +} + +#endif /* PIOS_INCLUDE_ADC */ + +#if defined(PIOS_INCLUDE_TIM) + +#include "pios_tim_priv.h" + +static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, +}; + +static const struct pios_tim_clock_cfg tim_1_cfg = { + .timer = TIM1, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_2_cfg = { + .timer = TIM2, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_3_cfg = { + .timer = TIM3, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_4_cfg = { + .timer = TIM4, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +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, + }, + }, + }, +}; + +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, + }, + }, + }, +}; + + +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, + }, + }, + }, + + // 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, + }, + }, + }, +}; + +#endif /* PIOS_INCLUDE_TIM */ + +#if defined(PIOS_INCLUDE_USART) + +#include "pios_usart_priv.h" + +static const struct pios_usart_cfg pios_usart_generic_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_generic_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, + }, + }, +}; + +#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_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 */ + +#endif /* PIOS_INCLUDE_USART */ + +#if defined(PIOS_INCLUDE_COM) + +#include "pios_com_priv.h" + +#endif /* PIOS_INCLUDE_COM */ + +#if defined(PIOS_INCLUDE_RTC) +/* + * Realtime Clock (RTC) + */ +#include + +void PIOS_RTC_IRQ_Handler (void); +void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); +static const struct pios_rtc_cfg pios_rtc_main_cfg = { + .clksrc = RCC_RTCCLKSource_HSE_Div128, + .prescaler = 100, + .irq = { + .init = { + .NVIC_IRQChannel = RTC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +void PIOS_RTC_IRQ_Handler (void) +{ + PIOS_RTC_irq_handler (); +} + +#endif + +#if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM) +/* + * Servo outputs + */ +#include + +const struct pios_servo_cfg pios_servo_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = NELEMENTS(pios_tim_servoport_all_pins), +}; + +const struct pios_servo_cfg pios_servo_rcvr_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_rcvrport_pins, + .num_channels = NELEMENTS(pios_tim_servoport_rcvrport_pins), +}; + +#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ + +/* + * PPM Inputs + */ +#if defined(PIOS_INCLUDE_PPM) +#include + +const struct pios_ppm_cfg pios_ppm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[0], + .num_channels = 1, +}; + +#endif /* PIOS_INCLUDE_PPM */ + +/* + * PWM Inputs + */ +#if defined(PIOS_INCLUDE_PWM) +#include + +const struct pios_pwm_cfg pios_pwm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_rcvrport_all_channels, + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), +}; +#endif + +#if defined(PIOS_INCLUDE_I2C) + +#include + +/* + * I2C Adapters + */ + +void PIOS_I2C_flexi_adapter_ev_irq_handler(void); +void PIOS_I2C_flexi_adapter_er_irq_handler(void); +void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexi_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_flexi_adapter_er_irq_handler"))); + +static const struct pios_i2c_adapter_cfg pios_i2c_flexi_adapter_cfg = { + .regs = I2C2, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +uint32_t pios_i2c_flexi_adapter_id; +void PIOS_I2C_flexi_adapter_ev_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_flexi_adapter_id); +} + +void PIOS_I2C_flexi_adapter_er_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_flexi_adapter_id); +} + +#endif /* PIOS_INCLUDE_I2C */ + +#if defined(PIOS_INCLUDE_GCSRCVR) +#include "pios_gcsrcvr_priv.h" +#endif /* PIOS_INCLUDE_GCSRCVR */ + +#if defined(PIOS_INCLUDE_RCVR) +#include "pios_rcvr_priv.h" + +#endif /* PIOS_INCLUDE_RCVR */ + +#if defined(PIOS_INCLUDE_USB) +#include "pios_usb_priv.h" + +static const struct pios_usb_cfg pios_usb_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +#include "pios_usb_board_data_priv.h" +#include "pios_usb_desc_hid_cdc_priv.h" +#include "pios_usb_desc_hid_only_priv.h" + +#endif /* PIOS_INCLUDE_USB */ + +#if defined(PIOS_INCLUDE_COM_MSG) + +#include + +#endif /* PIOS_INCLUDE_COM_MSG */ + +#if defined(PIOS_INCLUDE_USB_HID) +#include + +const struct pios_usb_hid_cfg pios_usb_hid_cfg = { + .data_if = 0, + .data_rx_ep = 1, + .data_tx_ep = 1, +}; +#endif /* PIOS_INCLUDE_USB_HID */ + +#if defined(PIOS_INCLUDE_USB_CDC) +#include + +const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { + .ctrl_if = 1, + .ctrl_tx_ep = 2, + + .data_if = 2, + .data_rx_ep = 3, + .data_tx_ep = 3, +}; +#endif /* PIOS_INCLUDE_USB_CDC */ + diff --git a/flight/board_hw_defs/ins/board_hw_defs.c b/flight/board_hw_defs/ins/board_hw_defs.c new file mode 100644 index 000000000..b1b5a166c --- /dev/null +++ b/flight/board_hw_defs/ins/board_hw_defs.c @@ -0,0 +1,496 @@ +#include + +#if defined(PIOS_INCLUDE_LED) + +#include +static const struct pios_led pios_leds[] = { + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_ALARM] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, +}; + +static const struct pios_led_cfg pios_led_cfg = { + .leds = pios_leds, + .num_leds = NELEMENTS(pios_leds), +}; + +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_SPI) + +#include + +/* SPI2 Interface + * - Used for mainboard communications and magnetometer + * + * NOTE: Leave this declared as const data so that it ends up in the + * .rodata section (ie. Flash) rather than in the .bss section (RAM). + */ +void PIOS_SPI_op_mag_irq_handler(void); +void DMA1_Channel5_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_mag_irq_handler"))); +void DMA1_Channel4_IRQHandler() __attribute__ ((alias("PIOS_SPI_op_mag_irq_handler"))); +static const struct pios_spi_cfg pios_spi_op_mag_cfg = { + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Slave, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Hard, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + }, + .use_crc = TRUE, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = + (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | + DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = + (uint32_t) & (SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = + DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = + DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = + DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = + (uint32_t) & (SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = + DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = + DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = + DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .ssel = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, +}; + +uint32_t pios_spi_op_mag_id; +void PIOS_SPI_op_mag_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_op_mag_id); +} + +/* SPI1 Interface + * - Used for BMA180 accelerometer + */ +void PIOS_SPI_accel_irq_handler(void); +void DMA1_Channel2_IRQHandler() __attribute__ ((alias("PIOS_SPI_accel_irq_handler"))); +void DMA1_Channel3_IRQHandler() __attribute__ ((alias("PIOS_SPI_accel_irq_handler"))); +static const struct pios_spi_cfg pios_spi_accel_cfg = { + .regs = SPI1, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_1Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + }, + .use_crc = FALSE, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), + .init = { + .NVIC_IRQChannel = DMA1_Channel2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel2, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel3, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .ssel = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, +}; + +static uint32_t pios_spi_accel_id; +void PIOS_SPI_accel_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_accel_id); +} + +#endif /* PIOS_INCLUDE_SPI */ + +#if defined(PIOS_INCLUDE_GPS) +#include + +/* + * GPS USART + */ +static const struct pios_usart_cfg pios_usart_gps_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, + }, + }, +}; + +#endif /* PIOS_INCLUDE_GPS */ + +#ifdef PIOS_COM_AUX +/* + * AUX USART + */ +static const struct pios_usart_cfg pios_usart_aux_cfg = { + .regs = USART4, + .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 = USART4_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_AF_PP, + }, + }, +}; + +#endif /* PIOS_COM_AUX */ + + +#if defined(PIOS_INCLUDE_COM) + +#include + +#endif /* PIOS_INCLUDE_COM */ + +#if defined(PIOS_INCLUDE_I2C) + +#include + +/* + * I2C Adapters + */ + +void PIOS_I2C_pres_mag_adapter_ev_irq_handler(void); +void PIOS_I2C_pres_mag_adapter_er_irq_handler(void); +void I2C1_EV_IRQHandler() + __attribute__ ((alias("PIOS_I2C_pres_mag_adapter_ev_irq_handler"))); +void I2C1_ER_IRQHandler() + __attribute__ ((alias("PIOS_I2C_pres_mag_adapter_er_irq_handler"))); + +static const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = { + .regs = I2C1, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 200000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C1_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +uint32_t pios_i2c_pres_mag_adapter_id; +void PIOS_I2C_pres_mag_adapter_ev_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_pres_mag_adapter_id); +} + +void PIOS_I2C_pres_mag_adapter_er_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_pres_mag_adapter_id); +} + + +void PIOS_I2C_gyro_adapter_ev_irq_handler(void); +void PIOS_I2C_gyro_adapter_er_irq_handler(void); +void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_er_irq_handler"))); + +static const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = { + .regs = I2C2, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +uint32_t pios_i2c_gyro_adapter_id; +void PIOS_I2C_gyro_adapter_ev_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_gyro_adapter_id); +} + +void PIOS_I2C_gyro_adapter_er_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_gyro_adapter_id); +} + +#endif /* PIOS_INCLUDE_I2C */ diff --git a/flight/board_hw_defs/openpilot/board_hw_defs.c b/flight/board_hw_defs/openpilot/board_hw_defs.c new file mode 100644 index 000000000..681089d09 --- /dev/null +++ b/flight/board_hw_defs/openpilot/board_hw_defs.c @@ -0,0 +1,1085 @@ +#include + +#if defined(PIOS_INCLUDE_LED) + +#include +static const struct pios_led pios_leds[] = { + [PIOS_LED_HEARTBEAT] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_ALARM] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, +}; + +static const struct pios_led_cfg pios_led_cfg = { + .leds = pios_leds, + .num_leds = NELEMENTS(pios_leds), +}; + +#endif /* PIOS_INCLUDE_LED */ + +#if defined(PIOS_INCLUDE_SPI) + +#include + +/* MicroSD Interface + * + * NOTE: Leave this declared as const data so that it ends up in the + * .rodata section (ie. Flash) rather than in the .bss section (RAM). + */ +void PIOS_SPI_sdcard_irq_handler(void); +void DMA1_Channel2_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler"))); +void DMA1_Channel3_IRQHandler() __attribute__ ((alias ("PIOS_SPI_sdcard_irq_handler"))); +static const struct pios_spi_cfg pios_spi_sdcard_cfg = { + .regs = SPI1, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256, /* Maximum divider (ie. slowest clock rate) */ + }, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), + .init = { + .NVIC_IRQChannel = DMA1_Channel2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel2, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel3, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .ssel = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, + .sclk = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .mosi = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, +}; + +/* AHRS Interface + * + * NOTE: Leave this declared as const data so that it ends up in the + * .rodata section (ie. Flash) rather than in the .bss section (RAM). + */ +void PIOS_SPI_ahrs_irq_handler(void); +void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler"))); +void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_ahrs_irq_handler"))); +static const struct pios_spi_cfg pios_spi_ahrs_cfg = { + .regs = SPI2, + .init = { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 7, + .SPI_CPOL = SPI_CPOL_High, + .SPI_CPHA = SPI_CPHA_2Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_16, + }, + .use_crc = TRUE, + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + + .irq = { + .flags = (DMA1_FLAG_TC4 | DMA1_FLAG_TE4 | DMA1_FLAG_HT4 | DMA1_FLAG_GL4), + .init = { + .NVIC_IRQChannel = DMA1_Channel4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel4, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel5, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI2->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + .ssel = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, + .sclk = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .mosi = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, +}; + +static uint32_t pios_spi_sdcard_id; +void PIOS_SPI_sdcard_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_sdcard_id); +} + +uint32_t pios_spi_ahrs_id; +void PIOS_SPI_ahrs_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_ahrs_id); +} + +#endif /* PIOS_INCLUDE_SPI */ + +#if defined(PIOS_INCLUDE_ADC) + +/* + * ADC system + */ +#include "pios_adc_priv.h" +extern void PIOS_ADC_handler(void); +void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); +// Remap the ADC DMA handler to this one +static const struct pios_adc_cfg pios_adc_cfg = { + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + .irq = { + .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), + .init = { + .NVIC_IRQChannel = DMA1_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA1_Channel1, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR, + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_Low, + .DMA_M2M = DMA_M2M_Disable, + }, + } + }, + .half_flag = DMA1_IT_HT1, + .full_flag = DMA1_IT_TC1, +}; + +struct pios_adc_dev pios_adc_devs[] = { + { + .cfg = &pios_adc_cfg, + .callback_function = NULL, + }, +}; + +uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs); + +void PIOS_ADC_handler() { + PIOS_ADC_DMA_Handler(); +} + +#endif /* PIOS_INCLUDE_ADC */ + +#if defined(PIOS_INCLUDE_TIM) + +#include "pios_tim_priv.h" + +static const TIM_TimeBaseInitTypeDef tim_4_8_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, +}; + +static const struct pios_tim_clock_cfg tim_4_cfg = { + .timer = TIM4, + .time_base_init = &tim_4_8_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_8_cfg = { + .timer = TIM8, + .time_base_init = &tim_4_8_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const TIM_TimeBaseInitTypeDef tim_1_3_5_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = 0xFFFF, + .TIM_RepetitionCounter = 0x0000, +}; + +static const struct pios_tim_clock_cfg tim_1_cfg = { + .timer = TIM1, + .time_base_init = &tim_1_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_3_cfg = { + .timer = TIM3, + .time_base_init = &tim_1_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_5_cfg = { + .timer = TIM5, + .time_base_init = &tim_1_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +#endif /* PIOS_INCLUDE_TIM */ + +#if defined(PIOS_INCLUDE_USART) + +#include "pios_usart_priv.h" + +/* + * Telemetry USART + */ +static const struct pios_usart_cfg pios_usart_telem_cfg = { + .regs = USART2, + .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 = USART2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_3, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, +}; + +/* + * GPS USART + */ +static const struct pios_usart_cfg pios_usart_gps_cfg = { + .regs = USART3, + .remap = GPIO_PartialRemap_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 = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, +}; + +#ifdef PIOS_COM_AUX +/* + * AUX USART + */ +static const struct pios_usart_cfg pios_usart_aux_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, + }, + }, + .remap = GPIO_Remap_USART1, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, +}; +#endif + +#if defined(PIOS_INCLUDE_RTC) +/* + * Realtime Clock (RTC) + */ +#include + +void PIOS_RTC_IRQ_Handler (void); +void RTC_IRQHandler() __attribute__ ((alias ("PIOS_RTC_IRQ_Handler"))); +static const struct pios_rtc_cfg pios_rtc_main_cfg = { + .clksrc = RCC_RTCCLKSource_HSE_Div128, + .prescaler = 100, + .irq = { + .init = { + .NVIC_IRQChannel = RTC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +void PIOS_RTC_IRQ_Handler (void) +{ + PIOS_RTC_irq_handler (); +} + +#endif + +#if defined(PIOS_INCLUDE_DSM) +/* + * Spektrum/JR DSM USART + */ +#include + +static const struct pios_usart_cfg pios_usart_dsm_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_cfg = { + .bind = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, +}; + +#endif /* PIOS_COM_DSM */ + +#if defined(PIOS_INCLUDE_SBUS) +#error PIOS_INCLUDE_SBUS not implemented +#endif /* PIOS_INCLUDE_SBUS */ + +#endif /* PIOS_INCLUDE_USART */ + +#if defined(PIOS_INCLUDE_COM) + +#include "pios_com_priv.h" + +#endif /* PIOS_INCLUDE_COM */ + +#if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM) + +/** + * Pios servo configuration structures + */ +#include +static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { + { + .timer = TIM4, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .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 = 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_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM8, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, +}; + +const struct pios_servo_cfg pios_servo_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_servoport_all_pins, + .num_channels = NELEMENTS(pios_tim_servoport_all_pins), +}; + +#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ + +/* + * PWM Inputs + */ +#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) +#include +static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { + { + .timer = TIM1, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM5, + .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 = TIM1, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .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 = 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_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, + { + .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, + }, +}; + +const struct pios_pwm_cfg pios_pwm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = pios_tim_rcvrport_all_channels, + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), +}; +#endif + +/* + * PPM Input + */ +#if defined(PIOS_INCLUDE_PPM) +#include +static const struct pios_ppm_cfg pios_ppm_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + .TIM_Channel = TIM_Channel_2, + }, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[0], + .num_channels = 1, +}; + +#endif //PPM + +#if defined(PIOS_INCLUDE_I2C) + +#include + +/* + * I2C Adapters + */ + +void PIOS_I2C_main_adapter_ev_irq_handler(void); +void PIOS_I2C_main_adapter_er_irq_handler(void); +void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_ev_irq_handler"))); +void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_main_adapter_er_irq_handler"))); + +static const struct pios_i2c_adapter_cfg pios_i2c_main_adapter_cfg = { + .regs = I2C2, + .init = { + .I2C_Mode = I2C_Mode_I2C, + .I2C_OwnAddress1 = 0, + .I2C_Ack = I2C_Ack_Enable, + .I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit, + .I2C_DutyCycle = I2C_DutyCycle_2, + .I2C_ClockSpeed = 400000, /* bits/s */ + }, + .transfer_timeout_ms = 50, + .scl = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .sda = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_OD, + }, + }, + .event = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_EV_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .error = { + .flags = 0, /* FIXME: check this */ + .init = { + .NVIC_IRQChannel = I2C2_ER_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +uint32_t pios_i2c_main_adapter_id; +void PIOS_I2C_main_adapter_ev_irq_handler(void) +{ +#ifdef I2C_DEBUG_PIN + PIOS_DEBUG_PinHigh(I2C_DEBUG_PIN); +#endif + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_EV_IRQ_Handler(pios_i2c_main_adapter_id); +#ifdef I2C_DEBUG_PIN + PIOS_DEBUG_PinLow(I2C_DEBUG_PIN); +#endif +} + +void PIOS_I2C_main_adapter_er_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_I2C_ER_IRQ_Handler(pios_i2c_main_adapter_id); +} + +#endif /* PIOS_INCLUDE_I2C */ + +#if defined(PIOS_ENABLE_DEBUG_PINS) + +static const struct stm32_gpio pios_debug_pins[] = { + #define PIOS_DEBUG_PIN_SERVO_1 0 + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, + #define PIOS_DEBUG_PIN_SERVO_2 1 + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, + #define PIOS_DEBUG_PIN_SERVO_3 2 + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, + #define PIOS_DEBUG_PIN_SERVO_4 3 + { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, + #define PIOS_DEBUG_PIN_SERVO_5 4 + { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, + #define PIOS_DEBUG_PIN_SERVO_6 5 + { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, + #define PIOS_DEBUG_PIN_SERVO_7 6 + { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, + #define PIOS_DEBUG_PIN_SERVO_8 7 + { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, +}; + +#endif /* PIOS_ENABLE_DEBUG_PINS */ + +#if defined(PIOS_INCLUDE_RCVR) +#include "pios_rcvr_priv.h" + +#endif /* PIOS_INCLUDE_RCVR */ + +#if defined(PIOS_INCLUDE_USB) +#include "pios_usb_priv.h" + +static const struct pios_usb_cfg pios_usb_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +#include "pios_usb_board_data_priv.h" +#include "pios_usb_desc_hid_cdc_priv.h" +#include "pios_usb_desc_hid_only_priv.h" +#endif /* PIOS_INCLUDE_USB */ + +#if defined(PIOS_INCLUDE_COM_MSG) + +#include + +#endif /* PIOS_INCLUDE_COM_MSG */ + +#if defined(PIOS_INCLUDE_USB_HID) +#include + +const struct pios_usb_hid_cfg pios_usb_hid_cfg = { + .data_if = 0, + .data_rx_ep = 1, + .data_tx_ep = 1, +}; + +#endif /* PIOS_INCLUDE_USB_HID */ + +#if defined(PIOS_INCLUDE_USB_CDC) +#include + +const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = { + .ctrl_if = 1, + .ctrl_tx_ep = 2, + + .data_if = 2, + .data_rx_ep = 3, + .data_tx_ep = 3, +}; +#endif /* PIOS_INCLUDE_USB_CDC */ diff --git a/flight/board_hw_defs/pipxtreme/board_hw_defs.c b/flight/board_hw_defs/pipxtreme/board_hw_defs.c new file mode 100644 index 000000000..8dc17d5ba --- /dev/null +++ b/flight/board_hw_defs/pipxtreme/board_hw_defs.c @@ -0,0 +1,273 @@ +#include + +#if defined(PIOS_INCLUDE_SPI) + +#include + +/* OP Interface + * + * NOTE: Leave this declared as const data so that it ends up in the + * .rodata section (ie. Flash) rather than in the .bss section (RAM). + */ +void PIOS_SPI_port_irq_handler(void); +void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_port_irq_handler"))); +void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_port_irq_handler"))); + +static const struct pios_spi_cfg pios_spi_port_cfg = +{ + .regs = SPI1, + + .init = + { + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + .SPI_CRCPolynomial = 0, + .SPI_CPOL = SPI_CPOL_Low, + .SPI_CPHA = SPI_CPHA_1Edge, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256, // slowest SCLK + }, + .use_crc = FALSE, + + .dma = + { + .ahb_clk = RCC_AHBPeriph_DMA1, + .irq = + { + .flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), + .init = { + .NVIC_IRQChannel = DMA1_Channel2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + + .rx = { + .channel = DMA1_Channel2, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + .tx = { + .channel = DMA1_Channel3, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR), + .DMA_DIR = DMA_DIR_PeripheralDST, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_M2M = DMA_M2M_Disable, + }, + }, + }, + + .ssel = + { + .gpio = GPIOA, + .init = + { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_Out_PP, + }, + }, + .sclk = + { + .gpio = GPIOA, + .init = + { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, + .miso = + { + .gpio = GPIOA, + .init = + { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .mosi = + { + .gpio = GPIOA, + .init = + { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Speed = GPIO_Speed_10MHz, + .GPIO_Mode = GPIO_Mode_AF_PP, + }, + }, +}; + +uint32_t pios_spi_port_id; +void PIOS_SPI_port_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_port_id); +} + +#endif /* PIOS_INCLUDE_SPI */ + +#if defined(PIOS_INCLUDE_ADC) + +/* + * ADC system + */ +#include "pios_adc_priv.h" +extern void PIOS_ADC_handler(void); +void DMA1_Channel1_IRQHandler() __attribute__ ((alias("PIOS_ADC_handler"))); +// Remap the ADC DMA handler to this one +static const struct pios_adc_cfg pios_adc_cfg = { + .dma = { + .ahb_clk = RCC_AHBPeriph_DMA1, + .irq = { + .flags = (DMA1_FLAG_TC1 | DMA1_FLAG_TE1 | DMA1_FLAG_HT1 | DMA1_FLAG_GL1), + .init = { + .NVIC_IRQChannel = DMA1_Channel1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA1_Channel1, + .init = { + .DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR, + .DMA_DIR = DMA_DIR_PeripheralSRC, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Word, + .DMA_Mode = DMA_Mode_Circular, + .DMA_Priority = DMA_Priority_High, + .DMA_M2M = DMA_M2M_Disable, + }, + } + }, + .half_flag = DMA1_IT_HT1, + .full_flag = DMA1_IT_TC1, +}; + +struct pios_adc_dev pios_adc_devs[] = { + { + .cfg = &pios_adc_cfg, + .callback_function = NULL, + }, +}; + +uint8_t pios_adc_num_devices = NELEMENTS(pios_adc_devs); + +void PIOS_ADC_handler() { + PIOS_ADC_DMA_Handler(); +} + +#endif /* PIOS_INCLUDE_ADC */ + +#if defined(PIOS_INCLUDE_USART) + +#include + +/* + * SERIAL USART + */ +static const struct pios_usart_cfg pios_usart_serial_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_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_AF_PP, + }, + }, +}; + +#endif /* PIOS_INCLUDE_USART */ + +#if defined(PIOS_INCLUDE_COM) + +#include + +#endif /* PIOS_INCLUDE_COM */ + +// *********************************************************************************** + +#if defined(PIOS_INCLUDE_USB) +#include "pios_usb_priv.h" + +static const struct pios_usb_cfg pios_usb_main_cfg = { + .irq = { + .init = { + .NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +#include "pios_usb_board_data_priv.h" +#include "pios_usb_desc_hid_only_priv.h" + +#endif /* PIOS_INCLUDE_USB */ + +#if defined(PIOS_INCLUDE_USB_HID) +#include + +const struct pios_usb_hid_cfg pios_usb_hid_cfg = { + .data_if = 0, + .data_rx_ep = 1, + .data_tx_ep = 1, +}; + +#endif /* PIOS_INCLUDE_USB_HID */ diff --git a/ground/openpilotgcs/share/openpilotgcs/dials/deluxe/lineardial-horizontal.svg b/ground/openpilotgcs/share/openpilotgcs/dials/deluxe/lineardial-horizontal.svg old mode 100644 new mode 100755 index 2f6f71b17..e37172ec0 --- a/ground/openpilotgcs/share/openpilotgcs/dials/deluxe/lineardial-horizontal.svg +++ b/ground/openpilotgcs/share/openpilotgcs/dials/deluxe/lineardial-horizontal.svg @@ -14,8 +14,8 @@ height="70.597504" id="svg10068" version="1.1" - inkscape:version="0.48.1 " - sodipodi:docname="lineardial-horizontal.svg" + inkscape:version="0.48.2 r9819" + sodipodi:docname="lineardial-horizontal-old2.svg" inkscape:export-filename="H:\Documents\Hobbies\W433\My Gauges\vbat-001.png" inkscape:export-xdpi="103.61" inkscape:export-ydpi="103.61" @@ -25,15 +25,19 @@ + offset="0.31684026" + style="stop-color:#a3a3a3;stop-opacity:1;" /> + @@ -57,15 +61,11 @@ + style="stop-color:#8a8a8a;stop-opacity:1;" /> - + id="stop4391" /> @@ -237,15 +237,15 @@ + style="stop-color:#676767;stop-opacity:1;" /> + style="stop-color:#7d7d7d;stop-opacity:1;" /> @@ -425,46 +425,34 @@ - + offset="0" + style="stop-color:#ffc001;stop-opacity:1;" /> - - + offset="0" + style="stop-color:#e60000;stop-opacity:1;" /> @@ -687,7 +675,7 @@ x2="87.074203" y2="168.83261" gradientUnits="userSpaceOnUse" - gradientTransform="matrix(0,0.99999995,-0.99999975,0,-11.23354,-270.8763)" /> + gradientTransform="matrix(0,1.0110991,-1.0062411,0,-10.360287,-272.19756)" /> + gradientTransform="matrix(0,1.0224134,-1.0048488,0,-10.554719,-273.54528)" /> + gradientTransform="matrix(0,1,-1.0055415,0,-10.459485,-270.8763)" /> + transform="translate(-368.2988,-507.08981)"> @@ -1225,17 +1212,16 @@ id="layer5" inkscape:label="Green Zone" style="display:inline" - transform="translate(-140.85549,-141.35611)" - sodipodi:insensitive="true"> + transform="translate(-140.85549,-141.35611)"> + transform="translate(-140.85549,-141.35611)"> + transform="translate(-140.85549,-141.35611)"> + transform="translate(-140.85549,-141.35611)"> + transform="matrix(0,1,-1,0,0,0)" + ry="2.5494981" + rx="1.9392394" /> + + + + + style="stop-color:#656565;stop-opacity:1;" /> + style="stop-color:#757575;stop-opacity:1;" /> @@ -83,15 +94,19 @@ + style="stop-color:#9c9c9c;stop-opacity:0;" /> + id="stop3921" + offset="0.359375" + style="stop-color:#6c6c6c;stop-opacity:0.88793105;" /> + + style="stop-color:#9c9c9c;stop-opacity:0;" /> @@ -215,46 +230,34 @@ - + offset="0" + style="stop-color:#ffc001;stop-opacity:1;" /> - - + offset="0" + style="stop-color:#e60000;stop-opacity:1;" /> @@ -475,7 +478,8 @@ y1="132.84332" x2="-58.661255" y2="169.46072" - gradientUnits="userSpaceOnUse" /> + gradientUnits="userSpaceOnUse" + gradientTransform="matrix(1.0045664,0,0,0.98072534,3.8285685,3.8328979)" /> + gradientUnits="userSpaceOnUse" + gradientTransform="matrix(1.0048708,0,0,1.0000284,3.816285,2.2385824)" /> + gradientUnits="userSpaceOnUse" + gradientTransform="matrix(1.0057658,0,0,0.99502095,3.8176724,2.8357789)" /> + y2="412.00528" + gradientTransform="matrix(1,0,0,0.98663274,0,3.8538758)" /> + x2="-136.75557" + y2="74.015358" /> + image/svg+xml - + Edouard Lafargue @@ -779,8 +796,7 @@ inkscape:label="Dark background" id="g2932" inkscape:groupmode="layer" - transform="translate(-357.06525,-236.21351)" - sodipodi:insensitive="true"> + transform="translate(-357.06525,-236.21351)"> @@ -789,39 +805,39 @@ style="fill:url(#linearGradient5344);fill-opacity:1;stroke:none" id="rect2936" width="318.58304" - height="46.756046" + height="46.131046" x="-556.79657" - y="358.44128" - ry="3.4504199" + y="357.50378" + ry="3.4042974" inkscape:export-filename="H:\Documents\Hobbies\W433\g9905.png" inkscape:export-xdpi="88.809998" inkscape:export-ydpi="88.809998" /> - + style="fill:url(#linearGradient3900);fill-opacity:1;stroke:none" /> + + transform="translate(-129.62194,129.52019)"> + style="display:inline" + transform="translate(-129.62194,129.52019)"> + transform="translate(-129.62194,129.52019)"> + width="34.173222" + height="5.9349518" + x="136.20108" + y="-114.68695" + inkscape:label="#rect5246" + ry="2.4981377" /> + transform="translate(-129.62194,129.52019)"> diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.cpp b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.cpp index edd65d3ce..d8e637802 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.cpp +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.cpp @@ -177,7 +177,7 @@ PluginManager *PluginManager::instance() Create a plugin manager. Should be done only once per application. */ PluginManager::PluginManager() - : d(new PluginManagerPrivate(this)) + : d(new PluginManagerPrivate(this)),m_allPluginsLoaded(false) { m_instance = this; } @@ -586,6 +586,8 @@ void PluginManagerPrivate::loadPlugins() loadPlugin(it.previous(), PluginSpec::Running); } emit q->pluginsChanged(); + q->m_allPluginsLoaded=true; + emit q->pluginsLoadEnded(); } /*! diff --git a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.h b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.h index 6182b388e..49bcd5c55 100644 --- a/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.h +++ b/ground/openpilotgcs/src/libs/extensionsystem/pluginmanager.h @@ -56,7 +56,7 @@ class EXTENSIONSYSTEM_EXPORT PluginManager : public QObject public: static PluginManager *instance(); - + bool allPluginsLoaded(){return m_allPluginsLoaded;} PluginManager(); virtual ~PluginManager(); @@ -115,6 +115,7 @@ signals: void aboutToRemoveObject(QObject *obj); void pluginsChanged(); + void pluginsLoadEnded(); private slots: void startTests(); @@ -122,6 +123,7 @@ private: Internal::PluginManagerPrivate *d; static PluginManager *m_instance; mutable QReadWriteLock m_lock; + bool m_allPluginsLoaded; friend class Internal::PluginManagerPrivate; }; diff --git a/ground/openpilotgcs/src/libs/qwt/qwtconfig.pri b/ground/openpilotgcs/src/libs/qwt/qwtconfig.pri index c59b8d2b8..10e0594f1 100644 --- a/ground/openpilotgcs/src/libs/qwt/qwtconfig.pri +++ b/ground/openpilotgcs/src/libs/qwt/qwtconfig.pri @@ -67,6 +67,14 @@ QWT_CONFIG += QwtDll QWT_CONFIG += QwtPlot +###################################################################### +# Build the static/shared libraries. +# If QwtDll is enabled, a shared library is built, otherwise +# it will be a static library. +###################################################################### + +QWT_CONFIG += QwtDll + ###################################################################### # QwtWidgets enables all classes, that are needed to use the all other # widgets (sliders, dials, ...), beside QwtPlot. diff --git a/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui b/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui index 118acb6e4..e64800124 100644 --- a/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui +++ b/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui @@ -1,353 +1,350 @@ - - - CC_HW_Widget - - - - 0 - 0 - 517 - 487 - - - - Form - - - - - - QFrame::StyledPanel - - - QFrame::Raised - - - - - - - - - - - :/configgadget/images/coptercontrol.svg - - - true - - - - - - - - - - - - - MainPort - - - Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft - - - - - - - - - - - - - - FlexiPort - - - Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - USB HID Port - - - Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft - - - - - - - - - - USB VCP Port - - - Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft - - - - - - - - - - RcvrPort - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - - - - - - - - - Telemetry speed: - - - - - - - - 55 - 0 - - - - GPS speed: - - - - - - - - 55 - 0 - - - - ComUsbBridge speed: - - - - - - - Select the speed here. - - - - - - - Select the speed here. - - - - - - - Select the speed here. - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - - - 75 - true - - - - - - - Qt::AutoText - - - true - - - - - - - - 75 - true - - - - Changes on this page only take effect after board reset or power cycle - - - true - - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - 0 - 0 - - - - - 32 - 32 - - - - - 32 - 32 - - - - - - - - :/core/images/helpicon.svg:/core/images/helpicon.svg - - - - 32 - 32 - - - - true - - - - - - - Send to OpenPilot but don't write in SD. -Beware of not locking yourself out! - - - true - - - - - - Apply - - - false - - - - - - - Applies and Saves all settings to SD. -Beware of not locking yourself out! - - - false - - - - - - Save - - - - - - - - - - - - - - - - + + + CC_HW_Widget + + + + 0 + 0 + 517 + 487 + + + + Form + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + + + + + + true + + + + + + + + + + + + + MainPort + + + Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft + + + + + + + + + + + + + + FlexiPort + + + Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + USB HID Port + + + Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft + + + + + + + + + + USB VCP Port + + + Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft + + + + + + + + + + RcvrPort + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + + + + + + + Telemetry speed: + + + + + + + + 55 + 0 + + + + GPS speed: + + + + + + + + 55 + 0 + + + + ComUsbBridge speed: + + + + + + + Select the speed here. + + + + + + + Select the speed here. + + + + + + + Select the speed here. + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + 75 + true + + + + + + + Qt::AutoText + + + true + + + + + + + + 75 + true + + + + Changes on this page only take effect after board reset or power cycle + + + true + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + + 32 + 32 + + + + + 32 + 32 + + + + + + + + :/core/images/helpicon.svg:/core/images/helpicon.svg + + + + 32 + 32 + + + + true + + + + + + + Send to OpenPilot but don't write in SD. +Beware of not locking yourself out! + + + true + + + + + + Apply + + + false + + + + + + + Applies and Saves all settings to SD. +Beware of not locking yourself out! + + + false + + + + + + Save + + + + + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp index ed6e6e551..36337d3fa 100644 --- a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp @@ -40,6 +40,9 @@ ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent) { m_telemetry = new Ui_CC_HW_Widget(); m_telemetry->setupUi(this); + + m_telemetry->label_2->setPixmap(QPixmap(":/configgadget/images/coptercontrol.svg")); + setupButtons(m_telemetry->saveTelemetryToRAM,m_telemetry->saveTelemetryToSD); addUAVObjectToWidgetRelation("HwSettings","CC_FlexiPort",m_telemetry->cbFlexi); addUAVObjectToWidgetRelation("HwSettings","CC_MainPort",m_telemetry->cbTele); diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h index f815786fb..7c74043b3 100644 --- a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.h @@ -50,6 +50,7 @@ private slots: private: Ui_CC_HW_Widget *m_telemetry; + QSvgRenderer *m_renderer; }; #endif // CONFIGCCHWWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.qrc b/ground/openpilotgcs/src/plugins/config/configgadget.qrc index f24cb68b8..6eb11f079 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.qrc +++ b/ground/openpilotgcs/src/plugins/config/configgadget.qrc @@ -17,5 +17,6 @@ images/gyroscope.png images/TX.svg images/camera.png + images/TX2.svg diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 863104447..4387d10f4 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -41,8 +41,8 @@ #include #include -#define ACCESS_MIN_MOVE -6 -#define ACCESS_MAX_MOVE 6 +#define ACCESS_MIN_MOVE -3 +#define ACCESS_MAX_MOVE 3 #define STICK_MIN_MOVE -8 #define STICK_MAX_MOVE 8 @@ -108,7 +108,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) m_renderer = new QSvgRenderer(); QGraphicsScene *l_scene = m_config->graphicsView->scene(); m_config->graphicsView->setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); - if (QFile::exists(":/configgadget/images/TX.svg") && m_renderer->load(QString(":/configgadget/images/TX.svg")) && m_renderer->isValid()) + if (QFile::exists(":/configgadget/images/TX2.svg") && m_renderer->load(QString(":/configgadget/images/TX2.svg")) && m_renderer->isValid()) { l_scene->clear(); // Deletes all items contained in the scene as well. @@ -401,6 +401,7 @@ void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) case wizardChooseMode: { m_config->graphicsView->setVisible(true); + m_config->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio ); setTxMovement(nothing); m_config->wzText->setText(tr("Please choose your transmiter type.\n" "Mode 1 means your throttle stick is on the right\n" @@ -718,7 +719,7 @@ void ConfigInputWidget::identifyControls() m_config->wzText->clear(); setTxMovement(nothing); - QTimer::singleShot(500, this, SLOT(wzNext())); + QTimer::singleShot(2500, this, SLOT(wzNext())); } void ConfigInputWidget::identifyLimits() diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp index cbe554f4e..69b0427e3 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp @@ -42,8 +42,7 @@ ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTa m_stabilization = new Ui_StabilizationWidget(); m_stabilization->setupUi(this); - - setupButtons(m_stabilization->saveStabilizationToRAM,m_stabilization->saveStabilizationToSD); + setupButtons(m_stabilization->saveStabilizationToRAM, m_stabilization->saveStabilizationToSD); addUAVObject("StabilizationSettings"); @@ -71,8 +70,6 @@ ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTa connect(m_stabilization->pitchKi, SIGNAL(valueChanged(double)), this, SLOT(updatePitchKI(double))); connect(m_stabilization->pitchILimit, SIGNAL(valueChanged(double)), this, SLOT(updatePitchILimit(double))); - // Connect the help button - connect(m_stabilization->stabilizationHelp, SIGNAL(clicked()), this, SLOT(openHelp())); addWidget(m_stabilization->rateRollKp); addWidget(m_stabilization->rateRollKi); addWidget(m_stabilization->rateRollILimit); @@ -102,6 +99,9 @@ ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTa addWidget(m_stabilization->maximumYaw); addWidget(m_stabilization->lowThrottleZeroIntegral); + // Connect buttons + connect(m_stabilization->stabilizationResetToDefaults, SIGNAL(clicked()), this, SLOT(resetToDefaults())); + connect(m_stabilization->stabilizationHelp, SIGNAL(clicked()), this, SLOT(openHelp())); } ConfigStabilizationWidget::~ConfigStabilizationWidget() @@ -195,21 +195,17 @@ void ConfigStabilizationWidget::updatePitchILimit(double val) } } - /******************************* * Stabilization Settings *****************************/ /** - Request stabilization settings from the board + * Refresh UI with new settings of StabilizationSettings object + * (either from active configuration or just loaded defaults + * to be applied or saved) */ -void ConfigStabilizationWidget::refreshWidgetsValues() +void ConfigStabilizationWidget::refreshUIValues(StabilizationSettings::DataFields &stabData) { - bool dirty=isDirty(); - // Not needed anymore as this slot is only called whenever we get - // a signal that the object was just updated - // stabSettings->requestUpdate(); - StabilizationSettings::DataFields stabData = stabSettings->getData(); // Now fill in all the fields, this is fairly tedious: m_stabilization->rateRollKp->setValue(stabData.RollRatePID[StabilizationSettings::ROLLRATEPID_KP]); m_stabilization->rateRollKi->setValue(stabData.RollRatePID[StabilizationSettings::ROLLRATEPID_KI]); @@ -247,15 +243,25 @@ void ConfigStabilizationWidget::refreshWidgetsValues() m_stabilization->maximumPitch->setValue(stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_PITCH]); m_stabilization->maximumYaw->setValue(stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_YAW]); m_stabilization->lowThrottleZeroIntegral->setChecked(stabData.LowThrottleZeroIntegral==StabilizationSettings::LOWTHROTTLEZEROINTEGRAL_TRUE ? true : false); - - setDirty(dirty); } +/** + Request stabilization settings from the board + */ +void ConfigStabilizationWidget::refreshWidgetsValues() +{ + bool dirty=isDirty(); + // Not needed anymore as this slot is only called whenever we get + // a signal that the object was just updated + // stabSettings->requestUpdate(); + StabilizationSettings::DataFields stabData = stabSettings->getData(); + refreshUIValues(stabData); + setDirty(dirty); +} /** Send telemetry settings to the board */ - void ConfigStabilizationWidget::updateObjectsFromWidgets() { StabilizationSettings::DataFields stabData = stabSettings->getData(); @@ -298,7 +304,6 @@ void ConfigStabilizationWidget::updateObjectsFromWidgets() stabData.LowThrottleZeroIntegral = (m_stabilization->lowThrottleZeroIntegral->isChecked() ? StabilizationSettings::LOWTHROTTLEZEROINTEGRAL_TRUE :StabilizationSettings::LOWTHROTTLEZEROINTEGRAL_FALSE); - stabSettings->setData(stabData); // this is atomic } @@ -311,9 +316,16 @@ void ConfigStabilizationWidget::realtimeUpdateToggle(bool state) } } -void ConfigStabilizationWidget::openHelp() +void ConfigStabilizationWidget::resetToDefaults() { - - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Stabilization+panel", QUrl::StrictMode) ); + StabilizationSettings stabDefaults; + StabilizationSettings::DataFields defaults = stabDefaults.getData(); + bool dirty=isDirty(); + refreshUIValues(defaults); + setDirty(dirty); } +void ConfigStabilizationWidget::openHelp() +{ + QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Stabilization+panel", QUrl::StrictMode) ); +} diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h index e512c5097..b502dd25e 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h @@ -49,11 +49,13 @@ private: Ui_StabilizationWidget *m_stabilization; StabilizationSettings* stabSettings; QTimer updateTimer; + void refreshUIValues(StabilizationSettings::DataFields &stabData); private slots: virtual void refreshWidgetsValues(); void updateObjectsFromWidgets(); void realtimeUpdateToggle(bool); + void resetToDefaults(); void openHelp(); void updateRateRollKP(double); @@ -73,4 +75,4 @@ private slots: void updatePitchILimit(double); }; -#endif // ConfigStabilizationWidget_H +#endif // CONFIGSTABILIZATIONWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/images/TX2.svg b/ground/openpilotgcs/src/plugins/config/images/TX2.svg new file mode 100644 index 000000000..a6c8a533a --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/images/TX2.svg @@ -0,0 +1,2852 @@ + + + +image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index 2fac45ec9..5b591089f 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -76,7 +76,7 @@ 0 - 70 + 90 diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 5c2de727e..f99b35b94 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -1,772 +1,829 @@ - - - StabilizationWidget - - - - 0 - 0 - 683 - 685 - - - - Form - - - - - - - 0 - 1 - - - - QFrame::NoFrame - - - true - - - - - 0 - 0 - 665 - 627 - - - - - 0 - 0 - - - - - 0 - - - 0 - - - - - - 0 - 0 - - - - - 0 - 150 - - - - Rate Stabilization Coefficients (Inner Loop) - - - - - - Kp - - - Qt::AlignCenter - - - - - - - Ki - - - Qt::AlignCenter - - - - - - - ILimit - - - Qt::AlignCenter - - - - - - - Roll - - - - - - - Slowly raise Kp until you start seeing clear oscillations when you fly. -Then lower the value by 20% or so. - - - 6 - - - 0.000100000000000 - - - - - - - I factor for rate stabilization is usually very low or even zero. - - - 6 - - - 0.000100000000000 - - - - - - - 6 - - - 0.000100000000000 - - - - - - - If checked, the Roll and Pitch factors will be identical. -When you change one, the other is updated. - - - Link - - - - - - - Pitch - - - - - - - Slowly raise Kp until you start seeing clear oscillations when you fly. -Then lower the value by 20% or so. - - - 6 - - - 0.000100000000000 - - - - - - - I factor for rate stabilization is usually very low or even zero. - - - 6 - - - 0.000100000000000 - - - - - - - 6 - - - 0.000100000000000 - - - - - - - Yaw - - - - - - - Slowly raise Kp until you start seeing clear oscillations when you fly. -Then lower the value by 20% or so. - -You can usually go for higher values for Yaw factors. - - - 6 - - - 0.000100000000000 - - - - - - - As a rule of thumb, you can set YawRate Ki at roughly the same -value as YawRate Kp. - - - 6 - - - 0.000100000000000 - - - - - - - 6 - - - 0.000100000000000 - - - - - - - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 13 - - - - - - - - - 0 - 0 - - - - - 0 - 150 - - - - Attitude Stabization Coefficients (Outer Loop) - - - - - - Once Rate stabilization is done, you should increase the Kp factor until the airframe oscillates again, and go back down 20% or so. - - - - 6 - - - 0.100000000000000 - - - - - - - Ki can usually be almost identical to Kp. - - - 6 - - - 0.100000000000000 - - - - - - - ILimit can be equal to three to four times Ki, but you can adjust -depending on whether your airframe is well balanced, and your -flying style. - - - 6 - - - 0.100000000000000 - - - - - - - Kp - - - Qt::AlignCenter - - - - - - - Ki - - - Qt::AlignCenter - - - - - - - ILimit - - - Qt::AlignCenter - - - - - - - ILimit can be equal to three to four times Ki, but you can adjust -depending on whether your airframe is well balanced, and your -flying style. - - - 6 - - - 0.100000000000000 - - - - - - - Ki can usually be almost identical to Kp. - - - 6 - - - 0.100000000000000 - - - - - - - Once Rate stabilization is done, you should increase the Kp factor until the airframe oscillates again, and go back down 20% or so. - - - - 6 - - - 0.100000000000000 - - - - - - - Once Rate stabilization is done, you should increase the Kp factor until the airframe oscillates again, and go back down 20% or so. - - - - 6 - - - 0.100000000000000 - - - - - - - Yaw - - - - - - - Pitch - - - - - - - Roll - - - - - - - Ki can usually be almost identical to Kp. - - - 6 - - - 0.100000000000000 - - - - - - - ILimit can be equal to three to four times Ki, but you can adjust -depending on whether your airframe is well balanced, and your -flying style. - - - 6 - - - 0.100000000000000 - - - - - - - If checked, the Roll and Pitch factors will be identical. -When you change one, the other is updated. - - - Link - - - - - - - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 13 - - - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - Stick range and limits - - - - QLayout::SetMinAndMaxSize - - - - - Roll - - - Qt::AlignCenter - - - - - - - Pitch - - - Qt::AlignCenter - - - - - - - Yaw - - - Qt::AlignCenter - - - - - - - 180 - - - - - - - 180 - - - - - - - 180 - - - - - - - - 150 - 0 - - - - - 50 - false - - - - Full stick angle (deg) - - - - - - - - 150 - 0 - - - - - 50 - false - - - - Full stick rate (deg/s) - - - - - - - 500 - - - - - - - 500 - - - - - - - 500 - - - - - - - - 150 - 0 - - - - - 50 - false - - - - - - - Maximum rate in attitude mode (deg/s) - - - - - - - 500 - - - - - - - 500 - - - - - - - 500 - - - - - - - - - - Zero the integral when throttle is low - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - - - - - - If you check this, the GCS will udpate the stabilization factors -automatically every 300ms, which will help for fast tuning. - - - Update in real time - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - 0 - 0 - - - - - 32 - 32 - - - - - - - - :/core/images/helpicon.svg:/core/images/helpicon.svg - - - - 32 - 32 - - - - true - - - - - - - Apply - - - - - - - Save - - - - - - - - - - - - + + + StabilizationWidget + + + + 0 + 0 + 683 + 685 + + + + Form + + + + + + + 0 + 1 + + + + QFrame::NoFrame + + + true + + + + + 0 + 0 + 665 + 627 + + + + + 0 + 0 + + + + + 0 + + + 0 + + + + + + 0 + 0 + + + + + 0 + 150 + + + + Rate Stabilization Coefficients (Inner Loop) + + + + + + Kp + + + Qt::AlignCenter + + + + + + + Ki + + + Qt::AlignCenter + + + + + + + ILimit + + + Qt::AlignCenter + + + + + + + Roll + + + + + + + Slowly raise Kp until you start seeing clear oscillations when you fly. +Then lower the value by 20% or so. + + + 6 + + + 0.000100000000000 + + + + + + + I factor for rate stabilization is usually very low or even zero. + + + 6 + + + 0.000100000000000 + + + + + + + 6 + + + 0.000100000000000 + + + + + + + If checked, the Roll and Pitch factors will be identical. +When you change one, the other is updated. + + + Link + + + + + + + Pitch + + + + + + + Slowly raise Kp until you start seeing clear oscillations when you fly. +Then lower the value by 20% or so. + + + 6 + + + 0.000100000000000 + + + + + + + I factor for rate stabilization is usually very low or even zero. + + + 6 + + + 0.000100000000000 + + + + + + + 6 + + + 0.000100000000000 + + + + + + + Yaw + + + + + + + Slowly raise Kp until you start seeing clear oscillations when you fly. +Then lower the value by 20% or so. + +You can usually go for higher values for Yaw factors. + + + 6 + + + 0.000100000000000 + + + + + + + As a rule of thumb, you can set YawRate Ki at roughly the same +value as YawRate Kp. + + + 6 + + + 0.000100000000000 + + + + + + + 6 + + + 0.000100000000000 + + + + + + + + + + Qt::Vertical + + + QSizePolicy::Fixed + + + + 20 + 13 + + + + + + + + + 0 + 0 + + + + + 0 + 150 + + + + Attitude Stabization Coefficients (Outer Loop) + + + + + + Once Rate stabilization is done, you should increase the Kp factor until the airframe oscillates again, and go back down 20% or so. + + + + 6 + + + 0.100000000000000 + + + + + + + Ki can usually be almost identical to Kp. + + + 6 + + + 0.100000000000000 + + + + + + + ILimit can be equal to three to four times Ki, but you can adjust +depending on whether your airframe is well balanced, and your +flying style. + + + 6 + + + 0.100000000000000 + + + + + + + Kp + + + Qt::AlignCenter + + + + + + + Ki + + + Qt::AlignCenter + + + + + + + ILimit + + + Qt::AlignCenter + + + + + + + ILimit can be equal to three to four times Ki, but you can adjust +depending on whether your airframe is well balanced, and your +flying style. + + + 6 + + + 0.100000000000000 + + + + + + + Ki can usually be almost identical to Kp. + + + 6 + + + 0.100000000000000 + + + + + + + Once Rate stabilization is done, you should increase the Kp factor until the airframe oscillates again, and go back down 20% or so. + + + + 6 + + + 0.100000000000000 + + + + + + + Once Rate stabilization is done, you should increase the Kp factor until the airframe oscillates again, and go back down 20% or so. + + + + 6 + + + 0.100000000000000 + + + + + + + Yaw + + + + + + + Pitch + + + + + + + Roll + + + + + + + Ki can usually be almost identical to Kp. + + + 6 + + + 0.100000000000000 + + + + + + + ILimit can be equal to three to four times Ki, but you can adjust +depending on whether your airframe is well balanced, and your +flying style. + + + 6 + + + 0.100000000000000 + + + + + + + If checked, the Roll and Pitch factors will be identical. +When you change one, the other is updated. + + + Link + + + + + + + + + + Qt::Vertical + + + QSizePolicy::Fixed + + + + 20 + 13 + + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + Stick range and limits + + + + QLayout::SetMinAndMaxSize + + + + + Roll + + + Qt::AlignCenter + + + + + + + Pitch + + + Qt::AlignCenter + + + + + + + Yaw + + + Qt::AlignCenter + + + + + + + 180 + + + + + + + 180 + + + + + + + 180 + + + + + + + + 150 + 0 + + + + + 50 + false + + + + Full stick angle (deg) + + + + + + + + 150 + 0 + + + + + 50 + false + + + + Full stick rate (deg/s) + + + + + + + 500 + + + + + + + 500 + + + + + + + 500 + + + + + + + + 150 + 0 + + + + + 50 + false + + + + + + + Maximum rate in attitude mode (deg/s) + + + + + + + 500 + + + + + + + 500 + + + + + + + 500 + + + + + + + + + + Zero the integral when throttle is low + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + + + + + + + + + If you check this, the GCS will udpate the stabilization factors +automatically every 300ms, which will help for fast tuning. + + + Update in real time + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + + 32 + 32 + + + + + + + + :/core/images/helpicon.svg:/core/images/helpicon.svg + + + + 32 + 32 + + + + true + + + + + + + Load default Stabilization settings + +Loaded settings are not applied automatically. You have to click the +Apply or Save button afterwards. + + + Reset To Defaults + + + + + + + Send settings to the board but do not save to the non-volatile memory + + + Apply + + + + + + + Send settings to the board and save to the non-volatile memory + + + Save + + + + + + + + + scrollArea + rateRollKp + rateRollKi + rateRollILimit + linkRateRP + ratePitchKp + ratePitchKi + ratePitchILimit + rateYawKp + rateYawKi + rateYawILimit + rollKp + rollKi + rollILimit + linkAttitudeRP + pitchKp + pitchKi + pitchILimit + yawKp + yawKi + yawILimit + rollMax + pitchMax + yawMax + manualRoll + manualPitch + manualYaw + maximumRoll + maximumPitch + maximumYaw + lowThrottleZeroIntegral + realTimeUpdates + stabilizationHelp + stabilizationResetToDefaults + saveStabilizationToRAM + saveStabilizationToSD + + + + + + diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp index 9dfa46518..f49d98c0e 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.cpp @@ -38,6 +38,7 @@ #include #include #include +#include namespace Core { @@ -333,6 +334,12 @@ void ConnectionManager::registerDevice(IConnection *conn, const QString &devN, c */ void ConnectionManager::devChanged(IConnection *connection) { + if(!ExtensionSystem::PluginManager::instance()->allPluginsLoaded()) + { + connectionBackup.append(connection); + connect(ExtensionSystem::PluginManager::instance(),SIGNAL(pluginsLoadEnded()),this,SLOT(connectionsCallBack()),Qt::UniqueConnection); + return; + } //clear device list combobox m_availableDevList->clear(); @@ -381,4 +388,14 @@ void ConnectionManager::devChanged(IConnection *connection) m_connectBtn->setEnabled(false); } +} + +void Core::ConnectionManager::connectionsCallBack() +{ + foreach(IConnection * con,connectionBackup) + { + devChanged(con); + } + connectionBackup.clear(); + disconnect(ExtensionSystem::PluginManager::instance(),SIGNAL(pluginsLoadEnded()),this,SLOT(connectionsCallBack())); } //namespace Core diff --git a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h index beb809915..289e65590 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h +++ b/ground/openpilotgcs/src/plugins/coreplugin/connectionmanager.h @@ -98,7 +98,7 @@ private slots: // void onConnectionClosed(QObject *obj); void onConnectionDestroyed(QObject *obj); - + void connectionsCallBack(); //used to call devChange after all the plugins are loaded protected: QComboBox *m_availableDevList; QPushButton *m_connectBtn; @@ -114,6 +114,7 @@ protected: private: bool connectDevice(); Internal::MainWindow *m_mainWindow; + QList connectionBackup; }; diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/images/joystick.svg b/ground/openpilotgcs/src/plugins/gcscontrol/images/joystick.svg index 386881587..29d5c8762 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/images/joystick.svg +++ b/ground/openpilotgcs/src/plugins/gcscontrol/images/joystick.svg @@ -1,5 +1,5 @@ - + image/svg+xml - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 50 % - - 75 % - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 75 % - - 50 % - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file + inkscape:version="0.48.1 " + sodipodi:docname="joystick.svg"> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.cpp index d738da556..571b07068 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.cpp @@ -7,7 +7,7 @@ * @{ * @addtogroup GCSControlGadgetPlugin GCSControl Gadget Plugin * @{ - * @brief A that mimics a transmitter joystick and updates the MCC + * @brief The plugin that mimics a transmitter joystick and updates the MCC *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -28,30 +28,24 @@ #include "joystickcontrol.h" #include "extensionsystem/pluginmanager.h" #include -#include #include -#include -#include -#include -#include -#include #include -#include +#include /** * @brief Constructor for JoystickControl widget. Sets up the image of a joystick */ -JoystickControl::JoystickControl(QWidget *parent) : - QGraphicsView(parent) +JoystickControl::JoystickControl(QWidget *parent) : QGraphicsView(parent) { - setMinimumSize(64,64); + setMinimumSize(64, 64); setSizePolicy(QSizePolicy::MinimumExpanding, QSizePolicy::MinimumExpanding); setScene(new QGraphicsScene(this)); setRenderHints(QPainter::Antialiasing); m_renderer = new QSvgRenderer(); bool test = m_renderer->load(QString(":/gcscontrol/images/joystick.svg")); - Q_ASSERT( test ); + Q_UNUSED(test); + Q_ASSERT(test); m_background = new QGraphicsSvgItem(); m_background->setSharedRenderer(m_renderer); @@ -60,29 +54,40 @@ JoystickControl::JoystickControl(QWidget *parent) : m_joystickEnd = new QGraphicsSvgItem(); m_joystickEnd->setSharedRenderer(m_renderer); m_joystickEnd->setElementId(QString("joystickEnd")); - m_joystickEnd->setPos(0,0); + + m_joystickArea = new QGraphicsSvgItem(); + m_joystickArea->setSharedRenderer(m_renderer); + m_joystickArea->setElementId(QString("joystickArea")); + m_joystickArea->setPos( + (m_background->boundingRect().width() - m_joystickArea->boundingRect().width()) * 0.5, + (m_background->boundingRect().height() - m_joystickArea->boundingRect().height()) * 0.5 + ); + m_joystickArea->setVisible(false); QGraphicsScene *l_scene = scene(); l_scene->clear(); // This also deletes all items contained in the scene. l_scene->addItem(m_background); + l_scene->addItem(m_joystickArea); l_scene->addItem(m_joystickEnd); l_scene->setSceneRect(m_background->boundingRect()); + + changePosition(0.0, 0.0); } JoystickControl::~JoystickControl() { - + // Do nothing } -/*! - \brief Enables/Disables OpenGL +/** + * @brief Enables/Disables OpenGL */ void JoystickControl::enableOpenGL(bool flag) { - if (flag) - setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); - else - setViewport(new QWidget); + if (flag) + setViewport(new QGLWidget(QGLFormat(QGL::SampleBuffers))); + else + setViewport(new QWidget); } /** @@ -90,11 +95,12 @@ void JoystickControl::enableOpenGL(bool flag) */ void JoystickControl::changePosition(double x, double y) { - QRectF sceneSize = scene()->sceneRect(); - - m_joystickEnd->setPos( - (x+1) / 2*sceneSize.width() - m_joystickEnd->boundingRect().width() / 2, - (-y+1) / 2*sceneSize.height() - m_joystickEnd->boundingRect().height() / 2); + QRectF areaSize = m_joystickArea->boundingRect(); + QPointF point( + ((1.0 + x) * areaSize.width() - m_joystickEnd->boundingRect().width()) * 0.5, + ((1.0 - y) * areaSize.height() - m_joystickEnd->boundingRect().height()) * 0.5 + ); + m_joystickEnd->setPos(m_joystickArea->mapToScene(point)); } /** @@ -102,17 +108,17 @@ void JoystickControl::changePosition(double x, double y) */ void JoystickControl::mouseMoveEvent(QMouseEvent *event) { - QPointF point = mapToScene(event->pos()); - QRectF sceneSize = scene()->sceneRect(); + QPointF point = m_joystickArea->mapFromScene(mapToScene(event->pos())); + QSizeF areaSize = m_joystickArea->boundingRect().size(); - double Y = - (point.y() / sceneSize.height() - .5) * 2; - double X = (point.x() / sceneSize.width() - .5) * 2; - if (Y<-1) Y = -1; - if (Y> 1) Y = 1; - if (X<-1) X = -1; - if (X> 1) X = 1; + double y = - (point.y() / areaSize.height() - 0.5) * 2.0; + double x = (point.x() / areaSize.width() - 0.5) * 2.0; + if (y < -1.0) y = -1.0; + if (y > 1.0) y = 1.0; + if (x < -1.0) x = -1.0; + if (x > 1.0) x = 1.0; - emit positionClicked(X, Y); + emit positionClicked(x, y); } /** @@ -120,12 +126,11 @@ void JoystickControl::mouseMoveEvent(QMouseEvent *event) */ void JoystickControl::mousePressEvent(QMouseEvent *event) { - if( event->button() == Qt::LeftButton ) { + if (event->button() == Qt::LeftButton) { mouseMoveEvent(event); } } - void JoystickControl::paint() { update(); @@ -133,9 +138,9 @@ void JoystickControl::paint() void JoystickControl::paintEvent(QPaintEvent *event) { - // Skip painting until the dial file is loaded - if (! m_renderer->isValid()) { - qDebug()<<"Image file not loaded, not rendering"; + // Skip painting until the image file is loaded + if (!m_renderer->isValid()) { + qDebug() << "Image file not loaded, not rendering"; } QGraphicsView::paintEvent(event); @@ -144,10 +149,9 @@ void JoystickControl::paintEvent(QPaintEvent *event) void JoystickControl::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - fitInView(m_background, Qt::IgnoreAspectRatio ); + fitInView(m_background, Qt::KeepAspectRatio); } - /** * @} * @} diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.h b/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.h index 8492474e6..9c6ac59d0 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/joystickcontrol.h @@ -7,7 +7,7 @@ * @{ * @addtogroup GCSControlGadgetPlugin GCSControl Gadget Plugin * @{ - * @brief A that mimics a transmitter joystick and updates the MCC + * @brief The plugin that mimics a transmitter joystick and updates the MCC *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify @@ -25,7 +25,6 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ - #ifndef JOYSTICKCONTROL_H #define JOYSTICKCONTROL_H @@ -46,26 +45,27 @@ class JoystickControl : public QGraphicsView public: explicit JoystickControl(QWidget *parent = 0); ~JoystickControl(); - void enableOpenGL(bool flag); - void paint(); + void enableOpenGL(bool flag); + void paint(); protected: - void mouseMoveEvent(QMouseEvent *event); - void mousePressEvent(QMouseEvent *event); - void paintEvent(QPaintEvent *event); - void resizeEvent(QResizeEvent *event); + void mouseMoveEvent(QMouseEvent *event); + void mousePressEvent(QMouseEvent *event); + void paintEvent(QPaintEvent *event); + void resizeEvent(QResizeEvent *event); public slots: - void changePosition (double X, double Y); + void changePosition(double x, double y); signals: - void positionClicked(double X, double Y); + void positionClicked(double x, double y); private: QSvgRenderer *m_renderer; QGraphicsSvgItem *m_background; + QGraphicsSvgItem *m_joystickArea; QGraphicsSvgItem *m_joystickEnd; - }; +}; #endif // JOYSTICKCONTROL_H diff --git a/ground/openpilotgcs/src/plugins/notify/notificationitem.cpp b/ground/openpilotgcs/src/plugins/notify/notificationitem.cpp new file mode 100644 index 000000000..a54e7ff89 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/notify/notificationitem.cpp @@ -0,0 +1,435 @@ +/** + ****************************************************************************** + * + * @file NotificationItem.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Notify Plugin configuration + * @see The GNU Public License (GPL) Version 3 + * @defgroup notifyplugin + * @{ + * + *****************************************************************************/ +/* + * 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 + */ + +//Qt headers +#include +#include + +// GCS headers +#include "extensionsystem/pluginmanager.h" +#include "utils/pathutils.h" +#include "uavobjectmanager.h" +#include "uavobject.h" + +// Notify plugin headers +#include "notificationitem.h" +#include "notifylogging.h" + + + + + +QStringList NotificationItem::sayOrderValues; +QStringList NotificationItem::retryValues; + + +NotificationItem::NotificationItem(QObject *parent) + : QObject(parent) + , _currentUpdatePlayed(false) + , isNowPlaying(0) + , _isPlayed(false) + , _timer(NULL) + , _expireTimer(NULL) + , _soundCollectionPath("") + , _currentLanguage("default") + , _dataObject("") + , _objectField("") + , _condition(0) + , _sound1("") + , _sound2("") + , _sound3("") + , _sayOrder(never) + , _singleValue(0) + , _valueRange2(0) + , _repeatValue(repeatInstantly) + , _expireTimeout(eDefaultTimeout) + , _mute(false) +{ + + NotificationItem::sayOrderValues.clear(); + NotificationItem::sayOrderValues.insert(never,QString(tr("Never"))); + NotificationItem::sayOrderValues.insert(beforeFirst,QString(tr("Before first"))); + NotificationItem::sayOrderValues.insert(beforeSecond,QString(tr("Before second"))); + NotificationItem::sayOrderValues.insert(afterSecond,QString(tr("After second"))); + + NotificationItem::retryValues.clear(); + NotificationItem::retryValues.insert(repeatOnce,QString(tr("Repeat Once"))); + NotificationItem::retryValues.insert(repeatOncePerUpdate,QString(tr("Repeat Once per update"))); + NotificationItem::retryValues.insert(repeatInstantly,QString(tr("Repeat Instantly"))); + NotificationItem::retryValues.insert(repeat10seconds,QString(tr("Repeat 10 seconds"))); + NotificationItem::retryValues.insert(repeat30seconds,QString(tr("Repeat 30 seconds"))); + NotificationItem::retryValues.insert(repeat1minute,QString(tr("Repeat 1 minute"))); + +} + +void NotificationItem::copyTo(NotificationItem* that) const +{ + that->isNowPlaying = isNowPlaying; + that->_isPlayed = _isPlayed; + that->_soundCollectionPath = _soundCollectionPath; + that->_currentLanguage = _currentLanguage; + that->_soundCollectionPath = _soundCollectionPath; + that->_dataObject = _dataObject; + that->_objectField = _objectField; + that->_condition = _condition; + that->_sound1 = _sound1; + that->_sound2 = _sound2; + that->_sound3 = _sound3; + that->_sayOrder = _sayOrder; + that->_singleValue = _singleValue; + that->_valueRange2 = _valueRange2; + that->_repeatValue = _repeatValue; + that->_expireTimeout = _expireTimeout; + that->_mute = _mute; + +} + + +void NotificationItem::saveState(QSettings* settings) const +{ + settings->setValue("SoundCollectionPath", Utils::PathUtils().RemoveDataPath(getSoundCollectionPath())); + settings->setValue(QLatin1String("CurrentLanguage"), getCurrentLanguage()); + settings->setValue(QLatin1String("ObjectField"), getObjectField()); + settings->setValue(QLatin1String("DataObject"), getDataObject()); + settings->setValue(QLatin1String("RangeLimit"), getCondition()); + settings->setValue(QLatin1String("Value1"), singleValue()); + settings->setValue(QLatin1String("Value2"), valueRange2()); + settings->setValue(QLatin1String("Sound1"), getSound1()); + settings->setValue(QLatin1String("Sound2"), getSound2()); + settings->setValue(QLatin1String("Sound3"), getSound3()); + settings->setValue(QLatin1String("SayOrder"), getSayOrder()); + settings->setValue(QLatin1String("Repeat"), retryValue()); + settings->setValue(QLatin1String("ExpireTimeout"), lifetime()); + settings->setValue(QLatin1String("Mute"), mute()); +} + +void NotificationItem::restoreState(QSettings* settings) +{ + //settings = Core::ICore::instance()->settings(); + setSoundCollectionPath(Utils::PathUtils().InsertDataPath(settings->value(QLatin1String("SoundCollectionPath"), tr("")).toString())); + setCurrentLanguage(settings->value(QLatin1String("CurrentLanguage"), tr("")).toString()); + setDataObject(settings->value(QLatin1String("DataObject"), tr("")).toString()); + setObjectField(settings->value(QLatin1String("ObjectField"), tr("")).toString()); + setCondition(settings->value(QLatin1String("RangeLimit"), tr("")).toInt()); + setSound1(settings->value(QLatin1String("Sound1"), tr("")).toString()); + setSound2(settings->value(QLatin1String("Sound2"), tr("")).toString()); + setSound3(settings->value(QLatin1String("Sound3"), tr("")).toString()); + setSayOrder(settings->value(QLatin1String("SayOrder"), tr("")).toInt()); + QVariant value = settings->value(QLatin1String("Value1"), tr("")); + setSingleValue(value); + setValueRange2(settings->value(QLatin1String("Value2"), tr("")).toDouble()); + setRetryValue(settings->value(QLatin1String("Repeat"), tr("")).toInt()); + setLifetime(settings->value(QLatin1String("ExpireTimeout"), tr("")).toInt()); + setMute(settings->value(QLatin1String("Mute"), tr("")).toInt()); +} + +void NotificationItem::serialize(QDataStream& stream) +{ + stream << this->_soundCollectionPath; + stream << this->_currentLanguage; + stream << this->_dataObject; + stream << this->_objectField; + stream << this->_condition; + qNotifyDebug()<<"getOptionsPageValues seriaize"<<_condition; + stream << this->_sound1; + stream << this->_sound2; + stream << this->_sound3; + stream << this->_sayOrder; + stream << this->_singleValue; + stream << this->_valueRange2; + stream << this->_repeatValue; + stream << this->_expireTimeout; + stream << this->_mute; +} + +void NotificationItem::deserialize(QDataStream& stream) +{ + stream >> this->_soundCollectionPath; + stream >> this->_currentLanguage; + stream >> this->_dataObject; + stream >> this->_objectField; + stream >> this->_condition; + stream >> this->_sound1; + stream >> this->_sound2; + stream >> this->_sound3; + stream >> this->_sayOrder; + stream >> this->_singleValue; + stream >> this->_valueRange2; + stream >> this->_repeatValue; + stream >> this->_expireTimeout; + stream >> this->_mute; +} + +void NotificationItem::startTimer(int msec) +{ + if (!_timer) { + _timer = new QTimer(this); + _timer->setInterval(msec); + } + if (!_timer->isActive()) + _timer->start(); +} + + +void NotificationItem::restartTimer() +{ + if (!_timer) { + if (!_timer->isActive()) + _timer->start(); + } +} + + +void NotificationItem::stopTimer() +{ + if (_timer) { + if (_timer->isActive()) + _timer->stop(); + } +} + +void NotificationItem::disposeTimer() +{ + if (_timer) { + _timer->stop(); + delete _timer; + _timer = NULL; + } +} + +void NotificationItem::startExpireTimer() +{ + if (!_expireTimer) { + _expireTimer = new QTimer(this); + } + _expireTimer->start(_expireTimeout * 1000); +} + +void NotificationItem::stopExpireTimer() +{ + if (_expireTimer) { + if (_expireTimer) + _expireTimer->stop(); + } +} + +void NotificationItem::disposeExpireTimer() +{ + if (_expireTimer) { + _expireTimer->stop(); + delete _expireTimer; + _expireTimer = NULL; + } +} + +int getValuePosition(QString sayOrder) +{ + return NotificationItem::sayOrderValues.indexOf(sayOrder)-1; +} + +QString NotificationItem::checkSoundExists(QString fileName) +{ + QString name(fileName + ".wav"); + QString filePath = QDir::toNativeSeparators(getSoundCollectionPath() + "/" + + getCurrentLanguage() + "/" + + name); + if(QFile::exists(filePath)) + return filePath; + else { + filePath = QDir::toNativeSeparators(getSoundCollectionPath() + + "/default/" + + name); + if(!QFile::exists(filePath)) + filePath.clear(); + } + return filePath; +} + +QStringList valueToSoundList(QString value) +{ + qNotifyDebug()<<"notificationItem valueToSoundList input param"< number < 1 + digitWavs.append(numberParts.at(1)); + } else { + // append fractional part of number + QString left = numberParts.at(1).left(1); + (left == "0") ? digitWavs.append(left) : digitWavs.append(left + '0'); + digitWavs.append(numberParts.at(1).right(1)); + } + } + qNotifyDebug()<<"notificationItem valueToSoundList return value"<getType()) { + if(!field->getOptions().contains(value.toString())) + return QString(); + str = value.toString(); + } else { + str = QString("%L1").arg(value.toDouble()); + } + return str; +} + +QString NotificationItem::toString() +{ + QString str; + UAVObjectField* field = getUAVObjectField(); + QString value = stringFromValue(singleValue(), field); + + int pos = getSayOrder()-1; + QStringList lst; + lst.append(getSoundCaption(getSound1())); + lst.append(getSoundCaption(getSound2())); + lst.append(getSoundCaption(getSound3())); + QStringList valueSounds = valueToSoundList(value); + bool missed = false; + foreach(QString sound, valueSounds) { + if(checkSoundExists(sound).isEmpty()) { + missed = true; + break; + } + } + + // if not "Never" case + if(-1 != pos) { + if(missed) + lst.insert(pos, "[missed]" + value); + else + lst.insert(pos, value); + } + str = lst.join(" "); + return str; +} + +QStringList& NotificationItem::toSoundList() +{ + // tips: + // check of *.wav files exist needed for playing phonon queues; + // if phonon player don't find next file in queue, it buzz + UAVObjectField* field = getUAVObjectField(); + QString value = stringFromValue(singleValue(), field); + + // generate queue of sound files to play + _messageSequence.clear(); + int pos = getSayOrder()-1; + QStringList lst; + if(!getSound1().isEmpty()) + lst.append(getSound1()); + if(!getSound2().isEmpty()) + lst.append(getSound2()); + if(!getSound3().isEmpty()) + lst.append(getSound3()); + + // if not "Never" case + if(-1 != pos) { + QStringList valueSounds = valueToSoundList(value); + foreach(QString sound, valueSounds) + lst.insert(pos++, sound); + } + + foreach(QString sound, lst) { + QString path = checkSoundExists(sound); + if (!path.isEmpty()) { + _messageSequence.append(path); + } else { + _messageSequence.clear(); + break; + } + } + return _messageSequence; +} + +QString NotificationItem::getSoundCaption(QString fileName) +{ + if(fileName.isEmpty()) return QString(); + if(checkSoundExists(fileName).isEmpty()) { + return QString("[missed]") + fileName; + } + return fileName; +} + +UAVObjectField* NotificationItem::getUAVObjectField() { + return getUAVObject()->getField(getObjectField()); +} + +UAVDataObject* NotificationItem::getUAVObject() { + return dynamic_cast((ExtensionSystem::PluginManager::instance()->getObject())->getObject(getDataObject())); +} diff --git a/ground/openpilotgcs/src/plugins/notify/notificationitem.h b/ground/openpilotgcs/src/plugins/notify/notificationitem.h new file mode 100644 index 000000000..1475fd64f --- /dev/null +++ b/ground/openpilotgcs/src/plugins/notify/notificationitem.h @@ -0,0 +1,217 @@ +/** + ****************************************************************************** + * + * @file NotificationItem.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Notify Plugin configuration header + * @see The GNU Public License (GPL) Version 3 + * @defgroup notifyplugin + * @{ + * + *****************************************************************************/ +/* + * 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 NOTIFICATION_ITEM_H +#define NOTIFICATION_ITEM_H + +#include +#include "qsettings.h" +#include +#include + +using namespace Core; + +#define DECLARE_SOUND(number) \ + QString getSound##number() const { return _sound##number; } \ + void setSound##number(QString text) { _sound##number = text; } \ + +class UAVDataObject; +class UAVObjectField; + +class NotificationItem : public QObject +{ + Q_OBJECT +public: + enum { eDefaultTimeout = 15 }; // in sec + + enum {never,beforeFirst,beforeSecond,afterSecond}; + enum {repeatOncePerUpdate,repeatOnce,repeatInstantly,repeat10seconds, + repeat30seconds,repeat1minute}; + + explicit NotificationItem(QObject *parent = 0); + + void copyTo(NotificationItem*) const; + + DECLARE_SOUND(1) + DECLARE_SOUND(2) + DECLARE_SOUND(3) + + bool getCurrentUpdatePlayed() const {return _currentUpdatePlayed;} + void setCurrentUpdatePlayed(bool value){_currentUpdatePlayed=value;} + + int getCondition() const { return _condition; } + void setCondition(int value) { _condition = value; } + + int getSayOrder() const { return _sayOrder; } + void setSayOrder(int text) { _sayOrder = text; } + + QVariant singleValue() const { return _singleValue; } + void setSingleValue(QVariant value) { _singleValue = value; } + + double valueRange2() const { return _valueRange2; } + void setValueRange2(double value) { _valueRange2 = value; } + + QString getDataObject() const { return _dataObject; } + void setDataObject(QString text) { _dataObject = text; } + + QString getObjectField() const { return _objectField; } + void setObjectField(QString text) { _objectField = text; } + + QString getSoundCollectionPath() const { return _soundCollectionPath; } + void setSoundCollectionPath(QString path) { _soundCollectionPath = path; } + + QString getCurrentLanguage() const { return _currentLanguage; } + void setCurrentLanguage(QString text) { _currentLanguage = text; } + + QStringList getMessageSequence() const { return _messageSequence; } + void setMessageSequence(QStringList sequence) { _messageSequence = sequence; } + + int retryValue() const { return _repeatValue; } + void setRetryValue(int value) { _repeatValue = value; } + + int lifetime() const { return _expireTimeout; } + void setLifetime(int value) { _expireTimeout = value; } + + bool mute() const { return _mute; } + void setMute(bool value) { _mute = value; } + + void saveState(QSettings* settings) const; + void restoreState(QSettings* settings); + + + UAVDataObject* getUAVObject(void); + UAVObjectField* getUAVObjectField(void); + + void serialize(QDataStream& stream); + void deserialize(QDataStream& stream); + + /** + * Convert notification item fields in single string, + * to show in table for example + * + * @return string which describe notification + */ + QString toString(); + + /** + * Generate list of sound files needed to play notification + * + * @return success - reference to non-empty _messageSequence; + * error - if one of sounds doesn't exist returns + * reference to empty _messageSequence; + */ + QStringList& toSoundList(); + + /** + * Returns sound caption name, needed to create string representation of notification. + * + * @return success - string == , if sound file exists + * error - string == [missind], if sound file doesn't exist + */ + QString getSoundCaption(QString fileName); + + + QTimer* getTimer() const { return _timer; } + void startTimer(int value); + void restartTimer(); + void stopTimer(); + void disposeTimer(); + + QTimer* getExpireTimer() const { return _expireTimer; } + void startExpireTimer(); + void stopExpireTimer(); + + void disposeExpireTimer(); + + bool isNowPlaying; + bool _isPlayed; + + static QStringList sayOrderValues; + static QStringList retryValues; + +private: + QString checkSoundExists(QString fileName); + +private: + + bool _currentUpdatePlayed; + + QTimer* _timer; + + //! time from putting notification in queue till moment when notification became out-of-date + //! NOTE: each notification has it lifetime, this time setups individually for each notification + //! according to its priority + QTimer* _expireTimer; + + //! list of wav files from which notification consists + QStringList _messageSequence; + + //! path to folder with sound files + QString _soundCollectionPath; + + //! language in what notifications will be spelled + QString _currentLanguage; + + //! one UAV object per one notification + QString _dataObject; + + //! one field value change can be assigned to one notification + QString _objectField; + + //! fire condition for UAV field value (lower, greater, in range) + int _condition; + + //! possible sounds(at least one required to play notification) + QString _sound1; + QString _sound2; + QString _sound3; + + //! order in what sounds 1-3 will be played + int _sayOrder; + + //! one-side range, value(numeric or ENUM type) maybe lower, greater or in range + QVariant _singleValue; + + //! both-side range, value should be inside the range + //double _valueRange1; + double _valueRange2; + + //! how often or what periodicaly notification should be played + int _repeatValue; + + //! time after event occured till notification became invalid + //! and will be removed from list + int _expireTimeout; + + //! enables/disables playing of current notification + bool _mute; +}; + +Q_DECLARE_METATYPE(NotificationItem*) + +#endif // NotificationItem_H diff --git a/ground/openpilotgcs/src/plugins/notify/notify.pro b/ground/openpilotgcs/src/plugins/notify/notify.pro index 9ebebfa22..fca92db99 100644 --- a/ground/openpilotgcs/src/plugins/notify/notify.pro +++ b/ground/openpilotgcs/src/plugins/notify/notify.pro @@ -10,15 +10,17 @@ QT += phonon HEADERS += notifyplugin.h \ notifypluginoptionspage.h \ - notifypluginconfiguration.h \ notifyitemdelegate.h \ - notifytablemodel.h + notifytablemodel.h \ + notificationitem.h \ + notifylogging.h SOURCES += notifyplugin.cpp \ notifypluginoptionspage.cpp \ - notifypluginconfiguration.cpp \ notifyitemdelegate.cpp \ - notifytablemodel.cpp + notifytablemodel.cpp \ + notificationitem.cpp \ + notifylogging.cpp OTHER_FILES += NotifyPlugin.pluginspec @@ -27,3 +29,5 @@ FORMS += \ RESOURCES += \ res.qrc + + diff --git a/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.cpp b/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.cpp index 7a2fc2ef4..da7177b35 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.cpp @@ -25,136 +25,132 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include "notifyitemdelegate.h" #include +#include "notifyitemdelegate.h" +#include "notifytablemodel.h" +#include "notifylogging.h" +#include "notificationitem.h" - - NotifyItemDelegate::NotifyItemDelegate(QStringList items,QObject *parent) - : QItemDelegate(parent), - m_parent(parent), - m_items(items) { - - } - - QWidget *NotifyItemDelegate::createEditor(QWidget *parent, - const QStyleOptionViewItem &, - const QModelIndex &index) const - { - if (index.column() == 1) { - QComboBox* editor = new QComboBox(parent); - editor->clear(); - editor->addItems(m_items); - //repeatEditor->setCurrentIndex(0); - //repeatEditor->setItemDelegate(new RepeatCounterDelegate()); - - //connect(repeatEditor,SIGNAL(activated (const QString& )),this,SLOT(selectRow(const QString& ))); - //QTableWidgetItem* item = qobject_cast(parent); - //((QTableWidgetItem*)parent)->setSelected(true); -// connect(editor, SIGNAL(editingFinished()), -// this, SLOT(commitAndCloseEditor())); - return editor; - } else - { - if(index.column() == 2) - { - QSpinBox* editor = new QSpinBox(parent); - connect(editor, SIGNAL(editingFinished()), - this, SLOT(commitAndCloseEditor())); - return editor; - } - - } - QLineEdit *editor = new QLineEdit(parent); -// connect(editor, SIGNAL(editingFinished()), -// this, SLOT(commitAndCloseEditor())); - return editor; - } - - void NotifyItemDelegate::commitAndCloseEditor() - { - QLineEdit *editor = qobject_cast(sender()); - if (editor) - { - emit commitData(editor); - emit closeEditor(editor); - - } else { - QComboBox* editor = qobject_cast(sender()); - if (editor) - { - emit commitData(editor); - emit closeEditor(editor); - } else { - QSpinBox* editor = qobject_cast(sender()); - if (editor) - { - emit commitData(editor); - emit closeEditor(editor); - } - } - } - } - - void NotifyItemDelegate::setEditorData(QWidget *editor, - const QModelIndex &index) const - { - QLineEdit *edit = qobject_cast(editor); - if (edit) { - edit->setText(index.model()->data(index, Qt::EditRole).toString()); - } else { - QComboBox * repeatEditor = qobject_cast(editor); - if (repeatEditor) - repeatEditor->setCurrentIndex(repeatEditor->findText(index.model()->data(index, Qt::EditRole).toString())); - else { - QSpinBox * expireEditor = qobject_cast(editor); - if (expireEditor) - expireEditor->setValue(index.model()->data(index, Qt::EditRole).toInt()); - } - } - } - - void NotifyItemDelegate::setModelData(QWidget *editor, - QAbstractItemModel *model, const QModelIndex &index) const - { - QLineEdit *edit = qobject_cast(editor); - if (edit) { - model->setData(index, edit->text()); - } else { - QComboBox * repeatEditor = qobject_cast(editor); - if (repeatEditor) { - model->setData(index, repeatEditor->currentText()); - - } else { - QSpinBox * expireEditor = qobject_cast(editor); - if (expireEditor) { - //expireEditor->interpretText(); - model->setData(index, expireEditor->value(), Qt::EditRole); - } - } - } - } - - -void NotifyItemDelegate::selectRow(const QString & text) +NotifyItemDelegate::NotifyItemDelegate(QObject* parent) + : QItemDelegate(parent) + , _parent(parent) { - //QList list = ((QTableWidget*)(sender()->parent()))->findItems(text,Qt::MatchExactly); - QComboBox* combo = qobject_cast(sender()); - QTableWidget* table = new QTableWidget; - table = (QTableWidget*)(combo->parent()); - qDebug()<columnCount(); - qDebug()<rowCount(); - qDebug()<currentRow(); - //table->setCurrentIndex(1); - //table->findItems(text,Qt::MatchExactly); - //item->model()->index() - //item->setSelected(true); } -QSize NotifyItemDelegate::sizeHint ( const QStyleOptionViewItem & option, - const QModelIndex & index ) const +QWidget *NotifyItemDelegate::createEditor(QWidget* parent, const QStyleOptionViewItem& /*none*/, + const QModelIndex& index) const { - QSize s = QItemDelegate::sizeHint(option, index); - s.setHeight(10); - - return s; + if (eRepeatValue == index.column()) { + QComboBox* editor = new QComboBox(parent); + editor->clear(); + editor->addItems(NotificationItem::retryValues); + return editor; + } else { + if (eExpireTimer == index.column()) { + QSpinBox* editor = new QSpinBox(parent); + connect(editor, SIGNAL(editingFinished()), this, SLOT(commitAndCloseEditor())); + return editor; + } else { + if (eTurnOn == index.column()) { + QCheckBox* editor = new QCheckBox(parent); + connect(editor, SIGNAL(editingFinished()), this, SLOT(commitAndCloseEditor())); + return editor; + } + } + } + QLineEdit *editor = new QLineEdit(parent); + return editor; +} + +void NotifyItemDelegate::commitAndCloseEditor() +{ + QLineEdit* editor = qobject_cast(sender()); + if (editor) { + emit commitData(editor); + emit closeEditor(editor); + } else { + QComboBox* editor = qobject_cast(sender()); + if (editor) + { + emit commitData(editor); + emit closeEditor(editor); + } else { + QSpinBox* editor = qobject_cast(sender()); + if (editor) + { + emit commitData(editor); + emit closeEditor(editor); + } else { + QCheckBox* editor = qobject_cast(sender()); + if (editor) + { + emit commitData(editor); + emit closeEditor(editor); + } + } + } + } +} + +void NotifyItemDelegate::setEditorData(QWidget *editor, const QModelIndex &index) const +{ + QLineEdit* edit = qobject_cast(editor); + if (edit) { + edit->setText(index.model()->data(index, Qt::EditRole).toString()); + } else { + QComboBox* repeatEditor = qobject_cast(editor); + if (repeatEditor) + repeatEditor->setCurrentIndex(repeatEditor->findText(index.model()->data(index, Qt::EditRole).toString())); + else { + QSpinBox* expireEditor = qobject_cast(editor); + if (expireEditor) + expireEditor->setValue(index.model()->data(index, Qt::EditRole).toInt()); + else { + QCheckBox* enablePlayEditor = qobject_cast(editor); + if (enablePlayEditor) + enablePlayEditor->setChecked(index.model()->data(index, Qt::EditRole).toBool()); + } + } + } +} + +void NotifyItemDelegate::setModelData(QWidget *editor, QAbstractItemModel *model, const QModelIndex &index) const +{ + QLineEdit *edit = qobject_cast(editor); + if (edit) { + model->setData(index, edit->text()); + } else { + QComboBox * repeatEditor = qobject_cast(editor); + if (repeatEditor) { + model->setData(index, repeatEditor->currentText()); + } else { + QSpinBox * expireEditor = qobject_cast(editor); + if (expireEditor) { + model->setData(index, expireEditor->value(), Qt::EditRole); + } else { + QCheckBox* enablePlayEditor = qobject_cast(editor); + if (enablePlayEditor) { + model->setData(index, enablePlayEditor->isChecked(), Qt::EditRole); + } + } + } + } +} + +void NotifyItemDelegate::selectRow(const QString& text) +{ + QComboBox* combo = qobject_cast(sender()); + QTableWidget* table = new QTableWidget; + table = (QTableWidget*)(combo->parent()); + + qNotifyDebug() << table->columnCount(); + qNotifyDebug() << table->rowCount(); + qNotifyDebug() << table->currentRow(); +} + +QSize NotifyItemDelegate::sizeHint(const QStyleOptionViewItem& option, const QModelIndex& index) const +{ + QSize s = QItemDelegate::sizeHint(option, index); + s.setHeight(10); + return s; } diff --git a/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.h b/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.h index 40d6ced20..ace6d4976 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.h +++ b/ground/openpilotgcs/src/plugins/notify/notifyitemdelegate.h @@ -31,46 +31,26 @@ #include #include -//class RepeatCounterDelegate : public QItemDelegate -//{ -// Q_OBJECT - -//public: -// RepeatCounterDelegate(QObject *parent = 0); -// QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &, -// const QModelIndex &index) const; -// void setEditorData(QWidget *editor, const QModelIndex &index) const; -// void setModelData(QWidget *editor, QAbstractItemModel *model, -// const QModelIndex &index) const; - -//private slots: -// void commitAndCloseEditor(); -//}; class NotifyItemDelegate : public QItemDelegate { - Q_OBJECT + Q_OBJECT public: - NotifyItemDelegate(QStringList items, QObject *parent = 0); - QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &, - const QModelIndex &index) const; - void setEditorData(QWidget *editor, const QModelIndex &index) const; - void setModelData(QWidget *editor, QAbstractItemModel *model, - const QModelIndex &index) const; -// bool editorEvent(QEvent * event, QAbstractItemModel * model, -// const QStyleOptionViewItem & option, const QModelIndex & index ); - QSize sizeHint ( const QStyleOptionViewItem & option, const QModelIndex & index ) const; - -private: - QObject* m_parent; - QStringList m_items; - QComboBox* repeatEditor; - + NotifyItemDelegate(QObject *parent = 0); + QWidget *createEditor(QWidget *parent, const QStyleOptionViewItem &, + const QModelIndex &index) const; + void setEditorData(QWidget *editor, const QModelIndex &index) const; + void setModelData(QWidget *editor, QAbstractItemModel *model, + const QModelIndex &index) const; + QSize sizeHint ( const QStyleOptionViewItem & option, const QModelIndex & index ) const; private slots: - void selectRow(const QString & text); - void commitAndCloseEditor(); + void selectRow(const QString & text); + void commitAndCloseEditor(); + +private: + QObject* _parent; }; #endif // NOTIFYITEMDELEGATE_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifylogging.cpp b/ground/openpilotgcs/src/plugins/notify/notifylogging.cpp new file mode 100644 index 000000000..3cb11d111 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/notify/notifylogging.cpp @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * + * @file notifylogging.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Uses to logging only inside notify plugin, + * can be convinient turned on/off + * @see The GNU Public License (GPL) Version 3 + * @defgroup notifyplugin + * @{ + * + *****************************************************************************/ +/* + * 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 + */ + +#include "notifylogging.h" + +#ifdef DEBUG_NOTIFIES_ENABLE +QDebug qNotifyDebug() +{ + return qDebug() << "[NOTIFY_PLG]"; +} +#endif + +#ifndef DEBUG_NOTIFIES_ENABLE +QNoDebug qNotifyDebug() +{ + return QNoDebug(); +} +#endif diff --git a/flight/CopterControl/System/inc/taskmonitor.h b/ground/openpilotgcs/src/plugins/notify/notifylogging.h similarity index 67% rename from flight/CopterControl/System/inc/taskmonitor.h rename to ground/openpilotgcs/src/plugins/notify/notifylogging.h index 511f552f7..43e54f3b7 100644 --- a/flight/CopterControl/System/inc/taskmonitor.h +++ b/ground/openpilotgcs/src/plugins/notify/notifylogging.h @@ -1,42 +1,46 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotSystem OpenPilot System - * @{ - * @addtogroup OpenPilotLibraries OpenPilot System Libraries - * @{ - * @file taskmonitor.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Include file of the task monitoring library - * @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 TASKMONITOR_H -#define TASKMONITOR_H - -#include "taskinfo.h" - -int32_t TaskMonitorInitialize(void); -int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle); -void TaskMonitorUpdateAll(void); - -#endif // TASKMONITOR_H - -/** - * @} - * @} - */ +/** + ****************************************************************************** + * + * @file notifylogging.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Uses to logging only inside notify plugin, + * can be convinient turned on/off + * @see The GNU Public License (GPL) Version 3 + * @defgroup notifyplugin + * @{ + * + *****************************************************************************/ +/* + * 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 NOTIFYLOGGING_H +#define NOTIFYLOGGING_H + +#include "qdebug.h" + +#define DEBUG_NOTIFIES_ENABLE + +#ifdef DEBUG_NOTIFIES_ENABLE +QDebug qNotifyDebug(); +#endif + +#ifndef DEBUG_NOTIFIES_ENABLE +QNoDebug qNotifyDebug(); +#endif + +#define qNotifyDebug_if(test) if(test) qNotifyDebug() + +#endif // NOTIFYLOGGING_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp b/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp index 8735f8b60..5c7d27e75 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifyplugin.cpp @@ -26,8 +26,9 @@ */ #include "notifyplugin.h" -#include "notifypluginconfiguration.h" +#include "notificationitem.h" #include "notifypluginoptionspage.h" +#include "notifylogging.h" #include #include #include @@ -37,16 +38,16 @@ #include #include "qxttimer.h" +#include "backendcapabilities.h" static const QString VERSION = "1.0.0"; //#define DEBUG_NOTIFIES + SoundNotifyPlugin::SoundNotifyPlugin() { - phonon.mo = NULL; - configured = false; - // Do nothing + phonon.mo = NULL; } SoundNotifyPlugin::~SoundNotifyPlugin() @@ -54,7 +55,6 @@ SoundNotifyPlugin::~SoundNotifyPlugin() Core::ICore::instance()->saveSettings(this); if (phonon.mo != NULL) delete phonon.mo; - // Do nothing } bool SoundNotifyPlugin::initialize(const QStringList& args, QString *errMsg) @@ -70,15 +70,12 @@ bool SoundNotifyPlugin::initialize(const QStringList& args, QString *errMsg) void SoundNotifyPlugin::extensionsInitialized() { - Core::ICore::instance()->readSettings(this); - if ( !configured ){ - readConfig_0_0_0(); - } + Core::ICore::instance()->readSettings(this); - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); - connect(pm, SIGNAL(objectAdded(QObject*)), this, SLOT(onTelemetryManagerAdded(QObject*))); - removedNotifies.clear(); - connectNotifications(); + ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); + connect(pm, SIGNAL(objectAdded(QObject*)), this, SLOT(onTelemetryManagerAdded(QObject*))); + _toRemoveNotifications.clear(); + connectNotifications(); } void SoundNotifyPlugin::saveConfig( QSettings* settings, UAVConfigInfo *configInfo){ @@ -95,22 +92,19 @@ void SoundNotifyPlugin::saveConfig( QSettings* settings, UAVConfigInfo *configIn settings->endGroup(); settings->beginWriteArray("listNotifies"); - for (int i = 0; i < lstNotifications.size(); i++) { - settings->setArrayIndex(i); - lstNotifications.at(i)->saveState(settings); + for (int i = 0; i < _notificationList.size(); i++) { + settings->setArrayIndex(i); + _notificationList.at(i)->saveState(settings); } settings->endArray(); settings->setValue(QLatin1String("EnableSound"), enableSound); } -void SoundNotifyPlugin::readConfig( QSettings* settings, UAVConfigInfo *configInfo){ - - if ( configInfo->version() == UAVConfigVersion() ){ - // Just for migration to the new format. - configured = false; - return; - } +void SoundNotifyPlugin::readConfig( QSettings* settings, UAVConfigInfo * /* configInfo */) +{ + // Just for migration to the new format. + //Q_ASSERT(configInfo->version() == UAVConfigVersion()); settings->beginReadArray("Current"); settings->setArrayIndex(0); @@ -120,52 +114,20 @@ void SoundNotifyPlugin::readConfig( QSettings* settings, UAVConfigInfo *configIn // read list of notifications from settings int size = settings->beginReadArray("listNotifies"); for (int i = 0; i < size; ++i) { - settings->setArrayIndex(i); - NotifyPluginConfiguration* notification = new NotifyPluginConfiguration; - notification->restoreState(settings); - lstNotifications.append(notification); + settings->setArrayIndex(i); + NotificationItem* notification = new NotificationItem; + notification->restoreState(settings); + _notificationList.append(notification); } settings->endArray(); setEnableSound(settings->value(QLatin1String("EnableSound"),0).toBool()); - - configured = true; } -void SoundNotifyPlugin::readConfig_0_0_0(){ - - settings = Core::ICore::instance()->settings(); - settings->beginGroup(QLatin1String("NotifyPlugin")); - - settings->beginReadArray("Current"); - settings->setArrayIndex(0); - currentNotification.restoreState(settings); - settings->endArray(); - - // read list of notifications from settings - int size = settings->beginReadArray("listNotifies"); - for (int i = 0; i < size; ++i) { - settings->setArrayIndex(i); - NotifyPluginConfiguration* notification = new NotifyPluginConfiguration; - notification->restoreState(settings); - lstNotifications.append(notification); - } - settings->endArray(); - setEnableSound(settings->value(QLatin1String("EnableSound"),0).toBool()); - settings->endGroup(); - - ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); - connect(pm, SIGNAL(objectAdded(QObject*)), this, SLOT(onTelemetryManagerAdded(QObject*))); - removedNotifies.clear(); - connectNotifications(); - - configured = true; - } - void SoundNotifyPlugin::onTelemetryManagerAdded(QObject* obj) { - telMngr = qobject_cast(obj); - if(telMngr) - connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); + telMngr = qobject_cast(obj); + if (telMngr) + connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect())); } void SoundNotifyPlugin::shutdown() @@ -175,340 +137,395 @@ void SoundNotifyPlugin::shutdown() void SoundNotifyPlugin::onAutopilotDisconnect() { - connectNotifications(); + connectNotifications(); } /*! - clear any notify timers from previous flight; - reset will be perform on start of option page + clear any notify timers from previous flight; + reset will be perform on start of option page */ void SoundNotifyPlugin::resetNotification(void) { - //first, reject empty args and unknown fields. - foreach(NotifyPluginConfiguration* notify,lstNotifications) { - if(notify->timer) - { - disconnect(notify->timer, SIGNAL(timeout()), this, SLOT(repeatTimerHandler())); - notify->timer->stop(); - delete notify->timer; - notify->timer = NULL; - } - if(notify->expireTimer) - { - disconnect(notify->expireTimer, SIGNAL(timeout()), this, SLOT(expireTimerHandler())); - notify->expireTimer->stop(); - delete notify->expireTimer; - notify->expireTimer = NULL; - } - } + //first, reject empty args and unknown fields. + foreach(NotificationItem* ntf, _notificationList) { + ntf->disposeTimer(); + disconnect(ntf->getTimer(), SIGNAL(timeout()), this, SLOT(on_timerRepeated_Notification())); + ntf->disposeExpireTimer(); + disconnect(ntf->getExpireTimer(), SIGNAL(timeout()), this, SLOT(on_timerRepeated_Notification())); + } } /*! - update list of notifies; - will be perform on OK or APPLY press of option page + update list of notifies; + will be perform on OK or APPLY press of option page */ -void SoundNotifyPlugin::updateNotificationList(QList list) +void SoundNotifyPlugin::updateNotificationList(QList list) { - removedNotifies.clear(); - resetNotification(); - lstNotifications.clear(); - lstNotifications=list; - connectNotifications(); + _toRemoveNotifications.clear(); + resetNotification(); + _notificationList.clear(); + _notificationList=list; + connectNotifications(); - Core::ICore::instance()->saveSettings(this); + Core::ICore::instance()->saveSettings(this); } void SoundNotifyPlugin::connectNotifications() { - foreach(UAVDataObject* obj,lstNotifiedUAVObjects) { - if (obj != NULL) - disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(appendNotification(UAVObject*))); - } - if(phonon.mo != NULL) { - delete phonon.mo; - phonon.mo = NULL; + foreach(UAVDataObject* obj,lstNotifiedUAVObjects) { + if (obj != NULL) + disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(on_arrived_Notification(UAVObject*))); + } + if (phonon.mo != NULL) { + delete phonon.mo; + phonon.mo = NULL; + } + + if (!enableSound) return; + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + + lstNotifiedUAVObjects.clear(); + _pendingNotifications.clear(); + _notificationList.append(_toRemoveNotifications); + _toRemoveNotifications.clear(); + + //first, reject empty args and unknown fields. + foreach(NotificationItem* notify, _notificationList) { + notify->_isPlayed = false; + notify->isNowPlaying=false; + + if(notify->mute()) continue; + // check is all sounds presented for notification, + // if not - we must not subscribe to it at all + if(notify->toSoundList().isEmpty()) continue; + + UAVDataObject* obj = dynamic_cast( objManager->getObject(notify->getDataObject()) ); + if (obj != NULL ) { + if (!lstNotifiedUAVObjects.contains(obj)) { + lstNotifiedUAVObjects.append(obj); + + connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(on_arrived_Notification(UAVObject*))); + } + } else { + qNotifyDebug() << "Error: Object is unknown (" << notify->getDataObject() << ")."; } + } - if(!enableSound) return; - - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - - lstNotifiedUAVObjects.clear(); - pendingNotifications.clear(); - lstNotifications.append(removedNotifies); - removedNotifies.clear(); - - //first, reject empty args and unknown fields. - foreach(NotifyPluginConfiguration* notify,lstNotifications) { - notify->firstStart=true; - notify->isNowPlaying=false; - -// if(notify->timer) -// { -// disconnect(notify->timer, SIGNAL(timeout()), this, SLOT(repeatTimerHandler())); -// notify->timer->stop(); -// delete notify->timer; -// notify->timer = NULL; -// } -// if(notify->expireTimer) -// { -// disconnect(notify->expireTimer, SIGNAL(timeout()), this, SLOT(expireTimerHandler())); -// notify->expireTimer->stop(); -// delete notify->expireTimer; -// notify->expireTimer = NULL; -// } - - UAVDataObject* obj = dynamic_cast( objManager->getObject(notify->getDataObject()) ); - if (obj != NULL ) { - if(!lstNotifiedUAVObjects.contains(obj)) { - lstNotifiedUAVObjects.append(obj); - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(appendNotification(UAVObject*))); - } - } else - std::cout << "Error: Object is unknown (" << notify->getDataObject().toStdString() << ")." << std::endl; - } - - if(lstNotifications.isEmpty()) return; - // set notification message to current event - phonon.mo = Phonon::createPlayer(Phonon::NotificationCategory); - phonon.mo->clearQueue(); - phonon.firstPlay = true; -#ifdef DEBUG_NOTIFIES - QList audioOutputDevices = - Phonon::BackendCapabilities::availableAudioOutputDevices(); - foreach(Phonon::AudioOutputDevice dev, audioOutputDevices) { - qDebug() << "Notify: Audio Output device: " << dev.name() << " - " << dev.description(); - } -#endif - connect(phonon.mo,SIGNAL(stateChanged(Phonon::State,Phonon::State)), - this,SLOT(stateChanged(Phonon::State,Phonon::State))); + if (_notificationList.isEmpty()) return; + // set notification message to current event + phonon.mo = Phonon::createPlayer(Phonon::NotificationCategory); + phonon.mo->clearQueue(); + phonon.firstPlay = true; + QList audioOutputDevices = + Phonon::BackendCapabilities::availableAudioOutputDevices(); + foreach(Phonon::AudioOutputDevice dev, audioOutputDevices) { + qNotifyDebug() << "Notify: Audio Output device: " << dev.name() << " - " << dev.description(); + } + connect(phonon.mo, SIGNAL(stateChanged(Phonon::State,Phonon::State)), + this, SLOT(stateChanged(Phonon::State,Phonon::State))); } -void SoundNotifyPlugin::appendNotification(UAVObject *object) +void SoundNotifyPlugin::on_arrived_Notification(UAVObject *object) { - disconnect(object, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(appendNotification(UAVObject*))); + foreach(NotificationItem* ntf, _notificationList) { + if (object->getName() != ntf->getDataObject()) + continue; - foreach(NotifyPluginConfiguration* notification, lstNotifications) { - if(object->getName()!=notification->getDataObject()) - continue; - if(nowPlayingConfiguration == notification) - continue; + // skip duplicate notifications + if (_nowPlayingNotification == ntf) + continue; - if(notification->getRepeatFlag()!= "Repeat Instantly" && - notification->getRepeatFlag()!= "Repeat Once" && !notification->firstStart) - continue; + // skip periodical notifications + // this condition accepts: + // 1. Periodical notifications played firstly; + // NOTE: At first time it will be played, then it played only by timer, + // when conditions became false firstStart flag has been cleared and + // notification can be accepted again; + // 2. Once time notifications, they removed immediately after first playing; + // 3. Instant notifications(played one by one without interval); + if (ntf->retryValue() != NotificationItem::repeatInstantly && ntf->retryValue() != NotificationItem::repeatOncePerUpdate && + ntf->retryValue() != NotificationItem::repeatOnce && ntf->_isPlayed) + continue; - checkNotificationRule(notification,object); - } - connect(object, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(appendNotification(UAVObject*))); + qNotifyDebug() << QString("new notification: | %1 | %2 | val1: %3 | val2: %4") + .arg(ntf->getDataObject()) + .arg(ntf->getObjectField()) + .arg(ntf->singleValue().toString()) + .arg(ntf->valueRange2()); + + checkNotificationRule(ntf, object); + } + connect(object, SIGNAL(objectUpdated(UAVObject*)), + this, SLOT(on_arrived_Notification(UAVObject*)), Qt::UniqueConnection); } -void SoundNotifyPlugin::checkNotificationRule(NotifyPluginConfiguration* notification, UAVObject* object) +void SoundNotifyPlugin::on_timerRepeated_Notification() { - bool condition=false; - double threshold; - QString direction; - QString fieldName; - threshold = notification->getSpinBoxValue(); - direction = notification->getValue(); - fieldName = notification->getObjectField(); - UAVObjectField* field = object->getField(fieldName); - if (field->getName()!="") { - double value = field->getDouble(); + NotificationItem* notification = static_cast(sender()->parent()); + if (!notification) + return; + // skip duplicate notifications + // WARNING: generally we shoudn't ever trap here + // this means, that timer fires to early and notification overlap itself + if (_nowPlayingNotification == notification) { + qNotifyDebug() << "WARN: on_timerRepeated - notification was skipped!"; + notification->restartTimer(); + return; + } - switch(direction[0].toAscii()) - { - case 'E': - if(value==threshold) - condition = true; - break; - case 'G': - if(value>threshold) - condition = true; - break; - case 'L': - if(valuegetDataObject()) + .arg(notification->getObjectField()) + .arg(notification->toString()); - if(condition) - { - if(!playNotification(notification)) - { - if(!pendingNotifications.contains(notification)) - { - if(notification->timer) - if(notification->timer->isActive()) - notification->timer->stop(); -#ifdef DEBUG_NOTIFIES - qDebug() << "add to pending list - " << notification->parseNotifyMessage(); -#endif - // if audio is busy, start expiration timer - //ms = (notification->getExpiredTimeout()[in sec])*1000 - //QxtTimer::singleShot(notification->getExpireTimeout()*1000, this, SLOT(expirationTimerHandler(NotifyPluginConfiguration*)), qVariantFromValue(notification)); - pendingNotifications.append(notification); - if(!notification->expireTimer) - { - notification->expireTimer = new QTimer(notification); - connect(notification->expireTimer, SIGNAL(timeout()), this, SLOT(expireTimerHandler())); - } - notification->expireTimer->start(notification->getExpireTimeout()*1000); - } - } - } + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objManager = pm->getObject(); + UAVObject* object = objManager->getObject(notification->getDataObject()); + if (object) + checkNotificationRule(notification,object); } -bool SoundNotifyPlugin::playNotification(NotifyPluginConfiguration* notification) + +void SoundNotifyPlugin::on_expiredTimer_Notification() { - // Check: race condition, if phonon.mo got deleted don't go further - if (phonon.mo == NULL) - return false; + // fire expire timer + NotificationItem* notification = static_cast(sender()->parent()); + if(!notification) + return; + notification->stopExpireTimer(); -#ifdef DEBUG_NOTIFIES - qDebug() << "Phonon State: " << phonon.mo->state(); -#endif - if((phonon.mo->state()==Phonon::PausedState) || - (phonon.mo->state()==Phonon::StoppedState) || - phonon.firstPlay) - { - // don't fire expire timer - //notification->expire = false; - nowPlayingConfiguration = notification; - if(notification->expireTimer) - notification->expireTimer->stop(); + volatile QMutexLocker lock(&_mutex); - if(notification->getRepeatFlag()=="Repeat Once") - { - removedNotifies.append(lstNotifications.takeAt(lstNotifications.indexOf(notification))); - //if(!notification->firstStart) return true; - } - else { - if(notification->getRepeatFlag()!="Repeat Instantly") - { - QRegExp rxlen("(\\d+)"); - QString value; - int timer_value; - int pos = rxlen.indexIn(notification->getRepeatFlag()); - if (pos > -1) { - value = rxlen.cap(1); // "189" - timer_value = (value.toInt()+8)*1000; //ms*1000 + average duration of meassage - } + if (!_pendingNotifications.isEmpty()) { + qNotifyDebug() << QString("expireTimer: %1% | %2 | %3").arg(notification->getDataObject()) + .arg(notification->getObjectField()) + .arg(notification->toString()); - if(!notification->timer) - { - notification->timer = new QTimer(notification); - notification->timer->setInterval(timer_value); - connect(notification->timer, SIGNAL(timeout()), this, SLOT(repeatTimerHandler())); - } - if(!notification->timer->isActive()) - notification->timer->start(); - - //QxtTimer::singleShot(timer_value, this, SLOT(repeatTimerHandler(NotifyPluginConfiguration*)), qVariantFromValue(notification)); - } - } - notification->firstStart=false; - phonon.mo->clear(); - QString str = notification->parseNotifyMessage(); -#ifdef DEBUG_NOTIFIES - qDebug() << "play notification - " << str; -#endif - foreach(QString item, notification->getNotifyMessageList()) { - Phonon::MediaSource *ms = new Phonon::MediaSource(item); - ms->setAutoDelete(true); - phonon.mo->enqueue(*ms); - } - phonon.mo->play(); - phonon.firstPlay = false; // On Linux, you sometimes have to nudge Phonon to play 1 time before - // the state is not "Loading" anymore. - } - else - return false; // if audio is busy - - return true; -} - -//void SoundNotifyPlugin::repeatTimerHandler(NotifyPluginConfiguration* notification) -//{ -// qDebug() << "repeatTimerHandler - " << notification->parseNotifyMessage(); - -// ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); -// UAVObjectManager *objManager = pm->getObject(); -// UAVObject* object = objManager->getObject(notification->getDataObject()); -// if(object) -// checkNotificationRule(notification,object); -//} - -void SoundNotifyPlugin::repeatTimerHandler() -{ - NotifyPluginConfiguration* notification = static_cast(sender()->parent()); -#ifdef DEBUG_NOTIFIES - qDebug() << "repeatTimerHandler - " << notification->parseNotifyMessage(); -#endif - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - UAVObject* object = objManager->getObject(notification->getDataObject()); - if(object) - checkNotificationRule(notification,object); -} - -void SoundNotifyPlugin::expireTimerHandler() -{ - // fire expire timer - NotifyPluginConfiguration* notification = static_cast(sender()->parent()); - notification->expireTimer->stop(); - - if(!pendingNotifications.isEmpty()) - { -#ifdef DEBUG_NOTIFIES - qDebug() << "expireTimerHandler - " << notification->parseNotifyMessage(); -#endif - pendingNotifications.removeOne(notification); - } + _pendingNotifications.removeOne(notification); + } } void SoundNotifyPlugin::stateChanged(Phonon::State newstate, Phonon::State oldstate) { Q_UNUSED(oldstate) -#ifdef DEBUG_NOTIFIES - qDebug() << "File length (ms): " << phonon.mo->totalTime(); - qDebug() << "New State: " << newstate; -#endif + //qNotifyDebug() << "File length (ms): " << phonon.mo->totalTime(); #ifndef Q_OS_WIN - // This is a hack to force Linux to wait until the end of the - // wav file before moving to the next in the queue. - // I wish I did not have to go through a #define, but I did not - // manage to make this work on both platforms any other way! - if (phonon.mo->totalTime()>0) - phonon.mo->setTransitionTime(phonon.mo->totalTime()); + // This is a hack to force Linux to wait until the end of the + // wav file before moving to the next in the queue. + // I wish I did not have to go through a #define, but I did not + // manage to make this work on both platforms any other way! + if (phonon.mo->totalTime()>0) + phonon.mo->setTransitionTime(phonon.mo->totalTime()); #endif - if((newstate == Phonon::PausedState) || - (newstate == Phonon::StoppedState)) - { - nowPlayingConfiguration=NULL; - if(!pendingNotifications.isEmpty()) - { - NotifyPluginConfiguration* notification = pendingNotifications.takeFirst(); -#ifdef DEBUG_NOTIFIES - qDebug() << "play audioFree - " << notification->parseNotifyMessage(); -#endif - playNotification(notification); + if ((newstate == Phonon::PausedState) || + (newstate == Phonon::StoppedState)) + { + qNotifyDebug() << "New State: " << QVariant(newstate).toString(); + + volatile QMutexLocker lock(&_mutex); + // assignment to NULL needed to detect that palying is finished + // it's useful in repeat timer handler, where we can detect + // that notification has not overlap with itself + _nowPlayingNotification = NULL; + + if (!_pendingNotifications.isEmpty()) + { + NotificationItem* notification = _pendingNotifications.takeFirst(); + qNotifyDebug_if(notification) << "play audioFree - " << notification->toString(); + playNotification(notification); + qNotifyDebug()<<"end playNotification"; + } + } else { + if (newstate == Phonon::ErrorState) { + if (phonon.mo->errorType()==0) { + qDebug() << "Phonon::ErrorState: ErrorType = " << phonon.mo->errorType(); + phonon.mo->clearQueue(); + } + } + } +} + +bool checkRange(QString fieldValue, QString enumValue, QStringList /* values */, int direction) +{ + + bool ret = false; + switch(direction) + { + case NotifyPluginOptionsPage::equal: + ret = !QString::compare(enumValue, fieldValue, Qt::CaseInsensitive) ? true : false; + break; + + default: + ret = true; + break; + }; + return ret; +} + +bool checkRange(double fieldValue, double min, double max, int direction) +{ + bool ret = false; + //Q_ASSERT(min < max); + switch(direction) + { + case NotifyPluginOptionsPage::equal: + ret = (fieldValue == min); + break; + + case NotifyPluginOptionsPage::bigger: + ret = (fieldValue > min); + break; + + case NotifyPluginOptionsPage::smaller: + ret = (fieldValue < min); + break; + + default: + ret = (fieldValue > min) && (fieldValue < max); + break; + }; + + return ret; +} + +void SoundNotifyPlugin::checkNotificationRule(NotificationItem* notification, UAVObject* object) +{ + if(notification->getDataObject()!=object->getName() || object->getField(notification->getObjectField())==NULL) + return; + bool condition=false; + + if (notification->mute()) + return; + + int direction = notification->getCondition(); + QString fieldName = notification->getObjectField(); + UAVObjectField* field = object->getField(fieldName); + + if (field->getName().isEmpty()) + return; + + QVariant value = field->getValue(); + if(UAVObjectField::ENUM == field->getType()) { + qNotifyDebug()<<"Check range ENUM"<singleValue().toString()<<"|"<getOptions()<<"|"<< + direction<singleValue().toString(), + field->getOptions(), + direction);; + condition = checkRange(value.toString(), + notification->singleValue().toString(), + field->getOptions(), + direction); + } else { + qNotifyDebug()<<"Check range VAL"<singleValue().toString()<<"|"<getOptions()<<"|"<< + direction<singleValue().toDouble(), + notification->valueRange2(), + direction); + condition = checkRange(value.toDouble(), + notification->singleValue().toDouble(), + notification->valueRange2(), + direction); + } + + notification->_isPlayed = condition; + // if condition has been changed, and already in false state + // we should reset _isPlayed flag and stop repeat timer + if (!notification->_isPlayed) { + notification->stopTimer(); + notification->setCurrentUpdatePlayed(false); + return; + } + if(notification->retryValue() == NotificationItem::repeatOncePerUpdate && notification->getCurrentUpdatePlayed()) + return; + volatile QMutexLocker lock(&_mutex); + + if (!playNotification(notification)) { + if (!_pendingNotifications.contains(notification) + && (_nowPlayingNotification != notification)) { + notification->stopTimer(); + + qNotifyDebug() << "add to pending list - " << notification->toString(); + // if audio is busy, start expiration timer + //ms = (notification->getExpiredTimeout()[in sec])*1000 + //QxtTimer::singleShot(notification->getExpireTimeout()*1000, this, SLOT(expirationTimerHandler(NotificationItem*)), qVariantFromValue(notification)); + _pendingNotifications.append(notification); + notification->startExpireTimer(); + connect(notification->getExpireTimer(), SIGNAL(timeout()), + this, SLOT(on_expiredTimer_Notification()), Qt::UniqueConnection); + } + } +} + +bool SoundNotifyPlugin::playNotification(NotificationItem* notification) +{ + if(!notification) + return false; + + // Check: race condition, if phonon.mo got deleted don't go further + if (phonon.mo == NULL) + return false; + + //qNotifyDebug() << "Phonon State: " << phonon.mo->state(); + + if ((phonon.mo->state()==Phonon::PausedState) + || (phonon.mo->state()==Phonon::StoppedState) + || phonon.firstPlay) + { + _nowPlayingNotification = notification; + notification->stopExpireTimer(); + + if (notification->retryValue() == NotificationItem::repeatOnce) { + _toRemoveNotifications.append(_notificationList.takeAt(_notificationList.indexOf(notification))); + } + else if(notification->retryValue() == NotificationItem::repeatOncePerUpdate) + notification->setCurrentUpdatePlayed(true); + else { + if (notification->retryValue() != NotificationItem::repeatInstantly) { + QRegExp rxlen("(\\d+)"); + QString value; + int timer_value=0; + int pos = rxlen.indexIn(NotificationItem::retryValues.at(notification->retryValue())); + if (pos > -1) { + value = rxlen.cap(1); // "189" + + // needs to correct repeat timer value, + // acording to message play duration, + // we don't measure duration of each message, + // simply take average duration + enum { eAverageDurationSec = 8 }; + + enum { eSecToMsec = 1000 }; + + timer_value = (value.toInt() + eAverageDurationSec) * eSecToMsec; } - } else if (newstate == Phonon::ErrorState) - { - if(phonon.mo->errorType()==0) { - qDebug() << "Phonon::ErrorState: ErrorType = " << phonon.mo->errorType(); - phonon.mo->clearQueue(); - } - } -// if(newstate == Phonon::BufferingState) -// qDebug() << "Phonon::BufferingState!!!"; + + notification->startTimer(timer_value); + connect(notification->getTimer(), SIGNAL(timeout()), + this, SLOT(on_timerRepeated_Notification()), Qt::UniqueConnection); + } + } + phonon.mo->clear(); + qNotifyDebug() << "play: " << notification->toString(); + foreach (QString item, notification->toSoundList()) { + Phonon::MediaSource *ms = new Phonon::MediaSource(item); + ms->setAutoDelete(true); + phonon.mo->enqueue(*ms); + } + qNotifyDebug()<<"begin play"; + phonon.mo->play(); + qNotifyDebug()<<"end play"; + phonon.firstPlay = false; // On Linux, you sometimes have to nudge Phonon to play 1 time before + // the state is not "Loading" anymore. + return true; + + } + + return false; } Q_EXPORT_PLUGIN(SoundNotifyPlugin) diff --git a/ground/openpilotgcs/src/plugins/notify/notifyplugin.h b/ground/openpilotgcs/src/plugins/notify/notifyplugin.h index 0d0eb50d8..6a142ff03 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifyplugin.h +++ b/ground/openpilotgcs/src/plugins/notify/notifyplugin.h @@ -32,7 +32,7 @@ #include "uavtalk/telemetrymanager.h" #include "uavobjectmanager.h" #include "uavobject.h" -#include "notifypluginconfiguration.h" +#include "notificationitem.h" #include #include @@ -44,71 +44,69 @@ class NotifyPluginOptionsPage; typedef struct { Phonon::MediaObject* mo; - NotifyPluginConfiguration* notify; - bool firstPlay; + NotificationItem* notify; + bool firstPlay; } PhononObject, *pPhononObject; + class SoundNotifyPlugin : public Core::IConfigurablePlugin -{ - Q_OBJECT -public: - SoundNotifyPlugin(); - ~SoundNotifyPlugin(); +{ + Q_OBJECT +public: + SoundNotifyPlugin(); + ~SoundNotifyPlugin(); - void extensionsInitialized(); - bool initialize(const QStringList & arguments, QString * errorString); - void readConfig( QSettings* qSettings, Core::UAVConfigInfo *configInfo); - void saveConfig( QSettings* qSettings, Core::UAVConfigInfo *configInfo); - void shutdown(); + void extensionsInitialized(); + bool initialize(const QStringList & arguments, QString * errorString); + void readConfig( QSettings* qSettings, Core::UAVConfigInfo *configInfo); + void saveConfig( QSettings* qSettings, Core::UAVConfigInfo *configInfo); + void shutdown(); - QList getListNotifications() { return lstNotifications; } - //void setListNotifications(QList& list_notify) { } - NotifyPluginConfiguration* getCurrentNotification(){ return ¤tNotification;} - - bool getEnableSound() const { return enableSound; } - void setEnableSound(bool value) {enableSound = value; } - + QList getListNotifications() { return _notificationList; } + NotificationItem* getCurrentNotification(){ return ¤tNotification;} + bool getEnableSound() const { return enableSound; } + void setEnableSound(bool value) {enableSound = value; } private: - bool configured; // just for migration,delete later - bool enableSound; - QList< QList* > lstMediaSource; - QStringList mediaSource; - //QMap mapMediaObjects; - QMultiMap mapMediaObjects; + Q_DISABLE_COPY(SoundNotifyPlugin) - QSettings* settings; - - QList lstNotifiedUAVObjects; - - QList lstNotifications; - QList pendingNotifications; - QList removedNotifies; - - NotifyPluginConfiguration currentNotification; - NotifyPluginConfiguration* nowPlayingConfiguration; - - QString m_field; - PhononObject phonon; - NotifyPluginOptionsPage *mop; - TelemetryManager* telMngr; - - bool playNotification(NotifyPluginConfiguration* notification); - void checkNotificationRule(NotifyPluginConfiguration* notification, UAVObject* object); - void readConfig_0_0_0(); + bool playNotification(NotificationItem* notification); + void checkNotificationRule(NotificationItem* notification, UAVObject* object); + void readConfig_0_0_0(); private slots: - void onTelemetryManagerAdded(QObject* obj); - void onAutopilotDisconnect(); - void connectNotifications(); - void updateNotificationList(QList list); - void resetNotification(void); - void appendNotification(UAVObject *object); - void repeatTimerHandler(void); - void expireTimerHandler(void); - void stateChanged(Phonon::State newstate, Phonon::State oldstate); + + void onTelemetryManagerAdded(QObject* obj); + void onAutopilotDisconnect(); + void connectNotifications(); + void updateNotificationList(QList list); + void resetNotification(void); + void on_arrived_Notification(UAVObject *object); + void on_timerRepeated_Notification(void); + void on_expiredTimer_Notification(void); + void stateChanged(Phonon::State newstate, Phonon::State oldstate); + +private: + bool enableSound; + QList< QList* > lstMediaSource; + QStringList mediaSource; + QMultiMap mapMediaObjects; + QSettings* settings; + + QList lstNotifiedUAVObjects; + QList _notificationList; + QList _pendingNotifications; + QList _toRemoveNotifications; + + NotificationItem currentNotification; + NotificationItem* _nowPlayingNotification; + + PhononObject phonon; + NotifyPluginOptionsPage* mop; + TelemetryManager* telMngr; + QMutex _mutex; }; #endif // SOUNDNOTIFYPLUGIN_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginconfiguration.cpp b/ground/openpilotgcs/src/plugins/notify/notifypluginconfiguration.cpp deleted file mode 100644 index 94b88421e..000000000 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginconfiguration.cpp +++ /dev/null @@ -1,245 +0,0 @@ -/** - ****************************************************************************** - * - * @file notifyPluginConfiguration.cpp - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Notify Plugin configuration - * @see The GNU Public License (GPL) Version 3 - * @defgroup notifyplugin - * @{ - * - *****************************************************************************/ -/* - * 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 - */ - -#include "notifypluginconfiguration.h" -#include -#include -#include "utils/pathutils.h" - - -NotifyPluginConfiguration::NotifyPluginConfiguration(QObject *parent) : - QObject(parent), - isNowPlaying(0), - firstStart(1), - soundCollectionPath(""), - currentLanguage("default"), - dataObject(""), - objectField(""), - value("Equal to"), - sound1(""), - sound2(""), - sound3(""), - sayOrder("Never"), - spinBoxValue(0), - repeatString("Repeat Instantly"), - repeatTimeout(true), - expireTimeout(15) - -{ - timer = NULL; - expireTimer = NULL; -} - -void NotifyPluginConfiguration::copyTo(NotifyPluginConfiguration* that) const -{ - - that->isNowPlaying = isNowPlaying; - that->firstStart = firstStart; - that->soundCollectionPath = soundCollectionPath; - that->currentLanguage = currentLanguage; - that->soundCollectionPath = soundCollectionPath; - that->dataObject = dataObject; - that->objectField = objectField; - that->value = value; - that->sound1 = sound1; - that->sound2 = sound2; - that->sound3 = sound3; - that->sayOrder = sayOrder; - that->spinBoxValue = spinBoxValue; - that->repeatString = repeatString; - that->repeatTimeout = repeatTimeout; - that->expireTimeout = expireTimeout; -} - - -void NotifyPluginConfiguration::saveState(QSettings* settings) const -{ - settings->setValue("SoundCollectionPath", Utils::PathUtils().RemoveDataPath(getSoundCollectionPath())); - settings->setValue(QLatin1String("CurrentLanguage"), getCurrentLanguage()); - settings->setValue(QLatin1String("ObjectField"), getObjectField()); - settings->setValue(QLatin1String("DataObject"), getDataObject()); - settings->setValue(QLatin1String("Value"), getValue()); - settings->setValue(QLatin1String("ValueSpinBox"), getSpinBoxValue()); - settings->setValue(QLatin1String("Sound1"), getSound1()); - settings->setValue(QLatin1String("Sound2"), getSound2()); - settings->setValue(QLatin1String("Sound3"), getSound3()); - settings->setValue(QLatin1String("SayOrder"), getSayOrder()); - settings->setValue(QLatin1String("Repeat"), getRepeatFlag()); - settings->setValue(QLatin1String("ExpireTimeout"), getExpireTimeout()); -} - -void NotifyPluginConfiguration::restoreState(QSettings* settings) -{ - //settings = Core::ICore::instance()->settings(); - setSoundCollectionPath(Utils::PathUtils().InsertDataPath(settings->value(QLatin1String("SoundCollectionPath"), tr("")).toString())); - setCurrentLanguage(settings->value(QLatin1String("CurrentLanguage"), tr("")).toString()); - setDataObject(settings->value(QLatin1String("DataObject"), tr("")).toString()); - setObjectField(settings->value(QLatin1String("ObjectField"), tr("")).toString()); - setValue(settings->value(QLatin1String("Value"), tr("")).toString()); - setSound1(settings->value(QLatin1String("Sound1"), tr("")).toString()); - setSound2(settings->value(QLatin1String("Sound2"), tr("")).toString()); - setSound3(settings->value(QLatin1String("Sound3"), tr("")).toString()); - setSayOrder(settings->value(QLatin1String("SayOrder"), tr("")).toString()); - setSpinBoxValue(settings->value(QLatin1String("ValueSpinBox"), tr("")).toDouble()); - setRepeatFlag(settings->value(QLatin1String("Repeat"), tr("")).toString()); - setExpireTimeout(settings->value(QLatin1String("ExpireTimeout"), tr("")).toInt()); -} - - -QString NotifyPluginConfiguration::parseNotifyMessage() -{ - // tips: - // check of *.wav files exist needed for playing phonon queues; - // if phonon player don't find next file in queue, it buzz - - QString str,str1; - str1= getSayOrder(); - str = QString("%L1 ").arg(getSpinBoxValue()); - int position = 0xFF; - // generate queue of sound files to play - notifyMessageList.clear(); - - if(QFile::exists(QDir::toNativeSeparators(getSoundCollectionPath() + "/" + getCurrentLanguage()+"/"+getSound1()+".wav"))) - notifyMessageList.append(QDir::toNativeSeparators(getSoundCollectionPath() + "/" + getCurrentLanguage()+"/"+getSound1()+".wav")); - else - if(QFile::exists(QDir::toNativeSeparators(getSoundCollectionPath() + "/default/"+getSound1()+".wav"))) - notifyMessageList.append(QDir::toNativeSeparators(getSoundCollectionPath() + "/default/"+getSound1()+".wav")); - - if(getSound2()!="") - { - if(QFile::exists(QDir::toNativeSeparators(getSoundCollectionPath() + "/" + getCurrentLanguage()+"/"+getSound2()+".wav"))) - notifyMessageList.append(QDir::toNativeSeparators(getSoundCollectionPath() + "/" + getCurrentLanguage()+"/"+getSound2()+".wav")); - else - if(QFile::exists(QDir::toNativeSeparators(getSoundCollectionPath() + "/default/"+getSound2()+".wav"))) - notifyMessageList.append(QDir::toNativeSeparators(getSoundCollectionPath() + "/default/"+getSound2()+".wav")); - } - - if(getSound3()!="") - { - if(QFile::exists(QDir::toNativeSeparators(getSoundCollectionPath()+ "/" + getCurrentLanguage()+"/"+getSound3()+".wav"))) - notifyMessageList.append(QDir::toNativeSeparators(getSoundCollectionPath()+ "/" + getCurrentLanguage()+"/"+getSound3()+".wav")); - else - if(QFile::exists(QDir::toNativeSeparators(getSoundCollectionPath()+"/default/"+getSound3()+".wav"))) - notifyMessageList.append(QDir::toNativeSeparators(getSoundCollectionPath()+"/default/"+getSound3()+".wav")); - } - - switch(str1.at(0).toAscii()) - { - case 'N'://NEVER: - str = getSound1()+" "+getSound2()+" "+getSound3(); - position = 0xFF; - break; - - case 'B'://BEFORE: - str = QString("%L1 ").arg(getSpinBoxValue())+getSound1()+" "+getSound2()+" "+getSound3(); - position = 0; - break; - - case 'A'://AFTER: - switch(str1.at(6).toAscii()) - { - case 'f': - str = getSound1()+QString(" %L1 ").arg(getSpinBoxValue())+getSound2()+" "+getSound3(); - position = 1; - break; - case 's': - str = getSound1()+" "+getSound2()+QString(" %L1").arg(getSpinBoxValue())+" "+getSound3(); - position = 2; - break; - case 't': - str = getSound1()+" "+getSound2()+" "+getSound3()+QString(" %L1").arg(getSpinBoxValue()); - position = 3; - break; - } - break; - } - - if(position!=0xFF) - { - QStringList numberParts = QString("%1").arg(getSpinBoxValue()).trimmed().split("."); - QStringList numberFiles; - - if((numberParts.at(0).size()==1) || (numberParts.at(0).toInt()<20)) - { - //if(numberParts.at(0)!="0") - numberFiles.append(numberParts.at(0)); - } else { - int i=0; - if(numberParts.at(0).right(2).toInt()<20 && numberParts.at(0).right(2).toInt()!=0) { - if(numberParts.at(0).right(2).toInt()<10) - numberFiles.append(numberParts.at(0).right(1)); - else - numberFiles.append(numberParts.at(0).right(2)); - i=2; - } - for(;i1) { - numberFiles.append("point"); - if((numberParts.at(1).size()==1) /*|| (numberParts.at(1).toInt()<20)*/) - numberFiles.append(numberParts.at(1)); - else { - if(numberParts.at(1).left(1)=="0") - numberFiles.append(numberParts.at(1).left(1)); - else - numberFiles.append(numberParts.at(1).left(1)+'0'); - numberFiles.append(numberParts.at(1).right(1)); - } - } - foreach(QString fileName,numberFiles) { - fileName+=".wav"; - QString filePath = QDir::toNativeSeparators(getSoundCollectionPath()+"/"+ getCurrentLanguage()+"/"+fileName); - if(QFile::exists(filePath)) - notifyMessageList.insert(position++,QDir::toNativeSeparators(getSoundCollectionPath()+ "/"+getCurrentLanguage()+"/"+fileName)); - else { - if(QFile::exists(QDir::toNativeSeparators(getSoundCollectionPath()+"/default/"+fileName))) - notifyMessageList.insert(position++,QDir::toNativeSeparators(getSoundCollectionPath()+"/default/"+fileName)); - else { - notifyMessageList.clear(); - break; // if no some of *.wav files, then don't play number! - } - } - } - } - - //str.replace(QString(".wav | .mp3"), QString("")); - return str; -} diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginconfiguration.h b/ground/openpilotgcs/src/plugins/notify/notifypluginconfiguration.h deleted file mode 100644 index ad13a82a0..000000000 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginconfiguration.h +++ /dev/null @@ -1,122 +0,0 @@ -/** - ****************************************************************************** - * - * @file notifypluginconfiguration.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Notify Plugin configuration header - * @see The GNU Public License (GPL) Version 3 - * @defgroup notifyplugin - * @{ - * - *****************************************************************************/ -/* - * 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 NOTIFYPLUGINCONFIGURATION_H -#define NOTIFYPLUGINCONFIGURATION_H -#include -#include "qsettings.h" -#include -#include - -using namespace Core; - -class NotifyPluginConfiguration : public QObject -{ - Q_OBJECT -public: - explicit NotifyPluginConfiguration(QObject *parent = 0); - - QTimer* timer; - QTimer* expireTimer; - bool isNowPlaying; // - bool firstStart; - - void copyTo(NotifyPluginConfiguration*) const; - - QString getSound1() const { return sound1; } - void setSound1(QString text) {sound1 = text; } - - QString getSound2() const { return sound2; } - void setSound2(QString text) {sound2 = text; } - - QString getSound3() const { return sound3; } - void setSound3(QString text) {sound3 = text; } - - QString getValue() const { return value; } - void setValue(QString text) {value = text; } - - QString getSayOrder() const { return sayOrder; } - void setSayOrder(QString text) {sayOrder = text; } - - double getSpinBoxValue() const { return spinBoxValue; } - void setSpinBoxValue(double value) {spinBoxValue = value; } - - - QString getDataObject() const { return dataObject; } - void setDataObject(QString text) {dataObject = text; } - - QString getObjectField() const { return objectField; } - void setObjectField(QString text) { objectField = text; } - - QString getSoundCollectionPath() const { return soundCollectionPath; } - void setSoundCollectionPath(QString text) { soundCollectionPath = text; } - - QString getCurrentLanguage() const { return currentLanguage; } - void setCurrentLanguage(QString text) { currentLanguage = text; } - - QStringList getNotifyMessageList() const { return notifyMessageList; } - void setNotifyMessageList(QStringList text) { notifyMessageList = text; } - - QString getRepeatFlag() const { return repeatString; } - void setRepeatFlag(QString value) { repeatString = value; } - - bool getRepeatTimeout() const { return repeatTimeout; } - void setRepeatTimeout(bool value) { repeatTimeout = value; } - - int getExpireTimeout() const { return expireTimeout; } - void setExpireTimeout(int value) { expireTimeout = value; } - - - - void saveState(QSettings* settings) const; - void restoreState(QSettings* settings); - QString parseNotifyMessage(); - -private: - QStringList notifyMessageList; - QString soundCollectionPath; - QString currentLanguage; - QString dataObject; - QString objectField; - - QString value; - QString sound1; - QString sound2; - QString sound3; - QString sayOrder; - double spinBoxValue; - QString repeatString; - bool repeatTimeout; - int repeatTimerValue; - int expireTimeout; - -}; - -Q_DECLARE_METATYPE(NotifyPluginConfiguration*) - -#endif // NOTIFYPLUGINCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp index 5dee5678a..fa8ff053f 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp @@ -27,7 +27,7 @@ #include "notifypluginoptionspage.h" #include -#include "notifypluginconfiguration.h" +#include "notificationitem.h" #include "ui_notifypluginoptionspage.h" #include "extensionsystem/pluginmanager.h" #include "utils/pathutils.h" @@ -39,438 +39,597 @@ #include #include #include +#include +#include #include "notifyplugin.h" #include "notifyitemdelegate.h" #include "notifytablemodel.h" +#include "notifylogging.h" -NotifyPluginOptionsPage::NotifyPluginOptionsPage(/*NotifyPluginConfiguration *config,*/ QObject *parent) : - IOptionsPage(parent), - owner((SoundNotifyPlugin*)parent), - currentCollectionPath(""), - privListNotifications(((SoundNotifyPlugin*)parent)->getListNotifications()) +QStringList NotifyPluginOptionsPage::conditionValues; + +NotifyPluginOptionsPage::NotifyPluginOptionsPage(QObject *parent) + : IOptionsPage(parent) + , _objManager(*ExtensionSystem::PluginManager::instance()->getObject()) + , _owner(qobject_cast(parent)) + , _dynamicFieldCondition(NULL) + , _dynamicFieldWidget(NULL) + , _dynamicFieldType(-1) + , _sayOrder(NULL) + , _form(NULL) + , _selectedNotification(NULL) { + NotifyPluginOptionsPage::conditionValues.insert(equal,tr("Equal to")); + NotifyPluginOptionsPage::conditionValues.insert(bigger,tr("Large than")); + NotifyPluginOptionsPage::conditionValues.insert(smaller,tr("Lower than")); + NotifyPluginOptionsPage::conditionValues.insert(inrange,tr("In range")); } +NotifyPluginOptionsPage::~NotifyPluginOptionsPage() +{} -//creates options page widget (uses the UI file) -QWidget *NotifyPluginOptionsPage::createPage(QWidget *parent) +QWidget *NotifyPluginOptionsPage::createPage(QWidget * /* parent */) { + _optionsPage.reset(new Ui::NotifyPluginOptionsPage()); + //main widget + QWidget* optionsPageWidget = new QWidget; + _dynamicFieldWidget = NULL; + _dynamicFieldCondition = NULL; + resetFieldType(); + //save ref to form, needed for binding dynamic fields in future + _form = optionsPageWidget; + //main layout + _optionsPage->setupUi(optionsPageWidget); - options_page = new Ui::NotifyPluginOptionsPage(); - //main widget - QWidget *optionsPageWidget = new QWidget; - //main layout - options_page->setupUi(optionsPageWidget); + _optionsPage->SoundDirectoryPathChooser->setExpectedKind(Utils::PathChooser::Directory); + _optionsPage->SoundDirectoryPathChooser->setPromptDialogTitle(tr("Choose sound collection directory")); - delegateItems.clear(); - listSoundFiles.clear(); + connect(_optionsPage->SoundDirectoryPathChooser, SIGNAL(changed(const QString&)), + this, SLOT(on_clicked_buttonSoundFolder(const QString&))); + connect(_optionsPage->SoundCollectionList, SIGNAL(currentIndexChanged (int)), + this, SLOT(on_changedIndex_soundLanguage(int))); - delegateItems << "Repeat Once" - << "Repeat Instantly" - << "Repeat 10 seconds" - << "Repeat 30 seconds" - << "Repeat 1 minute"; + connect(this, SIGNAL(updateNotifications(QList)), + _owner, SLOT(updateNotificationList(QList))); + //connect(this, SIGNAL(resetNotification()),owner, SLOT(resetNotification())); - options_page->chkEnableSound->setChecked(owner->getEnableSound()); - options_page->SoundDirectoryPathChooser->setExpectedKind(Utils::PathChooser::Directory); - options_page->SoundDirectoryPathChooser->setPromptDialogTitle(tr("Choose sound collection directory")); + _privListNotifications = _owner->getListNotifications(); + // [1] + setSelectedNotification(_owner->getCurrentNotification()); + addDynamicFieldLayout(); + // [2] + updateConfigView(_selectedNotification); - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - objManager = pm->getObject(); + initRulesTable(); + initButtons(); + initPhononPlayer(); - // Fills the combo boxes for the UAVObjects - QList< QList > objList = objManager->getDataObjects(); - foreach (QList list, objList) { - foreach (UAVDataObject* obj, list) { - options_page->UAVObject->addItem(obj->getName()); - } - } + int curr_row = _privListNotifications.indexOf(_selectedNotification); + _notifyRulesSelection->setCurrentIndex(_notifyRulesModel->index(curr_row, 0, QModelIndex()), + QItemSelectionModel::ClearAndSelect | QItemSelectionModel::Rows); - connect(options_page->SoundDirectoryPathChooser, SIGNAL(changed(const QString&)), this, SLOT(on_buttonSoundFolder_clicked(const QString&))); - connect(options_page->SoundCollectionList, SIGNAL(currentIndexChanged (int)), this, SLOT(on_soundLanguage_indexChanged(int))); - connect(options_page->buttonAdd, SIGNAL(pressed()), this, SLOT(on_buttonAddNotification_clicked())); - connect(options_page->buttonDelete, SIGNAL(pressed()), this, SLOT(on_buttonDeleteNotification_clicked())); - connect(options_page->buttonModify, SIGNAL(pressed()), this, SLOT(on_buttonModifyNotification_clicked())); -// connect(options_page->buttonTestSound1, SIGNAL(clicked()), this, SLOT(on_buttonTestSound1_clicked())); -// connect(options_page->buttonTestSound2, SIGNAL(clicked()), this, SLOT(on_buttonTestSound2_clicked())); - connect(options_page->buttonPlayNotification, SIGNAL(clicked()), this, SLOT(on_buttonTestSoundNotification_clicked())); - connect(options_page->chkEnableSound, SIGNAL(toggled(bool)), this, SLOT(on_chkEnableSound_toggled(bool))); - - - connect(options_page->UAVObject, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_UAVObject_indexChanged(QString))); - connect(this, SIGNAL(updateNotifications(QList)), - owner, SLOT(updateNotificationList(QList))); - connect(this, SIGNAL(resetNotification()),owner, SLOT(resetNotification())); - - //emit resetNotification(); - - - privListNotifications.clear(); - - for (int i = 0; i < owner->getListNotifications().size(); ++i) { - NotifyPluginConfiguration* notification = new NotifyPluginConfiguration(); - owner->getListNotifications().at(i)->copyTo(notification); - privListNotifications.append(notification); - } - - updateConfigView(owner->getCurrentNotification()); - - options_page->chkEnableSound->setChecked(owner->getEnableSound()); - - QStringList headerStrings; - headerStrings << "Name" << "Repeats" << "Lifetime,sec"; - - notifyRulesModel = new NotifyTableModel(&privListNotifications,headerStrings); - options_page->notifyRulesView->setModel(notifyRulesModel); - options_page->notifyRulesView->resizeRowsToContents(); - notifyRulesSelection = new QItemSelectionModel(notifyRulesModel); - connect(notifyRulesSelection, SIGNAL(selectionChanged ( const QItemSelection &, const QItemSelection & )), - this, SLOT(on_tableNotification_changeSelection( const QItemSelection & , const QItemSelection & ))); - connect(this, SIGNAL(entryUpdated(int)), - notifyRulesModel, SLOT(entryUpdated(int))); - connect(this, SIGNAL(entryAdded(int)), - notifyRulesModel, SLOT(entryAdded(int))); - - options_page->notifyRulesView->setSelectionModel(notifyRulesSelection); - options_page->notifyRulesView->setItemDelegate(new NotifyItemDelegate(delegateItems,this)); - - options_page->notifyRulesView->setColumnWidth(0,200); - options_page->notifyRulesView->setColumnWidth(1,150); - options_page->notifyRulesView->setColumnWidth(2,100); - - options_page->buttonModify->setEnabled(false); - options_page->buttonDelete->setEnabled(false); - options_page->buttonPlayNotification->setEnabled(false); - -// sound1 = Phonon::createPlayer(Phonon::NotificationCategory); -// sound2 = Phonon::createPlayer(Phonon::NotificationCategory); - notifySound = Phonon::createPlayer(Phonon::NotificationCategory); -// audioOutput = new Phonon::AudioOutput(Phonon::NotificationCategory, this); -// Phonon::createPath(sound1, audioOutput); -// Phonon::createPath(sound2, audioOutput); -// Phonon::createPath(notifySound, audioOutput); - -// connect(sound1,SIGNAL(stateChanged(Phonon::State,Phonon::State)),SLOT(changeButtonText(Phonon::State,Phonon::State))); -// connect(sound2,SIGNAL(stateChanged(Phonon::State,Phonon::State)),SLOT(changeButtonText(Phonon::State,Phonon::State))); - connect(notifySound,SIGNAL(stateChanged(Phonon::State,Phonon::State)), - this,SLOT(changeButtonText(Phonon::State,Phonon::State))); - - return optionsPageWidget; + return optionsPageWidget; } -void NotifyPluginOptionsPage::showPersistentComboBox( const QModelIndex & parent, int start, int end ) -{ -// for (int i=start; itableNotifications->openPersistentEditor(options_page->tableNotifications->item(i,1)); -// } -} - -void NotifyPluginOptionsPage::showPersistentComboBox2( const QModelIndex & topLeft, const QModelIndex & bottomRight ) -{ - //for (QModelIndex i=topLeft; itableNotifications->openPersistentEditor(options_page->tableNotifications->item(options_page->tableNotifications->currentRow(),1)); - } -} - - -void NotifyPluginOptionsPage::getOptionsPageValues(NotifyPluginConfiguration* notification) -{ - notification->setSoundCollectionPath(options_page->SoundDirectoryPathChooser->path()); - notification->setCurrentLanguage(options_page->SoundCollectionList->currentText()); - notification->setDataObject(options_page->UAVObject->currentText()); - notification->setObjectField(options_page->UAVObjectField->currentText()); - notification->setSound1(options_page->Sound1->currentText()); - notification->setSound2(options_page->Sound2->currentText()); - notification->setSound3(options_page->Sound3->currentText()); - notification->setSayOrder(options_page->SayOrder->currentText()); - notification->setValue(options_page->Value->currentText()); - notification->setSpinBoxValue(options_page->ValueSpinBox->value()); - -// if(notifyRulesSelection->currentIndex().row()>-1) -// { -// //qDebug() << "delegate value:" << options_page->tableNotifications->item(options_page->tableNotifications->currentRow(),1)->data(Qt::EditRole); -// notification->setRepeatFlag(notifyRulesModel->data(notifyRulesSelection->currentIndex(),Qt::DisplayRole).toString()); -// } -} - -//////////////////////////////////////////// -// Called when the user presses apply or OK. -// -// Saves the current values -// -//////////////////////////////////////////// void NotifyPluginOptionsPage::apply() { - - getOptionsPageValues(owner->getCurrentNotification()); - - owner->setEnableSound(options_page->chkEnableSound->isChecked()); - //owner->setListNotifications(privListNotifications); - emit updateNotifications(privListNotifications); + getOptionsPageValues(_owner->getCurrentNotification()); + _owner->setEnableSound(_optionsPage->chkEnableSound->isChecked()); + emit updateNotifications(_privListNotifications); } void NotifyPluginOptionsPage::finish() { - disconnect(notifySound,SIGNAL(stateChanged(Phonon::State,Phonon::State)), - this,SLOT(changeButtonText(Phonon::State,Phonon::State))); - if(notifySound) - { - notifySound->stop(); - notifySound->clear(); - } + disconnect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), + this, SLOT(on_changedIndex_UAVField(QString))); - delete options_page; -} - - - -////////////////////////////////////////////////////////////////////////////// -// Fills in the combo box when value is changed in the -// combo box -////////////////////////////////////////////////////////////////////////////// -void NotifyPluginOptionsPage::on_UAVObject_indexChanged(QString val) { - options_page->UAVObjectField->clear(); - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(val) ); - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - options_page->UAVObjectField->addItem(field->getName()); + disconnect(_testSound.data(),SIGNAL(stateChanged(Phonon::State,Phonon::State)), + this,SLOT(on_changed_playButtonText(Phonon::State,Phonon::State))); + if (_testSound) { + _testSound->stop(); + _testSound->clear(); } } -// locate collection folder on disk -void NotifyPluginOptionsPage::on_buttonSoundFolder_clicked(const QString& path) +void NotifyPluginOptionsPage::initButtons() { - QDir dirPath(path); - listDirCollections = dirPath.entryList(QDir::AllDirs | QDir::NoDotAndDotDot); - options_page->SoundCollectionList->clear(); - options_page->SoundCollectionList->addItems(listDirCollections); + _optionsPage->chkEnableSound->setChecked(_owner->getEnableSound()); + connect(_optionsPage->chkEnableSound, SIGNAL(toggled(bool)), + this, SLOT(on_toggled_checkEnableSound(bool))); + + _optionsPage->buttonModify->setEnabled(false); + _optionsPage->buttonDelete->setEnabled(false); + _optionsPage->buttonPlayNotification->setEnabled(false); + connect(_optionsPage->buttonAdd, SIGNAL(pressed()), + this, SLOT(on_clicked_buttonAddNotification())); + connect(_optionsPage->buttonDelete, SIGNAL(pressed()), + this, SLOT(on_clicked_buttonDeleteNotification())); + connect(_optionsPage->buttonModify, SIGNAL(pressed()), + this, SLOT(on_clicked_buttonModifyNotification())); + connect(_optionsPage->buttonPlayNotification, SIGNAL(clicked()), + this, SLOT(on_clicked_buttonTestSoundNotification())); } - -void NotifyPluginOptionsPage::on_soundLanguage_indexChanged(int index) +void NotifyPluginOptionsPage::initPhononPlayer() { - options_page->SoundCollectionList->setCurrentIndex(index); + _testSound.reset(Phonon::createPlayer(Phonon::NotificationCategory)); + connect(_testSound.data(),SIGNAL(stateChanged(Phonon::State,Phonon::State)), + this,SLOT(on_changed_playButtonText(Phonon::State,Phonon::State))); + connect(_testSound.data(), SIGNAL(finished(void)), this, SLOT(on_FinishedPlaying(void))); +} - currentCollectionPath = options_page->SoundDirectoryPathChooser->path() + - QDir::toNativeSeparators("/" + options_page->SoundCollectionList->currentText()); +void NotifyPluginOptionsPage::initRulesTable() +{ + qNotifyDebug_if(_notifyRulesModel.isNull()) << "_notifyRulesModel.isNull())"; + qNotifyDebug_if(!_notifyRulesSelection) << "_notifyRulesSelection.isNull())"; + _notifyRulesModel.reset(new NotifyTableModel(_privListNotifications)); + _notifyRulesSelection = new QItemSelectionModel(_notifyRulesModel.data()); - QDir dirPath(currentCollectionPath); - QStringList filters; - filters << "*.mp3" << "*.wav"; - dirPath.setNameFilters(filters); - listSoundFiles = dirPath.entryList(filters); - listSoundFiles.replaceInStrings(QRegExp(".mp3|.wav"), ""); - options_page->Sound1->clear(); - options_page->Sound2->clear(); - options_page->Sound3->clear(); - options_page->Sound1->addItems(listSoundFiles); - options_page->Sound2->addItem(""); - options_page->Sound2->addItems(listSoundFiles); - options_page->Sound3->addItem(""); - options_page->Sound3->addItems(listSoundFiles); + connect(_notifyRulesSelection, SIGNAL(selectionChanged ( const QItemSelection &, const QItemSelection & )), + this, SLOT(on_changedSelection_notifyTable( const QItemSelection & , const QItemSelection & ))); + connect(this, SIGNAL(entryUpdated(int)), + _notifyRulesModel.data(), SLOT(entryUpdated(int))); + + _optionsPage->notifyRulesView->setModel(_notifyRulesModel.data()); + _optionsPage->notifyRulesView->setSelectionModel(_notifyRulesSelection); + _optionsPage->notifyRulesView->setItemDelegate(new NotifyItemDelegate(this)); + + _optionsPage->notifyRulesView->resizeRowsToContents(); + _optionsPage->notifyRulesView->setColumnWidth(eMessageName,200); + _optionsPage->notifyRulesView->setColumnWidth(eRepeatValue,120); + _optionsPage->notifyRulesView->setColumnWidth(eExpireTimer,100); + _optionsPage->notifyRulesView->setColumnWidth(eTurnOn,60); + _optionsPage->notifyRulesView->setDragEnabled(true); + _optionsPage->notifyRulesView->setAcceptDrops(true); + _optionsPage->notifyRulesView->setDropIndicatorShown(true); + _optionsPage->notifyRulesView->setDragDropMode(QAbstractItemView::InternalMove); +} + +UAVObjectField* NotifyPluginOptionsPage::getObjectFieldFromSelected() +{ + return (_currUAVObject) ? _currUAVObject->getField(_selectedNotification->getObjectField()) : NULL; +} + +void NotifyPluginOptionsPage::setSelectedNotification(NotificationItem* ntf) +{ + _selectedNotification = ntf; + _currUAVObject = dynamic_cast(_objManager.getObject(_selectedNotification->getDataObject())); + if(!_currUAVObject) { + qNotifyDebug() << "no such UAVObject: " << _selectedNotification->getDataObject(); + } +} + +void NotifyPluginOptionsPage::addDynamicFieldLayout() +{ + // there is no need to check (objField == NULL), + // thus it doesn't use in this field directly + + QSizePolicy labelSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + labelSizePolicy.setHorizontalStretch(0); + labelSizePolicy.setVerticalStretch(0); +// labelSizePolicy.setHeightForWidth(UAVObject->sizePolicy().hasHeightForWidth()); + + + QLabel* labelSayOrder = new QLabel("Say order ", _form); + labelSayOrder->setSizePolicy(labelSizePolicy); + + _optionsPage->dynamicValueLayout->addWidget(labelSayOrder); + _sayOrder = new QComboBox(_form); + _optionsPage->dynamicValueLayout->addWidget(_sayOrder); + _sayOrder->addItems(NotificationItem::sayOrderValues); + + QLabel* labelValueIs = new QLabel("Value is ", _form); + labelValueIs->setSizePolicy(labelSizePolicy); + _optionsPage->dynamicValueLayout->addWidget(labelValueIs); + + _dynamicFieldCondition = new QComboBox(_form); + _optionsPage->dynamicValueLayout->addWidget(_dynamicFieldCondition); + UAVObjectField* field = getObjectFieldFromSelected(); + addDynamicField(field); +} + +void NotifyPluginOptionsPage::addDynamicField(UAVObjectField* objField) +{ + if(!objField) { + qNotifyDebug() << "addDynamicField | input objField == NULL"; + return; + } + if (objField->getType() == _dynamicFieldType) { + if (QComboBox* fieldValue = dynamic_cast(_dynamicFieldWidget)) { + fieldValue->clear(); + QStringList enumValues(objField->getOptions()); + fieldValue->addItems(enumValues); + } + return; + } + + disconnect(_dynamicFieldCondition, SIGNAL(currentIndexChanged(QString)), + this, SLOT(on_changedIndex_rangeValue(QString))); + + _dynamicFieldCondition->clear(); + _dynamicFieldCondition->addItems(NotifyPluginOptionsPage::conditionValues); + if (UAVObjectField::ENUM == objField->getType()) { + _dynamicFieldCondition->removeItem(smaller); + _dynamicFieldCondition->removeItem(bigger); + } + _dynamicFieldCondition->setCurrentIndex(_dynamicFieldCondition->findText(NotifyPluginOptionsPage::conditionValues.at(_selectedNotification->getCondition()))); + + connect(_dynamicFieldCondition, SIGNAL(currentIndexChanged(QString)), + this, SLOT(on_changedIndex_rangeValue(QString))); + + addDynamicFieldWidget(objField); +} + +void NotifyPluginOptionsPage::addDynamicFieldWidget(UAVObjectField* objField) +{ + if(!objField) { + qNotifyDebug() << "objField == NULL!"; + return; + } + // check if dynamic fileld already settled, + // so if its exists remove it and install new field + if (_dynamicFieldWidget) { + _optionsPage->dynamicValueLayout->removeWidget(_dynamicFieldWidget); + delete _dynamicFieldWidget; + _dynamicFieldWidget = NULL; + } + + QSizePolicy sizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed); + sizePolicy.setHorizontalStretch(0); + sizePolicy.setVerticalStretch(0); + + _dynamicFieldType = objField->getType(); + switch(_dynamicFieldType) + { + case UAVObjectField::ENUM: + { + _dynamicFieldWidget = new QComboBox(_form); + QStringList enumValues(objField->getOptions()); + (dynamic_cast(_dynamicFieldWidget))->addItems(enumValues); + } + break; + + default: + Q_ASSERT(_dynamicFieldCondition); + if (NotifyPluginOptionsPage::conditionValues.indexOf(_dynamicFieldCondition->currentText()) == NotifyPluginOptionsPage::inrange) { + _dynamicFieldWidget = new QLineEdit(_form); + + (static_cast(_dynamicFieldWidget))->setInputMask("#999.99 : #999.99;"); + (static_cast(_dynamicFieldWidget))->setText("0000000000"); + (static_cast(_dynamicFieldWidget))->setCursorPosition(0); + } else { + _dynamicFieldWidget = new QDoubleSpinBox(_form); + (dynamic_cast(_dynamicFieldWidget))->setRange(-999.99, 999.99); + } + break; + }; + enum { eDynamicFieldWidth = 100 }; + _dynamicFieldWidget->setSizePolicy(sizePolicy); + _dynamicFieldWidget->setFixedWidth(eDynamicFieldWidth); + _optionsPage->dynamicValueLayout->addWidget(_dynamicFieldWidget); +} + +void NotifyPluginOptionsPage::setDynamicFieldValue(NotificationItem* notification) +{ + if (QDoubleSpinBox* seValue = dynamic_cast(_dynamicFieldWidget)) + seValue->setValue(notification->singleValue().toDouble()); + else { + if (QComboBox* cbValue = dynamic_cast(_dynamicFieldWidget)) { + int idx = cbValue->findText(notification->singleValue().toString()); + if(-1 != idx) + cbValue->setCurrentIndex(idx); + } else { + if (QLineEdit* rangeValue = dynamic_cast(_dynamicFieldWidget)) { + QString str = QString("%1%2").arg(notification->singleValue().toDouble(), 5, 'f', 2, '0') + .arg(notification->valueRange2(), 5, 'f', 2, '0'); + rangeValue->setText(str); + } else { + qNotifyDebug() << "NotifyPluginOptionsPage::setDynamicValueField | unknown _fieldValue: " << _dynamicFieldWidget; + } + } + } +} + +void NotifyPluginOptionsPage::resetFieldType() +{ + _dynamicFieldType = -1; +} + +void NotifyPluginOptionsPage::getOptionsPageValues(NotificationItem* notification) +{ + Q_ASSERT(notification); + notification->setSoundCollectionPath(_optionsPage->SoundDirectoryPathChooser->path()); + notification->setCurrentLanguage(_optionsPage->SoundCollectionList->currentText()); + notification->setDataObject(_optionsPage->UAVObject->currentText()); + notification->setObjectField(_optionsPage->UAVObjectField->currentText()); + notification->setSound1(_optionsPage->Sound1->currentText()); + notification->setSound2(_optionsPage->Sound2->currentText()); + notification->setSound3(_optionsPage->Sound3->currentText()); + notification->setSayOrder(_sayOrder->currentIndex()); + notification->setCondition(NotifyPluginOptionsPage::conditionValues.indexOf(_dynamicFieldCondition->currentText())); + if (QDoubleSpinBox* spinValue = dynamic_cast(_dynamicFieldWidget)) + notification->setSingleValue(spinValue->value()); + else { + if (QComboBox* comboBoxValue = dynamic_cast(_dynamicFieldWidget)) + notification->setSingleValue(comboBoxValue->currentText()); + else { + if (QLineEdit* rangeValue = dynamic_cast(_dynamicFieldWidget)) { + QString str = rangeValue->text(); + QStringList range = str.split(':'); + notification->setSingleValue(range.at(0).toDouble()); + notification->setValueRange2(range.at(1).toDouble()); + } + } + } +} + +void NotifyPluginOptionsPage::updateConfigView(NotificationItem* notification) +{ + Q_ASSERT(notification); + disconnect(_optionsPage->UAVObject, SIGNAL(currentIndexChanged(QString)), + this, SLOT(on_changedIndex_UAVObject(QString))); + disconnect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), + this, SLOT(on_changedIndex_UAVField(QString))); + + QString path = notification->getSoundCollectionPath(); + if (path.isEmpty()) { + path = Utils::PathUtils().InsertDataPath("%%DATAPATH%%sounds"); + } + + _optionsPage->SoundDirectoryPathChooser->setPath(path); + + if (-1 != _optionsPage->SoundCollectionList->findText(notification->getCurrentLanguage())) { + _optionsPage->SoundCollectionList->setCurrentIndex(_optionsPage->SoundCollectionList->findText(notification->getCurrentLanguage())); + } else { + _optionsPage->SoundCollectionList->setCurrentIndex(_optionsPage->SoundCollectionList->findText("default")); + } + + // Fills the combo boxes for the UAVObjects + QList< QList > objList = _objManager.getDataObjects(); + foreach (QList list, objList) { + foreach (UAVDataObject* obj, list) { + _optionsPage->UAVObject->addItem(obj->getName()); + } + } + + if (-1 != _optionsPage->UAVObject->findText(notification->getDataObject())) { + _optionsPage->UAVObject->setCurrentIndex(_optionsPage->UAVObject->findText(notification->getDataObject())); + } + + _optionsPage->UAVObjectField->clear(); + if(_currUAVObject) { + QList fieldList = _currUAVObject->getFields(); + foreach (UAVObjectField* field, fieldList) + _optionsPage->UAVObjectField->addItem(field->getName()); + } + + if (-1 != _optionsPage->UAVObjectField->findText(notification->getObjectField())) { + _optionsPage->UAVObjectField->setCurrentIndex(_optionsPage->UAVObjectField->findText(notification->getObjectField())); + } + + if (-1 != _optionsPage->Sound1->findText(notification->getSound1())) { + _optionsPage->Sound1->setCurrentIndex(_optionsPage->Sound1->findText(notification->getSound1())); + } else { + // show item from default location + _optionsPage->SoundCollectionList->setCurrentIndex(_optionsPage->SoundCollectionList->findText("default")); + _optionsPage->Sound1->setCurrentIndex(_optionsPage->Sound1->findText(notification->getSound1())); + } + + if (-1 != _optionsPage->Sound2->findText(notification->getSound2())) { + _optionsPage->Sound2->setCurrentIndex(_optionsPage->Sound2->findText(notification->getSound2())); + } else { + // show item from default location + _optionsPage->SoundCollectionList->setCurrentIndex(_optionsPage->SoundCollectionList->findText("default")); + _optionsPage->Sound2->setCurrentIndex(_optionsPage->Sound2->findText(notification->getSound2())); + } + + if (-1 != _optionsPage->Sound3->findText(notification->getSound3())) { + _optionsPage->Sound3->setCurrentIndex(_optionsPage->Sound3->findText(notification->getSound3())); + } else { + // show item from default location + _optionsPage->SoundCollectionList->setCurrentIndex(_optionsPage->SoundCollectionList->findText("default")); + _optionsPage->Sound3->setCurrentIndex(_optionsPage->Sound3->findText(notification->getSound3())); + } + + _dynamicFieldCondition->setCurrentIndex(_dynamicFieldCondition->findText(NotifyPluginOptionsPage::conditionValues.at(notification->getCondition()))); + + _sayOrder->setCurrentIndex(notification->getSayOrder()); + + setDynamicFieldValue(notification); + + connect(_optionsPage->UAVObject, SIGNAL(currentIndexChanged(QString)), + this, SLOT(on_changedIndex_UAVObject(QString))); + connect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), + this, SLOT(on_changedIndex_UAVField(QString))); } -void NotifyPluginOptionsPage::changeButtonText(Phonon::State newstate, Phonon::State oldstate) +void NotifyPluginOptionsPage::on_changedIndex_rangeValue(QString /* rangeStr */) { - if(newstate == Phonon::PausedState || newstate == Phonon::StoppedState){ - options_page->buttonPlayNotification->setText("Play"); - options_page->buttonPlayNotification->setIcon(QPixmap(":/notify/images/play.png")); - } - else - if(newstate == Phonon::PlayingState) { - options_page->buttonPlayNotification->setText("Stop"); - options_page->buttonPlayNotification->setIcon(QPixmap(":/notify/images/stop.png")); - } + Q_ASSERT(_dynamicFieldWidget); + UAVObjectField* field = getObjectFieldFromSelected(); + Q_ASSERT(!!field); + addDynamicFieldWidget(field); + setDynamicFieldValue(_selectedNotification); } - -void NotifyPluginOptionsPage::on_buttonTestSoundNotification_clicked() +void NotifyPluginOptionsPage::on_changedIndex_UAVField(QString field) { - // QList messageNotify; - NotifyPluginConfiguration *notification; - - if(notifyRulesSelection->currentIndex().row()==-1) return; - - notifySound->clearQueue(); - notification = privListNotifications.at(notifyRulesSelection->currentIndex().row()); - notification->parseNotifyMessage(); - foreach(QString item, notification->getNotifyMessageList()) - notifySound->enqueue(Phonon::MediaSource(item)); - notifySound->play(); -} - -void NotifyPluginOptionsPage::on_chkEnableSound_toggled(bool state) -{ - bool state1 = 1^state; - - QList listOutputs = notifySound->outputPaths(); - Phonon::AudioOutput * audioOutput = (Phonon::AudioOutput*)listOutputs.last().sink(); - audioOutput->setMuted(state1); -} - -void NotifyPluginOptionsPage::updateConfigView(NotifyPluginConfiguration* notification) -{ - QString path = notification->getSoundCollectionPath(); - if(path=="") - { - //QDir dir = QDir::currentPath(); - //path = QDir::currentPath().left(QDir::currentPath().indexOf("OpenPilot",0,Qt::CaseSensitive))+"../share/sounds"; - path = Utils::PathUtils().InsertDataPath("%%DATAPATH%%sounds"); - } - options_page->SoundDirectoryPathChooser->setPath(path); - - if(options_page->SoundCollectionList->findText(notification->getCurrentLanguage())!=-1){ - options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText(notification->getCurrentLanguage())); - } - else - options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText("default")); - - - if(options_page->UAVObject->findText(notification->getDataObject())!=-1){ - options_page->UAVObject->setCurrentIndex(options_page->UAVObject->findText(notification->getDataObject())); - } - - // Now load the object field values: - options_page->UAVObjectField->clear(); - QString uavDataObject = notification->getDataObject(); - UAVDataObject* obj = dynamic_cast( objManager->getObject(uavDataObject/*objList.at(0).at(0)->getName()*/) ); - if (obj != NULL ) { - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - options_page->UAVObjectField->addItem(field->getName()); - } - } - - if(options_page->UAVObjectField->findText(notification->getObjectField())!=-1){ - options_page->UAVObjectField->setCurrentIndex(options_page->UAVObjectField->findText(notification->getObjectField())); - } - - if(options_page->Sound1->findText(notification->getSound1())!=-1){ - options_page->Sound1->setCurrentIndex(options_page->Sound1->findText(notification->getSound1())); - } - else - { - // show item from default location - options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText("default")); - options_page->Sound1->setCurrentIndex(options_page->Sound1->findText(notification->getSound1())); - - // don't show item if it wasn't find in stored location - //options_page->Sound1->setCurrentIndex(-1); - } - - if(options_page->Sound2->findText(notification->getSound2())!=-1) { - options_page->Sound2->setCurrentIndex(options_page->Sound2->findText(notification->getSound2())); - } - else { - // show item from default location - options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText("default")); - options_page->Sound2->setCurrentIndex(options_page->Sound2->findText(notification->getSound2())); - - // don't show item if it wasn't find in stored location - //options_page->Sound2->setCurrentIndex(-1); - } - - if(options_page->Sound3->findText(notification->getSound3())!=-1) { - options_page->Sound3->setCurrentIndex(options_page->Sound3->findText(notification->getSound3())); - } - else { - // show item from default location - options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText("default")); - options_page->Sound3->setCurrentIndex(options_page->Sound3->findText(notification->getSound3())); - } - - if(options_page->Value->findText(notification->getValue())!=-1) { - options_page->Value->setCurrentIndex(options_page->Value->findText(notification->getValue())); - } - - if(options_page->SayOrder->findText(notification->getSayOrder())!=-1) { - options_page->SayOrder->setCurrentIndex(options_page->SayOrder->findText(notification->getSayOrder())); - } - options_page->ValueSpinBox->setValue(notification->getSpinBoxValue()); -} - -void NotifyPluginOptionsPage::on_tableNotification_changeSelection( const QItemSelection & selected, const QItemSelection & deselected ) -{ - bool select = true; - notifySound->stop(); - if(selected.indexes().size()) - updateConfigView(privListNotifications.at(selected.indexes().at(0).row())); - else - select = false; - - options_page->buttonModify->setEnabled(select); - options_page->buttonDelete->setEnabled(select); - options_page->buttonPlayNotification->setEnabled(select); -} - - - -void NotifyPluginOptionsPage::on_buttonAddNotification_clicked() -{ - NotifyPluginConfiguration* notification = new NotifyPluginConfiguration; - - if(options_page->SoundDirectoryPathChooser->path()=="") - { - QPalette textPalette=options_page->SoundDirectoryPathChooser->palette(); - textPalette.setColor(QPalette::Normal,QPalette::Text, Qt::red); - options_page->SoundDirectoryPathChooser->setPalette(textPalette); - options_page->SoundDirectoryPathChooser->setPath("please select sound collection folder"); - return; - } - - notification->setSoundCollectionPath(options_page->SoundDirectoryPathChooser->path()); - notification->setCurrentLanguage(options_page->SoundCollectionList->currentText()); - notification->setDataObject(options_page->UAVObject->currentText()); - notification->setObjectField(options_page->UAVObjectField->currentText()); - notification->setValue(options_page->Value->currentText()); - notification->setSpinBoxValue(options_page->ValueSpinBox->value()); - - if(options_page->Sound1->currentText()!="") - notification->setSound1(options_page->Sound1->currentText()); - - notification->setSound2(options_page->Sound2->currentText()); - notification->setSound3(options_page->Sound3->currentText()); - - if(((options_page->Sound2->currentText()=="")&&(options_page->SayOrder->currentText()=="After second")) - || ((options_page->Sound3->currentText()=="")&&(options_page->SayOrder->currentText()=="After third"))) - return; - else - notification->setSayOrder(options_page->SayOrder->currentText()); - - privListNotifications.append(notification); - emit entryAdded(privListNotifications.size()-1); - notifyRulesSelection->setCurrentIndex(notifyRulesModel->index(privListNotifications.size()-1,0,QModelIndex()), - QItemSelectionModel::ClearAndSelect | QItemSelectionModel::Rows); -} - - -void NotifyPluginOptionsPage::on_buttonDeleteNotification_clicked() -{ - notifyRulesModel->removeRow(notifyRulesSelection->currentIndex().row()); - if(!notifyRulesModel->rowCount() && (notifyRulesSelection->currentIndex().row() > 0 && notifyRulesSelection->currentIndex().row() < notifyRulesModel->rowCount())) - { - options_page->buttonDelete->setEnabled(false); - options_page->buttonModify->setEnabled(false); - options_page->buttonPlayNotification->setEnabled(false); - } + resetFieldType(); + Q_ASSERT(_currUAVObject); + addDynamicField(_currUAVObject->getField(field)); } -void NotifyPluginOptionsPage::on_buttonModifyNotification_clicked() +void NotifyPluginOptionsPage::on_changedIndex_UAVObject(QString val) { - NotifyPluginConfiguration* notification = new NotifyPluginConfiguration; - getOptionsPageValues(notification); - notification->setRepeatFlag(privListNotifications.at(notifyRulesSelection->currentIndex().row())->getRepeatFlag()); - privListNotifications.replace(notifyRulesSelection->currentIndex().row(),notification); - entryUpdated(notifyRulesSelection->currentIndex().row()); - + resetFieldType(); + _currUAVObject = dynamic_cast( _objManager.getObject(val) ); + if(!_currUAVObject) { + qNotifyDebug() << "on_UAVObject_indexChanged | no such UAVOBject"; + return; + } + QList fieldList = _currUAVObject->getFields(); + disconnect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_changedIndex_UAVField(QString))); + _optionsPage->UAVObjectField->clear(); + foreach (UAVObjectField* field, fieldList) { + _optionsPage->UAVObjectField->addItem(field->getName()); + } + connect(_optionsPage->UAVObjectField, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_changedIndex_UAVField(QString))); + _selectedNotification->setObjectField(fieldList.at(0)->getName()); + addDynamicField(fieldList.at(0)); } + +void NotifyPluginOptionsPage::on_changedIndex_soundLanguage(int index) +{ + _optionsPage->SoundCollectionList->setCurrentIndex(index); + QString collectionPath = _optionsPage->SoundDirectoryPathChooser->path() + + QDir::toNativeSeparators("/" + _optionsPage->SoundCollectionList->currentText()); + + QDir dirPath(collectionPath); + QStringList filters; + filters << "*.mp3" << "*.wav"; + dirPath.setNameFilters(filters); + QStringList listSoundFiles = dirPath.entryList(filters); + listSoundFiles.replaceInStrings(QRegExp(".mp3|.wav"), ""); + _optionsPage->Sound1->clear(); + _optionsPage->Sound2->clear(); + _optionsPage->Sound3->clear(); + _optionsPage->Sound1->addItem(""); + _optionsPage->Sound1->addItems(listSoundFiles); + _optionsPage->Sound2->addItem(""); + _optionsPage->Sound2->addItems(listSoundFiles); + _optionsPage->Sound3->addItem(""); + _optionsPage->Sound3->addItems(listSoundFiles); +} + + +void NotifyPluginOptionsPage::on_changed_playButtonText(Phonon::State newstate, Phonon::State /* oldstate */) +{ + //Q_ASSERT(Phonon::ErrorState != newstate); + + if (newstate == Phonon::PausedState || newstate == Phonon::StoppedState) { + _optionsPage->buttonPlayNotification->setText("Play"); + _optionsPage->buttonPlayNotification->setIcon(QPixmap(":/notify/images/play.png")); + } else { + if (newstate == Phonon::PlayingState) { + _optionsPage->buttonPlayNotification->setText("Stop"); + _optionsPage->buttonPlayNotification->setIcon(QPixmap(":/notify/images/stop.png")); + } + } +} + +void NotifyPluginOptionsPage::on_changedSelection_notifyTable(const QItemSelection & selected, const QItemSelection & /* deselected */ ) +{ + bool select = false; + _testSound->stop(); + if (selected.indexes().size()) { + select = true; + setSelectedNotification(_privListNotifications.at(selected.indexes().at(0).row())); + UAVObjectField* field = getObjectFieldFromSelected(); + addDynamicField(field); + updateConfigView(_selectedNotification); + } + + _optionsPage->buttonModify->setEnabled(select); + _optionsPage->buttonDelete->setEnabled(select); + _optionsPage->buttonPlayNotification->setEnabled(select); +} + +void NotifyPluginOptionsPage::on_clicked_buttonSoundFolder(const QString& path) +{ + QDir dirPath(path); + QStringList listDirCollections = dirPath.entryList(QDir::AllDirs | QDir::NoDotAndDotDot); + _optionsPage->SoundCollectionList->clear(); + _optionsPage->SoundCollectionList->addItems(listDirCollections); +} + +void NotifyPluginOptionsPage::on_clicked_buttonTestSoundNotification() +{ + NotificationItem* notification = NULL; + qNotifyDebug() << "on_buttonTestSoundNotification_clicked"; + Q_ASSERT(-1 != _notifyRulesSelection->currentIndex().row()); + _testSound->clearQueue(); + notification = _privListNotifications.at(_notifyRulesSelection->currentIndex().row()); + QStringList sequence = notification->toSoundList(); + if (sequence.isEmpty()) { + qNotifyDebug() << "message sequense is empty!"; + return; + } + foreach(QString item, sequence) { + qNotifyDebug() << item; + _testSound->enqueue(Phonon::MediaSource(item)); + } + _testSound->play(); +} + +void NotifyPluginOptionsPage::on_clicked_buttonAddNotification() +{ + NotificationItem* notification = new NotificationItem; + + if (_optionsPage->SoundDirectoryPathChooser->path().isEmpty()) { + QPalette textPalette=_optionsPage->SoundDirectoryPathChooser->palette(); + textPalette.setColor(QPalette::Normal,QPalette::Text, Qt::red); + _optionsPage->SoundDirectoryPathChooser->setPalette(textPalette); + _optionsPage->SoundDirectoryPathChooser->setPath("please select sound collection folder"); + delete notification; + return; + } + getOptionsPageValues(notification); + + if ( ((!_optionsPage->Sound2->currentText().isEmpty()) && (_sayOrder->currentText()=="After second")) + || ((!_optionsPage->Sound3->currentText().isEmpty()) && (_sayOrder->currentText()=="After third")) ) { + delete notification; + return; + } else { + notification->setSayOrder(_sayOrder->currentIndex()); + } + + _notifyRulesModel->entryAdded(notification); + _notifyRulesSelection->setCurrentIndex(_notifyRulesModel->index(_privListNotifications.size()-1,0,QModelIndex()), + QItemSelectionModel::ClearAndSelect | QItemSelectionModel::Rows); +} + +void NotifyPluginOptionsPage::on_clicked_buttonDeleteNotification() +{ + _notifyRulesModel->removeRow(_notifyRulesSelection->currentIndex().row()); + if (!_notifyRulesModel->rowCount() + && (_notifyRulesSelection->currentIndex().row() > 0 + && _notifyRulesSelection->currentIndex().row() < _notifyRulesModel->rowCount()) ) + { + _optionsPage->buttonDelete->setEnabled(false); + _optionsPage->buttonModify->setEnabled(false); + _optionsPage->buttonPlayNotification->setEnabled(false); + } +} + +void NotifyPluginOptionsPage::on_clicked_buttonModifyNotification() +{ + NotificationItem* notification = new NotificationItem; + getOptionsPageValues(notification); + notification->setRetryValue(_privListNotifications.at(_notifyRulesSelection->currentIndex().row())->retryValue()); + notification->setLifetime(_privListNotifications.at(_notifyRulesSelection->currentIndex().row())->lifetime()); + notification->setMute(_privListNotifications.at(_notifyRulesSelection->currentIndex().row())->mute()); + + _privListNotifications.replace(_notifyRulesSelection->currentIndex().row(),notification); + entryUpdated(_notifyRulesSelection->currentIndex().row()); +} + +void NotifyPluginOptionsPage::on_FinishedPlaying() +{ + _testSound->clear(); +} + +void NotifyPluginOptionsPage::on_toggled_checkEnableSound(bool state) +{ + bool state1 = 1^state; + + QList listOutputs = _testSound->outputPaths(); + Phonon::AudioOutput * audioOutput = (Phonon::AudioOutput*)listOutputs.last().sink(); + audioOutput->setMuted(state1); +} diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp.orig b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp.orig new file mode 100644 index 000000000..2ff775e31 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.cpp.orig @@ -0,0 +1,512 @@ +/** + ****************************************************************************** + * + * @file notifypluginoptionspage.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Notify Plugin options page + * @see The GNU Public License (GPL) Version 3 + * @defgroup notifyplugin + * @{ + * + *****************************************************************************/ +/* + * 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 + */ + +#include "notifypluginoptionspage.h" +#include +#include "notificationitem.h" +#include "ui_notifypluginoptionspage.h" +#include "extensionsystem/pluginmanager.h" +#include "utils/pathutils.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "notifyplugin.h" +#include "notifyitemdelegate.h" +#include "notifytablemodel.h" +#include "notifylogging.h" + +NotifyPluginOptionsPage::NotifyPluginOptionsPage(/*NotificationItem *config,*/ QObject *parent) + : IOptionsPage(parent) + , objManager(*ExtensionSystem::PluginManager::instance()->getObject()) + , owner(qobject_cast(parent)) + , currentCollectionPath("") +{ +} + +NotifyPluginOptionsPage::~NotifyPluginOptionsPage() +{ +} + +//creates options page widget (uses the UI file) +QWidget *NotifyPluginOptionsPage::createPage(QWidget *parent) +{ + options_page.reset(new Ui::NotifyPluginOptionsPage()); + //main widget + QWidget *optionsPageWidget = new QWidget; + //main layout + options_page->setupUi(optionsPageWidget); + + listSoundFiles.clear(); + + options_page->SoundDirectoryPathChooser->setExpectedKind(Utils::PathChooser::Directory); + options_page->SoundDirectoryPathChooser->setPromptDialogTitle(tr("Choose sound collection directory")); + + // Fills the combo boxes for the UAVObjects + QList< QList > objList = objManager.getDataObjects(); + foreach (QList list, objList) { + foreach (UAVDataObject* obj, list) { + options_page->UAVObject->addItem(obj->getName()); + } + } + +<<<<<<< Updated upstream + connect(options_page->SoundDirectoryPathChooser, SIGNAL(changed(const QString&)), this, SLOT(on_buttonSoundFolder_clicked(const QString&))); + connect(options_page->SoundCollectionList, SIGNAL(currentIndexChanged (int)), this, SLOT(on_soundLanguage_indexChanged(int))); + connect(options_page->UAVObject, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_UAVObject_indexChanged(QString))); +======= + options_page = new Ui::NotifyPluginOptionsPage(); + //main widget + QWidget *optionsPageWidget = new QWidget; + //main layout + options_page->setupUi(optionsPageWidget); + + delegateItems.clear(); + listSoundFiles.clear(); + + options_page->horizontalLayout_3->addWidget(new QPushButton("testtt")); + delegateItems << "Repeat Once" + << "Repeat Instantly" + << "Repeat 10 seconds" + << "Repeat 30 seconds" + << "Repeat 1 minute"; + + options_page->chkEnableSound->setChecked(owner->getEnableSound()); + options_page->SoundDirectoryPathChooser->setExpectedKind(Utils::PathChooser::Directory); + options_page->SoundDirectoryPathChooser->setPromptDialogTitle(tr("Choose sound collection directory")); + + + + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + objManager = pm->getObject(); + + // Fills the combo boxes for the UAVObjects + QList< QList > objList = objManager->getDataObjects(); + foreach (QList list, objList) { + foreach (UAVDataObject* obj, list) { + options_page->UAVObject->addItem(obj->getName()); + } + } + + connect(options_page->SoundDirectoryPathChooser, SIGNAL(changed(const QString&)), this, SLOT(on_buttonSoundFolder_clicked(const QString&))); + connect(options_page->SoundCollectionList, SIGNAL(currentIndexChanged (int)), this, SLOT(on_soundLanguage_indexChanged(int))); + connect(options_page->buttonAdd, SIGNAL(pressed()), this, SLOT(on_buttonAddNotification_clicked())); + connect(options_page->buttonDelete, SIGNAL(pressed()), this, SLOT(on_buttonDeleteNotification_clicked())); + connect(options_page->buttonModify, SIGNAL(pressed()), this, SLOT(on_buttonModifyNotification_clicked())); +// connect(options_page->buttonTestSound1, SIGNAL(clicked()), this, SLOT(on_buttonTestSound1_clicked())); +// connect(options_page->buttonTestSound2, SIGNAL(clicked()), this, SLOT(on_buttonTestSound2_clicked())); + connect(options_page->buttonPlayNotification, SIGNAL(clicked()), this, SLOT(on_buttonTestSoundNotification_clicked())); + connect(options_page->chkEnableSound, SIGNAL(toggled(bool)), this, SLOT(on_chkEnableSound_toggled(bool))); + + + connect(options_page->UAVObject, SIGNAL(currentIndexChanged(QString)), this, SLOT(on_UAVObject_indexChanged(QString))); + connect(this, SIGNAL(updateNotifications(QList)), + owner, SLOT(updateNotificationList(QList))); + connect(this, SIGNAL(resetNotification()),owner, SLOT(resetNotification())); + + //emit resetNotification(); + + + privListNotifications.clear(); + + for (int i = 0; i < owner->getListNotifications().size(); ++i) { + NotifyPluginConfiguration* notification = new NotifyPluginConfiguration(); + owner->getListNotifications().at(i)->copyTo(notification); + privListNotifications.append(notification); + } + + updateConfigView(owner->getCurrentNotification()); + + options_page->chkEnableSound->setChecked(owner->getEnableSound()); + + QStringList headerStrings; + headerStrings << "Name" << "Repeats" << "Lifetime,sec"; +>>>>>>> Stashed changes + + connect(this, SIGNAL(updateNotifications(QList)), + owner, SLOT(updateNotificationList(QList))); + //connect(this, SIGNAL(resetNotification()),owner, SLOT(resetNotification())); + + +// privListNotifications = ((qobject_cast(parent))->getListNotifications()); + privListNotifications = owner->getListNotifications(); + + updateConfigView(owner->getCurrentNotification()); + + initRulesTable(); + initButtons(); + initPhononPlayer(); + + return optionsPageWidget; +} + +void NotifyPluginOptionsPage::initButtons() +{ + options_page->chkEnableSound->setChecked(owner->getEnableSound()); + connect(options_page->chkEnableSound, SIGNAL(toggled(bool)), this, SLOT(on_chkEnableSound_toggled(bool))); + + options_page->buttonModify->setEnabled(false); + options_page->buttonDelete->setEnabled(false); + options_page->buttonPlayNotification->setEnabled(false); + connect(options_page->buttonAdd, SIGNAL(pressed()), this, SLOT(on_buttonAddNotification_clicked())); + connect(options_page->buttonDelete, SIGNAL(pressed()), this, SLOT(on_buttonDeleteNotification_clicked())); + connect(options_page->buttonModify, SIGNAL(pressed()), this, SLOT(on_buttonModifyNotification_clicked())); + connect(options_page->buttonPlayNotification, SIGNAL(clicked()), this, SLOT(on_buttonTestSoundNotification_clicked())); +} + +void NotifyPluginOptionsPage::initPhononPlayer() +{ + notifySound.reset(Phonon::createPlayer(Phonon::NotificationCategory)); + connect(notifySound.data(),SIGNAL(stateChanged(Phonon::State,Phonon::State)), + this,SLOT(changeButtonText(Phonon::State,Phonon::State))); + connect(notifySound.data(), SIGNAL(finished(void)), this, SLOT(onFinishedPlaying(void))); +} + +void NotifyPluginOptionsPage::initRulesTable() +{ + qNotifyDebug_if(_notifyRulesModel.isNull()) << "_notifyRulesModel.isNull())"; + qNotifyDebug_if(!_notifyRulesSelection) << "_notifyRulesSelection.isNull())"; + //QItemSelectionModel* selection = _notifyRulesSelection.take(); + _notifyRulesModel.reset(new NotifyTableModel(privListNotifications)); + _notifyRulesSelection = new QItemSelectionModel(_notifyRulesModel.data()); + + connect(_notifyRulesSelection, SIGNAL(selectionChanged ( const QItemSelection &, const QItemSelection & )), + this, SLOT(on_tableNotification_changeSelection( const QItemSelection & , const QItemSelection & ))); + connect(this, SIGNAL(entryUpdated(int)), + _notifyRulesModel.data(), SLOT(entryUpdated(int))); + connect(this, SIGNAL(entryAdded(int)), + _notifyRulesModel.data(), SLOT(entryAdded(int))); + + options_page->notifyRulesView->setModel(_notifyRulesModel.data()); + options_page->notifyRulesView->setSelectionModel(_notifyRulesSelection); + options_page->notifyRulesView->setItemDelegate(new NotifyItemDelegate(this)); + + options_page->notifyRulesView->resizeRowsToContents(); + options_page->notifyRulesView->setColumnWidth(eMESSAGE_NAME,200); + options_page->notifyRulesView->setColumnWidth(eREPEAT_VALUE,120); + options_page->notifyRulesView->setColumnWidth(eEXPIRE_TIME,100); + options_page->notifyRulesView->setColumnWidth(eENABLE_NOTIFICATION,60); + options_page->notifyRulesView->setDragEnabled(true); + options_page->notifyRulesView->setAcceptDrops(true); + options_page->notifyRulesView->setDropIndicatorShown(true); + options_page->notifyRulesView->setDragDropMode(QAbstractItemView::InternalMove); + + +} + +void NotifyPluginOptionsPage::getOptionsPageValues(NotificationItem* notification) +{ + notification->setSoundCollectionPath(options_page->SoundDirectoryPathChooser->path()); + notification->setCurrentLanguage(options_page->SoundCollectionList->currentText()); + notification->setDataObject(options_page->UAVObject->currentText()); + notification->setObjectField(options_page->UAVObjectField->currentText()); + notification->setSound1(options_page->Sound1->currentText()); + notification->setSound2(options_page->Sound2->currentText()); + notification->setSound3(options_page->Sound3->currentText()); + notification->setSayOrder(options_page->SayOrder->currentText()); + notification->setValue(options_page->Value->currentText()); + notification->setSpinBoxValue(options_page->ValueSpinBox->value()); +} + +/*! +* Called when the user presses apply or OK. +* Saves the current values +*/ +void NotifyPluginOptionsPage::apply() +{ + getOptionsPageValues(owner->getCurrentNotification()); + owner->setEnableSound(options_page->chkEnableSound->isChecked()); + emit updateNotifications(privListNotifications); +} + +void NotifyPluginOptionsPage::finish() +{ + disconnect(notifySound.data(),SIGNAL(stateChanged(Phonon::State,Phonon::State)), + this,SLOT(changeButtonText(Phonon::State,Phonon::State))); + if (notifySound) { + notifySound->stop(); + notifySound->clear(); + } +} + +////////////////////////////////////////////////////////////////////////////// +// Fills in the combo box when value is changed in the +// combo box +////////////////////////////////////////////////////////////////////////////// +void NotifyPluginOptionsPage::on_UAVObject_indexChanged(QString val) { + options_page->UAVObjectField->clear(); + ExtensionSystem::PluginManager* pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager* objManager = pm->getObject(); + UAVDataObject* obj = dynamic_cast( objManager->getObject(val) ); + QList fieldList = obj->getFields(); + foreach (UAVObjectField* field, fieldList) { + options_page->UAVObjectField->addItem(field->getName()); + } +} + +// locate collection folder on disk +void NotifyPluginOptionsPage::on_buttonSoundFolder_clicked(const QString& path) +{ + QDir dirPath(path); + listDirCollections = dirPath.entryList(QDir::AllDirs | QDir::NoDotAndDotDot); + options_page->SoundCollectionList->clear(); + options_page->SoundCollectionList->addItems(listDirCollections); +} + + +void NotifyPluginOptionsPage::on_soundLanguage_indexChanged(int index) +{ + options_page->SoundCollectionList->setCurrentIndex(index); + + currentCollectionPath = options_page->SoundDirectoryPathChooser->path() + + QDir::toNativeSeparators("/" + options_page->SoundCollectionList->currentText()); + + QDir dirPath(currentCollectionPath); + QStringList filters; + filters << "*.mp3" << "*.wav"; + dirPath.setNameFilters(filters); + listSoundFiles = dirPath.entryList(filters); + listSoundFiles.replaceInStrings(QRegExp(".mp3|.wav"), ""); + options_page->Sound1->clear(); + options_page->Sound2->clear(); + options_page->Sound3->clear(); + options_page->Sound1->addItems(listSoundFiles); + options_page->Sound2->addItem(""); + options_page->Sound2->addItems(listSoundFiles); + options_page->Sound3->addItem(""); + options_page->Sound3->addItems(listSoundFiles); + +} + +void NotifyPluginOptionsPage::changeButtonText(Phonon::State newstate, Phonon::State oldstate) +{ + //Q_ASSERT(Phonon::ErrorState != newstate); + + if (newstate == Phonon::PausedState || newstate == Phonon::StoppedState) { + options_page->buttonPlayNotification->setText("Play"); + options_page->buttonPlayNotification->setIcon(QPixmap(":/notify/images/play.png")); + } else { + if (newstate == Phonon::PlayingState) { + options_page->buttonPlayNotification->setText("Stop"); + options_page->buttonPlayNotification->setIcon(QPixmap(":/notify/images/stop.png")); + } + } +} + +void NotifyPluginOptionsPage::onFinishedPlaying() +{ + notifySound->clear(); +} + +void NotifyPluginOptionsPage::on_buttonTestSoundNotification_clicked() +{ + NotificationItem* notification = NULL; + + if (-1 == _notifyRulesSelection->currentIndex().row()) + return; + notifySound->clearQueue(); + notification = privListNotifications.at(_notifyRulesSelection->currentIndex().row()); + notification->parseNotifyMessage(); + QStringList sequence = notification->getMessageSequence(); + Q_ASSERT(!!sequence.size()); + foreach(QString item, sequence) + notifySound->enqueue(Phonon::MediaSource(item)); + + notifySound->play(); +} + +void NotifyPluginOptionsPage::on_chkEnableSound_toggled(bool state) +{ + bool state1 = 1^state; + + QList listOutputs = notifySound->outputPaths(); + Phonon::AudioOutput * audioOutput = (Phonon::AudioOutput*)listOutputs.last().sink(); + audioOutput->setMuted(state1); +} + +void NotifyPluginOptionsPage::updateConfigView(NotificationItem* notification) +{ + QString path = notification->getSoundCollectionPath(); + if (path == "") { + //QDir dir = QDir::currentPath(); + //path = QDir::currentPath().left(QDir::currentPath().indexOf("OpenPilot",0,Qt::CaseSensitive))+"../share/sounds"; + path = Utils::PathUtils().InsertDataPath("%%DATAPATH%%sounds"); + } + + options_page->SoundDirectoryPathChooser->setPath(path); + + if (-1 != options_page->SoundCollectionList->findText(notification->getCurrentLanguage())){ + options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText(notification->getCurrentLanguage())); + } else { + options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText("default")); + } + + if (options_page->UAVObject->findText(notification->getDataObject())!=-1){ + options_page->UAVObject->setCurrentIndex(options_page->UAVObject->findText(notification->getDataObject())); + } + + // Now load the object field values: + options_page->UAVObjectField->clear(); + QString uavDataObject = notification->getDataObject(); + UAVDataObject* obj = dynamic_cast(objManager.getObject(uavDataObject)); + if (obj != NULL ) { + QList fieldList = obj->getFields(); + foreach (UAVObjectField* field, fieldList) { + options_page->UAVObjectField->addItem(field->getName()); + } + } + + if (-1 != options_page->UAVObjectField->findText(notification->getObjectField())) { + options_page->UAVObjectField->setCurrentIndex(options_page->UAVObjectField->findText(notification->getObjectField())); + } + + if (-1 != options_page->Sound1->findText(notification->getSound1())) { + options_page->Sound1->setCurrentIndex(options_page->Sound1->findText(notification->getSound1())); + } else { + // show item from default location + options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText("default")); + options_page->Sound1->setCurrentIndex(options_page->Sound1->findText(notification->getSound1())); + + // don't show item if it wasn't find in stored location + //options_page->Sound1->setCurrentIndex(-1); + } + + if (-1 != options_page->Sound2->findText(notification->getSound2())) { + options_page->Sound2->setCurrentIndex(options_page->Sound2->findText(notification->getSound2())); + } else { + // show item from default location + options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText("default")); + options_page->Sound2->setCurrentIndex(options_page->Sound2->findText(notification->getSound2())); + + // don't show item if it wasn't find in stored location + //options_page->Sound2->setCurrentIndex(-1); + } + + if (-1 != options_page->Sound3->findText(notification->getSound3())) { + options_page->Sound3->setCurrentIndex(options_page->Sound3->findText(notification->getSound3())); + } else { + // show item from default location + options_page->SoundCollectionList->setCurrentIndex(options_page->SoundCollectionList->findText("default")); + options_page->Sound3->setCurrentIndex(options_page->Sound3->findText(notification->getSound3())); + } + + if (-1 != options_page->Value->findText(notification->getValue())) { + options_page->Value->setCurrentIndex(options_page->Value->findText(notification->getValue())); + } + + if (-1 != options_page->SayOrder->findText(notification->getSayOrder())) { + options_page->SayOrder->setCurrentIndex(options_page->SayOrder->findText(notification->getSayOrder())); + } + + options_page->ValueSpinBox->setValue(notification->getSpinBoxValue()); +} + +void NotifyPluginOptionsPage::on_tableNotification_changeSelection( const QItemSelection & selected, const QItemSelection & deselected ) +{ + bool select = false; + notifySound->stop(); + if (selected.indexes().size()) { + select = true; + updateConfigView(privListNotifications.at(selected.indexes().at(0).row())); + } + + options_page->buttonModify->setEnabled(select); + options_page->buttonDelete->setEnabled(select); + options_page->buttonPlayNotification->setEnabled(select); +} + + +void NotifyPluginOptionsPage::on_buttonAddNotification_clicked() +{ + NotificationItem* notification = new NotificationItem; + + if (options_page->SoundDirectoryPathChooser->path()=="") { + QPalette textPalette=options_page->SoundDirectoryPathChooser->palette(); + textPalette.setColor(QPalette::Normal,QPalette::Text, Qt::red); + options_page->SoundDirectoryPathChooser->setPalette(textPalette); + options_page->SoundDirectoryPathChooser->setPath("please select sound collection folder"); + return; + } + + notification->setSoundCollectionPath(options_page->SoundDirectoryPathChooser->path()); + notification->setCurrentLanguage(options_page->SoundCollectionList->currentText()); + notification->setDataObject(options_page->UAVObject->currentText()); + notification->setObjectField(options_page->UAVObjectField->currentText()); + notification->setValue(options_page->Value->currentText()); + notification->setSpinBoxValue(options_page->ValueSpinBox->value()); + + if (options_page->Sound1->currentText().size() > 0) + notification->setSound1(options_page->Sound1->currentText()); + + notification->setSound2(options_page->Sound2->currentText()); + notification->setSound3(options_page->Sound3->currentText()); + +if ( ((!options_page->Sound2->currentText().size()) && (options_page->SayOrder->currentText()=="After second")) + || ((!options_page->Sound3->currentText().size()) && (options_page->SayOrder->currentText()=="After third")) ) { + return; + } else { + notification->setSayOrder(options_page->SayOrder->currentText()); + } + privListNotifications.append(notification); + emit entryAdded(privListNotifications.size() - 1); + _notifyRulesSelection->setCurrentIndex(_notifyRulesModel->index(privListNotifications.size()-1,0,QModelIndex()), + QItemSelectionModel::ClearAndSelect | QItemSelectionModel::Rows); +} + + +void NotifyPluginOptionsPage::on_buttonDeleteNotification_clicked() +{ + _notifyRulesModel->removeRow(_notifyRulesSelection->currentIndex().row()); + if (!_notifyRulesModel->rowCount() + && (_notifyRulesSelection->currentIndex().row() > 0 + && _notifyRulesSelection->currentIndex().row() < _notifyRulesModel->rowCount()) ) + { + options_page->buttonDelete->setEnabled(false); + options_page->buttonModify->setEnabled(false); + options_page->buttonPlayNotification->setEnabled(false); + } + +} + +void NotifyPluginOptionsPage::on_buttonModifyNotification_clicked() +{ + NotificationItem* notification = new NotificationItem; + getOptionsPageValues(notification); + notification->setRetryString(privListNotifications.at(_notifyRulesSelection->currentIndex().row())->retryString()); + notification->setLifetime(privListNotifications.at(_notifyRulesSelection->currentIndex().row())->lifetime()); + notification->setMute(privListNotifications.at(_notifyRulesSelection->currentIndex().row())->mute()); + + privListNotifications.replace(_notifyRulesSelection->currentIndex().row(),notification); + entryUpdated(_notifyRulesSelection->currentIndex().row()); + +} + diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.h b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.h index 9d159e2a4..fb3c2885d 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.h +++ b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.h @@ -42,83 +42,146 @@ #include #include #include +#include +#include class NotifyTableModel; - -class NotifyPluginConfiguration; +class NotificationItem; class SoundNotifyPlugin; namespace Ui { - class NotifyPluginOptionsPage; -} - + class NotifyPluginOptionsPage; +}; using namespace Core; class NotifyPluginOptionsPage : public IOptionsPage { - Q_OBJECT + Q_OBJECT + public: - explicit NotifyPluginOptionsPage(/*NotifyPluginConfiguration *config, */QObject *parent = 0); - - QString id() const { return QLatin1String("settings"); } - QString trName() const { return tr("settings"); } - QString category() const { return QLatin1String("Notify Plugin");} - QString trCategory() const { return tr("Notify Plugin");} - - + enum {equal,bigger,smaller,inrange}; + explicit NotifyPluginOptionsPage(QObject *parent = 0); + ~NotifyPluginOptionsPage(); + QString id() const { return QLatin1String("settings"); } + QString trName() const { return tr("settings"); } + QString category() const { return QLatin1String("Notify Plugin");} + QString trCategory() const { return tr("Notify Plugin");} QWidget *createPage(QWidget *parent); void apply(); - void finish(); - void restoreFromSettings(); - - void updateConfigView(NotifyPluginConfiguration* notification); - void getOptionsPageValues(NotifyPluginConfiguration* notification); - -private: - UAVObjectManager *objManager; - SoundNotifyPlugin* owner; - QStringList listDirCollections; - QStringList listSoundFiles; - QString currentCollectionPath; - int sizeNotifyList; - Phonon::MediaObject *sound1; - Phonon::MediaObject *sound2; - Phonon::MediaObject *notifySound; - Phonon::AudioOutput *audioOutput; - QStringList delegateItems; - NotifyTableModel* notifyRulesModel; - QItemSelectionModel *notifyRulesSelection; - QList privListNotifications; - - Ui::NotifyPluginOptionsPage *options_page; - //NotifyPluginConfiguration *notify; + void finish(); + void restoreFromSettings(); + static QStringList conditionValues; signals: - void updateNotifications(QList list); - void resetNotification(void); - void entryUpdated(int index); - void entryAdded(int position); - + void updateNotifications(QList list); + void entryUpdated(int index); private slots: - void showPersistentComboBox( const QModelIndex & parent, int start, int end ); - void showPersistentComboBox2 ( const QModelIndex & topLeft, const QModelIndex & bottomRight ); + void on_clicked_buttonTestSoundNotification(); + void on_clicked_buttonAddNotification(); + void on_clicked_buttonDeleteNotification(); + void on_clicked_buttonModifyNotification(); -// void on_buttonTestSound1_clicked(); -// void on_buttonTestSound2_clicked(); - void on_buttonTestSoundNotification_clicked(); + /** + * We can use continuous selection, to select simultaneously + * multiple rows to move them(using drag & drop) inside table ranges. + */ + void on_changedSelection_notifyTable( const QItemSelection & selected, const QItemSelection & deselected ); + + void on_changedIndex_soundLanguage(int index); + void on_clicked_buttonSoundFolder(const QString& path); + void on_changedIndex_UAVObject(QString val); + void on_changedIndex_UAVField(QString val); + void on_changed_playButtonText(Phonon::State newstate, Phonon::State oldstate); + void on_toggled_checkEnableSound(bool state); + + /** + * Important when we change to or from "In range" value + * For enums UI layout stayed the same, but for numeric values + * we need to change UI to show edit line, + * to have possibility assign range limits for value. + */ + void on_changedIndex_rangeValue(QString); + + void on_FinishedPlaying(void); + + +private: + Q_DISABLE_COPY(NotifyPluginOptionsPage) + + void initButtons(); + void initPhononPlayer(); + void initRulesTable(); + + void setSelectedNotification(NotificationItem* ntf); + void resetValueRange(); + void resetFieldType(); + + void updateConfigView(NotificationItem* notification); + void getOptionsPageValues(NotificationItem* notification); + UAVObjectField* getObjectFieldFromPage(); + UAVObjectField* getObjectFieldFromSelected(); + + void addDynamicFieldLayout(); + void addDynamicField(UAVObjectField* objField); + void addDynamicFieldWidget(UAVObjectField* objField); + void setDynamicFieldValue(NotificationItem* notification); + +private: + + UAVObjectManager& _objManager; + SoundNotifyPlugin* _owner; + + //! Media object uses to test sound playing + QScopedPointer _testSound; + + QScopedPointer _notifyRulesModel; + QItemSelectionModel* _notifyRulesSelection; + + /** + * Local copy of notification list, which owned by notify plugin. + * Notification list readed once on application loaded, during + * notify plugin startup, then on open options page. + * This copy is simple assignment, but due to implicitly sharing + * we don't have additional cost for that, copy will created + * only after modification of private notify list. + */ + QList _privListNotifications; + + QScopedPointer _optionsPage; + + //! Widget to convinient selection of condition for field value (equal, lower, greater) + QComboBox* _dynamicFieldCondition; + + //! Represents edit widget for dynamic UAVObjectfield, + //! can be spinbox - for numerics, combobox - enums, or + //! lineedit - for numerics with range constraints + QWidget* _dynamicFieldWidget; + + //! Type of UAVObjectField - numeric or ENUM, + //! this variable needs to correctly set appropriate dynamic UI element (_dynamicFieldWidget) + //! NOTE: ocassionaly it should be invalidated (= -1) to reset _dynamicFieldWidget + int _dynamicFieldType; + + //! Widget to convinient selection of position of + //! between sounds[1..3] + QComboBox* _sayOrder; + + //! Actualy reference to optionsPageWidget, + //! we MUST hold it beyond the scope of createPage func + //! to have possibility change dynamic parts of options page layout in future + QWidget* _form; + + //! Currently selected notification, all controls filled accroding to it. + //! On options page startup, always points to first row. + NotificationItem* _selectedNotification; + + //! Retrieved from UAVObjectManager by name from _selectedNotification, + //! if UAVObjectManager doesn't have such object, this field will be NULL + UAVDataObject* _currUAVObject; - void on_buttonAddNotification_clicked(); - void on_buttonDeleteNotification_clicked(); - void on_buttonModifyNotification_clicked(); - void on_tableNotification_changeSelection( const QItemSelection & selected, const QItemSelection & deselected ); - void on_soundLanguage_indexChanged(int index); - void on_buttonSoundFolder_clicked(const QString& path); - void on_UAVObject_indexChanged(QString val); - void changeButtonText(Phonon::State newstate, Phonon::State oldstate); - void on_chkEnableSound_toggled(bool state); }; #endif // NOTIFYPLUGINOPTIONSPAGE_H diff --git a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.ui b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.ui index b3f48432a..94560ff1a 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.ui +++ b/ground/openpilotgcs/src/plugins/notify/notifypluginoptionspage.ui @@ -6,7 +6,7 @@ 0 0 - 589 + 570 453 @@ -19,531 +19,364 @@ Form - - - - 10 - 10 - 501 - 81 - + + + QLayout::SetMinimumSize - - Sound Collection - - - - - 10 - 20 - 481 - 51 - - - - - 6 + + + + QLayout::SetFixedSize - - - - Language + + + + + 0 + 0 + - - - - - - true + + Sound Collection - - - 75 - 23 - - - - - - - - - 147 - 0 - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> + + + + + true + + + + 75 + 23 + + + + + 550 + 16777215 + + + + + + + + + 0 + 0 + + + + Language + + + + + + + + 147 + 0 + + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8.25pt; font-weight:400; font-style:normal;"> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:8pt;">Select the sound collection</span></p></body></html> + + + + + + + + + + + + + + + + + DataObject + + + + + + + + 0 + 0 + + + + + + + + ObjectField + + + + + + + + 0 + 0 + - - - - - - 10 - 220 - 501 - 211 - - - - Sound Notifications - - - - - 10 - 180 - 481 - 26 - - - + + + + + Qt::Horizontal + + + + + + + + - + + + + 0 + 0 + + - Enable Sounds + Sound1: - - - Qt::Horizontal + + + + 0 + 0 + - + - 138 - 20 + 110 + 0 - - - - - - Play - - - - :/notify/images/play.png:/notify/images/play.png - - - - - - - Qt::Horizontal - - + - 40 - 20 + 16777215 + 16777215 - + - - - Qt::Horizontal + + + + 0 + 0 + - + + Sound2: + + + + + + + + 0 + 0 + + + - 40 - 20 + 110 + 0 - - - - - - Add - - - - :/notify/images/add.png:/notify/images/add.png + + + 16777215 + 16777215 + - - - Modify + + + + 0 + 0 + - - - :/notify/images/modify.png:/notify/images/modify.png + + Sound3: - - - Delete + + + + 0 + 0 + - - - :/utils/images/removesubmitfield.png:/utils/images/removesubmitfield.png + + + 110 + 0 + + + + + 16777215 + 16777215 + - - - - - 10 - 20 - 481 - 151 - - - - QAbstractItemView::SingleSelection - - - QAbstractItemView::SelectRows - - - 22 - - - 22 - - - - - - - 10 - 100 - 501 - 31 - - - - - - - DataObject - - - - - - - - 0 - 0 - - - - - - - - ObjectField - - - - - - - - 0 - 0 - - - - - - - - - - 7 - 130 - 501 - 20 - - - - Qt::Horizontal - - - - - - 10 - 180 - 501 - 31 - - - - - - - - 0 - 0 - - - - Sound1: - - - - - - - - 0 - 0 - - - - - 110 - 0 - - - - - 16777215 - 16777215 - - - - - - - - Sound2: - - - - - - - - 0 - 0 - - - - - 110 - 0 - - - - - 16777215 - 16777215 - - - - - - - - Sound3: - - - - - - - - 0 - 0 - - - - - 110 - 0 - - - - - 16777215 - 16777215 - - - - - - - - - - 10 - 150 - 501 - 31 - - - - - - - true - - - - 0 - 0 - - - - - 40 - 0 - - - - Value is - - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 110 - 16777215 - - + + + + + + + + Sound Notifications + + - - Equal to - + + + QAbstractItemView::ContiguousSelection + + + QAbstractItemView::SelectRows + + + 22 + + + 22 + + - - Greater than - + + + + + Enable Sounds + + + + + + + Qt::Horizontal + + + + 138 + 20 + + + + + + + + Play + + + + :/notify/images/play.png:/notify/images/play.png + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Add + + + + :/notify/images/add.png:/notify/images/add.png + + + + + + + Modify + + + + :/notify/images/modify.png:/notify/images/modify.png + + + + + + + Delete + + + + :/utils/images/removesubmitfield.png:/utils/images/removesubmitfield.png + + + + - - - Less than - - - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 170 - 16777215 - - - - 2 - - - 9999.899999999999636 - - - 1.000000000000000 - - - - - - - - 0 - 0 - - - - Say Order - - - - - - - - 0 - 0 - - - - - 110 - 16777215 - - - - Select if the value of the object should be spoken and if so, either before the configured sound or after it. - - - - Never - - - - - Before first - - - - - After first - - - - - After second - - - - - After third - - - - - - + + + + diff --git a/ground/openpilotgcs/src/plugins/notify/notifytablemodel.cpp b/ground/openpilotgcs/src/plugins/notify/notifytablemodel.cpp index 6f0584d70..7b3da6d31 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifytablemodel.cpp +++ b/ground/openpilotgcs/src/plugins/notify/notifytablemodel.cpp @@ -26,110 +26,239 @@ */ #include "notifytablemodel.h" +#include "notifylogging.h" +#include +#include + +const char* mime_type_notify_table = "openpilot/notify_plugin_table"; + +NotifyTableModel::NotifyTableModel(QList& parentList, QObject* parent) + : QAbstractTableModel(parent) + , _list(parentList) +{ + _headerStrings << "Name" << "Repeats" << "Lifetime,sec" << "Mute"; + connect(this, SIGNAL(dragRows(int, int)), this, SLOT(dropRows(int, int))); +} + bool NotifyTableModel::setData(const QModelIndex &index, const QVariant &value, int role) { - if (index.isValid() && role == Qt::EditRole) { - if(index.column()==1) - _list->at(index.row())->setRepeatFlag(value.toString()); - else - if(index.column()==2) - _list->at(index.row())->setExpireTimeout(value.toInt()); - - emit dataChanged(index, index); - return true; - } - return false; + if (index.isValid() && role == Qt::DisplayRole) { + if (eMessageName == index.column()) { + emit dataChanged(index, index); + return true; + } + } + if (index.isValid() && role == Qt::EditRole) { + if (eRepeatValue == index.column()) + _list.at(index.row())->setRetryValue(NotificationItem::retryValues.indexOf(value.toString())); + else { + if (eExpireTimer == index.column()) + _list.at(index.row())->setLifetime(value.toInt()); + else { + if (eTurnOn == index.column()) + _list.at(index.row())->setMute(value.toBool()); + } + } + emit dataChanged(index, index); + return true; + } + return false; } QVariant NotifyTableModel::data(const QModelIndex &index, int role) const { + if (!index.isValid()) { + qWarning() << "NotifyTableModel::data - index.isValid()"; + return QVariant(); + } - if (!index.isValid()) - return QVariant(); + if (index.row() >= _list.size()) + return QVariant(); - if (index.row() >= _list->size()) - return QVariant(); + if (role == Qt::DisplayRole || role == Qt::EditRole) + { + switch(index.column()) + { + case eMessageName: + return _list.at(index.row())->toString(); - if (role == Qt::DisplayRole || role == Qt::EditRole) - { - switch(index.column()) - { - case 0: - return _list->at(index.row())->parseNotifyMessage(); + case eRepeatValue: + return (NotificationItem::retryValues.at(_list.at(index.row())->retryValue())); - case 1: - return _list->at(index.row())->getRepeatFlag(); + case eExpireTimer: + return _list.at(index.row())->lifetime(); - case 2: - return _list->at(index.row())->getExpireTimeout(); + case eTurnOn: + return _list.at(index.row())->mute(); - default: - return QVariant(); - } - } - else - { - if (Qt::SizeHintRole == role){ - //QVariant size = data(index, Qt::SizeHintRole); - return QVariant(10); - } - // if(role == Qt::DecorationRole) - // if (index.column() == 0) - // return defectsIcons[defectList->at(index.row()).id-1]; - } - return QVariant(); + default: + return QVariant(); + } + } + else + { + if (Qt::SizeHintRole == role){ + return QVariant(10); + } + } + return QVariant(); } QVariant NotifyTableModel::headerData(int section, Qt::Orientation orientation, int role) const { - if (role != Qt::DisplayRole) - return QVariant(); + if (role != Qt::DisplayRole) + return QVariant(); - if (orientation == Qt::Horizontal) - return headerStrings.at(section); - else - if(orientation == Qt::Vertical) - return QString("%1").arg(section); + if (orientation == Qt::Horizontal) + return _headerStrings.at(section); + else + if (orientation == Qt::Vertical) + return QString("%1").arg(section); - return QVariant(); + return QVariant(); } -bool NotifyTableModel::insertRows(int position, int rows, const QModelIndex &index) +bool NotifyTableModel::insertRows(int position, int rows, const QModelIndex& index) { - Q_UNUSED(index); - beginInsertRows(QModelIndex(), position, position+rows-1); + Q_UNUSED(index); -// for (int row=0; row < rows; ++row) { -// _list->append(position); -// } + if (-1 == position || -1 == rows) + return false; + beginInsertRows(QModelIndex(), position, position + rows - 1); - endInsertRows(); - return true; + for (int i = 0; i < rows; ++i) { + _list.insert(position + i, new NotificationItem()); + } + + endInsertRows(); + return true; } - bool NotifyTableModel::removeRows(int position, int rows, const QModelIndex &index) - { - Q_UNUSED(index); - beginRemoveRows(QModelIndex(), position, position+rows-1); +bool NotifyTableModel::removeRows(int position, int rows, const QModelIndex& index) +{ + Q_UNUSED(index); - for (int row=0; row < rows; ++row) { - _list->removeAt(position); - } + if ((-1 == position) || (-1 == rows) ) + return false; - endRemoveRows(); - return true; - } + beginRemoveRows(QModelIndex(), position, position + rows - 1); + + for (int row = 0; row < rows; ++row) { + _list.removeAt(position); + } + + endRemoveRows(); + return true; +} void NotifyTableModel::entryUpdated(int offset) { - QModelIndex idx = index(offset, 0); - emit dataChanged(idx, idx); + QModelIndex idx = index(offset, 0); + emit dataChanged(idx, idx); } -void NotifyTableModel::entryAdded(int position) +void NotifyTableModel::entryAdded(NotificationItem* item) { - insertRows(position, 1,QModelIndex()); + insertRows(rowCount(), 1, QModelIndex()); + NotificationItem* tmp = _list.at(rowCount() - 1); + _list.replace(rowCount() - 1, item); + delete tmp; + entryUpdated(rowCount() - 1); +} + +Qt::DropActions NotifyTableModel::supportedDropActions() const +{ + return Qt::MoveAction; +} + +QStringList NotifyTableModel::mimeTypes() const +{ + QStringList types; + types << mime_type_notify_table; + return types; +} + +bool NotifyTableModel::dropMimeData( const QMimeData * data, Qt::DropAction action, int row, + int column, const QModelIndex& parent) +{ + if (action == Qt::IgnoreAction) + return true; + + if (!data->hasFormat(mime_type_notify_table)) + return false; + + int beginRow = -1; + + if (row != -1) + beginRow = row; + else { + if (parent.isValid()) + beginRow = parent.row(); + else + beginRow = rowCount(QModelIndex()); + } + + if (-1 == beginRow) + return false; + + QByteArray encodedData = data->data(mime_type_notify_table); + QDataStream stream(&encodedData, QIODevice::ReadOnly); + int rows = beginRow; + // read next item from input MIME and drop into the table line by line + while(!stream.atEnd()) { + quintptr ptr; + stream >> ptr; + NotificationItem* item = reinterpret_cast(ptr); + int dragged = _list.indexOf(item); + // we can drag item from top rows to bottom (DOWN_DIRECTION), + // or from bottom rows to top rows (UP_DIRECTION) + enum { UP_DIRECTION, DOWN_DIRECTION }; + int direction = (dragged < rows) ? DOWN_DIRECTION : (dragged += 1, UP_DIRECTION); + // check drop bounds + if (dragged < 0 || ((dragged + 1) >= _list.size() && direction == DOWN_DIRECTION) || dragged == rows) { + qNotifyDebug() << "no such item"; + continue; + } + // addiional check in case dropping of multiple rows + if(rows + direction > _list.size()) continue; + + Q_ASSERT(insertRows(rows + direction, 1, QModelIndex())); + _list.replace(rows + direction, item); + Q_ASSERT(removeRows(dragged, 1, QModelIndex())); + if (direction == UP_DIRECTION) + ++rows; + }; + + QModelIndex idxTopLeft = index(beginRow, 0, QModelIndex()); + QModelIndex idxBotRight = index(beginRow, columnCount(QModelIndex()), QModelIndex()); + emit dataChanged(idxTopLeft, idxBotRight); + return true; +} + +QMimeData* NotifyTableModel::mimeData(const QModelIndexList& indexes) const +{ + QMimeData* mimeData = new QMimeData(); + QByteArray encodedData; + + QDataStream stream(&encodedData, QIODevice::WriteOnly); + int rows = 0; + foreach (const QModelIndex& index, indexes) { + if (!index.column()) { + quintptr item = reinterpret_cast(_list.at(index.row())); + stream << item; + ++rows; + } + } + mimeData->setData(mime_type_notify_table, encodedData); + return mimeData; +} + +void NotifyTableModel::dropRows(int position, int count) const +{ + for (int row = 0; row < count; ++row) { + _list.removeAt(position); + } } diff --git a/ground/openpilotgcs/src/plugins/notify/notifytablemodel.h b/ground/openpilotgcs/src/plugins/notify/notifytablemodel.h index 65aa82a86..dd2e765bd 100644 --- a/ground/openpilotgcs/src/plugins/notify/notifytablemodel.h +++ b/ground/openpilotgcs/src/plugins/notify/notifytablemodel.h @@ -31,49 +31,60 @@ #include #include -#include "notifypluginconfiguration.h" +#include "notificationitem.h" + +enum ColumnNames { eMessageName, eRepeatValue, eExpireTimer, eTurnOn }; class NotifyTableModel : public QAbstractTableModel { - Q_OBJECT - public: - NotifyTableModel(QList *parentList, const QStringList& parentHeaderList, QObject *parent = 0) - : QAbstractTableModel(parent), - _list(parentList), - headerStrings(parentHeaderList) - { } + Q_OBJECT - int rowCount(const QModelIndex &parent = QModelIndex()) const - { - return _list->count(); - } + enum {eColumnCount = 4 }; - int columnCount(const QModelIndex &/*parent*/) const - { - return 3; - } +public: - Qt::ItemFlags flags(const QModelIndex &index) const - { - if (!index.isValid()) - return Qt::ItemIsEnabled; + NotifyTableModel(QList& parentList, QObject* parent = 0); + int rowCount(const QModelIndex& parent = QModelIndex()) const + { + return _list.count(); + } - return QAbstractItemModel::flags(index) | Qt::ItemIsEditable; - } + int columnCount(const QModelIndex &/*parent*/) const + { + return eColumnCount; + } - bool setData(const QModelIndex &index, const QVariant &value, int role); - QVariant data(const QModelIndex &index, int role) const; - QVariant headerData(int section, Qt::Orientation orientation, int role) const; - bool insertRows(int position, int rows, const QModelIndex &index); - bool removeRows(int position, int rows, const QModelIndex &index); + Qt::ItemFlags flags(const QModelIndex &index) const + { + if (!index.isValid()) + return Qt::ItemIsEnabled | Qt::ItemIsDropEnabled; + + return QAbstractItemModel::flags(index) | Qt::ItemIsEditable | Qt::ItemIsDragEnabled | Qt::ItemIsDropEnabled; + } + QStringList mimeTypes() const; + Qt::DropActions supportedDropActions() const; + bool dropMimeData( const QMimeData * data, Qt::DropAction action, int row, + int column, const QModelIndex& parent); + QMimeData* mimeData(const QModelIndexList &indexes) const; + + + bool setData(const QModelIndex &index, const QVariant &value, int role); + QVariant data(const QModelIndex &index, int role) const; + QVariant headerData(int section, Qt::Orientation orientation, int role) const; + bool insertRows(int position, int rows, const QModelIndex &index); + bool removeRows(int position, int rows, const QModelIndex &index); + void entryAdded(NotificationItem* item); + +signals: + void dragRows(int position, int count); private slots: - void entryUpdated(int offset); - void entryAdded(int position); + void entryUpdated(int offset); + void dropRows(int position, int count) const; + private: - QList *_list; - QStringList headerStrings; + QList& _list; + QStringList _headerStrings; }; - #endif // NOTIFYTABLEMODEL_H diff --git a/ground/openpilotgcs/src/plugins/scope/plotdata.cpp b/ground/openpilotgcs/src/plugins/scope/plotdata.cpp index f2d4bb5af..e05bf7a1b 100644 --- a/ground/openpilotgcs/src/plugins/scope/plotdata.cpp +++ b/ground/openpilotgcs/src/plugins/scope/plotdata.cpp @@ -49,9 +49,14 @@ PlotData::PlotData(QString p_uavObject, QString p_uavField) xData = new QVector(); yData = new QVector(); + yDataHistory = new QVector(); curve = 0; scalePower = 0; + interpolationSamples = 1; + interpolationSum = 0.0f; + correctionSum = 0.0f; + correctionCount = 0; yMinimum = 0; yMaximum = 0; @@ -78,6 +83,7 @@ PlotData::~PlotData() { delete xData; delete yData; + delete yDataHistory; } @@ -91,7 +97,24 @@ bool SequencialPlotData::append(UAVObject* obj) if (field) { //Shift data forward and put the new value at the front - yData->append( valueAsDouble(obj, field) * pow(10, scalePower)); + + // calculate interpolated (smoothed) value + double currentValue = valueAsDouble(obj, field) * pow(10, scalePower); + yDataHistory->append( currentValue ); + interpolationSum += currentValue; + if(yDataHistory->size() > interpolationSamples) { + interpolationSum -= yDataHistory->first(); + yDataHistory->pop_front(); + } + // make sure to correct the sum every interpolationSamples steps to prevent it + // from running away due to flouting point rounding errors + correctionSum += currentValue; + if (++correctionCount >= interpolationSamples) { + interpolationSum = correctionSum; + correctionSum = 0.0f; + correctionCount = 0; + } + yData->append(interpolationSum/yDataHistory->size()); if (yData->size() > m_xWindowSize) { yData->pop_front(); } else @@ -117,8 +140,25 @@ bool ChronoPlotData::append(UAVObject* obj) //Put the new value at the front QDateTime NOW = QDateTime::currentDateTime(); + // calculate interpolated (smoothed) value + double currentValue = valueAsDouble(obj, field) * pow(10, scalePower); + yDataHistory->append( currentValue ); + interpolationSum += currentValue; + if(yDataHistory->size() > interpolationSamples) { + interpolationSum -= yDataHistory->first(); + yDataHistory->pop_front(); + } + // make sure to correct the sum every interpolationSamples steps to prevent it + // from running away due to flouting point rounding errors + correctionSum += currentValue; + if (++correctionCount >= interpolationSamples) { + interpolationSum = correctionSum; + correctionSum = 0.0f; + correctionCount = 0; + } + double valueX = NOW.toTime_t() + NOW.time().msec() / 1000.0; - double valueY = valueAsDouble(obj, field) * pow(10, scalePower); + double valueY = interpolationSum/yDataHistory->size(); xData->append(valueX); yData->append(valueY); diff --git a/ground/openpilotgcs/src/plugins/scope/plotdata.h b/ground/openpilotgcs/src/plugins/scope/plotdata.h index 2d61f12c6..a547f2b91 100644 --- a/ground/openpilotgcs/src/plugins/scope/plotdata.h +++ b/ground/openpilotgcs/src/plugins/scope/plotdata.h @@ -72,12 +72,17 @@ public: QString uavSubField; bool haveSubField; int scalePower; //This is the power to which each value must be raised + int interpolationSamples; + double interpolationSum; + double correctionSum; + int correctionCount; double yMinimum; double yMaximum; double m_xWindowSize; QwtPlotCurve* curve; QVector* xData; QVector* yData; + QVector* yDataHistory; virtual bool append(UAVObject* obj) = 0; virtual PlotType plotType() = 0; diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadget.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadget.cpp index ccf4a8b82..b4b1f24fa 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadget.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadget.cpp @@ -61,12 +61,14 @@ void ScopeGadget::loadConfiguration(IUAVGadgetConfiguration* config) QString uavObject = plotCurveConfig->uavObject; QString uavField = plotCurveConfig->uavField; int scale = plotCurveConfig->yScalePower; + int interpolation = plotCurveConfig->yInterpolationSamples; QRgb color = plotCurveConfig->color; widget->addCurvePlot( uavObject, uavField, scale, + interpolation, QPen( QBrush(QColor(color),Qt::SolidPattern), // (qreal)2, (qreal)1, diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.cpp index 63a48e495..3228497f3 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.cpp @@ -65,6 +65,8 @@ ScopeGadgetConfiguration::ScopeGadgetConfiguration(QString classId, QSettings* q color = qSettings->value("color").value(); plotCurveConf->color = color; plotCurveConf->yScalePower = qSettings->value("yScalePower").toInt(); + plotCurveConf->yInterpolationSamples = qSettings->value("yInterpolationSamples").toInt(); + if (!plotCurveConf->yInterpolationSamples) plotCurveConf->yInterpolationSamples = 1; // fallback for backward compatibility with earlier versions plotCurveConf->yMinimum = qSettings->value("yMinimum").toDouble(); plotCurveConf->yMaximum = qSettings->value("yMaximum").toDouble(); @@ -118,6 +120,7 @@ IUAVGadgetConfiguration *ScopeGadgetConfiguration::clone() newPlotCurveConf->uavField = currentPlotCurveConf->uavField; newPlotCurveConf->color = currentPlotCurveConf->color; newPlotCurveConf->yScalePower = currentPlotCurveConf->yScalePower; + newPlotCurveConf->yInterpolationSamples = currentPlotCurveConf->yInterpolationSamples; newPlotCurveConf->yMinimum = currentPlotCurveConf->yMinimum; newPlotCurveConf->yMaximum = currentPlotCurveConf->yMaximum; @@ -157,6 +160,7 @@ void ScopeGadgetConfiguration::saveConfig(QSettings* qSettings) const { qSettings->setValue("uavField", plotCurveConf->uavField); qSettings->setValue("color", plotCurveConf->color); qSettings->setValue("yScalePower", plotCurveConf->yScalePower); + qSettings->setValue("yInterpolationSamples", plotCurveConf->yInterpolationSamples); qSettings->setValue("yMinimum", plotCurveConf->yMinimum); qSettings->setValue("yMaximum", plotCurveConf->yMaximum); diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.h b/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.h index 1aadcaf5b..570ebe1f9 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.h @@ -41,6 +41,7 @@ struct PlotCurveConfiguration QString uavField; int yScalePower; //This is the power to which each value must be raised QRgb color; + int yInterpolationSamples; double yMinimum; double yMaximum; }; diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.cpp index e44ccb08f..d0d364b68 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.cpp @@ -101,9 +101,10 @@ QWidget* ScopeGadgetOptionsPage::createPage(QWidget *parent) QString uavObject = plotData->uavObject; QString uavField = plotData->uavField; int scale = plotData->yScalePower; + int interpolation = plotData->yInterpolationSamples; QVariant varColor = plotData->color; - addPlotCurveConfig(uavObject,uavField,scale,varColor); + addPlotCurveConfig(uavObject,uavField,scale,interpolation,varColor); } if(m_config->plotCurveConfigs().count() > 0) @@ -163,6 +164,10 @@ void ScopeGadgetOptionsPage::setYAxisWidgetFromPlotCurve() int rgb = varColor.toInt(&parseOK); setButtonColor(QColor((QRgb)rgb)); + + int interpolation = listItem->data(Qt::UserRole + 4).toInt(&parseOK); + if(!parseOK) interpolation = 1; + options_page->spnInterpolationSamples->setValue(interpolation); } void ScopeGadgetOptionsPage::setButtonColor(const QColor &color) @@ -235,6 +240,10 @@ void ScopeGadgetOptionsPage::apply() newPlotCurveConfigs->color = QColor(Qt::black).rgb(); else newPlotCurveConfigs->color = (QRgb)rgb; + + newPlotCurveConfigs->yInterpolationSamples = listItem->data(Qt::UserRole + 4).toInt(&parseOK); + if(!parseOK) + newPlotCurveConfigs->yInterpolationSamples = 1; plotCurveConfigs.append(newPlotCurveConfigs); } @@ -261,6 +270,7 @@ void ScopeGadgetOptionsPage::on_btnAddCurve_clicked() if(!parseOK) scale = 0; + int interpolation = options_page->spnInterpolationSamples->value(); QVariant varColor = (int)QColor(options_page->btnColor->text()).rgb(); @@ -270,27 +280,27 @@ void ScopeGadgetOptionsPage::on_btnAddCurve_clicked() options_page->lstCurves->currentItem()->text() == uavObject + "." + uavField) { QListWidgetItem *listWidgetItem = options_page->lstCurves->currentItem(); - setCurvePlotProperties(listWidgetItem,uavObject,uavField,scale,varColor); + setCurvePlotProperties(listWidgetItem,uavObject,uavField,scale,interpolation,varColor); }else { - addPlotCurveConfig(uavObject,uavField,scale,varColor); + addPlotCurveConfig(uavObject,uavField,scale,interpolation,varColor); options_page->lstCurves->setCurrentRow(options_page->lstCurves->count() - 1); } } -void ScopeGadgetOptionsPage::addPlotCurveConfig(QString uavObject, QString uavField, int scale, QVariant varColor) +void ScopeGadgetOptionsPage::addPlotCurveConfig(QString uavObject, QString uavField, int scale, int interpolation, QVariant varColor) { //Add a new curve config to the list QString listItemDisplayText = uavObject + "." + uavField; options_page->lstCurves->addItem(listItemDisplayText); QListWidgetItem *listWidgetItem = options_page->lstCurves->item(options_page->lstCurves->count() - 1); - setCurvePlotProperties(listWidgetItem,uavObject,uavField,scale,varColor); + setCurvePlotProperties(listWidgetItem,uavObject,uavField,scale,interpolation,varColor); } -void ScopeGadgetOptionsPage::setCurvePlotProperties(QListWidgetItem *listWidgetItem,QString uavObject, QString uavField, int scale, QVariant varColor) +void ScopeGadgetOptionsPage::setCurvePlotProperties(QListWidgetItem *listWidgetItem,QString uavObject, QString uavField, int scale, int interpolation, QVariant varColor) { bool parseOK = false; @@ -306,6 +316,7 @@ void ScopeGadgetOptionsPage::setCurvePlotProperties(QListWidgetItem *listWidgetI listWidgetItem->setData(Qt::UserRole + 1,QVariant(uavField)); listWidgetItem->setData(Qt::UserRole + 2,QVariant(scale)); listWidgetItem->setData(Qt::UserRole + 3,varColor); + listWidgetItem->setData(Qt::UserRole + 4,QVariant(interpolation)); } /*! diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.h b/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.h index cbbf9201b..39c0b3ea5 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.h +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.h @@ -66,8 +66,8 @@ private: Ui::ScopeGadgetOptionsPage *options_page; ScopeGadgetConfiguration *m_config; - void addPlotCurveConfig(QString uavObject, QString uavField, int scale, QVariant varColor); - void setCurvePlotProperties(QListWidgetItem *listWidgetItem, QString uavObject, QString uavField, int scale, QVariant varColor); + void addPlotCurveConfig(QString uavObject, QString uavField, int scale, int interpolation, QVariant varColor); + void setCurvePlotProperties(QListWidgetItem *listWidgetItem, QString uavObject, QString uavField, int scale, int interpolation, QVariant varColor); void setYAxisWidgetFromPlotCurve(); void setButtonColor(const QColor &color); diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.ui b/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.ui index 2aaee1942..b3d22faa0 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.ui +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.ui @@ -159,6 +159,32 @@ + + + + Display smoothed interpolation: + + + + + + + samples + + + 1 + + + 1001 + + + 10 + + + 1 + + + @@ -268,7 +294,7 @@ Update - Log data to csv file + Log data to csv file (not interpolated) diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp index b5f3bc9ee..140935da3 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp @@ -357,7 +357,7 @@ void ScopeGadgetWidget::setupChronoPlot() // scaleWidget->setMinBorderDist(0, fmw); } -void ScopeGadgetWidget::addCurvePlot(QString uavObject, QString uavFieldSubField, int scaleOrderFactor, QPen pen) +void ScopeGadgetWidget::addCurvePlot(QString uavObject, QString uavFieldSubField, int scaleOrderFactor, int interpolationSamples, QPen pen) { PlotData* plotData; @@ -370,6 +370,7 @@ void ScopeGadgetWidget::addCurvePlot(QString uavObject, QString uavFieldSubField plotData->m_xWindowSize = m_xWindowSize; plotData->scalePower = scaleOrderFactor; + plotData->interpolationSamples = interpolationSamples; //If the y-bounds are supplied, set them if (plotData->yMinimum != plotData->yMaximum) @@ -447,6 +448,7 @@ void ScopeGadgetWidget::uavObjectReceived(UAVObject* obj) foreach(PlotData* plotData, m_curvesData.values()) { if (plotData->append(obj)) m_csvLoggingDataUpdated=1; } + csvLoggingAddData(); } void ScopeGadgetWidget::replotNewData() @@ -609,6 +611,7 @@ int ScopeGadgetWidget::csvLoggingStart() m_csvLoggingStartTime = NOW; m_csvLoggingHeaderSaved=0; m_csvLoggingDataSaved=0; + m_csvLoggingBuffer.clear(); QDir PathCheck(m_csvLoggingPath); if (!PathCheck.exists()) { @@ -676,13 +679,50 @@ int ScopeGadgetWidget::csvLoggingInsertHeader() return 0; } +int ScopeGadgetWidget::csvLoggingAddData() +{ + if (!m_csvLoggingStarted) return -1; + m_csvLoggingDataValid=0; + QDateTime NOW = QDateTime::currentDateTime(); + QString tempString; + + QTextStream ss( &tempString ); + ss << NOW.toString("yyyy-MM-dd") << ", " << NOW.toString("hh:mm:ss.z") << ", " ; + +#if QT_VERSION >= 0x040700 + ss <<(NOW.toMSecsSinceEpoch() - m_csvLoggingStartTime.toMSecsSinceEpoch())/1000.00; +#else + ss <<(NOW.toTime_t() - m_csvLoggingStartTime.toTime_t()); +#endif + ss << ", " << m_csvLoggingConnected << ", " << m_csvLoggingDataUpdated; + m_csvLoggingDataUpdated=0; + + foreach(PlotData* plotData2, m_curvesData.values()) + { + ss << ", "; + if (plotData2->xData->isEmpty ()) + { + } + else + { + ss << QString().sprintf("%3.6g",plotData2->yDataHistory->last()); + m_csvLoggingDataValid=1; + } + } + ss << endl; + if (m_csvLoggingDataValid) + { + QTextStream ts( &m_csvLoggingBuffer ); + ts << tempString; + } + + return 0; +} + int ScopeGadgetWidget::csvLoggingInsertData() { if (!m_csvLoggingStarted) return -1; m_csvLoggingDataSaved=1; - m_csvLoggingDataValid=0; - QDateTime NOW = QDateTime::currentDateTime(); - QString tempString; if(m_csvLoggingFile.open(QIODevice::WriteOnly | QIODevice::Append)== FALSE) { @@ -690,38 +730,11 @@ int ScopeGadgetWidget::csvLoggingInsertData() } else { - QTextStream ss( &tempString ); - ss << NOW.toString("yyyy-MM-dd") << ", " << NOW.toString("hh:mm:ss.z") << ", " ; - -#if QT_VERSION >= 0x040700 - ss <<(NOW.toMSecsSinceEpoch() - m_csvLoggingStartTime.toMSecsSinceEpoch())/1000.00; -#else - ss <<(NOW.toTime_t() - m_csvLoggingStartTime.toTime_t()); -#endif - ss << ", " << m_csvLoggingConnected << ", " << m_csvLoggingDataUpdated; - m_csvLoggingDataUpdated=0; - - foreach(PlotData* plotData2, m_curvesData.values()) - { - ss << ", "; - if (plotData2->xData->isEmpty ()) - { - } - else - { - ss << QString().sprintf("%3.6g",plotData2->yData->last()); - m_csvLoggingDataValid=1; - } - } - ss << endl; - if (m_csvLoggingDataValid) - { - QTextStream ts( &m_csvLoggingFile ); - ts << tempString; - } + QTextStream ts( &m_csvLoggingFile ); + ts << m_csvLoggingBuffer; m_csvLoggingFile.close(); } - + m_csvLoggingBuffer.clear(); return 0; } @@ -744,4 +757,4 @@ void ScopeGadgetWidget::csvLoggingDisconnect() m_csvLoggingConnected=0; if (m_csvLoggingNewFileOnConnect)csvLoggingStop(); return; -} \ No newline at end of file +} diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.h b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.h index f75db9c3c..98da3beb6 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.h @@ -110,7 +110,7 @@ public: int refreshInterval(){return m_refreshInterval;} - void addCurvePlot(QString uavObject, QString uavFieldSubField, int scaleOrderFactor = 0, QPen pen = QPen(Qt::black)); + void addCurvePlot(QString uavObject, QString uavFieldSubField, int scaleOrderFactor = 0, int interpolationSamples = 1, QPen pen = QPen(Qt::black)); //void removeCurvePlot(QString uavObject, QString uavField); void clearCurvePlots(); int csvLoggingStart(); @@ -166,11 +166,13 @@ private: QString m_csvLoggingName; QString m_csvLoggingPath; + QString m_csvLoggingBuffer; QFile m_csvLoggingFile; QMutex mutex; int csvLoggingInsertHeader(); + int csvLoggingAddData(); int csvLoggingInsertData(); void deleteLegend(); @@ -178,4 +180,4 @@ private: }; -#endif /* SCOPEGADGETWIDGET_H_ */ \ No newline at end of file +#endif /* SCOPEGADGETWIDGET_H_ */ diff --git a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp index 9e57a4225..f1d28ec90 100644 --- a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp @@ -111,7 +111,7 @@ void deviceWidget::populate() // display a nice icon: myDevice->gVDevice->scene()->clear(); myDevice->lblDevName->setText(deviceDescriptorStruct::idToBoardName(id)); - myDevice->lblHWRev->setText(QString(tr("HW Revision: "))+QString::number(id & 0x0011, 16)); + myDevice->lblHWRev->setText(QString(tr("HW Revision: "))+QString::number(id & 0x00FF, 16)); devicePic = new QGraphicsSvgItem(); devicePic->setSharedRenderer(new QSvgRenderer()); @@ -358,8 +358,10 @@ void deviceWidget::loadFirmware() */ void deviceWidget::uploadFirmware() { + myDevice->updateButton->setEnabled(false); if (!m_dfu->devices[deviceID].Writable) { status("Device not writable!", STATUSICON_FAIL); + myDevice->updateButton->setEnabled(true); return; } @@ -378,6 +380,7 @@ void deviceWidget::uploadFirmware() int firmwareBoard = ((desc.at(12)&0xff)<<8) + (desc.at(13)&0xff); if (firmwareBoard != board) { status("Error: firmware does not match board", STATUSICON_FAIL); + myDevice->updateButton->setEnabled(true); return; } // Check the firmware embedded in the file: @@ -385,6 +388,7 @@ void deviceWidget::uploadFirmware() QByteArray fileHash = QCryptographicHash::hash(loadedFW.left(loadedFW.length()-100), QCryptographicHash::Sha1); if (firmwareHash != fileHash) { status("Error: firmware file corrupt", STATUSICON_FAIL); + myDevice->updateButton->setEnabled(true); return; } } else { @@ -400,6 +404,7 @@ void deviceWidget::uploadFirmware() if(!m_dfu->enterDFU(deviceID)) { status("Error:Could not enter DFU mode", STATUSICON_FAIL); + myDevice->updateButton->setEnabled(true); return; } OP_DFU::Status ret=m_dfu->StatusRequest(); @@ -412,6 +417,7 @@ void deviceWidget::uploadFirmware() bool retstatus = m_dfu->UploadFirmware(filename,verify, deviceID); if(!retstatus ) { status("Could not start upload", STATUSICON_FAIL); + myDevice->updateButton->setEnabled(true); return; } status("Uploading, please wait...", STATUSICON_RUNNING); @@ -465,6 +471,7 @@ void deviceWidget::downloadFinished() */ void deviceWidget::uploadFinished(OP_DFU::Status retstatus) { + myDevice->updateButton->setEnabled(true); disconnect(m_dfu, SIGNAL(uploadFinished(OP_DFU::Status)), this, SLOT(uploadFinished(OP_DFU::Status))); disconnect(m_dfu, SIGNAL(progressUpdated(int)), this, SLOT(setProgress(int))); disconnect(m_dfu, SIGNAL(operationProgress(QString)), this, SLOT(dfuStatus(QString))); diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp index 0143a180b..bff5d04d5 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp @@ -37,6 +37,7 @@ DFUObject::DFUObject(bool _debug,bool _use_serial,QString portname): debug(_debug),use_serial(_use_serial),mready(true) { info = NULL; + numberOfDevices = 0; qRegisterMetaType("Status"); diff --git a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp index fcde00125..9cb351bc1 100644 --- a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp @@ -73,7 +73,7 @@ void runningDeviceWidget::populate() myDevice->lblDeviceID->setText(QString("Device ID: ") + QString::number(id, 16)); myDevice->lblBoardName->setText(deviceDescriptorStruct::idToBoardName(id)); - myDevice->lblHWRev->setText(QString(tr("HW Revision: "))+QString::number(id & 0x0011, 16)); + myDevice->lblHWRev->setText(QString(tr("HW Revision: "))+QString::number(id & 0x00FF, 16)); qDebug()<<"CRC"<getFirmwareCRC(); myDevice->lblCRC->setText(QString(tr("Firmware CRC: "))+QVariant(utilMngr->getFirmwareCRC()).toString()); // DeviceID tells us what sort of HW we have detected: diff --git a/ground/openpilotgcs/src/plugins/uploader/uploader.ui b/ground/openpilotgcs/src/plugins/uploader/uploader.ui index b0dffd4a0..540d69302 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploader.ui +++ b/ground/openpilotgcs/src/plugins/uploader/uploader.ui @@ -6,7 +6,7 @@ 0 0 - 580 + 583 350 @@ -144,6 +144,26 @@ halting a running board. + + + + + + + + :/core/images/helpicon.svg:/core/images/helpicon.svg + + + + 30 + 30 + + + + true + + + @@ -166,14 +186,14 @@ halting a running board. <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">To upgrade the firmware in your boards, proceed as follows:</p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">- Connect telemetry</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">- Once telemetry is running, press &quot;Halt&quot; above</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">- You will get a list of devices.</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">- You can then upload/download to/from each board as you wish</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">- You can resume operations by pressing &quot;Boot&quot;</p></body></html> +</style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8.25pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">To upgrade the firmware in your boards, proceed as follows:</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">- Connect telemetry</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">- Once telemetry is running, press &quot;Halt&quot; above</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">- You will get a list of devices.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">- You can then upload/download to/from each board as you wish</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">- You can resume operations by pressing &quot;Boot&quot;</span></p></body></html> @@ -188,14 +208,16 @@ p, li { white-space: pre-wrap; } <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'Ubuntu'; font-size:11pt; font-weight:400; font-style:normal;"> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"></p></body></html> +</style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8.25pt; font-weight:400; font-style:normal;"> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"></p></body></html> - + + + diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index 182753b24..851334c86 100755 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -64,6 +64,7 @@ UploaderGadgetWidget::UploaderGadgetWidget(QWidget *parent) : QWidget(parent) connect(m_config->refreshPorts, SIGNAL(clicked()), this, SLOT(getSerialPorts())); + connect(m_config->pbHelp,SIGNAL(clicked()),this,SLOT(openHelp())); // And check whether by any chance we are not already connected if (telMngr->isConnected()) { @@ -650,3 +651,8 @@ void UploaderGadgetWidget::versionMatchCheck() QString(tr("FW Versions: ")) + boardDescription.gitTag+":"+boardDescription.buildDate); } } +void UploaderGadgetWidget::openHelp() +{ + + QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Uploader+Plugin", QUrl::StrictMode) ); +} diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h index 998166d69..2c6814827 100755 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.h @@ -55,6 +55,7 @@ #include "devicedescriptorstruct.h" #include #include +#include using namespace OP_DFU; @@ -74,6 +75,7 @@ public slots: void onAutopilotConnect(); void onAutopilotDisconnect(); void populate(); + void openHelp(); private: Ui_UploaderWidget *m_config; DFUObject *dfu; diff --git a/make/boards/openpilot/board-info.mk b/make/boards/openpilot/board-info.mk index d9cff166d..e98ac4480 100644 --- a/make/boards/openpilot/board-info.mk +++ b/make/boards/openpilot/board-info.mk @@ -1,6 +1,6 @@ BOARD_TYPE := 0x01 BOARD_REVISION := 0x01 -BOOTLOADER_VERSION := 0x00 +BOOTLOADER_VERSION := 0x01 HW_TYPE := 0x00 MCU := cortex-m3 diff --git a/make/boards/pipxtreme/board-info.mk b/make/boards/pipxtreme/board-info.mk index f9635109e..dbcb314df 100644 --- a/make/boards/pipxtreme/board-info.mk +++ b/make/boards/pipxtreme/board-info.mk @@ -1,6 +1,6 @@ BOARD_TYPE := 0x03 BOARD_REVISION := 0x01 -BOOTLOADER_VERSION := 0x00 +BOOTLOADER_VERSION := 0x01 HW_TYPE := 0x01 MCU := cortex-m3 diff --git a/package/Makefile.linux b/package/Makefile.linux index fba2ef3ff..78e902d71 100644 --- a/package/Makefile.linux +++ b/package/Makefile.linux @@ -49,7 +49,7 @@ linux_deb_package: deb_build gcs deb_build: | $(DEB_BUILD_DIR) $(ALL_DEB_FILES) $(BUILD_DIR)/build @echo $@ starting - $(V1)$(shell echo $(PACKAGE_DIR) > $(BUILD_DIR)/package_dir) + $(V1)$(shell echo $(FW_DIR) > $(BUILD_DIR)/package_dir) $(V1)sed -i -e "$(SED_SCRIPT)" $(DEB_BUILD_DIR)/changelog $(BUILD_DIR)/build: package_flight diff --git a/package/linux/deb_common/rules b/package/linux/deb_common/rules old mode 100755 new mode 100644 index c4a7d9bd9..ba22c1e9c --- a/package/linux/deb_common/rules +++ b/package/linux/deb_common/rules @@ -43,8 +43,10 @@ install: cp -arp package/linux/openpilot_menu.png debian/openpilot/usr/share/pixmaps cp -arp package/linux/openpilot_menu.menu debian/openpilot/etc/xdg/menus/applications-merged cp -arp package/linux/openpilot_menu.directory debian/openpilot/usr/share/desktop-directories -ifdef $(PACKAGE_DIR) - cp -ar $(PACKAGE_DIR)/* debian/openpilot/usr/local/OpenPilot/firmware/ +ifdef PACKAGE_DIR + cp -a $(PACKAGE_DIR)/*.opfw debian/openpilot/usr/local/OpenPilot/firmware/ +else + $(error PACKAGE_DIR not defined! $(PACKAGE_DIR)) endif ln -s /usr/local/OpenPilot/bin/openpilotgcs.bin `pwd`/debian/openpilot/usr/bin/openpilot-gcs rm -rf debian/openpilot/usr/local/OpenPilot/share/openpilotgcs/sounds/sounds diff --git a/package/osx/libraries b/package/osx/libraries index 5c87ba15b..fc21721e9 100755 --- a/package/osx/libraries +++ b/package/osx/libraries @@ -5,7 +5,7 @@ PLUGINS="${APP}/Contents/Plugins" OP_PLUGINS="${APP}/Contents/Plugins/OpenPilot" QT_LIBS="QtGui QtTest QtCore QtSvg QtSql QtOpenGL QtNetwork QtXml QtDBus QtScript phonon" QT_DIR=$(otool -L "${APP}/Contents/MacOS/OpenPilot GCS" | sed -n -e 's/\/QtCore\.framework.*//p' | sed -n -E 's:^.::p') -QT_EXTRA="accessible/libqtaccessiblewidgets.dylib bearer/libqgenericbearer.dylib codecs/libqcncodecs.dylib codecs/libqjpcodecs.dylib codecs/libqkrcodecs.dylib codecs/libqtwcodecs.dylib graphicssystems/libqtracegraphicssystem.dylib imageformats/libqgif.dylib imageformats/libqico.dylib imageformats/libqjpeg.dylib imageformats/libqmng.dylib imageformats/libqtiff.dylib qmltooling/libqmldbg_inspector.dylib qmltooling/libqmldbg_tcp.dylib" +QT_EXTRA="accessible/libqtaccessiblewidgets.dylib bearer/libqgenericbearer.dylib codecs/libqcncodecs.dylib codecs/libqjpcodecs.dylib codecs/libqkrcodecs.dylib codecs/libqtwcodecs.dylib graphicssystems/libqtracegraphicssystem.dylib imageformats/libqgif.dylib imageformats/libqico.dylib imageformats/libqjpeg.dylib imageformats/libqmng.dylib imageformats/libqtiff.dylib imageformats/libqsvg.dylib qmltooling/libqmldbg_inspector.dylib qmltooling/libqmldbg_tcp.dylib" echo "Qt library directory is \"${QT_DIR}\"" @@ -50,10 +50,21 @@ done for f in ${QT_EXTRA} do + echo "Copying package ${f}" + cp "${QT_DIR}/../plugins/${f}" "${APP}/Contents/Plugins/${f}" echo "Changing package identification of ${f}" install_name_tool -id \ @executable_path/../Plugins/${f} \ "${PLUGINS}/${f}" + + echo "Changing package linkages" + for g in $QT_LIBS + do + install_name_tool -change \ + "${QT_DIR}/${g}.framework/Versions/4/${g}" \ + @executable_path/../Frameworks/${g}.framework/Versions/4/${g} \ + "${APP}/Contents/Plugins/${f}" + done done echo "Copying SDL" diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 1e34e0808..455e23cd7 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -12,10 +12,11 @@ + - +