void jump_to_app() { const struct pios_board_info *bdinfo = &pios_board_info_blob; PIOS_LED_On(PIOS_LED_HEARTBEAT); // Look at cm3_vectors struct in startup. In a fw image the first uint32_t contains the address of the top of irqstack uint32_t fwIrqStackBase = (*(__IO uint32_t *)bdinfo->fw_base) & 0xFFFE0000; // Check for the two possible irqstack locations (sram or core coupled sram) if (fwIrqStackBase == 0x20000000 || fwIrqStackBase == 0x10000000) { /* Jump to user application */ FLASH_Lock(); RCC_APB2PeriphResetCmd(0xffffffff, ENABLE); RCC_APB1PeriphResetCmd(0xffffffff, ENABLE); RCC_APB2PeriphResetCmd(0xffffffff, DISABLE); RCC_APB1PeriphResetCmd(0xffffffff, DISABLE); #ifdef PIOS_INCLUDE_USB PIOS_USBHOOK_Deactivate(); #endif JumpAddress = *(__IO uint32_t *)(bdinfo->fw_base + 4); Jump_To_Application = (pFunction)JumpAddress; /* Initialize user application's Stack Pointer */ __set_MSP(*(__IO uint32_t *)bdinfo->fw_base); Jump_To_Application(); } else { DeviceState = failed_jump; return; } }
/** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ void PIOS_Board_Init(void) { /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); PIOS_LED_On(LED1); /* Delay system */ PIOS_DELAY_Init(); /* Communication system */ #if !defined(PIOS_ENABLE_DEBUG_PINS) #if defined(PIOS_INCLUDE_COM) if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id)) { PIOS_DEBUG_Assert(0); } #endif /* PIOS_INCLUDE_COM */ #endif /* IAP System Setup */ PIOS_IAP_Init(); /* ADC system */ PIOS_ADC_Init(); extern uint8_t adc_oversampling; PIOS_ADC_Config(adc_oversampling); extern void adc_callback(float *); PIOS_ADC_SetCallback(adc_callback); /* ADC buffer */ extern t_fifo_buffer adc_fifo_buffer; fifoBuf_init(&adc_fifo_buffer, adc_fifo_buf, sizeof(adc_fifo_buf)); /* Setup the Accelerometer FS (Full-Scale) GPIO */ PIOS_GPIO_Enable(0); SET_ACCEL_6G; #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) /* Magnetic sensor system */ if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) { PIOS_DEBUG_Assert(0); } PIOS_HMC5843_Init(); #endif #if defined(PIOS_INCLUDE_SPI) #include "ahrs_spi_comm.h" AhrsInitComms(); /* 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); } AhrsConnect(pios_spi_op_id); #endif }
void error(int led) { for (;;) { PIOS_LED_On(led); PIOS_DELAY_WaitmS(500); PIOS_LED_Off(led); PIOS_DELAY_WaitmS(500); } }
void PIOS_Board_Init() { /* Delay system */ PIOS_DELAY_Init(); #if defined(PIOS_INCLUDE_LED) PIOS_LED_Init(&pios_led_cfg); #endif /* PIOS_INCLUDE_LED */ PWR_BackupAccessCmd(ENABLE); RCC_LSEConfig(RCC_LSE_OFF); PIOS_LED_On(PIOS_LED_HEARTBEAT); #if defined(PIOS_INCLUDE_SPI) /* Set up the SPI interface to the flash */ if (PIOS_SPI_Init(&pios_spi_flash_id, &pios_spi_flash_cfg)) { PIOS_DEBUG_Assert(0); } #endif /* PIOS_INCLUDE_SPI */ #if defined(PIOS_INCLUDE_FLASH) /* Inititialize all flash drivers */ PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg); #if defined(PIOS_INCLUDE_SPI) PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_flash_id, 0, &flash_mx25_cfg); #endif /* PIOS_INCLUDE_SPI */ /* Register the partition table */ PIOS_FLASH_register_partition_table(pios_flash_partition_table, NELEMENTS(pios_flash_partition_table)); #endif /* PIOS_INCLUDE_FLASH */ #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(); uintptr_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_HID) && defined(PIOS_INCLUDE_COM_MSG) uintptr_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_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_MSG */ PIOS_USBHOOK_Activate(); #endif /* PIOS_INCLUDE_USB */ }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { portTickType lastSysTime; /* create all modules thread */ MODULE_TASKCREATE_ALL // Initialize vars idleCounter = 0; idleCounterClear = 0; lastSysTime = xTaskGetTickCount(); // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectCallback(&objectUpdatedCb); // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(DIAGNOSTICS) updateI2Cstats(); updateWDGstats(); #endif // Update the task status object TaskMonitorUpdateAll(); // Flash the heartbeat LED PIOS_LED_Toggle(LED1); // Turn on the error LED if an alarm is set #if (PIOS_LED_NUM > 1) if (AlarmsHasErrors()) { PIOS_LED_Toggle(LED2); } else if (AlarmsHasWarnings()) { PIOS_LED_On(LED2); } else { PIOS_LED_Off(LED2); } #endif FlightStatusData flightStatus; FlightStatusGet(&flightStatus); // Wait until next period if(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) ); } else { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS); } } }
void error(int led, int code) { for (;;) { PIOS_DELAY_WaitmS(1000); for (int x = 0; x < code; x++) { PIOS_LED_On(led); PIOS_DELAY_WaitmS(200); PIOS_LED_Off(led); PIOS_DELAY_WaitmS(1000); } PIOS_DELAY_WaitmS(3000); } }
/** * @brief Bootloader Main function */ int main() { uint8_t GO_dfu = false; /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); /* Enable Prefetch Buffer */ FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable); /* Flash 2 wait state */ FLASH_SetLatency(FLASH_Latency_2); RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); /* Delay system */ PIOS_DELAY_Init(); for (uint32_t t = 0; t < 10000000; ++t) { if (NSS_HOLD_STATE == 1) GO_dfu = TRUE; else { GO_dfu = FALSE; 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) { jump_to_app(); } if (PIOS_IAP_CheckRequest()) { PIOS_DELAY_WaitmS(1000); PIOS_IAP_ClearRequest(); } PIOS_Board_Init(); boot_status = idle; Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc(); PIOS_LED_On(LED1); while (1) { process_spi_request(); } return 0; }
/** * Reports the name of the source file and the source line number * where the assert_param error has occurred. * \param[in] file pointer to the source file name * \param[in] line assert_param error line source number * \retval None */ void assert_failed(uint8_t *file, uint32_t line) { /* When serial debugging is implemented, use something like this. */ /* printf("Wrong parameters value: file %s on line %d\r\n", file, 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); /* Infinite loop */ while (1) { PIOS_LED_Toggle(LED1); PIOS_LED_Toggle(LED2); for (int i = 0; i < 1000000; i++) { ; } } }
void jump_to_app() { const struct pios_board_info * bdinfo = &pios_board_info_blob; 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); PIOS_USBHOOK_Deactivate(); JumpAddress = *(__IO uint32_t*) (bdinfo->fw_base + 4); Jump_To_Application = (pFunction) JumpAddress; /* Initialize user application's Stack Pointer */ __set_MSP(*(__IO uint32_t*) bdinfo->fw_base); Jump_To_Application(); } else { DeviceState = failed_jump; return; } }
void jump_to_app() { const struct pios_board_info * bdinfo = &pios_board_info_blob; PIOS_LED_On(LED1); 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; /* Initialize user application's Stack Pointer */ __set_MSP(*(__IO uint32_t*) bdinfo->fw_base); Jump_To_Application(); } else { boot_status = jump_failed; return; } }
int main() { PIOS_SYS_Init(); PIOS_Board_Init(); PIOS_LED_On(PIOS_LED_HEARTBEAT); PIOS_DELAY_WaitmS(3000); PIOS_LED_Off(PIOS_LED_HEARTBEAT); /// Self overwrite check uint32_t base_address = SCB->VTOR; if ((0x08000000 + embedded_image_size) > base_address) error(PIOS_LED_HEARTBEAT, 1); /// /* * Make sure the bootloader we're carrying is for the same * board type and board revision as the one we're running on. * * Assume the bootloader in flash and the bootloader contained in * the updater both carry a board_info_blob at the end of the image. */ /* Calculate how far the board_info_blob is from the beginning of the bootloader */ uint32_t board_info_blob_offset = (uint32_t) &pios_board_info_blob - (uint32_t)0x08000000; /* Use the same offset into our embedded bootloader image */ struct pios_board_info * new_board_info_blob = (struct pios_board_info *) ((uint32_t)embedded_image_start + board_info_blob_offset); /* Compare the two board info blobs to make sure they're for the same HW revision */ if ((pios_board_info_blob.magic != new_board_info_blob->magic) || (pios_board_info_blob.board_type != new_board_info_blob->board_type) || (pios_board_info_blob.board_rev != new_board_info_blob->board_rev)) { error(PIOS_LED_HEARTBEAT, 2); } /* Embedded bootloader looks like it's the right one for this HW, proceed... */ FLASH_Unlock(); /// Bootloader memory space erase uint32_t pageAddress; pageAddress = 0x08000000; bool fail = false; while ((pageAddress < base_address) && (fail == false)) { for (int retry = 0; retry < MAX_DEL_RETRYS; ++retry) { if (FLASH_ErasePage(pageAddress) == FLASH_COMPLETE) { fail = false; break; } else { fail = true; } } #ifdef STM32F10X_HD pageAddress += 2048; #elif defined (STM32F10X_MD) pageAddress += 1024; #endif } if (fail == true) error(PIOS_LED_HEARTBEAT, 3); /// /// Bootloader programing for (uint32_t offset = 0; offset < embedded_image_size / sizeof(uint32_t); ++offset) { bool result = false; 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]) == FLASH_COMPLETE) ? true : false; } } if (result == false) error(PIOS_LED_HEARTBEAT, 4); } /// for (uint8_t x = 0; x < 3; ++x) { PIOS_LED_On(PIOS_LED_HEARTBEAT); PIOS_DELAY_WaitmS(1000); PIOS_LED_Off(PIOS_LED_HEARTBEAT); PIOS_DELAY_WaitmS(1000); } /// Invalidate the bootloader updater so we won't run /// the update again on the next power cycle. FLASH_ProgramWord(base_address, 0); FLASH_Lock(); for (;;) { PIOS_DELAY_WaitmS(1000); } }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { /* create all modules thread */ MODULE_TASKCREATE_ALL; if (PIOS_heap_malloc_failed_p()) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } #if defined(PIOS_INCLUDE_IAP) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif // Initialize vars idleCounter = 0; idleCounterClear = 0; // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectQueue(objectPersistenceQueue); #if (defined(COPTERCONTROL) || defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX)) // Run this initially to make sure the configuration is checked configuration_check(); // Whenever the configuration changes, make sure it is safe to fly if (StabilizationSettingsHandle()) StabilizationSettingsConnectCallback(configurationUpdatedCb); if (SystemSettingsHandle()) SystemSettingsConnectCallback(configurationUpdatedCb); if (ManualControlSettingsHandle()) ManualControlSettingsConnectCallback(configurationUpdatedCb); if (FlightStatusHandle()) FlightStatusConnectCallback(configurationUpdatedCb); #endif #if (defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX)) if (StateEstimationHandle()) StateEstimationConnectCallback(configurationUpdatedCb); #endif // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(WDG_STATS_DIAGNOSTICS) updateWDGstats(); #endif #if defined(DIAG_TASKS) // Update the task status object TaskMonitorUpdateAll(); #endif // Flash the heartbeat LED #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF); #endif /* PIOS_LED_HEARTBEAT */ // Turn on the error LED if an alarm is set if (indicateError()) { #if defined (PIOS_LED_ALARM) PIOS_LED_On(PIOS_LED_ALARM); #endif /* PIOS_LED_ALARM */ } else { #if defined (PIOS_LED_ALARM) PIOS_LED_Off(PIOS_LED_ALARM); #endif /* PIOS_LED_ALARM */ } FlightStatusData flightStatus; FlightStatusGet(&flightStatus); UAVObjEvent ev; int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ? SYSTEM_UPDATE_PERIOD_MS / (LED_BLINK_RATE_HZ * 2) : SYSTEM_UPDATE_PERIOD_MS; if (PIOS_Queue_Receive(objectPersistenceQueue, &ev, delayTime) == true) { // If object persistence is updated call the callback objectUpdatedCb(&ev); } } }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { /* create all modules thread */ MODULE_TASKCREATE_ALL; if (mallocFailed) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } #if defined(PIOS_INCLUDE_IAP) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif // Initialize vars idleCounter = 0; idleCounterClear = 0; // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectQueue(objectPersistenceQueue); // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(I2C_WDG_STATS_DIAGNOSTICS) updateI2Cstats(); updateWDGstats(); #endif #if defined(DIAG_TASKS) // Update the task status object TaskMonitorUpdateAll(); #endif // Flash the heartbeat LED #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 defined (PIOS_LED_ALARM) if (AlarmsHasWarnings()) { PIOS_LED_On(PIOS_LED_ALARM); } else { PIOS_LED_Off(PIOS_LED_ALARM); } #endif /* PIOS_LED_ALARM */ FlightStatusData flightStatus; FlightStatusGet(&flightStatus); UAVObjEvent ev; int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ? SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) : SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS; if(xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) { // If object persistence is updated call the callback objectUpdatedCb(&ev); } } }
void PIOS_Board_Init(void) { /* Delay system */ PIOS_DELAY_Init(); const struct pios_board_info * bdinfo = &pios_board_info_blob; #if defined(PIOS_INCLUDE_LED) const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); PIOS_Assert(led_cfg); PIOS_LED_Init(led_cfg); #endif /* PIOS_INCLUDE_LED */ /* Set up the SPI interface to the gyro/acelerometer */ if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } /* Set up the SPI interface to the flash and rfm22b */ if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_FLASH) /* Inititialize all flash drivers */ if (PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_telem_flash_id, 1, &flash_m25p_cfg) != 0) panic(1); if (PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg) != 0) panic(1); /* Register the partition table */ const struct pios_flash_partition * flash_partition_table; uint32_t num_partitions; flash_partition_table = PIOS_BOARD_HW_DEFS_GetPartitionTable(bdinfo->board_rev, &num_partitions); PIOS_FLASH_register_partition_table(flash_partition_table, num_partitions); /* Mount all filesystems */ if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_settings_cfg, FLASH_PARTITION_LABEL_SETTINGS) != 0) panic(1); if (PIOS_FLASHFS_Logfs_Init(&pios_waypoints_settings_fs_id, &flashfs_waypoints_cfg, FLASH_PARTITION_LABEL_WAYPOINTS) != 0) panic(1); #if defined(ERASE_FLASH) PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); #endif #endif /* PIOS_INCLUDE_FLASH */ /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); /* Initialize the alarms library */ AlarmsInitialize(); HwFreedomInitialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) PIOS_RTC_Init(&pios_rtc_main_cfg); #endif #ifndef ERASE_FLASH /* Initialize watchdog as early as possible to catch faults during init * but do it only if there is no debugger connected */ if ((CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) == 0) { PIOS_WDG_Init(); } #endif // /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_3_cfg); /* IAP System Setup */ 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 hw config to defaults */ HwFreedomSetDefaults(HwFreedomHandle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; #endif uintptr_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwFreedomUSB_VCPPortGet(&hw_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hw_usb_vcpport = HWFREEDOM_USB_VCPPORT_DISABLED; } uintptr_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } switch (hw_usb_vcpport) { case HWFREEDOM_USB_VCPPORT_DISABLED: break; case HWFREEDOM_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_PICOC: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_PICOC_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_PICOC_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_picoc_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_PICOC_RX_BUF_LEN, tx_buffer, PIOS_COM_PICOC_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 hw_usb_hidport; HwFreedomUSB_HIDPortGet(&hw_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hw_usb_hidport = HWFREEDOM_USB_HIDPORT_DISABLED; } uintptr_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } switch (hw_usb_hidport) { case HWFREEDOM_USB_HIDPORT_DISABLED: break; case HWFREEDOM_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_HID */ if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } #endif /* PIOS_INCLUDE_USB */ /* Configure IO ports */ uint8_t hw_DSMxBind; HwFreedomDSMxBindGet(&hw_DSMxBind); /* Configure FlexiPort */ uint8_t hw_mainport; HwFreedomMainPortGet(&hw_mainport); switch (hw_mainport) { case HWFREEDOM_MAINPORT_DISABLED: break; case HWFREEDOM_MAINPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWFREEDOM_MAINPORT_GPS: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_RX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_MAINPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { PIOS_Board_configure_dsm(&pios_usart_dsm_hsum_main_cfg, &pios_dsm_main_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFREEDOM_MAINPORT_HOTTSUMD: case HWFREEDOM_MAINPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_mainport) { case HWFREEDOM_MAINPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFREEDOM_MAINPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_main_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFREEDOM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_MAINPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; case HWFREEDOM_MAINPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWFREEDOM_MAINPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_GPS) #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); pios_com_mavlink_id = pios_com_gps_id; #endif /* PIOS_INCLUDE_MAVLINK */ #endif /* PIOS_INCLUDE_GPS */ break; case HWFREEDOM_MAINPORT_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); #endif /* PIOS_INCLUDE_HOTT */ break; case HWFREEDOM_MAINPORT_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sensor_hub_id); #endif /* PIOS_INCLUDE_FRSKY_SENSOR_HUB */ break; case HWFREEDOM_MAINPORT_LIGHTTELEMETRYTX: #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_lighttelemetry_id); #endif break; case HWFREEDOM_MAINPORT_PICOC: #if defined(PIOS_INCLUDE_PICOC) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_PICOC_RX_BUF_LEN, PIOS_COM_PICOC_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_picoc_id); #endif /* PIOS_INCLUDE_PICOC */ break; case HWFREEDOM_MAINPORT_FRSKYSPORTTELEMETRY: #if defined(PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_FRSKYSPORT_RX_BUF_LEN, PIOS_COM_FRSKYSPORT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sport_id); #endif /* PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY */ break; } /* hw_freedom_mainport */ /* Configure flexi USART port */ uint8_t hw_flexiport; HwFreedomFlexiPortGet(&hw_flexiport); switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_DISABLED: break; case HWFREEDOM_FLEXIPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWFREEDOM_FLEXIPORT_GPS: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_RX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_FLEXIPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_hsum_flexi_cfg, &pios_dsm_flexi_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFREEDOM_FLEXIPORT_HOTTSUMD: case HWFREEDOM_FLEXIPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFREEDOM_FLEXIPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_flexi_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFREEDOM_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_I2C */ break; case HWFREEDOM_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_FLEXIPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; case HWFREEDOM_FLEXIPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWFREEDOM_FLEXIPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_GPS) #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); pios_com_mavlink_id = pios_com_gps_id; #endif /* PIOS_INCLUDE_MAVLINK */ #endif /* PIOS_INCLUDE_GPS */ break; case HWFREEDOM_FLEXIPORT_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); #endif /* PIOS_INCLUDE_HOTT */ break; case HWFREEDOM_FLEXIPORT_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sensor_hub_id); #endif /* PIOS_INCLUDE_FRSKY_SENSOR_HUB */ break; case HWFREEDOM_FLEXIPORT_LIGHTTELEMETRYTX: #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_lighttelemetry_id); #endif break; case HWFREEDOM_FLEXIPORT_PICOC: #if defined(PIOS_INCLUDE_PICOC) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_PICOC_RX_BUF_LEN, PIOS_COM_PICOC_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_picoc_id); #endif /* PIOS_INCLUDE_PICOC */ break; case HWFREEDOM_FLEXIPORT_FRSKYSPORTTELEMETRY: #if defined(PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_FRSKYSPORT_RX_BUF_LEN, PIOS_COM_FRSKYSPORT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sport_id); #endif /* PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY */ break; } /* hw_freedom_flexiport */ /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) RFM22BStatusInitialize(); RFM22BStatusCreateInstance(); // Initialize out status object. RFM22BStatusData rfm22bstatus; RFM22BStatusGet(&rfm22bstatus); RFM22BStatusInstSet(1,&rfm22bstatus); HwFreedomData hwFreedom; HwFreedomGet(&hwFreedom); // Initialize out status object. rfm22bstatus.BoardType = bdinfo->board_type; rfm22bstatus.BoardRevision = bdinfo->board_rev; if (hwFreedom.Radio == HWFREEDOM_RADIO_DISABLED || hwFreedom.MaxRfPower == HWFREEDOM_MAXRFPOWER_0) { // When radio disabled, it is ok for init to fail. This allows boards without populating // this component. const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg) == 0) { PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); rfm22bstatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); rfm22bstatus.BoardRevision = PIOS_RFM22B_ModuleVersion(pios_rfm22b_id); } else { pios_rfm22b_id = 0; } rfm22bstatus.LinkState = RFM22BSTATUS_LINKSTATE_DISABLED; } else { // always allow receiving PPM when radio is on bool ppm_mode = hwFreedom.Radio == HWFREEDOM_RADIO_TELEMPPM || hwFreedom.Radio == HWFREEDOM_RADIO_PPM; bool ppm_only = hwFreedom.Radio == HWFREEDOM_RADIO_PPM; bool is_oneway = hwFreedom.Radio == HWFREEDOM_RADIO_PPM; // Sparky2 can only receive PPM only /* Configure the RFM22B device. */ const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { panic(6); } rfm22bstatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); rfm22bstatus.BoardRevision = PIOS_RFM22B_ModuleVersion(pios_rfm22b_id); /* Configure the radio com interface */ uint8_t *rx_buffer = (uint8_t *)PIOS_malloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *)PIOS_malloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { panic(6); } /* Set Telemetry to use RFM22b if no other telemetry is configured (USB always overrides anyway) */ if (!pios_com_telem_rf_id) { pios_com_telem_rf_id = pios_com_rf_id; } rfm22bstatus.LinkState = RFM22BSTATUS_LINKSTATE_ENABLED; enum rfm22b_datarate datarate = RFM22_datarate_64000; switch (hwFreedom.MaxRfSpeed) { case HWFREEDOM_MAXRFSPEED_9600: datarate = RFM22_datarate_9600; break; case HWFREEDOM_MAXRFSPEED_19200: datarate = RFM22_datarate_19200; break; case HWFREEDOM_MAXRFSPEED_32000: datarate = RFM22_datarate_32000; break; case HWFREEDOM_MAXRFSPEED_64000: datarate = RFM22_datarate_64000; break; case HWFREEDOM_MAXRFSPEED_100000: datarate = RFM22_datarate_100000; break; case HWFREEDOM_MAXRFSPEED_192000: datarate = RFM22_datarate_192000; break; } /* Set the radio configuration parameters. */ PIOS_RFM22B_Config(pios_rfm22b_id, datarate, hwFreedom.MinChannel, hwFreedom.MaxChannel, hwFreedom.CoordID, is_oneway, ppm_mode, ppm_only); /* Set the modem Tx poer level */ switch (hwFreedom.MaxRfPower) { case HWFREEDOM_MAXRFPOWER_125: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); break; case HWFREEDOM_MAXRFPOWER_16: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); break; case HWFREEDOM_MAXRFPOWER_316: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); break; case HWFREEDOM_MAXRFPOWER_63: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); break; case HWFREEDOM_MAXRFPOWER_126: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); break; case HWFREEDOM_MAXRFPOWER_25: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); break; case HWFREEDOM_MAXRFPOWER_50: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); break; case HWFREEDOM_MAXRFPOWER_100: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); break; default: // do nothing break; } /* Reinitialize the modem. */ PIOS_RFM22B_Reinit(pios_rfm22b_id); #if defined(PIOS_INCLUDE_RFM22B_RCVR) { uintptr_t pios_rfm22brcvr_id; PIOS_RFM22B_Rcvr_Init(&pios_rfm22brcvr_id, pios_rfm22b_id); uintptr_t pios_rfm22brcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_rfm22brcvr_rcvr_id, &pios_rfm22b_rcvr_driver, pios_rfm22brcvr_id)) { panic(6); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_RFM22B] = pios_rfm22brcvr_rcvr_id; } } RFM22BStatusInstSet(1,&rfm22bstatus); #endif /* PIOS_INCLUDE_RFM22B_RCVR */ #endif /* PIOS_INCLUDE_RFM22B */ PIOS_WDG_Clear(); /* Configure input receiver USART port */ uint8_t hw_rcvrport; HwFreedomRcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_DISABLED: break; case HWFREEDOM_RCVRPORT_PPM: { uintptr_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); uintptr_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } break; case HWFREEDOM_RCVRPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_hsum_rcvr_cfg, &pios_dsm_rcvr_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMRCVRPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFREEDOM_RCVRPORT_HOTTSUMD: case HWFREEDOM_RCVRPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFREEDOM_RCVRPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_rcvr_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFREEDOM_RCVRPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_rcvr_cfg)) { PIOS_Assert(0); } uintptr_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } uintptr_t pios_sbus_rcvr_id; if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; } #endif break; } if (hw_rcvrport != HWFREEDOM_RCVRPORT_SBUS) { GPIO_Init(pios_sbus_cfg.inv.gpio, (GPIO_InitTypeDef*)&pios_sbus_cfg.inv.init); GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); } #if defined(PIOS_OVERO_SPI) /* Set up the SPI based PIOS_COM interface to the overo */ { bool overo_enabled = false; #ifdef MODULE_OveroSync_BUILTIN overo_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_OVEROSYNC] == MODULESETTINGS_ADMINSTATE_ENABLED) { overo_enabled = true; } else { overo_enabled = false; } #endif if (overo_enabled) { if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { PIOS_DEBUG_Assert(0); } const uint32_t PACKET_SIZE = 1024; uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PACKET_SIZE); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PACKET_SIZE); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_overo_id, &pios_overo_com_driver, pios_overo_id, rx_buffer, PACKET_SIZE, tx_buffer, PACKET_SIZE)) { PIOS_Assert(0); } } } #endif #if defined(PIOS_INCLUDE_GCSRCVR) GCSReceiverInitialize(); uintptr_t pios_gcsrcvr_id; PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); uintptr_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS uint8_t hw_output_port; HwFreedomOutputGet(&hw_output_port); switch (hw_output_port) { case HWFREEDOM_OUTPUT_DISABLED: break; case HWFREEDOM_OUTPUT_PWM: /* Set up the servo outputs */ PIOS_Servo_Init(&pios_servo_cfg); break; default: break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif PIOS_DELAY_WaitmS(200); PIOS_SENSORS_Init(); #if defined(PIOS_INCLUDE_MPU6000) if (PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg) != 0) panic(2); if (PIOS_MPU6000_Test() != 0) panic(2); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwFreedomGyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWFREEDOM_GYRORANGE_250: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWFREEDOM_GYRORANGE_500: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWFREEDOM_GYRORANGE_1000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWFREEDOM_GYRORANGE_2000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwFreedomAccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWFREEDOM_ACCELRANGE_2G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWFREEDOM_ACCELRANGE_4G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWFREEDOM_ACCELRANGE_8G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWFREEDOM_ACCELRANGE_16G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_16G); break; } // the filter has to be set before rate else divisor calculation will fail uint8_t hw_mpu6000_dlpf; HwFreedomMPU6000DLPFGet(&hw_mpu6000_dlpf); enum pios_mpu60x0_filter mpu6000_dlpf = \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ pios_mpu6000_cfg.default_filter; PIOS_MPU6000_SetLPF(mpu6000_dlpf); uint8_t hw_mpu6000_samplerate; HwFreedomMPU6000RateGet(&hw_mpu6000_samplerate); uint16_t mpu6000_samplerate = \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_200) ? 200 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_333) ? 333 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_500) ? 500 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_666) ? 666 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_1000) ? 1000 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_2000) ? 2000 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_4000) ? 4000 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_8000) ? 8000 : \ pios_mpu6000_cfg.default_samplerate; PIOS_MPU6000_SetSampleRate(mpu6000_samplerate); #endif if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_mag_pressure_adapter_id) != 0) panic(5); PIOS_DELAY_WaitmS(50); #if defined(PIOS_INCLUDE_ADC) uint32_t internal_adc_id; PIOS_INTERNAL_ADC_Init(&internal_adc_id, &pios_adc_cfg); PIOS_ADC_Init(&pios_internal_adc_id, &pios_internal_adc_driver, internal_adc_id); #endif PIOS_LED_On(0); #if defined(PIOS_INCLUDE_HMC5883) if (PIOS_HMC5883_Init(pios_i2c_mag_pressure_adapter_id, &pios_hmc5883_cfg) != 0) panic(3); #endif PIOS_LED_On(1); #if defined(PIOS_INCLUDE_MS5611) if (PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id) != 0) panic(4); #endif PIOS_LED_On(2); }
void PIOS_Board_Init(void) { /* Delay system */ PIOS_DELAY_Init(); const struct pios_board_info * bdinfo = &pios_board_info_blob; #if defined(PIOS_INCLUDE_LED) const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); PIOS_Assert(led_cfg); PIOS_LED_Init(led_cfg); #endif /* PIOS_INCLUDE_LED */ /* Set up the SPI interface to the gyro/acelerometer */ if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } /* Set up the SPI interface to the flash and rfm22b */ if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_FLASH) /* Connect flash to the approrpiate interface and configure it */ uintptr_t flash_id; if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1, &flash_m25p_cfg) != 0) panic(1); uintptr_t fs_id; if (PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id) != 0) panic(1); #endif /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); HwFreedomInitialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* Initialize the alarms library */ AlarmsInitialize(); /* Initialize the task monitor library */ TaskMonitorInitialize(); // /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_3_cfg); /* IAP System Setup */ 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 hw config to defaults */ HwFreedomSetDefaults(HwFreedomHandle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; #endif uint32_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwFreedomUSB_VCPPortGet(&hw_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hw_usb_vcpport = HWFREEDOM_USB_VCPPORT_DISABLED; } switch (hw_usb_vcpport) { case HWFREEDOM_USB_VCPPORT_DISABLED: break; case HWFREEDOM_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_telem_usb_id); #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_vcp_id); #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uint32_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwFreedomUSB_HIDPortGet(&hw_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hw_usb_hidport = HWFREEDOM_USB_HIDPORT_DISABLED; } switch (hw_usb_hidport) { case HWFREEDOM_USB_HIDPORT_DISABLED: break; case HWFREEDOM_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 */ if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } #endif /* PIOS_INCLUDE_USB */ /* Configure IO ports */ uint8_t hw_DSMxBind; HwFreedomDSMxBindGet(&hw_DSMxBind); /* Configure FlexiPort */ uint8_t hw_mainport; HwFreedomMainPortGet(&hw_mainport); switch (hw_mainport) { case HWFREEDOM_MAINPORT_DISABLED: break; case HWFREEDOM_MAINPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; break; case HWFREEDOM_MAINPORT_GPS: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_MAINPORT_DSM2: case HWFREEDOM_MAINPORT_DSMX10BIT: case HWFREEDOM_MAINPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hw_mainport) { case HWFREEDOM_MAINPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWFREEDOM_MAINPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWFREEDOM_MAINPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hw_DSMxBind); } break; case HWFREEDOM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_MAINPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; } /* hw_freedom_mainport */ /* Configure flexi USART port */ uint8_t hw_flexiport; HwFreedomFlexiPortGet(&hw_flexiport); switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_DISABLED: break; case HWFREEDOM_FLEXIPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWFREEDOM_FLEXIPORT_GPS: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_FLEXIPORT_DSM2: case HWFREEDOM_FLEXIPORT_DSMX10BIT: case HWFREEDOM_FLEXIPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWFREEDOM_FLEXIPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWFREEDOM_FLEXIPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,&hw_DSMxBind); } break; case HWFREEDOM_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_I2C */ case HWFREEDOM_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_FLEXIPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; } /* hw_freedom_flexiport */ /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) uint8_t hwsettings_radioport; HwFreedomRadioPortGet(&hwsettings_radioport); switch (hwsettings_radioport) { case HWFREEDOM_RADIOPORT_DISABLED: break; case HWFREEDOM_RADIOPORT_TELEMETRY: { const struct pios_board_info * bdinfo = &pios_board_info_blob; const struct pios_rfm22b_cfg *pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) { PIOS_Assert(0); } uint8_t *rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { PIOS_Assert(0); } break; } } #endif /* PIOS_INCLUDE_RFM22B */ /* Configure input receiver USART port */ uint8_t hw_rcvrport; HwFreedomRcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_DISABLED: break; case HWFREEDOM_RCVRPORT_PPM: { uint32_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); uint32_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } break; case HWFREEDOM_RCVRPORT_DSM2: case HWFREEDOM_RCVRPORT_DSMX10BIT: case HWFREEDOM_RCVRPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWFREEDOM_RCVRPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWFREEDOM_RCVRPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_rcvr_cfg, &pios_dsm_rcvr_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,&hw_DSMxBind); } break; case HWFREEDOM_RCVRPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) { uint32_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_rcvr_cfg)) { PIOS_Assert(0); } uint32_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } uint32_t pios_sbus_rcvr_id; if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; } #endif break; } if (hw_rcvrport != HWFREEDOM_RCVRPORT_SBUS) { GPIO_Init(pios_sbus_cfg.inv.gpio, (GPIO_InitTypeDef*)&pios_sbus_cfg.inv.init); GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); } #if defined(PIOS_OVERO_SPI) /* Set up the SPI based PIOS_COM interface to the overo */ { bool overo_enabled = false; #ifdef MODULE_OveroSync_BUILTIN overo_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_OVEROSYNC] == MODULESETTINGS_ADMINSTATE_ENABLED) { overo_enabled = true; } else { overo_enabled = false; } #endif if (overo_enabled) { if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { PIOS_DEBUG_Assert(0); } const uint32_t PACKET_SIZE = 1024; uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_overo_id, &pios_overo_com_driver, pios_overo_id, rx_buffer, PACKET_SIZE, tx_buffer, PACKET_SIZE)) { PIOS_Assert(0); } } } #endif #if defined(PIOS_INCLUDE_GCSRCVR) GCSReceiverInitialize(); uint32_t pios_gcsrcvr_id; PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); uint32_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS uint8_t hw_output_port; HwFreedomOutputGet(&hw_output_port); switch (hw_output_port) { case HWFREEDOM_OUTPUT_DISABLED: break; case HWFREEDOM_OUTPUT_PWM: /* Set up the servo outputs */ PIOS_Servo_Init(&pios_servo_cfg); break; default: break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif PIOS_DELAY_WaitmS(200); PIOS_SENSORS_Init(); #if defined(PIOS_INCLUDE_MPU6000) if (PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg) != 0) panic(2); if (PIOS_MPU6000_Test() != 0) panic(2); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwFreedomGyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWFREEDOM_GYRORANGE_250: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWFREEDOM_GYRORANGE_500: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWFREEDOM_GYRORANGE_1000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWFREEDOM_GYRORANGE_2000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwFreedomAccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWFREEDOM_ACCELRANGE_2G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWFREEDOM_ACCELRANGE_4G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWFREEDOM_ACCELRANGE_8G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWFREEDOM_ACCELRANGE_16G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_16G); break; } uint8_t hw_mpu6000_rate; HwFreedomMPU6000RateGet(&hw_mpu6000_rate); uint16_t hw_mpu6000_divisor = \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_500) ? 15 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_666) ? 11 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_1000) ? 7 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_2000) ? 3 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_4000) ? 1 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_8000) ? 0 : \ 15; PIOS_MPU6000_SetDivisor(hw_mpu6000_divisor); uint8_t hw_dlpf; HwFreedomMPU6000DLPFGet(&hw_dlpf); uint16_t hw_mpu6000_dlpf = \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ PIOS_MPU60X0_LOWPASS_256_HZ; PIOS_MPU6000_SetLPF(hw_mpu6000_dlpf); #endif if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_mag_pressure_adapter_id) != 0) panic(5); PIOS_DELAY_WaitmS(50); #if defined(PIOS_INCLUDE_ADC) PIOS_ADC_Init(&pios_adc_cfg); #endif PIOS_LED_On(0); #if defined(PIOS_INCLUDE_HMC5883) if (PIOS_HMC5883_Init(pios_i2c_mag_pressure_adapter_id, &pios_hmc5883_cfg) != 0) panic(3); #endif PIOS_LED_On(1); #if defined(PIOS_INCLUDE_MS5611) if (PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id) != 0) panic(4); #endif PIOS_LED_On(2); }
int main() { PIOS_SYS_Init(); PIOS_Board_Init(); PIOS_IAP_Init(); USB_connected = PIOS_USB_CheckAvailable(0); if (PIOS_IAP_CheckRequest() == true) { PIOS_DELAY_WaitmS(1000); User_DFU_request = true; PIOS_IAP_ClearRequest(); } GO_dfu = (USB_connected == true) || (User_DFU_request == true); if (GO_dfu == true) { PIOS_Board_Init(); if (User_DFU_request == true) DeviceState = DFUidle; else DeviceState = BLidle; } else JumpToApp = true; uint32_t stopwatch = 0; uint32_t prev_ticks = PIOS_DELAY_GetuS(); while (true) { /* Update the stopwatch */ uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); prev_ticks += elapsed_ticks; stopwatch += elapsed_ticks; if (JumpToApp == true) jump_to_app(); switch (DeviceState) { case Last_operation_Success: case uploadingStarting: case DFUidle: period1 = 5000; sweep_steps1 = 100; PIOS_LED_Off(PIOS_LED_HEARTBEAT); period2 = 0; break; case uploading: period1 = 5000; sweep_steps1 = 100; period2 = 2500; sweep_steps2 = 50; break; case downloading: period1 = 2500; sweep_steps1 = 50; PIOS_LED_Off(PIOS_LED_HEARTBEAT); period2 = 0; break; case BLidle: period1 = 0; PIOS_LED_On(PIOS_LED_HEARTBEAT); period2 = 0; break; default://error period1 = 5000; sweep_steps1 = 100; period2 = 5000; sweep_steps2 = 100; } if (period1 != 0) { if (LedPWM(period1, sweep_steps1, stopwatch)) PIOS_LED_On(PIOS_LED_HEARTBEAT); else PIOS_LED_Off(PIOS_LED_HEARTBEAT); } else PIOS_LED_On(PIOS_LED_HEARTBEAT); if (period2 != 0) { if (LedPWM(period2, sweep_steps2, stopwatch)) PIOS_LED_On(PIOS_LED_HEARTBEAT); else PIOS_LED_Off(PIOS_LED_HEARTBEAT); } else PIOS_LED_Off(PIOS_LED_HEARTBEAT); if (stopwatch > 50 * 1000 * 1000) stopwatch = 0; if ((stopwatch > 6 * 1000 * 1000) && (DeviceState == BLidle)) JumpToApp = true; processRX(); DataDownload(start); } }
/** * System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS */ static void systemTask(void *parameters) { portTickType lastSysTime; /* create all modules thread */ MODULE_TASKCREATE_ALL; if (mallocFailed) { /* We failed to malloc during task creation, * system behaviour is undefined. Reset and let * the BootFault code recover for us. */ PIOS_SYS_Reset(); } #if defined(PIOS_INCLUDE_IAP) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif // Initialize vars idleCounter = 0; idleCounterClear = 0; lastSysTime = xTaskGetTickCount(); // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectCallback(&objectUpdatedCb); // Main system loop while (1) { // Update the system statistics updateStats(); // Update the system alarms updateSystemAlarms(); #if defined(DIAGNOSTICS) updateI2Cstats(); updateWDGstats(); #endif #if defined(DIAG_TASKS) // Update the task status object TaskMonitorUpdateAll(); #endif // Flash the heartbeat LED #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 defined (PIOS_LED_ALARM) if (AlarmsHasWarnings()) { PIOS_LED_On(PIOS_LED_ALARM); } else { PIOS_LED_Off(PIOS_LED_ALARM); } #endif /* PIOS_LED_ALARM */ FlightStatusData flightStatus; FlightStatusGet(&flightStatus); // Wait until next period if(flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2) ); } else { vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS); } } }
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; PIOS_IAP_Init(); if (PIOS_IAP_CheckRequest() == TRUE) { PIOS_Board_Init(); PIOS_DELAY_WaitmS(1000); User_DFU_request = TRUE; PIOS_IAP_ClearRequest(); } GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); if (GO_dfu == TRUE) { PIOS_Board_Init(); if(User_DFU_request == TRUE) DeviceState = DFUidle; else DeviceState = BLidle; STOPWATCH_Init(100,LED_PWM_TIMER); } else JumpToApp = TRUE; STOPWATCH_Reset(LED_PWM_TIMER); while (TRUE) { if (JumpToApp == TRUE) jump_to_app(); //pwm_period = 50; // *100 uS -> 5 mS //pwm_sweep_steps =100; // * 5 mS -> 500 mS switch (DeviceState) { case Last_operation_Success: case uploadingStarting: case DFUidle: period1 = 50; sweep_steps1 = 100; PIOS_LED_Off(BLUE); period2 = 0; break; case uploading: period1 = 50; sweep_steps1 = 100; period2 = 25; sweep_steps2 = 50; break; case downloading: period1 = 25; sweep_steps1 = 50; PIOS_LED_Off(BLUE); period2 = 0; break; case BLidle: period1 = 0; PIOS_LED_On(BLUE); period2 = 0; break; default://error period1 = 50; sweep_steps1 = 100; period2 = 50; sweep_steps2 = 100; } if (period1 != 0) { if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER))) PIOS_LED_On(BLUE); else PIOS_LED_Off(BLUE); } else PIOS_LED_On(BLUE); if (period2 != 0) { if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER))) PIOS_LED_On(BLUE); else PIOS_LED_Off(BLUE); } else PIOS_LED_Off(BLUE); if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100) STOPWATCH_Reset(LED_PWM_TIMER); if ((STOPWATCH_ValueGet(LED_PWM_TIMER) > 60000) && (DeviceState == BLidle)) JumpToApp = TRUE; processRX(); DataDownload(start); } }
int main() { float gyro[3], accel[3], mag[3]; float vel[3] = { 0, 0, 0 }; /* Normaly we get/set UAVObjects but this one only needs to be set. We will never expect to get this from another module*/ AttitudeActualData attitude_actual; AHRSSettingsData ahrs_settings; /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); /* Delay system */ PIOS_DELAY_Init(); /* Communication system */ PIOS_COM_Init(); /* ADC system */ AHRS_ADC_Config( adc_oversampling ); /* Setup the Accelerometer FS (Full-Scale) GPIO */ PIOS_GPIO_Enable( 0 ); SET_ACCEL_2G; #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) /* Magnetic sensor system */ PIOS_I2C_Init(); PIOS_HMC5843_Init(); // Get 3 ID bytes strcpy(( char * )mag_data.id, "ZZZ" ); PIOS_HMC5843_ReadID( mag_data.id ); #endif /* SPI link to master */ // PIOS_SPI_Init(); AhrsInitComms(); AHRSCalibrationConnectCallback( calibration_callback ); GPSPositionConnectCallback( gps_callback ); ahrs_state = AHRS_IDLE; while( !AhrsLinkReady() ) { AhrsPoll(); while( ahrs_state != AHRS_DATA_READY ) ; ahrs_state = AHRS_PROCESSING; downsample_data(); ahrs_state = AHRS_IDLE; if(( total_conversion_blocks % 50 ) == 0 ) PIOS_LED_Toggle( LED1 ); } AHRSSettingsGet(&ahrs_settings); /* Use simple averaging filter for now */ for( int i = 0; i < adc_oversampling; i++ ) fir_coeffs[i] = 1; fir_coeffs[adc_oversampling] = adc_oversampling; if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS) { // compute a data point and initialize INS downsample_data(); converge_insgps(); } #ifdef DUMP_RAW int previous_conversion; while( 1 ) { AhrsPoll(); int result; uint8_t framing[16] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }; while( ahrs_state != AHRS_DATA_READY ) ; ahrs_state = AHRS_PROCESSING; if( total_conversion_blocks != previous_conversion + 1 ) PIOS_LED_On( LED1 ); // not keeping up else PIOS_LED_Off( LED1 ); previous_conversion = total_conversion_blocks; downsample_data(); ahrs_state = AHRS_IDLE;; // Dump raw buffer result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 ); // framing header result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) ); // dump block number result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & valid_data_buffer[0], ADC_OVERSAMPLE * ADC_CONTINUOUS_CHANNELS * sizeof( valid_data_buffer[0] ) ); if( result == 0 ) PIOS_LED_Off( LED1 ); else { PIOS_LED_On( LED1 ); } } #endif timer_start(); /******************* Main EKF loop ****************************/ while( 1 ) { AhrsPoll(); AHRSCalibrationData calibration; AHRSCalibrationGet( &calibration ); BaroAltitudeData baro_altitude; BaroAltitudeGet( &baro_altitude ); GPSPositionData gps_position; GPSPositionGet( &gps_position ); AHRSSettingsGet(&ahrs_settings); // Alive signal if(( total_conversion_blocks % 100 ) == 0 ) PIOS_LED_Toggle( LED1 ); #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) // Get magnetic readings if( PIOS_HMC5843_NewDataAvailable() ) { PIOS_HMC5843_ReadMag( mag_data.raw.axis ); mag_data.updated = 1; } attitude_raw.magnetometers[0] = mag_data.raw.axis[0]; attitude_raw.magnetometers[2] = mag_data.raw.axis[1]; attitude_raw.magnetometers[2] = mag_data.raw.axis[2]; #endif // Delay for valid data counter_val = timer_count(); running_counts = counter_val - last_counter_idle_end; last_counter_idle_start = counter_val; while( ahrs_state != AHRS_DATA_READY ) ; counter_val = timer_count(); idle_counts = counter_val - last_counter_idle_start; last_counter_idle_end = counter_val; ahrs_state = AHRS_PROCESSING; downsample_data(); /***************** SEND BACK SOME RAW DATA ************************/ // Hacky - grab one sample from buffer to populate this. Need to send back // all raw data if it's happening accel_data.raw.x = valid_data_buffer[0]; accel_data.raw.y = valid_data_buffer[2]; accel_data.raw.z = valid_data_buffer[4]; gyro_data.raw.x = valid_data_buffer[1]; gyro_data.raw.y = valid_data_buffer[3]; gyro_data.raw.z = valid_data_buffer[5]; gyro_data.temp.xy = valid_data_buffer[6]; gyro_data.temp.z = valid_data_buffer[7]; if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS) { /******************** INS ALGORITHM **************************/ // format data for INS algo gyro[0] = gyro_data.filtered.x; gyro[1] = gyro_data.filtered.y; gyro[2] = gyro_data.filtered.z; accel[0] = accel_data.filtered.x, accel[1] = accel_data.filtered.y, accel[2] = accel_data.filtered.z, // Note: The magnetometer driver returns registers X,Y,Z from the chip which are // (left, backward, up). Remapping to (forward, right, down). mag[0] = -( mag_data.raw.axis[1] - calibration.mag_bias[1] ); mag[1] = -( mag_data.raw.axis[0] - calibration.mag_bias[0] ); mag[2] = -( mag_data.raw.axis[2] - calibration.mag_bias[2] ); INSStatePrediction( gyro, accel, 1 / ( float )EKF_RATE ); INSCovariancePrediction( 1 / ( float )EKF_RATE ); if( gps_updated && gps_position.Status == GPSPOSITION_STATUS_FIX3D ) { // Compute velocity from Heading and groundspeed vel[0] = gps_position.Groundspeed * cos( gps_position.Heading * M_PI / 180 ); vel[1] = gps_position.Groundspeed * sin( gps_position.Heading * M_PI / 180 ); // Completely unprincipled way to make the position variance // increase as data quality decreases but keep it bounded // Variance becomes 40 m^2 and 40 (m/s)^2 when no gps INSSetPosVelVar( 0.004 ); HomeLocationData home; HomeLocationGet( &home ); float ned[3]; double lla[3] = {( double ) gps_position.Latitude / 1e7, ( double ) gps_position.Longitude / 1e7, ( double )( gps_position.GeoidSeparation + gps_position.Altitude )}; // convert from cm back to meters double ecef[3] = {( double )( home.ECEF[0] / 100 ), ( double )( home.ECEF[1] / 100 ), ( double )( home.ECEF[2] / 100 )}; LLA2Base( lla, ecef, ( float( * )[3] ) home.RNE, ned ); if( gps_updated ) { //FIXME: Is this correct? //TOOD: add check for altitude updates FullCorrection( mag, ned, vel, baro_altitude.Altitude ); gps_updated = false; } else { GpsBaroCorrection( ned, vel, baro_altitude.Altitude ); } gps_updated = false; mag_data.updated = 0; } else if( gps_position.Status == GPSPOSITION_STATUS_FIX3D && mag_data.updated == 1 ) { MagCorrection( mag ); // only trust mags if outdoors mag_data.updated = 0; } else { // Indoors, update with zero position and velocity and high covariance INSSetPosVelVar( 0.1 ); vel[0] = 0; vel[1] = 0; vel[2] = 0; VelBaroCorrection( vel, baro_altitude.Altitude ); // MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors } attitude_actual.q1 = Nav.q[0]; attitude_actual.q2 = Nav.q[1]; attitude_actual.q3 = Nav.q[2]; attitude_actual.q4 = Nav.q[3]; } else if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_SIMPLE ) { float q[4]; float rpy[3]; /***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/ /* Very simple computation of the heading and attitude from accel. */ rpy[2] = atan2(( mag_data.raw.axis[0] ), ( -1 * mag_data.raw.axis[1] ) ) * 180 / M_PI; rpy[1] = atan2( accel_data.filtered.x, accel_data.filtered.z ) * 180 / M_PI; rpy[0] = atan2( accel_data.filtered.y, accel_data.filtered.z ) * 180 / M_PI; RPY2Quaternion( rpy, q ); attitude_actual.q1 = q[0]; attitude_actual.q2 = q[1]; attitude_actual.q3 = q[2]; attitude_actual.q4 = q[3]; } ahrs_state = AHRS_IDLE; #ifdef DUMP_FRIENDLY PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "b: %d\r\n", total_conversion_blocks ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "a: %d %d %d\r\n", ( int16_t )( accel_data.filtered.x * 1000 ), ( int16_t )( accel_data.filtered.y * 1000 ), ( int16_t )( accel_data.filtered.z * 1000 ) ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "g: %d %d %d\r\n", ( int16_t )( gyro_data.filtered.x * 1000 ), ( int16_t )( gyro_data.filtered.y * 1000 ), ( int16_t )( gyro_data.filtered.z * 1000 ) ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "m: %d %d %d\r\n", mag_data.raw.axis[0], mag_data.raw.axis[1], mag_data.raw.axis[2] ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "q: %d %d %d %d\r\n", ( int16_t )( Nav.q[0] * 1000 ), ( int16_t )( Nav.q[1] * 1000 ), ( int16_t )( Nav.q[2] * 1000 ), ( int16_t )( Nav.q[3] * 1000 ) ); #endif #ifdef DUMP_EKF uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 }; extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances extern float K[NUMX][NUMV]; // feedback gain matrix // Dump raw buffer int8_t result; result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 ); // framing header result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) ); // dump block number result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & mag_data, sizeof( mag_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & gps_data, sizeof( gps_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & accel_data, sizeof( accel_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & gyro_data, sizeof( gyro_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & Q, sizeof( float ) * NUMX * NUMX ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & K, sizeof( float ) * NUMX * NUMV ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & X, sizeof( float ) * NUMX * NUMX ); if( result == 0 ) PIOS_LED_Off( LED1 ); else { PIOS_LED_On( LED1 ); } #endif AttitudeActualSet( &attitude_actual ); /*FIXME: This is dangerous. There is no locking for UAVObjects so it could stomp all over the airspeed/climb rate etc. This used to be done in the OP module which was bad. Having ~4ms latency for the round trip makes it worse here. */ PositionActualData pos; PositionActualGet( &pos ); for( int ct = 0; ct < 3; ct++ ) { pos.NED[ct] = Nav.Pos[ct]; pos.Vel[ct] = Nav.Vel[ct]; } PositionActualSet( &pos ); static bool was_calibration = false; AhrsStatusData status; AhrsStatusGet( &status ); if( was_calibration != status.CalibrationSet ) { was_calibration = status.CalibrationSet; if( status.CalibrationSet ) { calibrate_sensors(); AhrsStatusGet( &status ); status.CalibrationSet = true; } } status.CPULoad = (( float )running_counts / ( float )( idle_counts + running_counts ) ) * 100; status.IdleTimePerCyle = idle_counts / ( TIMER_RATE / 10000 ); status.RunningTimePerCyle = running_counts / ( TIMER_RATE / 10000 ); status.DroppedUpdates = ekf_too_slow; AhrsStatusSet( &status ); } return 0; }
int main() { PIOS_SYS_Init(); PIOS_Board_Init(); PIOS_IAP_Init(); // Make sure the brown out reset value for this chip // is 2.7 volts check_bor(); #ifdef PIOS_INCLUDE_USB USB_connected = PIOS_USB_CheckAvailable(0); #endif if (PIOS_IAP_CheckRequest() == true) { PIOS_DELAY_WaitmS(1000); User_DFU_request = true; PIOS_IAP_ClearRequest(); } GO_dfu = (USB_connected == true) || (User_DFU_request == true); if (GO_dfu == true) { if (User_DFU_request == true) { DeviceState = DFUidle; } else { DeviceState = BLidle; } } else { JumpToApp = true; } uint32_t stopwatch = 0; uint32_t prev_ticks = PIOS_DELAY_GetuS(); while (true) { /* Update the stopwatch */ uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks); prev_ticks += elapsed_ticks; stopwatch += elapsed_ticks; if (JumpToApp == true) { jump_to_app(); } switch (DeviceState) { case Last_operation_Success: case uploadingStarting: case DFUidle: period1 = 5000; sweep_steps1 = 100; PIOS_LED_Off(PIOS_LED_HEARTBEAT); period2 = 0; break; case uploading: period1 = 5000; sweep_steps1 = 100; period2 = 2500; sweep_steps2 = 50; break; case downloading: period1 = 2500; sweep_steps1 = 50; PIOS_LED_Off(PIOS_LED_HEARTBEAT); period2 = 0; break; case BLidle: period1 = 0; PIOS_LED_On(PIOS_LED_HEARTBEAT); period2 = 0; break; default: // error period1 = 5000; sweep_steps1 = 100; period2 = 5000; sweep_steps2 = 100; } if (period1 != 0) { if (LedPWM(period1, sweep_steps1, stopwatch)) { PIOS_LED_On(PIOS_LED_HEARTBEAT); } else { PIOS_LED_Off(PIOS_LED_HEARTBEAT); } } else { PIOS_LED_On(PIOS_LED_HEARTBEAT); } if (period2 != 0) { if (LedPWM(period2, sweep_steps2, stopwatch)) { PIOS_LED_On(PIOS_LED_HEARTBEAT); } else { PIOS_LED_Off(PIOS_LED_HEARTBEAT); } } else { PIOS_LED_Off(PIOS_LED_HEARTBEAT); } if (stopwatch > 50 * 1000 * 1000) { stopwatch = 0; } if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) { JumpToApp = true; } processRX(); DataDownload(start); } }
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; PIOS_IAP_Init(); if (PIOS_IAP_CheckRequest() == TRUE) { PIOS_Board_Init(); PIOS_DELAY_WaitmS(1000); User_DFU_request = TRUE; PIOS_IAP_ClearRequest(); } GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); if (GO_dfu == TRUE) { if (USB_connected) ProgPort = Usb; else ProgPort = Serial; PIOS_Board_Init(); if(User_DFU_request == TRUE) DeviceState = DFUidle; else DeviceState = BLidle; STOPWATCH_Init(100,LED_PWM_TIMER); if (ProgPort == Serial) { fifoBuf_init(&ssp_buffer,rx_buffer,UART_BUFFER_SIZE); STOPWATCH_Init(100,SSP_TIMER);//nao devia ser 1000? STOPWATCH_Reset(SSP_TIMER); ssp_Init(&ssp_port, &SSP_PortConfig); } PIOS_OPAHRS_ForceSlaveSelected(true); } else JumpToApp = TRUE; STOPWATCH_Reset(LED_PWM_TIMER); while (TRUE) { if (ProgPort == Serial) { ssp_ReceiveProcess(&ssp_port); status=ssp_SendProcess(&ssp_port); while((status!=SSP_TX_IDLE) && (status!=SSP_TX_ACKED)){ ssp_ReceiveProcess(&ssp_port); status=ssp_SendProcess(&ssp_port); } } if (JumpToApp == TRUE) jump_to_app(); //pwm_period = 50; // *100 uS -> 5 mS //pwm_sweep_steps =100; // * 5 mS -> 500 mS switch (DeviceState) { case Last_operation_Success: case uploadingStarting: case DFUidle: period1 = 50; sweep_steps1 = 100; PIOS_LED_Off(RED); period2 = 0; break; case uploading: period1 = 50; sweep_steps1 = 100; period2 = 25; sweep_steps2 = 50; break; case downloading: period1 = 25; sweep_steps1 = 50; PIOS_LED_Off(RED); period2 = 0; break; case BLidle: period1 = 0; PIOS_LED_On(BLUE); period2 = 0; break; default://error period1 = 50; sweep_steps1 = 100; period2 = 50; sweep_steps2 = 100; } if (period1 != 0) { if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER))) PIOS_LED_On(BLUE); else PIOS_LED_Off(BLUE); } else PIOS_LED_On(BLUE); if (period2 != 0) { if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER))) PIOS_LED_On(RED); else PIOS_LED_Off(RED); } else PIOS_LED_Off(RED); if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100) STOPWATCH_Reset(LED_PWM_TIMER); if ((STOPWATCH_ValueGet(LED_PWM_TIMER) > 60000) && (DeviceState == BLidle)) JumpToApp = TRUE; processRX(); DataDownload(start); } }
int main() { PIOS_SYS_Init(); if (BSL_HOLD_STATE == 0) USB_connected = TRUE; PIOS_IAP_Init(); if (PIOS_IAP_CheckRequest() == TRUE) { PIOS_Board_Init(); PIOS_DELAY_WaitmS(1000); User_DFU_request = TRUE; PIOS_IAP_ClearRequest(); } GO_dfu = (USB_connected == TRUE) || (User_DFU_request == TRUE); if (GO_dfu == TRUE) { PIOS_Board_Init(); if (User_DFU_request == TRUE) DeviceState = DFUidle; else DeviceState = BLidle; STOPWATCH_Init(100, LED_PWM_TIMER); } else JumpToApp = TRUE; STOPWATCH_Reset(LED_PWM_TIMER); while (TRUE) { if (JumpToApp == TRUE) jump_to_app(); switch (DeviceState) { case Last_operation_Success: case uploadingStarting: case DFUidle: period1 = 50; sweep_steps1 = 100; PIOS_LED_Off(BLUE); period2 = 0; break; case uploading: period1 = 50; sweep_steps1 = 100; period2 = 25; sweep_steps2 = 50; break; case downloading: period1 = 25; sweep_steps1 = 50; PIOS_LED_Off(BLUE); period2 = 0; break; case BLidle: period1 = 0; PIOS_LED_On(BLUE); period2 = 0; break; default://error period1 = 50; sweep_steps1 = 100; period2 = 50; sweep_steps2 = 100; } if (period1 != 0) { if (LedPWM(period1, sweep_steps1, STOPWATCH_ValueGet(LED_PWM_TIMER))) PIOS_LED_On(BLUE); else PIOS_LED_Off(BLUE); } else PIOS_LED_On(BLUE); if (period2 != 0) { if (LedPWM(period2, sweep_steps2, STOPWATCH_ValueGet(LED_PWM_TIMER))) PIOS_LED_On(BLUE); else PIOS_LED_Off(BLUE); } else PIOS_LED_Off(BLUE); if (STOPWATCH_ValueGet(LED_PWM_TIMER) > 100 * 50 * 100) STOPWATCH_Reset(LED_PWM_TIMER); if ((STOPWATCH_ValueGet(LED_PWM_TIMER) > 60000) && (DeviceState == BLidle)) JumpToApp = TRUE; processRX(); DataDownload(start); } }
void process_spi_request(void) { const struct pios_board_info * bdinfo = &pios_board_info_blob; bool msg_to_process = FALSE; PIOS_IRQ_Disable(); /* Figure out if we're in an interesting stable state */ switch (lfsm_get_state()) { case LFSM_STATE_USER_BUSY: msg_to_process = TRUE; break; case LFSM_STATE_INACTIVE: /* Queue up a receive buffer */ lfsm_user_set_rx_v0(&user_rx_v0); lfsm_user_done(); break; case LFSM_STATE_STOPPED: /* Get things going */ lfsm_set_link_proto_v0(&link_tx_v0, &link_rx_v0); break; default: /* Not a stable state */ break; } PIOS_IRQ_Enable(); if (!msg_to_process) { /* Nothing to do */ //PIOS_COM_SendFormattedString(PIOS_COM_AUX, "."); return; } if (user_rx_v0.tail.magic != OPAHRS_MSG_MAGIC_TAIL) { return; } switch (user_rx_v0.payload.user.t) { case OPAHRS_MSG_V0_REQ_FWUP_VERIFY: opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWUP_STATUS); Fw_crc = PIOS_BL_HELPER_CRC_Memory_Calc(); lfsm_user_set_tx_v0(&user_tx_v0); boot_status = idle; PIOS_LED_Off(LED1); user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status; break; case OPAHRS_MSG_V0_REQ_RESET: PIOS_DELAY_WaitmS(user_rx_v0.payload.user.v.req.reset.reset_delay_in_ms); 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) | BOARD_REVISION; user_tx_v0.payload.user.v.rsp.versions.fw_crc = Fw_crc; lfsm_user_set_tx_v0(&user_tx_v0); break; case OPAHRS_MSG_V0_REQ_MEM_MAP: opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_MEM_MAP); user_tx_v0.payload.user.v.rsp.mem_map.density = bdinfo->hw_type; user_tx_v0.payload.user.v.rsp.mem_map.rw_flags = (BOARD_READABLE | (BOARD_WRITABLE << 1)); user_tx_v0.payload.user.v.rsp.mem_map.size_of_code_memory = bdinfo->fw_size; user_tx_v0.payload.user.v.rsp.mem_map.size_of_description = bdinfo->desc_size; user_tx_v0.payload.user.v.rsp.mem_map.start_of_user_code = bdinfo->fw_base; lfsm_user_set_tx_v0(&user_tx_v0); break; case OPAHRS_MSG_V0_REQ_SERIAL: opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_SERIAL); PIOS_SYS_SerialNumberGet( (char *) &(user_tx_v0.payload.user.v.rsp.serial.serial_bcd)); lfsm_user_set_tx_v0(&user_tx_v0); break; case OPAHRS_MSG_V0_REQ_FWUP_STATUS: 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); break; case OPAHRS_MSG_V0_REQ_FWUP_DATA: PIOS_LED_On(LED1); 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)) { for (uint8_t x = 0; x < user_rx_v0.payload.user.v.req.fwup_data.size; ++x) { if (FLASH_ProgramWord( (user_rx_v0.payload.user.v.req.fwup_data.adress + ((uint32_t)(x * 4))), user_rx_v0.payload.user.v.req.fwup_data.data[x]) != FLASH_COMPLETE) { boot_status = write_error; break; } } } else { boot_status = outside_dev_capabilities; } PIOS_LED_Off(LED1); user_tx_v0.payload.user.v.rsp.fwup_status.status = boot_status; lfsm_user_set_tx_v0(&user_tx_v0); break; case OPAHRS_MSG_V0_REQ_FWDN_DATA: opahrs_msg_v0_init_user_tx(&user_tx_v0, OPAHRS_MSG_V0_RSP_FWDN_DATA); uint32_t adr = user_rx_v0.payload.user.v.req.fwdn_data.adress; for (uint8_t x = 0; x < 4; ++x) { user_tx_v0.payload.user.v.rsp.fw_dn.data[x] = *PIOS_BL_HELPER_FLASH_If_Read(adr + x); } lfsm_user_set_tx_v0(&user_tx_v0); break; case OPAHRS_MSG_V0_REQ_FWUP_START: FLASH_Unlock(); 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); if (PIOS_BL_HELPER_FLASH_Start() == TRUE) { boot_status = started; PIOS_LED_Off(LED1); } else { boot_status = start_failed; break; } break; case OPAHRS_MSG_V0_REQ_BOOT: PIOS_DELAY_WaitmS(user_rx_v0.payload.user.v.req.boot.boot_delay_in_ms); FLASH_Lock(); jump_to_app(); break; default: break; } /* Finished processing the received message, requeue it */ lfsm_user_set_rx_v0(&user_rx_v0); lfsm_user_done(); return; }