diff --git a/flight/Bootloaders/OpenPilot/main.c b/flight/Bootloaders/OpenPilot/main.c index 47487200a..d6d69759d 100644 --- a/flight/Bootloaders/OpenPilot/main.c +++ b/flight/Bootloaders/OpenPilot/main.c @@ -125,7 +125,6 @@ int main() { else ProgPort = Serial; PIOS_Board_Init(); - PIOS_OPAHRS_Init(); if(User_DFU_request == TRUE) DeviceState = DFUidle; else @@ -137,7 +136,7 @@ int main() { STOPWATCH_Reset(SSP_TIMER); ssp_Init(&ssp_port, &SSP_PortConfig); } - PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0); + PIOS_OPAHRS_ForceSlaveSelected(true); } else JumpToApp = TRUE; diff --git a/flight/Bootloaders/OpenPilot/op_dfu.c b/flight/Bootloaders/OpenPilot/op_dfu.c index fc4b7da30..84f689ad2 100644 --- a/flight/Bootloaders/OpenPilot/op_dfu.c +++ b/flight/Bootloaders/OpenPilot/op_dfu.c @@ -518,7 +518,7 @@ void OPDfuIni(uint8_t discover) { } } } else - PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0); + PIOS_OPAHRS_ForceSlaveSelected(true); } //TODO check other devices trough spi or whatever } diff --git a/flight/Bootloaders/OpenPilot/pios_board.c b/flight/Bootloaders/OpenPilot/pios_board.c index 8272b8421..7096d9f73 100644 --- a/flight/Bootloaders/OpenPilot/pios_board.c +++ b/flight/Bootloaders/OpenPilot/pios_board.c @@ -26,142 +26,12 @@ */ #include -#include -#include -#include -#include //#include //#include -/** - * PIOS_Board_Init() - * initializes all the core subsystems on this specific hardware - * called from System/openpilot.c - */ -void PIOS_Board_Init(void) { +#if defined(PIOS_INCLUDE_SPI) - - /* Enable Prefetch Buffer */ - FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); - - /* Flash 2 wait state */ - FLASH_SetLatency(FLASH_Latency_2); - - /* Delay system */ - PIOS_DELAY_Init(); - - /* SPI Init */ - PIOS_SPI_Init(); - - - /* Initialize the PiOS library */ - PIOS_COM_Init(); - PIOS_GPIO_Init(); - -#if defined(PIOS_INCLUDE_USB_HID) - PIOS_USB_HID_Init(0); -#endif - //PIOS_I2C_Init(); - RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar -} - -/* 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"))); -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 = 7 << 3, /* Maximum divider (ie. slowest clock rate) */ - }, - .dma = { - .ahb_clk = RCC_AHBPeriph_DMA1, - - .irq = { - .handler = PIOS_SPI_sdcard_irq_handler, - .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_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, - }, - }, -}; +#include /* AHRS Interface * @@ -182,7 +52,7 @@ const struct pios_spi_cfg pios_spi_ahrs_cfg = { .SPI_CRCPolynomial = 7, .SPI_CPOL = SPI_CPOL_High, .SPI_CPHA = SPI_CPHA_2Edge, - .SPI_BaudRatePrescaler = 7 << 3, /* Maximum divider (ie. slowest clock rate) */ + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8, }, .use_crc = TRUE, .dma = { @@ -262,32 +132,20 @@ const struct pios_spi_cfg pios_spi_ahrs_cfg = { }, }; -/* - * Board specific number of devices. - */ -struct pios_spi_dev pios_spi_devs[] = { - { - .cfg = &pios_spi_sdcard_cfg, - }, - { - .cfg = &pios_spi_ahrs_cfg, - }, -}; - -uint8_t pios_spi_num_devices = NELEMENTS(pios_spi_devs); - -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_SDCARD_SPI); -} - +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_OPAHRS_SPI); + /* 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 */ @@ -334,200 +192,74 @@ const struct pios_usart_cfg pios_usart_telem_cfg = { }, }; - - -#ifdef PIOS_COM_AUX -/* - * AUX USART - */ -void PIOS_USART_aux_irq_handler(void); -void USART1_IRQHandler() __attribute__ ((alias ("PIOS_USART_aux_irq_handler"))); -const struct pios_usart_cfg pios_usart_aux_cfg = { - .regs = USART1, - .init = { - #if defined (PIOS_COM_AUX_BAUDRATE) - .USART_BaudRate = PIOS_COM_AUX_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 = { - .handler = PIOS_USART_aux_irq_handler, - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .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 - -/* - * Board specific number of devices. - */ -struct pios_usart_dev pios_usart_devs[] = { -#define PIOS_USART_TELEM 0 - { - .cfg = &pios_usart_telem_cfg, - }, -#ifdef PIOS_COM_AUX -#define PIOS_USART_AUX 1 - { - .cfg = &pios_usart_aux_cfg, - }, -#endif -}; - -uint8_t pios_usart_num_devices = NELEMENTS(pios_usart_devs); - +static uint32_t pios_usart_telem_rf_id; void PIOS_USART_telem_irq_handler(void) { - PIOS_USART_IRQ_Handler(PIOS_USART_TELEM); + PIOS_USART_IRQ_Handler(pios_usart_telem_rf_id); } +#endif /* PIOS_INCLUDE_USART */ -#ifdef PIOS_COM_AUX -void PIOS_USART_aux_irq_handler(void) -{ - PIOS_USART_IRQ_Handler(PIOS_USART_AUX); -} -#endif +#if defined(PIOS_INCLUDE_COM) +#include "pios_com_priv.h" -/* - * COM devices - */ +#endif /* PIOS_INCLUDE_COM */ -/* - * Board specific number of devices. - */ -extern const struct pios_com_driver pios_usart_com_driver; extern const struct pios_com_driver pios_usb_com_driver; -struct pios_com_dev pios_com_devs[] = { - { - .id = PIOS_USART_TELEM, - .driver = &pios_usart_com_driver, - }, +uint32_t pios_com_telem_rf_id; +uint32_t pios_com_telem_usb_id; + +#include "pios_opahrs.h" + +/** + * PIOS_Board_Init() + * initializes all the core subsystems on this specific hardware + * called from System/openpilot.c + */ +void PIOS_Board_Init(void) { + + + /* Enable Prefetch Buffer */ + FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); + + /* Flash 2 wait state */ + FLASH_SetLatency(FLASH_Latency_2); + + /* Delay system */ + PIOS_DELAY_Init(); + + /* Initialize the PiOS library */ +#if defined(PIOS_INCLUDE_COM) + if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) { + PIOS_DEBUG_Assert(0); + } + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) { + PIOS_DEBUG_Assert(0); + } +#endif /* PIOS_INCLUDE_COM */ + + PIOS_GPIO_Init(); + #if defined(PIOS_INCLUDE_USB_HID) - { - .id = 0, - .driver = &pios_usb_com_driver, - }, -#endif -#ifdef PIOS_COM_AUX - { - .id = PIOS_USART_AUX, - .driver = &pios_usart_com_driver, - }, -#endif + PIOS_USB_HID_Init(0); +#if defined(PIOS_INCLUDE_COM) + if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) { + PIOS_DEBUG_Assert(0); + } +#endif /* PIOS_INCLUDE_COM */ +#endif /* PIOS_INCLUDE_USB_HID */ -}; + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE);//TODO Tirar -const uint8_t pios_com_num_devices = NELEMENTS(pios_com_devs); + /* Set up the SPI interface to the AHRS */ + if (PIOS_SPI_Init(&pios_spi_ahrs_id, &pios_spi_ahrs_cfg)) { + PIOS_DEBUG_Assert(0); + } -/* - * 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"))); - -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 = { - .handler = PIOS_I2C_main_adapter_ev_irq_handler, - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_EV_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .error = { - .handler = PIOS_I2C_main_adapter_er_irq_handler, - .flags = 0, /* FIXME: check this */ - .init = { - .NVIC_IRQChannel = I2C2_ER_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, -}; - -/* - * Board specific number of devices. - */ -struct pios_i2c_adapter pios_i2c_adapters[] = { - { - .cfg = &pios_i2c_main_adapter_cfg, - }, -}; - -uint8_t pios_i2c_num_adapters = NELEMENTS(pios_i2c_adapters); - -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); -} - -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); + /* Bind the AHRS comms layer to the AHRS SPI link */ + PIOS_OPAHRS_Attach(pios_spi_ahrs_id); } /** diff --git a/flight/PiOS/Common/pios_opahrs_v0.c b/flight/PiOS/Common/pios_opahrs_v0.c index 94e013021..3dbc71a1d 100644 --- a/flight/PiOS/Common/pios_opahrs_v0.c +++ b/flight/PiOS/Common/pios_opahrs_v0.c @@ -36,9 +36,20 @@ #include "pios_opahrs_proto.h" #include "pios_opahrs.h" -void PIOS_OPAHRS_Init(void) +static uint32_t PIOS_OPAHRS_SPI; + +void PIOS_OPAHRS_Attach(uint32_t spi_id) { - PIOS_SPI_SetClockSpeed(PIOS_OPAHRS_SPI, PIOS_SPI_PRESCALER_8); + PIOS_OPAHRS_SPI = spi_id; +} + +void PIOS_OPAHRS_ForceSlaveSelected(bool selected) +{ + if (selected) { + PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 0); + } else { + PIOS_SPI_RC_PinSet(PIOS_OPAHRS_SPI, 1); + } } static int32_t opahrs_msg_txrx(const uint8_t * tx, uint8_t * rx, uint32_t len) diff --git a/flight/PiOS/inc/pios_opahrs.h b/flight/PiOS/inc/pios_opahrs.h index f0193c8c0..3b995ba7a 100644 --- a/flight/PiOS/inc/pios_opahrs.h +++ b/flight/PiOS/inc/pios_opahrs.h @@ -38,7 +38,8 @@ enum opahrs_result { OPAHRS_RESULT_FAILED, }; -extern void PIOS_OPAHRS_Init(void); +extern void PIOS_OPAHRS_Attach(uint32_t spi_id); +extern void PIOS_OPAHRS_ForceSlaveSelected(bool selected); /* * Protocol V0 messages used to talk to bootloaders