/** * 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(); #if defined(PIOS_INCLUDE_LED) PIOS_LED_Init(&pios_led_cfg); #endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_LED_HEARTBEAT) PIOS_LED_On(PIOS_LED_HEARTBEAT); #endif /* PIOS_LED_HEARTBEAT */ /* Delay system */ PIOS_DELAY_Init(); /* Communication system */ #if !defined(PIOS_ENABLE_DEBUG_PINS) #if defined(PIOS_INCLUDE_COM) { uint32_t pios_usart_aux_id; 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, NULL, 0, pios_com_aux_tx_buffer, sizeof(pios_com_aux_tx_buffer))) { 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 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 */ #if defined(PIOS_INCLUDE_I2C) if (PIOS_I2C_Init(&pios_i2c_internal_id, &pios_i2c_internal_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_internal_id) != 0) panic(3); #endif #if defined(PIOS_INCLUDE_CAN) if (PIOS_CAN_Init(&pios_can_id, &pios_can_cfg) != 0) panic(6); uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_CAN_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_CAN_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_can_id, &pios_can_com_driver, pios_can_id, rx_buffer, PIOS_COM_CAN_RX_BUF_LEN, tx_buffer, PIOS_COM_CAN_TX_BUF_LEN)) panic(6); pios_com_bridge_id = pios_com_can_id; #endif #if defined(PIOS_INCLUDE_FLASH) /* Inititialize all flash drivers */ if (PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg) != 0) panic(5); /* Register the partition table */ PIOS_FLASH_register_partition_table(pios_flash_partition_table, NELEMENTS(pios_flash_partition_table)); /* Mount all filesystems */ if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_settings_cfg, FLASH_PARTITION_LABEL_SETTINGS) != 0) panic(5); if (PIOS_FLASHFS_Logfs_Init(&pios_waypoints_settings_fs_id, &flashfs_internal_waypoints_cfg, FLASH_PARTITION_LABEL_WAYPOINTS) != 0) panic(5); #endif /* PIOS_INCLUDE_FLASH */ /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); /* Initialize the alarms library. Reads RCC reset flags */ AlarmsInitialize(); PIOS_RESET_Clear(); // Clear the RCC reset flags after use. /* Initialize the hardware UAVOs */ HwSparkyBGCInitialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ 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 #if defined(PIOS_INCLUDE_BRUSHLESS) /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_2_brushless_cfg); PIOS_TIM_InitClock(&tim_3_brushless_cfg); #endif /* PIOS_INCLUDE_BRUSHLESS */ /* 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 */ HwSparkyBGCSetDefaults(HwSparkyBGCHandle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } #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; #if defined(PIOS_INCLUDE_USB_CDC) bool usb_cdc_present = false; 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 */ HwSparkyBGCUSB_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 = HWSPARKYBGC_USB_VCPPORT_DISABLED; } switch (hw_usb_vcpport) { case HWSPARKYBGC_USB_VCPPORT_DISABLED: break; case HWSPARKYBGC_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { 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); } 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 HWSPARKYBGC_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { 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); } 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 HWSPARKYBGC_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { 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); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwSparkyBGCUSB_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 = HWSPARKYBGC_USB_HIDPORT_DISABLED; } switch (hw_usb_hidport) { case HWSPARKYBGC_USB_HIDPORT_DISABLED: break; case HWSPARKYBGC_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { 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); } 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 */ #endif /* PIOS_INCLUDE_USB */ /* Configure the IO ports */ HwSparkyBGCDSMxModeOptions hw_DSMxMode; HwSparkyBGCDSMxModeGet(&hw_DSMxMode); /* UART1 Port */ uint8_t hw_flexi; HwSparkyBGCFlexiPortGet(&hw_flexi); switch (hw_flexi) { case HWSPARKYBGC_FLEXIPORT_DISABLED: break; case HWSPARKYBGC_FLEXIPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_flexi_usart_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWSPARKYBGC_FLEXIPORT_GPS: #if defined(PIOS_INCLUDE_GPS) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_flexi_usart_cfg, PIOS_COM_GPS_RX_BUF_LEN, 0, &pios_usart_com_driver, &pios_com_gps_id); #endif break; case HWSPARKYBGC_FLEXIPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) && defined(PIOS_INCLUDE_USART) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_flexi_sbus_cfg)) { PIOS_Assert(0); } uintptr_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_flexi_sbus_aux_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 /* PIOS_INCLUDE_SBUS */ break; case HWSPARKYBGC_FLEXIPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { PIOS_Board_configure_dsm(&pios_flexi_dsm_cfg, &pios_flexi_dsm_aux_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSM, &hw_DSMxMode); } #endif /* PIOS_INCLUDE_DSM */ break; case HWSPARKYBGC_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_flexi_usart_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWSPARKYBGC_FLEXIPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_flexi_usart_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); #endif break; case HWSPARKYBGC_FLEXIPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_flexi_usart_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWSPARKYBGC_FLEXIPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_GPS) #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_flexi_usart_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; } /* Configure the rcvr port */ uint8_t hw_rcvrport; HwSparkyBGCRcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWSPARKYBGC_RCVRPORT_DISABLED: break; case HWSPARKYBGC_RCVRPORT_PPM: #if defined(PIOS_INCLUDE_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; } #endif /* PIOS_INCLUDE_PPM */ break; case HWSPARKYBGC_RCVRPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { PIOS_Board_configure_dsm(&pios_rcvr_dsm_cfg, &pios_rcvr_dsm_aux_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSM, &hw_DSMxMode); } #endif /* PIOS_INCLUDE_DSM */ break; case HWSPARKYBGC_RCVRPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) && defined(PIOS_INCLUDE_USART) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_rcvr_sbus_cfg)) { PIOS_Assert(0); } uintptr_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_rcvr_sbus_aux_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 /* PIOS_INCLUDE_SBUS */ break; break; } #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 #ifdef PIOS_INCLUDE_SERVO PIOS_Servo_Init(&pios_servo_cfg); #endif #ifdef PIOS_INCLUDE_BRUSHLESS if (PIOS_Brushless_Init(&pios_brushless_cfg) != 0) panic(8); #endif #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif PIOS_WDG_Clear(); PIOS_DELAY_WaitmS(200); PIOS_WDG_Clear(); #if defined(PIOS_INCLUDE_MPU9150) int retval; retval = PIOS_MPU9150_Init(pios_i2c_internal_id, PIOS_MPU9150_I2C_ADD_A0_LOW, &pios_mpu9150_cfg); if (retval == -10) panic(1); // indicate missing IRQ separately if (retval != 0) panic(2); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwSparkyBGCGyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWSPARKYBGC_GYRORANGE_250: PIOS_MPU9150_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWSPARKYBGC_GYRORANGE_500: PIOS_MPU9150_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWSPARKYBGC_GYRORANGE_1000: PIOS_MPU9150_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWSPARKYBGC_GYRORANGE_2000: PIOS_MPU9150_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; uint8_t hw_accel_range; HwSparkyBGCAccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWSPARKYBGC_ACCELRANGE_2G: PIOS_MPU9150_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWSPARKYBGC_ACCELRANGE_4G: PIOS_MPU9150_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWSPARKYBGC_ACCELRANGE_8G: PIOS_MPU9150_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWSPARKYBGC_ACCELRANGE_16G: PIOS_MPU9150_SetAccelRange(PIOS_MPU60X0_ACCEL_16G); break; } uint8_t hw_mpu9150_dlpf; HwSparkyBGCMPU9150DLPFGet(&hw_mpu9150_dlpf); enum pios_mpu60x0_filter mpu9150_dlpf = \ (hw_mpu9150_dlpf == HWSPARKYBGC_MPU9150DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_mpu9150_dlpf == HWSPARKYBGC_MPU9150DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_mpu9150_dlpf == HWSPARKYBGC_MPU9150DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_mpu9150_dlpf == HWSPARKYBGC_MPU9150DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_mpu9150_dlpf == HWSPARKYBGC_MPU9150DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_mpu9150_dlpf == HWSPARKYBGC_MPU9150DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_mpu9150_dlpf == HWSPARKYBGC_MPU9150DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ pios_mpu9150_cfg.default_filter; PIOS_MPU9150_SetLPF(mpu9150_dlpf); uint8_t hw_mpu9150_samplerate; HwSparkyBGCMPU9150RateGet(&hw_mpu9150_samplerate); uint16_t mpu9150_samplerate = \ (hw_mpu9150_samplerate == HWSPARKYBGC_MPU9150RATE_200) ? 200 : \ (hw_mpu9150_samplerate == HWSPARKYBGC_MPU9150RATE_333) ? 333 : \ (hw_mpu9150_samplerate == HWSPARKYBGC_MPU9150RATE_500) ? 500 : \ (hw_mpu9150_samplerate == HWSPARKYBGC_MPU9150RATE_666) ? 666 : \ (hw_mpu9150_samplerate == HWSPARKYBGC_MPU9150RATE_1000) ? 1000 : \ (hw_mpu9150_samplerate == HWSPARKYBGC_MPU9150RATE_2000) ? 2000 : \ (hw_mpu9150_samplerate == HWSPARKYBGC_MPU9150RATE_4000) ? 4000 : \ (hw_mpu9150_samplerate == HWSPARKYBGC_MPU9150RATE_8000) ? 8000 : \ pios_mpu9150_cfg.default_samplerate; PIOS_MPU9150_SetSampleRate(mpu9150_samplerate); } #endif /* PIOS_INCLUDE_MPU9150 */ //I2C is slow, sensor init as well, reset watchdog to prevent reset here PIOS_WDG_Clear(); #if defined(PIOS_INCLUDE_GPIO) PIOS_GPIO_Init(); #endif /* Make sure we have at least one telemetry link configured or else fail initialization */ PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); }
/** * 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(); /* Delay system */ PIOS_DELAY_Init(); /* IAP System Setup */ PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_GPS) uint32_t pios_usart_gps_id; if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, pios_com_gps_rx_buffer, sizeof(pios_com_gps_rx_buffer), NULL, 0)) { PIOS_DEBUG_Assert(0); } #endif /* PIOS_INCLUDE_GPS */ #endif /* PIOS_INCLUDE_COM */ #if defined (PIOS_INCLUDE_I2C) if (PIOS_I2C_Init(&pios_i2c_pres_mag_adapter_id, &pios_i2c_pres_mag_adapter_cfg)) { PIOS_DEBUG_Assert(0); } #if defined (PIOS_INCLUDE_BMP085) PIOS_BMP085_Init(); #endif /* PIOS_INCLUDE_BMP085 */ #if defined (PIOS_INCLUDE_HMC5883) PIOS_HMC5883_Init(); #endif /* PIOS_INCLUDE_HMC5883 */ #if defined(PIOS_INCLUDE_IMU3000) if (PIOS_I2C_Init(&pios_i2c_gyro_adapter_id, &pios_i2c_gyro_adapter_cfg)) { PIOS_DEBUG_Assert(0); } PIOS_IMU3000_Init(); #endif /* PIOS_INCLUDE_IMU3000 */ #endif /* PIOS_INCLUDE_I2C */ #if defined(PIOS_INCLUDE_SPI) /* Set up the SPI interface to the accelerometer*/ if (PIOS_SPI_Init(&pios_spi_accel_id, &pios_spi_accel_cfg)) { PIOS_DEBUG_Assert(0); } PIOS_BMA180_Attach(pios_spi_accel_id); // #include "ahrs_spi_comm.h" // InsInitComms(); // // /* 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); // } // // InsConnect(pios_spi_op_id); #endif /* PIOS_INCLUDE_SPI */ }
/** * Initialise a single USART device */ int32_t PIOS_USART_Init(uintptr_t * usart_id, const struct pios_usart_cfg * cfg) { PIOS_DEBUG_Assert(usart_id); PIOS_DEBUG_Assert(cfg); struct pios_usart_dev * usart_dev; usart_dev = (struct pios_usart_dev *) PIOS_USART_alloc(); if (!usart_dev) goto out_fail; /* Bind the configuration to the device instance */ usart_dev->cfg = cfg; /* Map pins to USART function */ /* note __builtin_ctz() due to the difference between GPIO_PinX and GPIO_PinSourceX */ if (usart_dev->cfg->remap) { if (usart_dev->cfg->rx.gpio != 0) GPIO_PinAFConfig(usart_dev->cfg->rx.gpio, __builtin_ctz(usart_dev->cfg->rx.init.GPIO_Pin), usart_dev->cfg->remap); if (usart_dev->cfg->tx.gpio != 0) GPIO_PinAFConfig(usart_dev->cfg->tx.gpio, __builtin_ctz(usart_dev->cfg->tx.init.GPIO_Pin), usart_dev->cfg->remap); } /* Initialize the USART Rx and Tx pins */ if (usart_dev->cfg->rx.gpio != 0) GPIO_Init(usart_dev->cfg->rx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->rx.init); if (usart_dev->cfg->tx.gpio != 0) GPIO_Init(usart_dev->cfg->tx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->tx.init); /* Configure the USART */ USART_Init(usart_dev->cfg->regs, (USART_InitTypeDef *)&usart_dev->cfg->init); *usart_id = (uintptr_t)usart_dev; /* Configure USART Interrupts */ switch ((uint32_t)usart_dev->cfg->regs) { case (uint32_t)USART1: PIOS_USART_1_id = (uintptr_t)usart_dev; break; case (uint32_t)USART2: PIOS_USART_2_id = (uintptr_t)usart_dev; break; case (uint32_t)USART3: PIOS_USART_3_id = (uintptr_t)usart_dev; break; case (uint32_t)UART4: PIOS_USART_4_id = (uintptr_t)usart_dev; break; case (uint32_t)UART5: PIOS_USART_5_id = (uintptr_t)usart_dev; break; case (uint32_t)USART6: PIOS_USART_6_id = (uintptr_t)usart_dev; break; } NVIC_Init((NVIC_InitTypeDef *)&(usart_dev->cfg->irq.init)); USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); // FIXME XXX Clear / reset uart here - sends NUL char else /* Enable USART */ USART_Cmd(usart_dev->cfg->regs, ENABLE); return(0); out_fail: return(-1); }
/** * Initialise a single USART device */ int32_t PIOS_USART_Init(uintptr_t * usart_id, const struct pios_usart_cfg * cfg) { PIOS_DEBUG_Assert(usart_id); PIOS_DEBUG_Assert(cfg); struct pios_usart_dev * usart_dev; usart_dev = (struct pios_usart_dev *) PIOS_USART_alloc(); if (!usart_dev) goto out_fail; /* Bind the configuration to the device instance */ usart_dev->cfg = cfg; /* Map pins to USART function */ if (usart_dev->cfg->remap) { if (usart_dev->cfg->rx.gpio != 0) GPIO_PinAFConfig(usart_dev->cfg->rx.gpio, usart_dev->cfg->rx.pin_source, usart_dev->cfg->remap); if (usart_dev->cfg->tx.gpio != 0) GPIO_PinAFConfig(usart_dev->cfg->tx.gpio, usart_dev->cfg->tx.pin_source, usart_dev->cfg->remap); } /* Initialize the USART Rx and Tx pins */ if (usart_dev->cfg->rx.gpio != 0) GPIO_Init(usart_dev->cfg->rx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->rx.init); if (usart_dev->cfg->tx.gpio != 0) GPIO_Init(usart_dev->cfg->tx.gpio, (GPIO_InitTypeDef *)&usart_dev->cfg->tx.init); /* Apply inversion and swap settings */ if (usart_dev->cfg->rx_invert == true) USART_InvPinCmd(usart_dev->cfg->regs, USART_InvPin_Rx, ENABLE); else USART_InvPinCmd(usart_dev->cfg->regs, USART_InvPin_Rx, DISABLE); if (usart_dev->cfg->tx_invert == true) USART_InvPinCmd(usart_dev->cfg->regs, USART_InvPin_Tx, ENABLE); else USART_InvPinCmd(usart_dev->cfg->regs, USART_InvPin_Tx, DISABLE); if (usart_dev->cfg->rxtx_swap == true) USART_SWAPPinCmd(usart_dev->cfg->regs, ENABLE); else USART_SWAPPinCmd(usart_dev->cfg->regs, DISABLE); /* Configure the USART */ USART_Init(usart_dev->cfg->regs, (USART_InitTypeDef *)&usart_dev->cfg->init); *usart_id = (uintptr_t)usart_dev; /* Configure USART Interrupts */ switch ((uint32_t)usart_dev->cfg->regs) { case (uint32_t)USART1: PIOS_USART_1_id = (uintptr_t)usart_dev; break; case (uint32_t)USART2: PIOS_USART_2_id = (uintptr_t)usart_dev; break; case (uint32_t)USART3: PIOS_USART_3_id = (uintptr_t)usart_dev; break; case (uint32_t)UART4: PIOS_UART_4_id = (uintptr_t)usart_dev; break; case (uint32_t)UART5: PIOS_UART_5_id = (uintptr_t)usart_dev; break; } NVIC_Init((NVIC_InitTypeDef *)&(usart_dev->cfg->irq.init)); USART_ITConfig(usart_dev->cfg->regs, USART_IT_RXNE, ENABLE); USART_ITConfig(usart_dev->cfg->regs, USART_IT_TXE, ENABLE); // FIXME XXX Clear / reset uart here - sends NUL char else /* Enable USART */ USART_Cmd(usart_dev->cfg->regs, ENABLE); return(0); out_fail: return(-1); }
int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg *cfg) { PIOS_DEBUG_Assert(cfg); /* Enable appropriate clock to timer module */ switch ((uint32_t)cfg->timer) { case (uint32_t)TIM1: RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); break; case (uint32_t)TIM2: RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); break; case (uint32_t)TIM3: RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); break; case (uint32_t)TIM4: RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); break; case (uint32_t)TIM5: RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); break; case (uint32_t)TIM6: RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); break; case (uint32_t)TIM7: RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); break; case (uint32_t)TIM8: RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); break; case (uint32_t)TIM9: RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM9, ENABLE); break; case (uint32_t)TIM10: RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM10, ENABLE); break; case (uint32_t)TIM11: RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM11, ENABLE); break; case (uint32_t)TIM12: RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12, ENABLE); break; case (uint32_t)TIM13: RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM13, ENABLE); break; case (uint32_t)TIM14: RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM14, ENABLE); break; } /* Configure the dividers for this timer */ TIM_TimeBaseInit(cfg->timer, cfg->time_base_init); /* Configure internal timer clocks */ TIM_InternalClockConfig(cfg->timer); /* Enable timers */ TIM_Cmd(cfg->timer, ENABLE); /* Enable Interrupts */ NVIC_Init(&cfg->irq.init); return 0; }
void PIOS_Board_Init(void) { const struct pios_board_info * bdinfo = &pios_board_info_blob; /* Delay system */ PIOS_DELAY_Init(); PIOS_LED_Init(&pios_led_cfg); /* Set up the SPI interface to the accelerometer*/ if (PIOS_SPI_Init(&pios_spi_accel_id, &pios_spi_accel_cfg)) { PIOS_DEBUG_Assert(0); } /* Set up the SPI interface to the gyro */ if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } #if !defined(PIOS_FLASH_ON_ACCEL) /* Set up the SPI interface to the flash */ if (PIOS_SPI_Init(&pios_spi_flash_id, &pios_spi_flash_cfg)) { PIOS_DEBUG_Assert(0); } /* Inititialize all flash drivers */ PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_flash_id, 0, &flash_m25p_cfg); #else /* Inititialize all flash drivers */ PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_accel_id, 1, &flash_m25p_cfg); #endif PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg); /* 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 */ PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_settings_cfg, FLASH_PARTITION_LABEL_SETTINGS); PIOS_FLASHFS_Logfs_Init(&pios_waypoints_settings_fs_id, &flashfs_waypoints_cfg, FLASH_PARTITION_LABEL_WAYPOINTS); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); HwRevolutionInitialize(); 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); PIOS_TIM_InitClock(&tim_4_cfg); PIOS_TIM_InitClock(&tim_5_cfg); PIOS_TIM_InitClock(&tim_9_cfg); PIOS_TIM_InitClock(&tim_10_cfg); PIOS_TIM_InitClock(&tim_11_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 */ HwRevolutionSetDefaults(HwRevolutionHandle(), 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_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwRevolutionUSB_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 = HWREVOLUTION_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 HWREVOLUTION_USB_VCPPORT_DISABLED: break; case HWREVOLUTION_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWREVOLUTION_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWREVOLUTION_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { 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; HwRevolutionUSB_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 = HWREVOLUTION_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 HWREVOLUTION_USB_HIDPORT_DISABLED: break; case HWREVOLUTION_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { 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; HwRevolutionDSMxBindGet(&hw_DSMxBind); /* Configure Telemetry port */ uint8_t hw_telemetryport; HwRevolutionTelemetryPortGet(&hw_telemetryport); switch (hw_telemetryport){ case HWREVOLUTION_TELEMETRYPORT_DISABLED: break; case HWREVOLUTION_TELEMETRYPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_telem_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 HWREVOLUTION_TELEMETRYPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) PIOS_Board_configure_com(&pios_usart_telem_cfg, 0, PIOS_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWREVOLUTION_TELEMETRYPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; } /* hw_telemetryport */ /* Configure GPS port */ uint8_t hw_gpsport; HwRevolutionGPSPortGet(&hw_gpsport); switch (hw_gpsport){ case HWREVOLUTION_GPSPORT_DISABLED: break; case HWREVOLUTION_GPSPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_gps_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 HWREVOLUTION_GPSPORT_GPS: PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, 0, &pios_usart_com_driver, &pios_com_gps_id); break; case HWREVOLUTION_GPSPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) PIOS_Board_configure_com(&pios_usart_gps_cfg, 0, PIOS_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWREVOLUTION_GPSPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; }/* hw_gpsport */ /* Configure AUXPort */ uint8_t hw_auxport; HwRevolutionAuxPortGet(&hw_auxport); switch (hw_auxport) { case HWREVOLUTION_AUXPORT_DISABLED: break; case HWREVOLUTION_AUXPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_aux_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 HWREVOLUTION_AUXPORT_DSM2: case HWREVOLUTION_AUXPORT_DSMX10BIT: case HWREVOLUTION_AUXPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) { enum pios_dsm_proto proto; switch (hw_auxport) { case HWREVOLUTION_AUXPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWREVOLUTION_AUXPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWREVOLUTION_AUXPORT_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_hsum_aux_cfg, &pios_dsm_aux_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWREVOLUTION_AUXPORT_HOTTSUMD: case HWREVOLUTION_AUXPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_auxport) { case HWREVOLUTION_AUXPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWREVOLUTION_AUXPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_aux_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWREVOLUTION_AUXPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_aux_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWREVOLUTION_AUXPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) && defined(PIOS_INCLUDE_GPS) PIOS_Board_configure_com(&pios_usart_aux_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 */ break; case HWREVOLUTION_AUXPORT_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); #endif /* PIOS_INCLUDE_HOTT */ break; case HWREVOLUTION_AUXPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) PIOS_Board_configure_com(&pios_usart_aux_cfg, 0, PIOS_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWREVOLUTION_AUXPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; } /* hw_auxport */ /* Configure AUXSbusPort */ //TODO: ensure that the serial invertion pin is setted correctly uint8_t hw_auxsbusport; HwRevolutionAuxSBusPortGet(&hw_auxsbusport); switch (hw_auxsbusport) { case HWREVOLUTION_AUXSBUSPORT_DISABLED: break; case HWREVOLUTION_AUXSBUSPORT_SBUS: #ifdef PIOS_INCLUDE_SBUS { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_auxsbus_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 /* PIOS_INCLUDE_SBUS */ break; case HWREVOLUTION_AUXSBUSPORT_DSM2: case HWREVOLUTION_AUXSBUSPORT_DSMX10BIT: case HWREVOLUTION_AUXSBUSPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) { enum pios_dsm_proto proto; switch (hw_auxsbusport) { case HWREVOLUTION_AUXSBUSPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWREVOLUTION_AUXSBUSPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWREVOLUTION_AUXSBUSPORT_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_hsum_auxsbus_cfg, &pios_dsm_auxsbus_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWREVOLUTION_AUXSBUSPORT_HOTTSUMD: case HWREVOLUTION_AUXSBUSPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_auxsbusport) { case HWREVOLUTION_AUXSBUSPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWREVOLUTION_AUXSBUSPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_auxsbus_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWREVOLUTION_AUXSBUSPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWREVOLUTION_AUXSBUSPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) && defined(PIOS_INCLUDE_GPS) PIOS_Board_configure_com(&pios_usart_auxsbus_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 */ break; case HWREVOLUTION_AUXSBUSPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, 0, PIOS_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWREVOLUTION_AUXSBUSPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; case HWREVOLUTION_AUXSBUSPORT_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); #endif /* PIOS_INCLUDE_HOTT */ break; } /* hw_auxport */ /* Configure FlexiPort */ uint8_t hw_flexiport; HwRevolutionFlexiPortGet(&hw_flexiport); switch (hw_flexiport) { case HWREVOLUTION_FLEXIPORT_DISABLED: break; case HWREVOLUTION_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 HWREVOLUTION_FLEXIPORT_DSM2: case HWREVOLUTION_FLEXIPORT_DSMX10BIT: case HWREVOLUTION_FLEXIPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) { enum pios_dsm_proto proto; switch (hw_flexiport) { case HWREVOLUTION_FLEXIPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWREVOLUTION_FLEXIPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWREVOLUTION_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_hsum_flexi_cfg, &pios_dsm_flexi_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWREVOLUTION_FLEXIPORT_HOTTSUMD: case HWREVOLUTION_FLEXIPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_flexiport) { case HWREVOLUTION_FLEXIPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWREVOLUTION_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 HWREVOLUTION_FLEXIPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && 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 HWREVOLUTION_FLEXIPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) && defined(PIOS_INCLUDE_GPS) 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 */ break; case HWREVOLUTION_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWREVOLUTION_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 HWREVOLUTION_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_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); #endif /* PIOS_INCLUDE_HOTT */ break; } /* hw_flexiport */ /* Configure the receiver port*/ uint8_t hw_rcvrport; HwRevolutionRcvrPortGet(&hw_rcvrport); // switch (hw_rcvrport){ case HWREVOLUTION_RCVRPORT_DISABLED: break; case HWREVOLUTION_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) { /* Set up the receiver port. Later this should be optional */ uintptr_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); uintptr_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } #endif /* PIOS_INCLUDE_PWM */ break; case HWREVOLUTION_RCVRPORT_PPM: case HWREVOLUTION_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_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; } #endif /* PIOS_INCLUDE_PPM */ case HWREVOLUTION_RCVRPORT_OUTPUTS: break; } #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(); 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 switch (hw_rcvrport) { case HWREVOLUTION_RCVRPORT_DISABLED: case HWREVOLUTION_RCVRPORT_PWM: case HWREVOLUTION_RCVRPORT_PPM: /* Set up the servo outputs */ PIOS_Servo_Init(&pios_servo_cfg); break; case HWREVOLUTION_RCVRPORT_PPMOUTPUTS: case HWREVOLUTION_RCVRPORT_OUTPUTS: //PIOS_Servo_Init(&pios_servo_rcvr_cfg); //TODO: Prepare the configurations on board_hw_defs and handle here: PIOS_Servo_Init(&pios_servo_cfg); break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif if (PIOS_I2C_Init(&pios_i2c_mag_adapter_id, &pios_i2c_mag_adapter_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_Init(&pios_i2c_pressure_adapter_id, &pios_i2c_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } PIOS_DELAY_WaitmS(50); PIOS_SENSORS_Init(); #if defined(PIOS_INCLUDE_ADC) uint32_t internal_adc_id; if(PIOS_INTERNAL_ADC_Init(&internal_adc_id, &pios_adc_cfg) < 0) PIOS_Assert(0); PIOS_ADC_Init(&pios_internal_adc_id, &pios_internal_adc_driver, internal_adc_id); #endif #if defined(PIOS_INCLUDE_HMC5883) PIOS_HMC5883_Init(PIOS_I2C_MAIN_ADAPTER, &pios_hmc5883_cfg); #endif #if defined(PIOS_INCLUDE_MS5611) PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id); #endif switch(bdinfo->board_rev) { case 0x01: #if defined(PIOS_INCLUDE_L3GD20) PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg); PIOS_Assert(PIOS_L3GD20_Test() == 0); #endif #if defined(PIOS_INCLUDE_BMA180) PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg); PIOS_Assert(PIOS_BMA180_Test() == 0); #endif break; case 0x02: #if defined(PIOS_INCLUDE_MPU6000) PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwRevolutionGyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWREVOLUTION_GYRORANGE_250: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWREVOLUTION_GYRORANGE_500: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWREVOLUTION_GYRORANGE_1000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWREVOLUTION_GYRORANGE_2000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwRevolutionAccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWREVOLUTION_ACCELRANGE_2G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWREVOLUTION_ACCELRANGE_4G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWREVOLUTION_ACCELRANGE_8G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWREVOLUTION_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; HwRevolutionMPU6000DLPFGet(&hw_mpu6000_dlpf); enum pios_mpu60x0_filter mpu6000_dlpf = \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ pios_mpu6000_cfg.default_filter; PIOS_MPU6000_SetLPF(mpu6000_dlpf); uint8_t hw_mpu6000_samplerate; HwRevolutionMPU6000RateGet(&hw_mpu6000_samplerate); uint16_t mpu6000_samplerate = \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_200) ? 200 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_333) ? 333 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_500) ? 500 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_666) ? 666 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_1000) ? 1000 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_2000) ? 2000 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_4000) ? 4000 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_8000) ? 8000 : \ pios_mpu6000_cfg.default_samplerate; PIOS_MPU6000_SetSampleRate(mpu6000_samplerate); #endif break; default: PIOS_DEBUG_Assert(0); } }
static void SensorsTask(void *parameters) { portTickType lastSysTime; uint32_t accel_samples = 0; uint32_t gyro_samples = 0; int32_t accel_accum[3] = {0, 0, 0}; int32_t gyro_accum[3] = {0,0,0}; float gyro_scaling = 0; float accel_scaling = 0; static int32_t timeval; AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); UAVObjEvent ev; settingsUpdatedCb(&ev); const struct pios_board_info * bdinfo = &pios_board_info_blob; switch(bdinfo->board_rev) { case 0x01: #if defined(PIOS_INCLUDE_L3GD20) gyro_test = PIOS_L3GD20_Test(); #endif #if defined(PIOS_INCLUDE_BMA180) accel_test = PIOS_BMA180_Test(); #endif break; case 0x02: #if defined(PIOS_INCLUDE_MPU6000) gyro_test = PIOS_MPU6000_Test(); accel_test = gyro_test; #endif break; default: PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_HMC5883) mag_test = PIOS_HMC5883_Test(); #else mag_test = 0; #endif if(accel_test < 0 || gyro_test < 0 || mag_test < 0) { AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); while(1) { PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); vTaskDelay(10); } } // Main task loop lastSysTime = xTaskGetTickCount(); bool error = false; uint32_t mag_update_time = PIOS_DELAY_GetRaw(); while (1) { // TODO: add timeouts to the sensor reads and set an error if the fail sensor_dt_us = PIOS_DELAY_DiffuS(timeval); timeval = PIOS_DELAY_GetRaw(); if (error) { PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); lastSysTime = xTaskGetTickCount(); vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS); AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); error = false; } else { AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); } for (int i = 0; i < 3; i++) { accel_accum[i] = 0; gyro_accum[i] = 0; } accel_samples = 0; gyro_samples = 0; AccelsData accelsData; GyrosData gyrosData; switch(bdinfo->board_rev) { case 0x01: // L3GD20 + BMA180 board #if defined(PIOS_INCLUDE_BMA180) { struct pios_bma180_data accel; int32_t read_good; int32_t count; count = 0; while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error; if (error) { // Unfortunately if the BMA180 ever misses getting read, then it will not // trigger more interrupts. In this case we must force a read to kickstarts // it. struct pios_bma180_data data; PIOS_BMA180_ReadAccels(&data); continue; } while(read_good == 0) { count++; accel_accum[1] += accel.x; accel_accum[0] += accel.y; accel_accum[2] -= accel.z; read_good = PIOS_BMA180_ReadFifo(&accel); } accel_samples = count; accel_scaling = PIOS_BMA180_GetScale(); // Get temp from last reading accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f; } #endif #if defined(PIOS_INCLUDE_L3GD20) { struct pios_l3gd20_data gyro; gyro_samples = 0; xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue(); if(xQueueReceive(gyro_queue, (void *) &gyro, 4) == errQUEUE_EMPTY) { error = true; continue; } gyro_samples = 1; gyro_accum[1] += gyro.gyro_x; gyro_accum[0] += gyro.gyro_y; gyro_accum[2] -= gyro.gyro_z; gyro_scaling = PIOS_L3GD20_GetScale(); // Get temp from last reading gyrosData.temperature = gyro.temperature; } #endif break; case 0x02: // MPU6000 board case 0x03: // MPU6000 board #if defined(PIOS_INCLUDE_MPU6000) { struct pios_mpu6000_data mpu6000_data; xQueueHandle queue = PIOS_MPU6000_GetQueue(); while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY) { gyro_accum[0] += mpu6000_data.gyro_x; gyro_accum[1] += mpu6000_data.gyro_y; gyro_accum[2] += mpu6000_data.gyro_z; accel_accum[0] += mpu6000_data.accel_x; accel_accum[1] += mpu6000_data.accel_y; accel_accum[2] += mpu6000_data.accel_z; gyro_samples ++; accel_samples ++; } if (gyro_samples == 0) { PIOS_MPU6000_ReadGyros(&mpu6000_data); error = true; continue; } gyro_scaling = PIOS_MPU6000_GetScale(); accel_scaling = PIOS_MPU6000_GetAccelScale(); gyrosData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; accelsData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; } #endif /* PIOS_INCLUDE_MPU6000 */ break; default: PIOS_DEBUG_Assert(0); } // Scale the accels float accels[3] = {(float) accel_accum[0] / accel_samples, (float) accel_accum[1] / accel_samples, (float) accel_accum[2] / accel_samples}; float accels_out[3] = {accels[0] * accel_scaling * accel_scale[0] - accel_bias[0], accels[1] * accel_scaling * accel_scale[1] - accel_bias[1], accels[2] * accel_scaling * accel_scale[2] - accel_bias[2]}; if (rotate) { rot_mult(R, accels_out, accels); accelsData.x = accels[0]; accelsData.y = accels[1]; accelsData.z = accels[2]; } else { accelsData.x = accels_out[0]; accelsData.y = accels_out[1]; accelsData.z = accels_out[2]; } AccelsSet(&accelsData); // Scale the gyros float gyros[3] = {(float) gyro_accum[0] / gyro_samples, (float) gyro_accum[1] / gyro_samples, (float) gyro_accum[2] / gyro_samples}; float gyros_out[3] = {gyros[0] * gyro_scaling, gyros[1] * gyro_scaling, gyros[2] * gyro_scaling}; if (rotate) { rot_mult(R, gyros_out, gyros); gyrosData.x = gyros[0]; gyrosData.y = gyros[1]; gyrosData.z = gyros[2]; } else { gyrosData.x = gyros_out[0]; gyrosData.y = gyros_out[1]; gyrosData.z = gyros_out[2]; } if (bias_correct_gyro) { // Apply bias correction to the gyros from the state estimator GyrosBiasData gyrosBias; GyrosBiasGet(&gyrosBias); gyrosData.x -= gyrosBias.x; gyrosData.y -= gyrosBias.y; gyrosData.z -= gyrosBias.z; } GyrosSet(&gyrosData); // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) #if defined(PIOS_INCLUDE_HMC5883) MagnetometerData mag; if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { int16_t values[3]; PIOS_HMC5883_ReadMag(values); float mags[3] = {(float) values[1] * mag_scale[0] - mag_bias[0], (float) values[0] * mag_scale[1] - mag_bias[1], -(float) values[2] * mag_scale[2] - mag_bias[2]}; if (rotate) { float mag_out[3]; rot_mult(R, mags, mag_out); mag.x = mag_out[0]; mag.y = mag_out[1]; mag.z = mag_out[2]; } else { mag.x = mags[0]; mag.y = mags[1]; mag.z = mags[2]; } // Correct for mag bias and update if the rate is non zero if(cal.MagBiasNullingRate > 0) magOffsetEstimation(&mag); MagnetometerSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); } #endif PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); lastSysTime = xTaskGetTickCount(); } }
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 appropriate interface and configure it */ uintptr_t flash_id; // Initialize the external USER flash if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1)) { PIOS_DEBUG_Assert(0); } if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_external_system_cfg, &pios_jedec_flash_driver, flash_id)) { PIOS_DEBUG_Assert(0); } if (PIOS_FLASHFS_Logfs_Init(&pios_user_fs_id, &flashfs_external_user_cfg, &pios_jedec_flash_driver, flash_id)) { PIOS_DEBUG_Assert(0); } #endif /* if defined(PIOS_INCLUDE_FLASH) */ #if defined(PIOS_INCLUDE_RTC) PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* IAP System Setup */ PIOS_IAP_Init(); // check for safe mode commands from gcs if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); PIOS_IAP_WriteBootCmd(0, 0); PIOS_IAP_WriteBootCmd(1, 0); PIOS_IAP_WriteBootCmd(2, 0); } #ifdef PIOS_INCLUDE_WDG PIOS_WDG_Init(); #endif /* Initialize the task monitor */ if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { PIOS_Assert(0); } /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); HwSettingsInitialize(); #if defined(PIOS_INCLUDE_RFM22B) OPLinkSettingsInitialize(); OPLinkStatusInitialize(); #endif /* PIOS_INCLUDE_RFM22B */ /* Initialize the alarms library */ AlarmsInitialize(); /* Initialize the delayed callback library */ CallbackSchedulerInitialize(); /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_3_cfg); PIOS_TIM_InitClock(&tim_4_cfg); PIOS_TIM_InitClock(&tim_5_cfg); PIOS_TIM_InitClock(&tim_8_cfg); PIOS_TIM_InitClock(&tim_9_cfg); PIOS_TIM_InitClock(&tim_10_cfg); PIOS_TIM_InitClock(&tim_11_cfg); PIOS_TIM_InitClock(&tim_12_cfg); uint16_t boot_count = PIOS_IAP_ReadBootCount(); if (boot_count < 3) { PIOS_IAP_WriteBootCount(++boot_count); AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); } else { /* Too many failed boot attempts, force hwsettings to defaults */ HwSettingsSetDefaults(HwSettingsHandle(), 0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } // 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 hwsettings_usb_vcpport; /* Configure the USB VCP port */ HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; } 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); } 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); } switch (hwsettings_usb_vcpport) { case HWSETTINGS_USB_VCPPORT_DISABLED: break; case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWSETTINGS_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { 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 hwsettings_usb_hidport; HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; } switch (hwsettings_usb_hidport) { case HWSETTINGS_USB_HIDPORT_DISABLED: break; case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { 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 hwsettings_DSMxBind; HwSettingsDSMxBindGet(&hwsettings_DSMxBind); /* Configure main USART port */ uint8_t hwsettings_mainport; HwSettingsRM_MainPortGet(&hwsettings_mainport); switch (hwsettings_mainport) { case HWSETTINGS_RM_MAINPORT_DISABLED: break; case HWSETTINGS_RM_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 HWSETTINGS_RM_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 HWSETTINGS_RM_MAINPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) { uint32_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { PIOS_Assert(0); } uint32_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } uint32_t pios_sbus_rcvr_id; if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; } #endif break; case HWSETTINGS_RM_MAINPORT_DSM2: case HWSETTINGS_RM_MAINPORT_DSMX10BIT: case HWSETTINGS_RM_MAINPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hwsettings_mainport) { case HWSETTINGS_RM_MAINPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWSETTINGS_RM_MAINPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWSETTINGS_RM_MAINPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } // Force binding to zero on the main port hwsettings_DSMxBind = 0; // TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); } break; case HWSETTINGS_RM_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 HWSETTINGS_RM_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 HWSETTINGS_RM_MAINPORT_OSDHK: PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); break; } /* hwsettings_rm_mainport */ if (hwsettings_mainport != HWSETTINGS_RM_MAINPORT_SBUS) { GPIO_Init(pios_sbus_cfg.inv.gpio, &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); } /* Configure FlexiPort */ uint8_t hwsettings_flexiport; HwSettingsRM_FlexiPortGet(&hwsettings_flexiport); switch (hwsettings_flexiport) { case HWSETTINGS_RM_FLEXIPORT_DISABLED: break; case HWSETTINGS_RM_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 HWSETTINGS_RM_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 HWSETTINGS_RM_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 HWSETTINGS_RM_FLEXIPORT_DSM2: case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: case HWSETTINGS_RM_FLEXIPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hwsettings_flexiport) { case HWSETTINGS_RM_FLEXIPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWSETTINGS_RM_FLEXIPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWSETTINGS_RM_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, &hwsettings_DSMxBind); } break; case HWSETTINGS_RM_FLEXIPORT_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 HWSETTINGS_RM_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 HWSETTINGS_RM_FLEXIPORT_OSDHK: PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id); break; } /* hwsettings_rm_flexiport */ /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) /* Fetch the OPinkSettings object. */ OPLinkSettingsData oplinkSettings; OPLinkSettingsGet(&oplinkSettings); // Initialize out status object. OPLinkStatusData oplinkStatus; OPLinkStatusGet(&oplinkStatus); oplinkStatus.BoardType = bdinfo->board_type; PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); oplinkStatus.BoardRevision = bdinfo->board_rev; /* Is the radio turned on? */ bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); bool is_oneway = (oplinkSettings.OneWay == OPLINKSETTINGS_ONEWAY_TRUE); bool ppm_mode = (oplinkSettings.PPM == OPLINKSETTINGS_PPM_TRUE); bool ppm_only = (oplinkSettings.PPMOnly == OPLINKSETTINGS_PPMONLY_TRUE); if (oplinkSettings.MaxRFPower != OPLINKSETTINGS_MAXRFPOWER_0) { /* 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)) { PIOS_Assert(0); } /* Configure the radio com interface */ 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); } oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED; // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. enum rfm22b_datarate datarate = RFM22_datarate_64000; switch (oplinkSettings.ComSpeed) { case OPLINKSETTINGS_COMSPEED_4800: datarate = RFM22_datarate_9600; break; case OPLINKSETTINGS_COMSPEED_9600: datarate = RFM22_datarate_19200; break; case OPLINKSETTINGS_COMSPEED_19200: datarate = RFM22_datarate_32000; break; case OPLINKSETTINGS_COMSPEED_38400: datarate = RFM22_datarate_64000; break; case OPLINKSETTINGS_COMSPEED_57600: datarate = RFM22_datarate_100000; break; case OPLINKSETTINGS_COMSPEED_115200: datarate = RFM22_datarate_192000; break; } /* Set the radio configuration parameters. */ PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only); PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); /* Set the PPM callback if we should be receiving PPM. */ if (ppm_mode) { PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback); } /* Set the modem Tx poer level */ switch (oplinkSettings.MaxRFPower) { case OPLINKSETTINGS_MAXRFPOWER_125: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); break; case OPLINKSETTINGS_MAXRFPOWER_16: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); break; case OPLINKSETTINGS_MAXRFPOWER_316: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); break; case OPLINKSETTINGS_MAXRFPOWER_63: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); break; case OPLINKSETTINGS_MAXRFPOWER_126: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); break; case OPLINKSETTINGS_MAXRFPOWER_25: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); break; case OPLINKSETTINGS_MAXRFPOWER_50: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); break; case OPLINKSETTINGS_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); } else { oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; } OPLinkStatusSet(&oplinkStatus); #endif /* PIOS_INCLUDE_RFM22B */ #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PWM) const struct pios_servo_cfg *pios_servo_cfg; // default to servo outputs only pios_servo_cfg = &pios_servo_cfg_out; #endif /* Configure the receiver port*/ uint8_t hwsettings_rcvrport; HwSettingsRM_RcvrPortGet(&hwsettings_rcvrport); // switch (hwsettings_rcvrport) { case HWSETTINGS_RM_RCVRPORT_DISABLED: break; case HWSETTINGS_RM_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) /* Set up the receiver port. Later this should be optional */ PIOS_Board_configure_pwm(&pios_pwm_cfg); #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_RM_RCVRPORT_PPM: case HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS: case HWSETTINGS_RM_RCVRPORT_PPMPWM: #if defined(PIOS_INCLUDE_PPM) if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) { // configure servo outputs and the remaining 5 inputs as outputs pios_servo_cfg = &pios_servo_cfg_out_in_ppm; } PIOS_Board_configure_ppm(&pios_ppm_cfg); // enable pwm on the remaining channels if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) { PIOS_Board_configure_pwm(&pios_pwm_ppm_cfg); } break; #endif /* PIOS_INCLUDE_PPM */ case HWSETTINGS_RM_RCVRPORT_OUTPUTS: // configure only the servo outputs pios_servo_cfg = &pios_servo_cfg_out_in; break; } #if defined(PIOS_INCLUDE_GCSRCVR) GCSReceiverInitialize(); uint32_t pios_gcsrcvr_id; PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); uint32_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ #if defined(PIOS_INCLUDE_OPLINKRCVR) { OPLinkReceiverInitialize(); uint32_t pios_oplinkrcvr_id; PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id); uint32_t pios_oplinkrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_oplinkrcvr_rcvr_id, &pios_oplinkrcvr_rcvr_driver, pios_oplinkrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_oplinkrcvr_rcvr_id; } #endif /* PIOS_INCLUDE_OPLINKRCVR */ #ifndef PIOS_ENABLE_DEBUG_PINS // pios_servo_cfg points to the correct configuration based on input port settings PIOS_Servo_Init(pios_servo_cfg); #else PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins)); #endif if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } PIOS_DELAY_WaitmS(50); #if defined(PIOS_INCLUDE_ADC) PIOS_ADC_Init(&pios_adc_cfg); #endif #if defined(PIOS_INCLUDE_HMC5883) PIOS_HMC5883_Init(&pios_hmc5883_cfg); #endif #if defined(PIOS_INCLUDE_MS5611) PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id); #endif #if defined(PIOS_INCLUDE_MPU6000) PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg); PIOS_MPU6000_CONFIG_Configure(); #endif }
void PIOS_Board_Init(void) { // Delay system PIOS_DELAY_Init(); PIOS_LED_Init(&pios_led_cfg); #if defined(PIOS_INCLUDE_SPI) /* Set up the SPI interface to the SD card */ if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) { PIOS_Assert(0); } #if defined(PIOS_INCLUDE_SDCARD) /* Enable and mount the SDCard */ PIOS_SDCARD_Init(pios_spi_sdcard_id); PIOS_SDCARD_MountFS(0); #endif #endif /* PIOS_INCLUDE_SPI */ #ifdef PIOS_INCLUDE_FLASH_LOGFS_SETTINGS uintptr_t flash_id; PIOS_Flash_Internal_Init(&flash_id, &flash_internal_cfg); PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id); #elif !defined(PIOS_USE_SETTINGS_ON_SDCARD) #error No setting storage specified. (define PIOS_USE_SETTINGS_ON_SDCARD or INCLUDE_FLASH_SECTOR_SETTINGS) #endif /* Initialize the task monitor */ if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) { PIOS_Assert(0); } /* Initialize the delayed callback library */ PIOS_CALLBACKSCHEDULER_Initialize(); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); HwSettingsInitialize(); #ifdef PIOS_INCLUDE_WDG /* Initialize watchdog as early as possible to catch faults during init */ PIOS_WDG_Init(); #endif /* PIOS_INCLUDE_WDG */ /* Initialize the alarms library */ AlarmsInitialize(); /* 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 hwsettings to defaults */ HwSettingsSetDefaults(HwSettingsHandle(), 0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ PIOS_RTC_Init(&pios_rtc_main_cfg); if (!PIOS_RTC_RegisterTickCallback(Clock, 0)) { PIOS_DEBUG_Assert(0); } #endif #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_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hwsettings_usb_vcpport; /* Configure the USB VCP port */ HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; } switch (hwsettings_usb_vcpport) { case HWSETTINGS_USB_VCPPORT_DISABLED: break; case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint32_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWSETTINGS_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { uint32_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hwsettings_usb_hidport; HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; } switch (hwsettings_usb_hidport) { case HWSETTINGS_USB_HIDPORT_DISABLED: break; case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint32_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_HID */ if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } #endif /* PIOS_INCLUDE_USB */ #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_GPS) uint32_t pios_usart_gps_id; if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { PIOS_Assert(0); } uint8_t *gps_rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_GPS_RX_BUF_LEN); PIOS_Assert(gps_rx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, gps_rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, NULL, 0)) { PIOS_Assert(0); } #endif /* PIOS_INCLUDE_GPS */ #if defined(PIOS_INCLUDE_COM_AUX) { uint32_t pios_usart_aux_id; if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) { PIOS_DEBUG_Assert(0); } uint8_t *aux_rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_AUX_RX_BUF_LEN); uint8_t *aux_tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_AUX_TX_BUF_LEN); PIOS_Assert(aux_rx_buffer); PIOS_Assert(aux_tx_buffer); if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id, aux_rx_buffer, PIOS_COM_AUX_RX_BUF_LEN, aux_tx_buffer, PIOS_COM_AUX_TX_BUF_LEN)) { PIOS_DEBUG_Assert(0); } } #else pios_com_aux_id = 0; #endif /* PIOS_INCLUDE_COM_AUX */ #if defined(PIOS_INCLUDE_COM_TELEM) { /* Eventually add switch for this port function */ uint32_t pios_usart_telem_rf_id; if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_main_cfg)) { PIOS_Assert(0); } uint8_t *telem_rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); uint8_t *telem_tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); PIOS_Assert(telem_rx_buffer); PIOS_Assert(telem_tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id, telem_rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, telem_tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { PIOS_Assert(0); } } #else pios_com_telem_rf_id = 0; #endif /* PIOS_INCLUDE_COM_TELEM */ #endif /* PIOS_INCLUDE_COM */ /* Configure FlexiPort */ /* uint8_t hwsettings_rv_flexiport; HwSettingsRV_FlexiPortGet(&hwsettings_rv_flexiport); switch (hwsettings_rv_flexiport) { case HWSETTINGS_RV_FLEXIPORT_DISABLED: break; case HWSETTINGS_RV_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 HWSETTINGS_RV_FLEXIPORT_DSM: //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, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hwsettings_DSMxBind); break; case HWSETTINGS_RV_FLEXIPORT_COMAUX: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); break; case HWSETTINGS_RV_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; }*/ /* hwsettings_rv_flexiport */ #if defined(PIOS_INCLUDE_WAVE) PIOS_WavPlay_Init(&pios_dac_cfg); #endif // ADC system #if defined(PIOS_INCLUDE_ADC) PIOS_ADC_Init(&pios_adc_cfg); #endif #if defined(PIOS_INCLUDE_VIDEO) PIOS_TIM_InitClock(&tim_8_cfg); PIOS_Servo_Init(&pios_servo_cfg); // Start the pixel and line clock counter // PIOS_TIM_InitClock(&pios_tim4_cfg); PIOS_Video_Init(&pios_video_cfg); #endif }
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 */ #if defined(PIOS_INCLUDE_FLASH) /* Inititialize all flash drivers */ 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 defined(PIOS_INCLUDE_EXTERNAL_FLASH_WAYPOINTS) /* Use external flash chip to store waypoints */ if (PIOS_FLASHFS_Logfs_Init(&pios_waypoints_settings_fs_id, &flashfs_external_waypoints_cfg, FLASH_PARTITION_LABEL_WAYPOINTS) != 0) panic(1); #endif /* PIOS_INCLUDE_EXTERNAL_FLASH_WAYPOINTS */ #endif /* PIOS_INCLUDE_FLASH */ /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); /* Initialize the alarms library */ AlarmsInitialize(); HwFlyingF4Initialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ 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 */ //inputs PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_4_cfg); PIOS_TIM_InitClock(&tim_8_cfg); PIOS_TIM_InitClock(&tim_9_cfg); //outputs PIOS_TIM_InitClock(&tim_1_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 */ HwFlyingF4SetDefaults(HwFlyingF4Handle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } #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 */ HwFlyingF4USB_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 = HWFLYINGF4_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 HWFLYINGF4_USB_VCPPORT_DISABLED: break; case HWFLYINGF4_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 HWFLYINGF4_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 HWFLYINGF4_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 HWFLYINGF4_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; HwFlyingF4USB_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 = HWFLYINGF4_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 HWFLYINGF4_USB_HIDPORT_DISABLED: break; case HWFLYINGF4_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 the IO ports */ uint8_t hw_DSMxBind; HwFlyingF4DSMxBindGet(&hw_DSMxBind); /* init sensor queue registration */ PIOS_SENSORS_Init(); /* UART1 Port */ uint8_t hw_uart1; HwFlyingF4Uart1Get(&hw_uart1); switch (hw_uart1) { case HWFLYINGF4_UART1_DISABLED: break; case HWFLYINGF4_UART1_GPS: #if defined(PIOS_INCLUDE_GPS) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart1_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); #endif break; case HWFLYINGF4_UART1_SBUS: //hardware signal inverter required #if defined(PIOS_INCLUDE_SBUS) && defined(PIOS_INCLUDE_USART) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart1_sbus_cfg)) { PIOS_Assert(0); } uintptr_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_usart1_sbus_aux_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 /* PIOS_INCLUDE_SBUS */ break; case HWFLYINGF4_UART1_DSM: #if defined(PIOS_INCLUDE_DSM) { PIOS_Board_configure_dsm(&pios_usart1_dsm_hsum_cfg, &pios_usart1_dsm_aux_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFLYINGF4_UART1_HOTTSUMD: case HWFLYINGF4_UART1_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_uart1) { case HWFLYINGF4_UART1_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFLYINGF4_UART1_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart1_dsm_hsum_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFLYINGF4_UART1_PICOC: #if defined(PIOS_INCLUDE_PICOC) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart1_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; } /* UART2 Port */ uint8_t hw_uart2; HwFlyingF4Uart2Get(&hw_uart2); switch (hw_uart2) { case HWFLYINGF4_UART2_DISABLED: break; case HWFLYINGF4_UART2_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart2_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWFLYINGF4_UART2_GPS: #if defined(PIOS_INCLUDE_GPS) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart2_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); #endif break; case HWFLYINGF4_UART2_DSM: #if defined(PIOS_INCLUDE_DSM) { PIOS_Board_configure_dsm(&pios_usart2_dsm_hsum_cfg, &pios_usart2_dsm_aux_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFLYINGF4_UART2_HOTTSUMD: case HWFLYINGF4_UART2_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_uart2) { case HWFLYINGF4_UART2_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFLYINGF4_UART2_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart2_dsm_hsum_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFLYINGF4_UART2_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart2_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFLYINGF4_UART2_COMBRIDGE: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart2_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); #endif break; case HWFLYINGF4_UART2_MAVLINKTX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart2_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWFLYINGF4_UART2_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) && defined(PIOS_INCLUDE_GPS) PIOS_Board_configure_com(&pios_usart2_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 */ break; case HWFLYINGF4_UART2_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart2_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 HWFLYINGF4_UART2_LIGHTTELEMETRYTX: #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) PIOS_Board_configure_com(&pios_usart2_cfg, 0, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_lighttelemetry_id); #endif break; case HWFLYINGF4_UART2_PICOC: #if defined(PIOS_INCLUDE_PICOC) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart2_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 HWFLYINGF4_UART2_FRSKYSPORTTELEMETRY: #if defined(PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY) PIOS_Board_configure_com(&pios_usart2_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; } /* UART3 Port */ uint8_t hw_uart3; HwFlyingF4Uart3Get(&hw_uart3); switch (hw_uart3) { case HWFLYINGF4_UART3_DISABLED: break; case HWFLYINGF4_UART3_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWFLYINGF4_UART3_GPS: #if defined(PIOS_INCLUDE_GPS) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); #endif break; case HWFLYINGF4_UART3_DSM: #if defined(PIOS_INCLUDE_DSM) { PIOS_Board_configure_dsm(&pios_usart3_dsm_hsum_cfg, &pios_usart3_dsm_aux_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ case HWFLYINGF4_UART3_HOTTSUMD: case HWFLYINGF4_UART3_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_uart3) { case HWFLYINGF4_UART3_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFLYINGF4_UART3_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart3_dsm_hsum_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFLYINGF4_UART3_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFLYINGF4_UART3_COMBRIDGE: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); #endif break; case HWFLYINGF4_UART3_MAVLINKTX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart3_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWFLYINGF4_UART3_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) && defined(PIOS_INCLUDE_GPS) PIOS_Board_configure_com(&pios_usart3_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 */ break; case HWFLYINGF4_UART3_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_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 HWFLYINGF4_UART3_LIGHTTELEMETRYTX: #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) PIOS_Board_configure_com(&pios_usart3_cfg, 0, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_lighttelemetry_id); #endif break; case HWFLYINGF4_UART3_PICOC: #if defined(PIOS_INCLUDE_PICOC) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_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 HWFLYINGF4_UART3_FRSKYSPORTTELEMETRY: #if defined(PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY) PIOS_Board_configure_com(&pios_usart3_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 */ } /* Configure the rcvr port */ uint8_t hw_rcvrport; HwFlyingF4RcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWFLYINGF4_RCVRPORT_DISABLED: break; case HWFLYINGF4_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) { uintptr_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); uintptr_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } #endif /* PIOS_INCLUDE_PWM */ break; case HWFLYINGF4_RCVRPORT_PPM: case HWFLYINGF4_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_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; } #endif /* PIOS_INCLUDE_PPM */ break; case HWFLYINGF4_RCVRPORT_PPMPWM: /* This is a combination of PPM and PWM inputs */ #if defined(PIOS_INCLUDE_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; } #endif /* PIOS_INCLUDE_PPM */ #if defined(PIOS_INCLUDE_PWM) { uintptr_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); uintptr_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } #endif /* PIOS_INCLUDE_PWM */ break; } #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 switch (hw_rcvrport) { case HWFLYINGF4_RCVRPORT_DISABLED: case HWFLYINGF4_RCVRPORT_PWM: case HWFLYINGF4_RCVRPORT_PPM: /* Set up the servo outputs */ #ifdef PIOS_INCLUDE_SERVO PIOS_Servo_Init(&pios_servo_cfg); #endif break; case HWFLYINGF4_RCVRPORT_PPMOUTPUTS: case HWFLYINGF4_RCVRPORT_OUTPUTS: #ifdef PIOS_INCLUDE_SERVO PIOS_Servo_Init(&pios_servo_rcvr_cfg); #endif break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif PIOS_WDG_Clear(); PIOS_DELAY_WaitmS(200); PIOS_WDG_Clear(); #if defined(PIOS_INCLUDE_I2C) if (PIOS_I2C_Init(&pios_i2c_10dof_adapter_id, &pios_i2c_10dof_adapter_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_10dof_adapter_id) != 0) panic(5); #if defined(PIOS_INCLUDE_MPU6050) if (PIOS_MPU6050_Init(pios_i2c_10dof_adapter_id, PIOS_MPU6050_I2C_ADD_A0_LOW, &pios_mpu6050_cfg) != 0) panic(2); if (PIOS_MPU6050_Test() != 0) panic(2); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwFlyingF4GyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWFLYINGF4_GYRORANGE_250: PIOS_MPU6050_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWFLYINGF4_GYRORANGE_500: PIOS_MPU6050_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWFLYINGF4_GYRORANGE_1000: PIOS_MPU6050_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWFLYINGF4_GYRORANGE_2000: PIOS_MPU6050_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwFlyingF4AccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWFLYINGF4_ACCELRANGE_2G: PIOS_MPU6050_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWFLYINGF4_ACCELRANGE_4G: PIOS_MPU6050_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWFLYINGF4_ACCELRANGE_8G: PIOS_MPU6050_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWFLYINGF4_ACCELRANGE_16G: PIOS_MPU6050_SetAccelRange(PIOS_MPU60X0_ACCEL_16G); break; } // the filter has to be set before rate else divisor calculation will fail uint8_t hw_mpu6050_dlpf; HwFlyingF4MPU6050DLPFGet(&hw_mpu6050_dlpf); enum pios_mpu60x0_filter mpu6050_dlpf = \ (hw_mpu6050_dlpf == HWFLYINGF4_MPU6050DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_mpu6050_dlpf == HWFLYINGF4_MPU6050DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_mpu6050_dlpf == HWFLYINGF4_MPU6050DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_mpu6050_dlpf == HWFLYINGF4_MPU6050DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_mpu6050_dlpf == HWFLYINGF4_MPU6050DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_mpu6050_dlpf == HWFLYINGF4_MPU6050DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_mpu6050_dlpf == HWFLYINGF4_MPU6050DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ pios_mpu6050_cfg.default_filter; PIOS_MPU6050_SetLPF(mpu6050_dlpf); uint8_t hw_mpu6050_samplerate; HwFlyingF4MPU6050RateGet(&hw_mpu6050_samplerate); uint16_t mpu6050_samplerate = \ (hw_mpu6050_samplerate == HWFLYINGF4_MPU6050RATE_200) ? 200 : \ (hw_mpu6050_samplerate == HWFLYINGF4_MPU6050RATE_333) ? 333 : \ (hw_mpu6050_samplerate == HWFLYINGF4_MPU6050RATE_500) ? 500 : \ (hw_mpu6050_samplerate == HWFLYINGF4_MPU6050RATE_666) ? 666 : \ (hw_mpu6050_samplerate == HWFLYINGF4_MPU6050RATE_1000) ? 1000 : \ (hw_mpu6050_samplerate == HWFLYINGF4_MPU6050RATE_2000) ? 2000 : \ (hw_mpu6050_samplerate == HWFLYINGF4_MPU6050RATE_4000) ? 4000 : \ (hw_mpu6050_samplerate == HWFLYINGF4_MPU6050RATE_8000) ? 8000 : \ pios_mpu6050_cfg.default_samplerate; PIOS_MPU6050_SetSampleRate(mpu6050_samplerate); PIOS_MPU6050_SetPassThrough(true); #endif /* PIOS_INCLUDE_MPU6050 */ PIOS_WDG_Clear(); PIOS_DELAY_WaitmS(50); PIOS_WDG_Clear(); #if defined(PIOS_INCLUDE_HMC5883) { uint8_t Magnetometer; HwFlyingF4MagnetometerGet(&Magnetometer); if (Magnetometer == HWFLYINGF4_MAGNETOMETER_EXTERNALI2C) { if (PIOS_HMC5883_Init(pios_i2c_10dof_adapter_id, &pios_hmc5883_external_cfg) != 0) panic(3); if (PIOS_HMC5883_Test() != 0) panic(3); // setup sensor orientation uint8_t ExtMagOrientation; HwFlyingF4ExtMagOrientationGet(&ExtMagOrientation); enum pios_hmc5883_orientation hmc5883_orientation = \ (ExtMagOrientation == HWFLYINGF4_EXTMAGORIENTATION_TOP0DEGCW) ? PIOS_HMC5883_TOP_0DEG : \ (ExtMagOrientation == HWFLYINGF4_EXTMAGORIENTATION_TOP90DEGCW) ? PIOS_HMC5883_TOP_90DEG : \ (ExtMagOrientation == HWFLYINGF4_EXTMAGORIENTATION_TOP180DEGCW) ? PIOS_HMC5883_TOP_180DEG : \ (ExtMagOrientation == HWFLYINGF4_EXTMAGORIENTATION_TOP270DEGCW) ? PIOS_HMC5883_TOP_270DEG : \ (ExtMagOrientation == HWFLYINGF4_EXTMAGORIENTATION_BOTTOM0DEGCW) ? PIOS_HMC5883_BOTTOM_0DEG : \ (ExtMagOrientation == HWFLYINGF4_EXTMAGORIENTATION_BOTTOM90DEGCW) ? PIOS_HMC5883_BOTTOM_90DEG : \ (ExtMagOrientation == HWFLYINGF4_EXTMAGORIENTATION_BOTTOM180DEGCW) ? PIOS_HMC5883_BOTTOM_180DEG : \ (ExtMagOrientation == HWFLYINGF4_EXTMAGORIENTATION_BOTTOM270DEGCW) ? PIOS_HMC5883_BOTTOM_270DEG : \ pios_hmc5883_external_cfg.Default_Orientation; PIOS_HMC5883_SetOrientation(hmc5883_orientation); } } #endif //I2C is slow, sensor init as well, reset watchdog to prevent reset here PIOS_WDG_Clear(); #if defined(PIOS_INCLUDE_MS5611) if (PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_10dof_adapter_id) != 0) panic(4); if (PIOS_MS5611_Test() != 0) panic(4); #endif //I2C is slow, sensor init as well, reset watchdog to prevent reset here PIOS_WDG_Clear(); #endif /* PIOS_INCLUDE_I2C */ #if defined(PIOS_INCLUDE_GPIO) PIOS_GPIO_Init(); #endif #if defined(PIOS_INCLUDE_ADC) PIOS_ADC_Init(&pios_adc_cfg); #endif /* Make sure we have at least one telemetry link configured or else fail initialization */ PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); }
/** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ void PIOS_Board_Init(void) { /* Delay system */ PIOS_DELAY_Init(); /* Set up the SPI interface to the serial flash */ if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg)) { PIOS_DEBUG_Assert(0); } PIOS_Flash_W25X_Init(pios_spi_flash_accel_id); PIOS_ADXL345_Attach(pios_spi_flash_accel_id); #if defined(PIOS_INCLUDE_SPEKTRUM) /* SPEKTRUM init must come before comms */ PIOS_SPEKTRUM_Init(); if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_COM_Init(&pios_com_spektrum_id, &pios_usart_com_driver, pios_usart_spektrum_id)) { PIOS_DEBUG_Assert(0); } #endif /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); UAVObjectsInitializeAll(); /* Initialize the alarms library */ AlarmsInitialize(); /* Initialize the task monitor library */ TaskMonitorInitialize(); /* 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); } #if defined(PIOS_INCLUDE_GPS) if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) { PIOS_DEBUG_Assert(0); } #endif /* PIOS_INCLUDE_GPS */ #endif /* PIOS_INCLUDE_COM */ /* Remap AFIO pin */ GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); PIOS_Servo_Init(); PIOS_ADC_Init(); PIOS_GPIO_Init(); #if defined(PIOS_INCLUDE_PWM) PIOS_PWM_Init(); #endif #if defined(PIOS_INCLUDE_PPM) PIOS_PPM_Init(); #endif #if defined(PIOS_INCLUDE_USB_HID) 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 #if defined(PIOS_INCLUDE_I2C) if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) { PIOS_DEBUG_Assert(0); } #endif /* PIOS_INCLUDE_I2C */ PIOS_IAP_Init(); PIOS_WDG_Init(); }
/** * Initialises all the pins */ int32_t PIOS_PWM_Init(uint32_t *pwm_id, const struct pios_pwm_cfg *cfg) { PIOS_DEBUG_Assert(pwm_id); PIOS_DEBUG_Assert(cfg); struct pios_pwm_dev *pwm_dev; pwm_dev = (struct pios_pwm_dev *)PIOS_PWM_alloc(); if (!pwm_dev) { goto out_fail; } /* Bind the configuration to the device instance */ pwm_dev->cfg = cfg; for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { /* Flush counter variables */ pwm_dev->CaptureState[i] = 0; pwm_dev->RiseValue[i] = 0; pwm_dev->FallValue[i] = 0; pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; } uint32_t tim_id; if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) { return -1; } /* Configure the channels to be in capture/compare mode */ for (uint8_t i = 0; i < cfg->num_channels; i++) { const struct pios_tim_channel *chan = &cfg->channels[i]; /* Configure timer for input capture */ TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; TIM_ICInitStructure.TIM_Channel = chan->timer_chan; TIM_ICInit(chan->timer, &TIM_ICInitStructure); /* Enable the Capture Compare Interrupt Request */ switch (chan->timer_chan) { case TIM_Channel_1: TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); break; case TIM_Channel_2: TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); break; case TIM_Channel_3: TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); break; case TIM_Channel_4: TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); break; } // Need the update event for that timer to detect timeouts TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); } *pwm_id = (uint32_t)pwm_dev; return 0; out_fail: return -1; }
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 */ #if defined(PIOS_INCLUDE_SPI) /* Set up the SPI interface to the serial flash */ switch(bdinfo->board_rev) { case BOARD_REVISION_CC: if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) { PIOS_Assert(0); } break; case BOARD_REVISION_CC3D: if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) { PIOS_Assert(0); } break; default: PIOS_Assert(0); } #endif switch(bdinfo->board_rev) { case BOARD_REVISION_CC: PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_flash_accel_id, 1, &flash_w25x_cfg); break; case BOARD_REVISION_CC3D: PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_flash_accel_id, 0, &flash_m25p_cfg); break; default: PIOS_DEBUG_Assert(0); } PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg); /* 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 */ PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_settings_cfg, FLASH_PARTITION_LABEL_SETTINGS); /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); /* Initialize the alarms library. Reads RCC reset flags */ AlarmsInitialize(); PIOS_RESET_Clear(); // Clear the RCC reset flags after use. /* Initialize the hardware UAVOs */ HwCopterControlInitialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ PIOS_RTC_Init(&pios_rtc_main_cfg); #endif #ifndef ERASE_FLASH /* Initialize watchdog as early as possible to catch faults during init */ PIOS_WDG_Init(); #endif /* Check for repeated boot failures */ 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 configuration to defaults */ HwCopterControlSetDefaults(HwCopterControlHandle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } /* 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); PIOS_TIM_InitClock(&tim_4_cfg); #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; #endif uintptr_t pios_usb_id; switch(bdinfo->board_rev) { case BOARD_REVISION_CC: PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); break; case BOARD_REVISION_CC3D: PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); break; default: PIOS_Assert(0); } #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwCopterControlUSB_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 = HWCOPTERCONTROL_USB_VCPPORT_DISABLED; } switch (hw_usb_vcpport) { case HWCOPTERCONTROL_USB_VCPPORT_DISABLED: break; case HWCOPTERCONTROL_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { 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); } 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 HWCOPTERCONTROL_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { 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); } 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 HWCOPTERCONTROL_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { 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); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwCopterControlUSB_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 = HWCOPTERCONTROL_USB_HIDPORT_DISABLED; } switch (hw_usb_hidport) { case HWCOPTERCONTROL_USB_HIDPORT_DISABLED: break; case HWCOPTERCONTROL_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { 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); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWCOPTERCONTROL_USB_HIDPORT_RCTRANSMITTER: #if defined(PIOS_INCLUDE_USB_RCTX) { if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_USB_RCTX */ break; } #endif /* PIOS_INCLUDE_USB_HID */ #endif /* PIOS_INCLUDE_USB */ /* Configure the main IO port */ HwCopterControlDSMxModeOptions hw_DSMxMode; HwCopterControlDSMxModeGet(&hw_DSMxMode); uint8_t hw_mainport; HwCopterControlMainPortGet(&hw_mainport); switch (hw_mainport) { case HWCOPTERCONTROL_MAINPORT_DISABLED: break; case HWCOPTERCONTROL_MAINPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWCOPTERCONTROL_MAINPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_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 /* PIOS_INCLUDE_SBUS */ break; case HWCOPTERCONTROL_MAINPORT_GPS: #if defined(PIOS_INCLUDE_GPS) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_GPS_RX_BUF_LEN); PIOS_Assert(rx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, NULL, 0)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_GPS */ break; case HWCOPTERCONTROL_MAINPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { uintptr_t pios_usart_dsm_id; if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_hsum_main_cfg)) { PIOS_Assert(0); } if (hw_DSMxMode >= HWCOPTERCONTROL_DSMXMODE_BIND3PULSES) { hw_DSMxMode = HWCOPTERCONTROL_DSMXMODE_AUTODETECT; /* Do not try to bind through XOR */ } uintptr_t pios_dsm_id; if (PIOS_DSM_Init(&pios_dsm_id, &pios_dsm_main_cfg, &pios_usart_com_driver, pios_usart_dsm_id, hw_DSMxMode)) { PIOS_Assert(0); } uintptr_t pios_dsm_rcvr_id; if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSM] = pios_dsm_rcvr_id; } #endif /* PIOS_INCLUDE_DSM */ break; case HWCOPTERCONTROL_MAINPORT_HOTTSUMD: case HWCOPTERCONTROL_MAINPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; proto = (hw_mainport == HWCOPTERCONTROL_MAINPORT_HOTTSUMD) ? PIOS_HSUM_PROTO_SUMD : PIOS_HSUM_PROTO_SUMH; uintptr_t pios_usart_hsum_id; if (PIOS_USART_Init(&pios_usart_hsum_id, &pios_usart_dsm_hsum_main_cfg)) { PIOS_Assert(0); } uintptr_t pios_hsum_id; if (PIOS_HSUM_Init(&pios_hsum_id, &pios_usart_com_driver, pios_usart_hsum_id, proto)) { PIOS_Assert(0); } uintptr_t pios_hsum_rcvr_id; if (PIOS_RCVR_Init(&pios_hsum_rcvr_id, &pios_hsum_rcvr_driver, pios_hsum_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM] = pios_hsum_rcvr_id; } #endif /* PIOS_INCLUDE_HSUM */ break; case HWCOPTERCONTROL_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_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 HWCOPTERCONTROL_MAINPORT_COMBRIDGE: { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); PIOS_Assert(rx_buffer); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } break; case HWCOPTERCONTROL_MAINPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_MAVLINK) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_MAVLINK_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_MAVLINK_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWCOPTERCONTROL_MAINPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_GPS) #if defined(PIOS_INCLUDE_MAVLINK) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_GPS_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_MAVLINK_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, tx_buffer, PIOS_COM_MAVLINK_TX_BUF_LEN)) { PIOS_Assert(0); } pios_com_mavlink_id = pios_com_gps_id; } #endif /* PIOS_INCLUDE_MAVLINK */ #endif /* PIOS_INCLUDE_GPS */ break; case HWCOPTERCONTROL_MAINPORT_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_frsky_sensor_hub_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_FRSKYSENSORHUB */ break; case HWCOPTERCONTROL_MAINPORT_LIGHTTELEMETRYTX: { #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_lighttelemetry_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN)) { PIOS_Assert(0); } #endif } break; } /* Configure the flexi port */ uint8_t hw_flexiport; HwCopterControlFlexiPortGet(&hw_flexiport); switch (hw_flexiport) { case HWCOPTERCONTROL_FLEXIPORT_DISABLED: break; case HWCOPTERCONTROL_FLEXIPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWCOPTERCONTROL_FLEXIPORT_COMBRIDGE: { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } break; case HWCOPTERCONTROL_FLEXIPORT_GPS: #if defined(PIOS_INCLUDE_GPS) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_GPS_RX_BUF_LEN); PIOS_Assert(rx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, NULL, 0)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_GPS */ break; case HWCOPTERCONTROL_FLEXIPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { uintptr_t pios_usart_dsm_id; if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_hsum_flexi_cfg)) { PIOS_Assert(0); } uintptr_t pios_dsm_id; if (PIOS_DSM_Init(&pios_dsm_id, &pios_dsm_flexi_cfg, &pios_usart_com_driver, pios_usart_dsm_id, hw_DSMxMode)) { PIOS_Assert(0); } uintptr_t pios_dsm_rcvr_id; if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSM] = pios_dsm_rcvr_id; } #endif /* PIOS_INCLUDE_DSM */ break; case HWCOPTERCONTROL_FLEXIPORT_HOTTSUMD: case HWCOPTERCONTROL_FLEXIPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; proto = (hw_flexiport == HWCOPTERCONTROL_FLEXIPORT_HOTTSUMD) ? PIOS_HSUM_PROTO_SUMD : PIOS_HSUM_PROTO_SUMH; uintptr_t pios_usart_hsum_id; if (PIOS_USART_Init(&pios_usart_hsum_id, &pios_usart_dsm_hsum_flexi_cfg)) { PIOS_Assert(0); } uintptr_t pios_hsum_id; if (PIOS_HSUM_Init(&pios_hsum_id, &pios_usart_com_driver, pios_usart_hsum_id, proto)) { PIOS_Assert(0); } uintptr_t pios_hsum_rcvr_id; if (PIOS_RCVR_Init(&pios_hsum_rcvr_id, &pios_hsum_rcvr_driver, pios_hsum_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM] = pios_hsum_rcvr_id; } #endif /* PIOS_INCLUDE_HSUM */ break; case HWCOPTERCONTROL_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usart_com_driver, pios_usart_generic_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 HWCOPTERCONTROL_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_I2C */ #if defined(PIOS_INCLUDE_PCF8591) { uintptr_t pcf8591_adc_id; if(PIOS_PCF8591_ADC_Init(&pcf8591_adc_id, &pios_8591_cfg) < 0) PIOS_Assert(0); PIOS_ADC_Init(&pios_pcf8591_adc_id, &pios_pcf8591_adc_driver, pcf8591_adc_id); } #endif break; case HWCOPTERCONTROL_FLEXIPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_MAVLINK) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_MAVLINK_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_MAVLINK_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWCOPTERCONTROL_FLEXIPORT_LIGHTTELEMETRYTX: { #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_lighttelemetry_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN)) { PIOS_Assert(0); } #endif case HWCOPTERCONTROL_FLEXIPORT_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_frsky_sensor_hub_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_FRSKYSENSORHUB */ break; } break; } /* Configure the rcvr port */ uint8_t hw_rcvrport; HwCopterControlRcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWCOPTERCONTROL_RCVRPORT_DISABLED: break; case HWCOPTERCONTROL_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) { uintptr_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); uintptr_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } #endif /* PIOS_INCLUDE_PWM */ break; case HWCOPTERCONTROL_RCVRPORT_PPM: case HWCOPTERCONTROL_RCVRPORT_PPMONPIN8: case HWCOPTERCONTROL_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_PPM) { uintptr_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, (hw_rcvrport == HWCOPTERCONTROL_RCVRPORT_PPMONPIN8) ? &pios_ppm_pin8_cfg : &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; } #endif /* PIOS_INCLUDE_PPM */ break; case HWCOPTERCONTROL_RCVRPORT_PPMPWM: /* This is a combination of PPM and PWM inputs */ #if defined(PIOS_INCLUDE_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; } #endif /* PIOS_INCLUDE_PPM */ #if defined(PIOS_INCLUDE_PWM) { uintptr_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); uintptr_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } #endif /* PIOS_INCLUDE_PWM */ break; } #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 */ /* Remap AFIO pin for PB4 (Servo 5 Out)*/ GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS switch (hw_rcvrport) { case HWCOPTERCONTROL_RCVRPORT_DISABLED: case HWCOPTERCONTROL_RCVRPORT_PWM: case HWCOPTERCONTROL_RCVRPORT_PPM: case HWCOPTERCONTROL_RCVRPORT_PPMONPIN8: case HWCOPTERCONTROL_RCVRPORT_PPMPWM: PIOS_Servo_Init(&pios_servo_cfg); break; case HWCOPTERCONTROL_RCVRPORT_PPMOUTPUTS: case HWCOPTERCONTROL_RCVRPORT_OUTPUTS: PIOS_Servo_Init(&pios_servo_rcvr_cfg); break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */ PIOS_SENSORS_Init(); switch(bdinfo->board_rev) { case BOARD_REVISION_CC: // Revision 1 with invensense gyros, start the ADC #if defined(PIOS_INCLUDE_ADC) { uint32_t internal_adc_id; PIOS_INTERNAL_ADC_Init(&internal_adc_id, &internal_adc_cfg); PIOS_ADC_Init(&pios_internal_adc_id, &pios_internal_adc_driver, internal_adc_id); } #endif #if defined(PIOS_INCLUDE_ADXL345) PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0); #endif break; case BOARD_REVISION_CC3D: // Revision 2 with L3GD20 gyros, start a SPI interface and connect to it GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); #if defined(PIOS_INCLUDE_MPU6000) // Set up the SPI interface to the serial flash if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_Assert(0); } PIOS_MPU6000_Init(pios_spi_gyro_id,0,&pios_mpu6000_cfg); init_test = PIOS_MPU6000_Test(); uint8_t hw_gyro_range; HwCopterControlGyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWCOPTERCONTROL_GYRORANGE_250: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWCOPTERCONTROL_GYRORANGE_500: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWCOPTERCONTROL_GYRORANGE_1000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWCOPTERCONTROL_GYRORANGE_2000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwCopterControlAccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWCOPTERCONTROL_ACCELRANGE_2G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWCOPTERCONTROL_ACCELRANGE_4G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWCOPTERCONTROL_ACCELRANGE_8G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWCOPTERCONTROL_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; HwCopterControlMPU6000DLPFGet(&hw_mpu6000_dlpf); enum pios_mpu60x0_filter mpu6000_dlpf = \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ pios_mpu6000_cfg.default_filter; PIOS_MPU6000_SetLPF(mpu6000_dlpf); uint8_t hw_mpu6000_samplerate; HwCopterControlMPU6000RateGet(&hw_mpu6000_samplerate); uint16_t mpu6000_samplerate = \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_200) ? 200 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_333) ? 333 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_500) ? 500 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_666) ? 666 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_1000) ? 1000 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_2000) ? 2000 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_4000) ? 4000 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_8000) ? 8000 : \ pios_mpu6000_cfg.default_samplerate; PIOS_MPU6000_SetSampleRate(mpu6000_samplerate); #endif /* PIOS_INCLUDE_MPU6000 */ break; default: PIOS_Assert(0); } PIOS_GPIO_Init(); /* Make sure we have at least one telemetry link configured or else fail initialization */ PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); }
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 */ #if defined(PIOS_INCLUDE_SPI) /* Set up the SPI interface to the serial flash */ switch(bdinfo->board_rev) { case BOARD_REVISION_CC: if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) { PIOS_Assert(0); } break; case BOARD_REVISION_CC3D: if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) { PIOS_Assert(0); } break; default: PIOS_Assert(0); } #endif switch(bdinfo->board_rev) { case BOARD_REVISION_CC: PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 1, &flash_w25x_cfg); PIOS_FLASHFS_Init(&flashfs_w25x_cfg); break; case BOARD_REVISION_CC3D: PIOS_Flash_Jedec_Init(pios_spi_flash_accel_id, 0, &flash_m25p_cfg); PIOS_FLASHFS_Init(&flashfs_m25p_cfg); break; default: PIOS_DEBUG_Assert(0); } /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ PIOS_RTC_Init(&pios_rtc_main_cfg); #endif HwSettingsInitialize(); #ifndef ERASE_FLASH /* Initialize watchdog as early as possible to catch faults during init */ PIOS_WDG_Init(); #endif /* Initialize the alarms library */ AlarmsInitialize(); /* Check for repeated boot failures */ PIOS_IAP_Init(); uint16_t boot_count = PIOS_IAP_ReadBootCount(); if (boot_count < 3) { PIOS_IAP_WriteBootCount(++boot_count); AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); } else { /* Too many failed boot attempts, force hwsettings to defaults */ HwSettingsSetDefaults(HwSettingsHandle(), 0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_3_cfg); PIOS_TIM_InitClock(&tim_4_cfg); #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; #endif uint32_t pios_usb_id; switch(bdinfo->board_rev) { case BOARD_REVISION_CC: PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); break; case BOARD_REVISION_CC3D: PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); break; default: PIOS_Assert(0); } #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hwsettings_usb_vcpport; /* Configure the USB VCP port */ HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; } switch (hwsettings_usb_vcpport) { case HWSETTINGS_USB_VCPPORT_DISABLED: break; case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint32_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWSETTINGS_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { uint32_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hwsettings_usb_hidport; HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; } switch (hwsettings_usb_hidport) { case HWSETTINGS_USB_HIDPORT_DISABLED: break; case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint32_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWSETTINGS_USB_HIDPORT_RCTRANSMITTER: #if defined(PIOS_INCLUDE_USB_RCTX) { if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_USB_RCTX */ break; } #endif /* PIOS_INCLUDE_USB_HID */ #endif /* PIOS_INCLUDE_USB */ /* Configure the main IO port */ uint8_t hwsettings_DSMxBind; HwSettingsDSMxBindGet(&hwsettings_DSMxBind); uint8_t hwsettings_cc_mainport; HwSettingsCC_MainPortGet(&hwsettings_cc_mainport); switch (hwsettings_cc_mainport) { case HWSETTINGS_CC_MAINPORT_DISABLED: break; case HWSETTINGS_CC_MAINPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) { uint32_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWSETTINGS_CC_MAINPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) { uint32_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { PIOS_Assert(0); } uint32_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } uint32_t pios_sbus_rcvr_id; if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; } #endif /* PIOS_INCLUDE_SBUS */ break; case HWSETTINGS_CC_MAINPORT_GPS: #if defined(PIOS_INCLUDE_GPS) { uint32_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); PIOS_Assert(rx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, NULL, 0)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_GPS */ break; case HWSETTINGS_CC_MAINPORT_DSM2: case HWSETTINGS_CC_MAINPORT_DSMX10BIT: case HWSETTINGS_CC_MAINPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) { enum pios_dsm_proto proto; switch (hwsettings_cc_mainport) { case HWSETTINGS_CC_MAINPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWSETTINGS_CC_MAINPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWSETTINGS_CC_MAINPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } uint32_t pios_usart_dsm_id; if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_main_cfg)) { PIOS_Assert(0); } uint32_t pios_dsm_id; if (PIOS_DSM_Init(&pios_dsm_id, &pios_dsm_main_cfg, &pios_usart_com_driver, pios_usart_dsm_id, proto, 0)) { PIOS_Assert(0); } uint32_t pios_dsm_rcvr_id; if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT] = pios_dsm_rcvr_id; } #endif /* PIOS_INCLUDE_DSM */ break; case HWSETTINGS_CC_MAINPORT_COMAUX: break; case HWSETTINGS_CC_MAINPORT_COMBRIDGE: { uint32_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); PIOS_Assert(rx_buffer); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } break; } /* Configure the flexi port */ uint8_t hwsettings_cc_flexiport; HwSettingsCC_FlexiPortGet(&hwsettings_cc_flexiport); switch (hwsettings_cc_flexiport) { case HWSETTINGS_CC_FLEXIPORT_DISABLED: break; case HWSETTINGS_CC_FLEXIPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) { uint32_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWSETTINGS_CC_FLEXIPORT_COMBRIDGE: { uint32_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } break; case HWSETTINGS_CC_FLEXIPORT_GPS: #if defined(PIOS_INCLUDE_GPS) { uint32_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); PIOS_Assert(rx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, NULL, 0)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_GPS */ break; case HWSETTINGS_CC_FLEXIPORT_DSM2: case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT: case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) { enum pios_dsm_proto proto; switch (hwsettings_cc_flexiport) { case HWSETTINGS_CC_FLEXIPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWSETTINGS_CC_FLEXIPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWSETTINGS_CC_FLEXIPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } uint32_t pios_usart_dsm_id; if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_flexi_cfg)) { PIOS_Assert(0); } uint32_t pios_dsm_id; if (PIOS_DSM_Init(&pios_dsm_id, &pios_dsm_flexi_cfg, &pios_usart_com_driver, pios_usart_dsm_id, proto, hwsettings_DSMxBind)) { PIOS_Assert(0); } uint32_t pios_dsm_rcvr_id; if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT] = pios_dsm_rcvr_id; } #endif /* PIOS_INCLUDE_DSM */ break; case HWSETTINGS_CC_FLEXIPORT_COMAUX: break; case HWSETTINGS_CC_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_I2C */ break; } /* Configure the rcvr port */ uint8_t hwsettings_rcvrport; HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport); switch (hwsettings_rcvrport) { case HWSETTINGS_CC_RCVRPORT_DISABLED: break; case HWSETTINGS_CC_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) { uint32_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); uint32_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_CC_RCVRPORT_PPM: case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_PPM) { uint32_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); uint32_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } #endif /* PIOS_INCLUDE_PPM */ break; case HWSETTINGS_CC_RCVRPORT_PPMPWM: /* This is a combination of PPM and PWM inputs */ #if defined(PIOS_INCLUDE_PPM) { uint32_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); uint32_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } #endif /* PIOS_INCLUDE_PPM */ #if defined(PIOS_INCLUDE_PWM) { uint32_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); uint32_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } #endif /* PIOS_INCLUDE_PWM */ break; } #if defined(PIOS_INCLUDE_GCSRCVR) GCSReceiverInitialize(); uint32_t pios_gcsrcvr_id; PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); uint32_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ /* Remap AFIO pin for PB4 (Servo 5 Out)*/ GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS switch (hwsettings_rcvrport) { case HWSETTINGS_CC_RCVRPORT_DISABLED: case HWSETTINGS_CC_RCVRPORT_PWM: case HWSETTINGS_CC_RCVRPORT_PPM: case HWSETTINGS_CC_RCVRPORT_PPMPWM: PIOS_Servo_Init(&pios_servo_cfg); break; case HWSETTINGS_CC_RCVRPORT_PPMOUTPUTS: case HWSETTINGS_CC_RCVRPORT_OUTPUTS: PIOS_Servo_Init(&pios_servo_rcvr_cfg); break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */ switch(bdinfo->board_rev) { case BOARD_REVISION_CC: // Revision 1 with invensense gyros, start the ADC #if defined(PIOS_INCLUDE_ADC) PIOS_ADC_Init(&pios_adc_cfg); #endif #if defined(PIOS_INCLUDE_ADXL345) PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0); #endif break; case BOARD_REVISION_CC3D: // Revision 2 with L3GD20 gyros, start a SPI interface and connect to it GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); #if defined(PIOS_INCLUDE_MPU6000) // Set up the SPI interface to the serial flash if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_Assert(0); } PIOS_MPU6000_Init(pios_spi_gyro_id,0,&pios_mpu6000_cfg); init_test = PIOS_MPU6000_Test(); #endif /* PIOS_INCLUDE_MPU6000 */ break; default: PIOS_Assert(0); } PIOS_GPIO_Init(); /* Make sure we have at least one telemetry link configured or else fail initialization */ PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); }
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 */ #if defined(PIOS_INCLUDE_SPI) if (PIOS_SPI_Init(&pios_spi_internal_id, &pios_spi_internal_cfg)) { PIOS_DEBUG_Assert(0); } #endif #if defined(PIOS_INCLUDE_I2C) if (PIOS_I2C_Init(&pios_i2c_internal_id, &pios_i2c_internal_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_internal_id) != 0) panic(3); if (PIOS_I2C_Init(&pios_i2c_external_id, &pios_i2c_external_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_external_id) != 0) panic(4); #endif #if defined(PIOS_INCLUDE_CAN) if (PIOS_CAN_Init(&pios_can_id, &pios_can_cfg) != 0) panic(6); uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_CAN_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_CAN_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_can_id, &pios_can_com_driver, pios_can_id, rx_buffer, PIOS_COM_CAN_RX_BUF_LEN, tx_buffer, PIOS_COM_CAN_TX_BUF_LEN)) panic(6); pios_com_bridge_id = pios_com_can_id; #endif #if defined(PIOS_INCLUDE_FLASH) /* Inititialize all flash drivers */ PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg); /* 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 */ PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_settings_cfg, FLASH_PARTITION_LABEL_SETTINGS); PIOS_FLASHFS_Logfs_Init(&pios_waypoints_settings_fs_id, &flashfs_internal_waypoints_cfg, FLASH_PARTITION_LABEL_WAYPOINTS); #if defined(ERASE_FLASH) PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); #endif #endif /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); /* Initialize the alarms library */ AlarmsInitialize(); HwFlyingF3Initialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ 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 */ //inputs PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_8_cfg); PIOS_TIM_InitClock(&tim_15_cfg); PIOS_TIM_InitClock(&tim_16_cfg); PIOS_TIM_InitClock(&tim_17_cfg); //outputs PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_3_cfg); PIOS_TIM_InitClock(&tim_4_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 */ HwFlyingF3SetDefaults(HwFlyingF3Handle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } #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; #if defined(PIOS_INCLUDE_USB_CDC) bool usb_cdc_present = false; 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 */ HwFlyingF3USB_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 = HWFLYINGF3_USB_VCPPORT_DISABLED; } switch (hw_usb_vcpport) { case HWFLYINGF3_USB_VCPPORT_DISABLED: break; case HWFLYINGF3_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { 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); } 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 HWFLYINGF3_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { 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); } 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 HWFLYINGF3_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { 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); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwFlyingF3USB_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 = HWFLYINGF3_USB_HIDPORT_DISABLED; } switch (hw_usb_hidport) { case HWFLYINGF3_USB_HIDPORT_DISABLED: break; case HWFLYINGF3_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { 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); } 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 */ #endif /* PIOS_INCLUDE_USB */ /* Configure the IO ports */ HwFlyingF3DSMxModeOptions hw_DSMxMode; HwFlyingF3DSMxModeGet(&hw_DSMxMode); /* UART1 Port */ uint8_t hw_uart1; HwFlyingF3Uart1Get(&hw_uart1); switch (hw_uart1) { case HWFLYINGF3_UART1_DISABLED: break; case HWFLYINGF3_UART1_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart1_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWFLYINGF3_UART1_GPS: #if defined(PIOS_INCLUDE_GPS) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart1_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); #endif break; case HWFLYINGF3_UART1_SBUS: #if defined(PIOS_INCLUDE_SBUS) && defined(PIOS_INCLUDE_USART) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart1_sbus_cfg)) { PIOS_Assert(0); } uintptr_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_usart1_sbus_aux_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 /* PIOS_INCLUDE_SBUS */ break; case HWFLYINGF3_UART1_DSM: #if defined(PIOS_INCLUDE_DSM) { PIOS_Board_configure_dsm(&pios_usart1_dsm_hsum_cfg, &pios_usart1_dsm_aux_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSM, &hw_DSMxMode); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFLYINGF3_UART1_HOTTSUMD: case HWFLYINGF3_UART1_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_uart1) { case HWFLYINGF3_UART1_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFLYINGF3_UART1_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart1_dsm_hsum_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFLYINGF3_UART1_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart1_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFLYINGF3_UART1_COMBRIDGE: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart1_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); #endif break; case HWFLYINGF3_UART1_MAVLINKTX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart1_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWFLYINGF3_UART1_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) && defined(PIOS_INCLUDE_GPS) PIOS_Board_configure_com(&pios_usart1_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 */ break; case HWFLYINGF3_UART1_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart1_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 HWFLYINGF3_UART1_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart1_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 HWFLYINGF3_UART1_LIGHTTELEMETRYTX: #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) PIOS_Board_configure_com(&pios_usart1_cfg, 0, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_lighttelemetry_id); #endif break; case HWFLYINGF3_UART1_FRSKYSPORTTELEMETRY: #if defined(PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY) PIOS_Board_configure_com(&pios_usart1_sport_cfg, PIOS_COM_FRSKYSPORT_RX_BUF_LEN, PIOS_COM_FRSKYSPORT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sport_id); #endif break; } /* UART2 Port */ uint8_t hw_uart2; HwFlyingF3Uart2Get(&hw_uart2); switch (hw_uart2) { case HWFLYINGF3_UART2_DISABLED: break; case HWFLYINGF3_UART2_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart2_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWFLYINGF3_UART2_GPS: #if defined(PIOS_INCLUDE_GPS) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart2_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); #endif break; case HWFLYINGF3_UART2_SBUS: #if defined(PIOS_INCLUDE_SBUS) && defined(PIOS_INCLUDE_USART) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart2_sbus_cfg)) { PIOS_Assert(0); } uintptr_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_usart2_sbus_aux_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 /* PIOS_INCLUDE_SBUS */ break; case HWFLYINGF3_UART2_DSM: #if defined(PIOS_INCLUDE_DSM) { PIOS_Board_configure_dsm(&pios_usart2_dsm_hsum_cfg, &pios_usart2_dsm_aux_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSM, &hw_DSMxMode); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFLYINGF3_UART2_HOTTSUMD: case HWFLYINGF3_UART2_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_uart2) { case HWFLYINGF3_UART2_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFLYINGF3_UART2_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart2_dsm_hsum_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFLYINGF3_UART2_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart2_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFLYINGF3_UART2_COMBRIDGE: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart2_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); #endif break; case HWFLYINGF3_UART2_MAVLINKTX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart2_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWFLYINGF3_UART2_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) && defined(PIOS_INCLUDE_GPS) PIOS_Board_configure_com(&pios_usart2_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 */ break; case HWFLYINGF3_UART2_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart2_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 HWFLYINGF3_UART2_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart2_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 HWFLYINGF3_UART2_LIGHTTELEMETRYTX: #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) PIOS_Board_configure_com(&pios_usart2_cfg, 0, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_lighttelemetry_id); #endif break; case HWFLYINGF3_UART2_FRSKYSPORTTELEMETRY: #if defined(PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY) PIOS_Board_configure_com(&pios_usart2_sport_cfg, PIOS_COM_FRSKYSPORT_RX_BUF_LEN, PIOS_COM_FRSKYSPORT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sport_id); #endif break; } /* UART3 Port */ uint8_t hw_uart3; HwFlyingF3Uart3Get(&hw_uart3); switch (hw_uart3) { case HWFLYINGF3_UART3_DISABLED: break; case HWFLYINGF3_UART3_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWFLYINGF3_UART3_GPS: #if defined(PIOS_INCLUDE_GPS) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); #endif break; case HWFLYINGF3_UART3_SBUS: #if defined(PIOS_INCLUDE_SBUS) && defined(PIOS_INCLUDE_USART) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart3_sbus_cfg)) { PIOS_Assert(0); } uintptr_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_usart3_sbus_aux_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 /* PIOS_INCLUDE_SBUS */ break; case HWFLYINGF3_UART3_DSM: #if defined(PIOS_INCLUDE_DSM) { PIOS_Board_configure_dsm(&pios_usart3_dsm_hsum_cfg, &pios_usart3_dsm_aux_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSM, &hw_DSMxMode); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFLYINGF3_UART3_HOTTSUMD: case HWFLYINGF3_UART3_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_uart3) { case HWFLYINGF3_UART3_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFLYINGF3_UART3_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart3_dsm_hsum_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFLYINGF3_UART3_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFLYINGF3_UART3_COMBRIDGE: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); #endif break; case HWFLYINGF3_UART3_MAVLINKTX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart3_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWFLYINGF3_UART3_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) && defined(PIOS_INCLUDE_GPS) PIOS_Board_configure_com(&pios_usart3_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 */ break; case HWFLYINGF3_UART3_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_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 HWFLYINGF3_UART3_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_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 HWFLYINGF3_UART3_LIGHTTELEMETRYTX: #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) PIOS_Board_configure_com(&pios_usart3_cfg, 0, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_lighttelemetry_id); #endif break; case HWFLYINGF3_UART3_FRSKYSPORTTELEMETRY: #if defined(PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY) PIOS_Board_configure_com(&pios_usart3_sport_cfg, PIOS_COM_FRSKYSPORT_RX_BUF_LEN, PIOS_COM_FRSKYSPORT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sport_id); #endif break; } /* UART4 Port */ uint8_t hw_uart4; HwFlyingF3Uart4Get(&hw_uart4); switch (hw_uart4) { case HWFLYINGF3_UART4_DISABLED: break; case HWFLYINGF3_UART4_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart4_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWFLYINGF3_UART4_GPS: #if defined(PIOS_INCLUDE_GPS) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart4_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); #endif break; case HWFLYINGF3_UART4_SBUS: #if defined(PIOS_INCLUDE_SBUS) && defined(PIOS_INCLUDE_USART) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart4_sbus_cfg)) { PIOS_Assert(0); } uintptr_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_usart4_sbus_aux_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 /* PIOS_INCLUDE_SBUS */ break; case HWFLYINGF3_UART4_DSM: #if defined(PIOS_INCLUDE_DSM) { PIOS_Board_configure_dsm(&pios_usart4_dsm_hsum_cfg, &pios_usart4_dsm_aux_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSM, &hw_DSMxMode); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFLYINGF3_UART4_HOTTSUMD: case HWFLYINGF3_UART4_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_uart4) { case HWFLYINGF3_UART4_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFLYINGF3_UART4_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart4_dsm_hsum_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFLYINGF3_UART4_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart4_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFLYINGF3_UART4_COMBRIDGE: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart4_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); #endif break; case HWFLYINGF3_UART4_MAVLINKTX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart4_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWFLYINGF3_UART4_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) && defined(PIOS_INCLUDE_GPS) PIOS_Board_configure_com(&pios_usart4_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 */ break; case HWFLYINGF3_UART4_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart4_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 HWFLYINGF3_UART4_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart4_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 HWFLYINGF3_UART4_LIGHTTELEMETRYTX: #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) PIOS_Board_configure_com(&pios_usart4_cfg, 0, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_lighttelemetry_id); #endif break; case HWFLYINGF3_UART4_FRSKYSPORTTELEMETRY: #if defined(PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY) PIOS_Board_configure_com(&pios_usart4_sport_cfg, PIOS_COM_FRSKYSPORT_RX_BUF_LEN, PIOS_COM_FRSKYSPORT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sport_id); #endif break; } /* UART5 Port */ uint8_t hw_uart5; HwFlyingF3Uart5Get(&hw_uart5); switch (hw_uart5) { case HWFLYINGF3_UART5_DISABLED: break; case HWFLYINGF3_UART5_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart5_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWFLYINGF3_UART5_GPS: #if defined(PIOS_INCLUDE_GPS) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart5_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); #endif break; case HWFLYINGF3_UART5_SBUS: #if defined(PIOS_INCLUDE_SBUS) && defined(PIOS_INCLUDE_USART) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart5_sbus_cfg)) { PIOS_Assert(0); } uintptr_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_usart5_sbus_aux_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 /* PIOS_INCLUDE_SBUS */ break; case HWFLYINGF3_UART5_DSM: #if defined(PIOS_INCLUDE_DSM) { PIOS_Board_configure_dsm(&pios_usart5_dsm_hsum_cfg, &pios_usart5_dsm_aux_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSM, &hw_DSMxMode); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFLYINGF3_UART5_HOTTSUMD: case HWFLYINGF3_UART5_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_uart5) { case HWFLYINGF3_UART5_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFLYINGF3_UART5_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart5_dsm_hsum_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFLYINGF3_UART5_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart5_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFLYINGF3_UART5_COMBRIDGE: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart5_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); #endif break; case HWFLYINGF3_UART5_MAVLINKTX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart5_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWFLYINGF3_UART5_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) && defined(PIOS_INCLUDE_GPS) PIOS_Board_configure_com(&pios_usart5_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 */ break; case HWFLYINGF3_UART5_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart5_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 HWFLYINGF3_UART5_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart5_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 HWFLYINGF3_UART5_LIGHTTELEMETRYTX: #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) PIOS_Board_configure_com(&pios_usart5_cfg, 0, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_lighttelemetry_id); #endif break; case HWFLYINGF3_UART5_FRSKYSPORTTELEMETRY: #if defined(PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY) PIOS_Board_configure_com(&pios_usart5_sport_cfg, PIOS_COM_FRSKYSPORT_RX_BUF_LEN, PIOS_COM_FRSKYSPORT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sport_id); #endif break; } /* Configure the rcvr port */ uint8_t hw_rcvrport; HwFlyingF3RcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWFLYINGF3_RCVRPORT_DISABLED: break; case HWFLYINGF3_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) { uintptr_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); uintptr_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } #endif /* PIOS_INCLUDE_PWM */ break; case HWFLYINGF3_RCVRPORT_PPM: case HWFLYINGF3_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_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; } #endif /* PIOS_INCLUDE_PPM */ break; case HWFLYINGF3_RCVRPORT_PPMPWM: /* This is a combination of PPM and PWM inputs */ #if defined(PIOS_INCLUDE_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; } #endif /* PIOS_INCLUDE_PPM */ #if defined(PIOS_INCLUDE_PWM) { uintptr_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_cfg); uintptr_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } #endif /* PIOS_INCLUDE_PWM */ break; } #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 switch (hw_rcvrport) { case HWFLYINGF3_RCVRPORT_DISABLED: case HWFLYINGF3_RCVRPORT_PWM: case HWFLYINGF3_RCVRPORT_PPM: /* Set up the servo outputs */ #ifdef PIOS_INCLUDE_SERVO PIOS_Servo_Init(&pios_servo_cfg); #endif break; case HWFLYINGF3_RCVRPORT_PPMOUTPUTS: case HWFLYINGF3_RCVRPORT_OUTPUTS: #ifdef PIOS_INCLUDE_SERVO PIOS_Servo_Init(&pios_servo_rcvr_cfg); #endif break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif PIOS_WDG_Clear(); PIOS_DELAY_WaitmS(200); PIOS_WDG_Clear(); PIOS_SENSORS_Init(); #if defined(PIOS_INCLUDE_L3GD20) && defined(PIOS_INCLUDE_SPI) if (PIOS_L3GD20_Init(pios_spi_internal_id, 0, &pios_l3gd20_cfg) != 0) panic(1); if (PIOS_L3GD20_Test() != 0) panic(1); uint8_t hw_l3gd20_samplerate; HwFlyingF3L3GD20RateGet(&hw_l3gd20_samplerate); enum pios_l3gd20_rate l3gd20_samplerate = PIOS_L3GD20_RATE_380HZ_100HZ; switch(hw_l3gd20_samplerate) { case HWFLYINGF3_L3GD20RATE_380: l3gd20_samplerate = PIOS_L3GD20_RATE_380HZ_100HZ; break; case HWFLYINGF3_L3GD20RATE_760: l3gd20_samplerate = PIOS_L3GD20_RATE_760HZ_100HZ; break; } PIOS_Assert(PIOS_L3GD20_SetSampleRate(l3gd20_samplerate) == 0); // To be safe map from UAVO enum to driver enum /* * FIXME: add support for this to l3gd20 driver uint8_t hw_gyro_range; HwFlyingF3GyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWFLYINGF3_GYRORANGE_250: PIOS_L3GD20_SetRange(PIOS_L3GD20_SCALE_250_DEG); break; case HWFLYINGF3_GYRORANGE_500: PIOS_L3GD20_SetRange(PIOS_L3GD20_SCALE_500_DEG); break; case HWFLYINGF3_GYRORANGE_1000: //FIXME: how to behave in this case? PIOS_L3GD20_SetRange(PIOS_L3GD20_SCALE_2000_DEG); break; case HWFLYINGF3_GYRORANGE_2000: PIOS_L3GD20_SetRange(PIOS_L3GD20_SCALE_2000_DEG); break; } */ PIOS_WDG_Clear(); PIOS_DELAY_WaitmS(50); PIOS_WDG_Clear(); #endif /* PIOS_INCLUDE_L3GD20 && PIOS_INCLUDE_I2C*/ #if defined(PIOS_INCLUDE_LSM303) && defined(PIOS_INCLUDE_I2C) if (PIOS_LSM303_Init(pios_i2c_internal_id, &pios_lsm303_cfg) != 0) panic(2); if (PIOS_LSM303_Accel_Test() != 0) panic(2); if (PIOS_LSM303_Mag_Test() != 0) panic(2); uint8_t hw_accel_range; HwFlyingF3AccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWFLYINGF3_ACCELRANGE_2G: PIOS_LSM303_Accel_SetRange(PIOS_LSM303_ACCEL_2G); break; case HWFLYINGF3_ACCELRANGE_4G: PIOS_LSM303_Accel_SetRange(PIOS_LSM303_ACCEL_4G); break; case HWFLYINGF3_ACCELRANGE_8G: PIOS_LSM303_Accel_SetRange(PIOS_LSM303_ACCEL_8G); break; case HWFLYINGF3_ACCELRANGE_16G: PIOS_LSM303_Accel_SetRange(PIOS_LSM303_ACCEL_16G); break; } //there is no setting for the mag scale yet PIOS_LSM303_Mag_SetRange(PIOS_LSM303_MAG_1_9GA); PIOS_WDG_Clear(); PIOS_DELAY_WaitmS(50); PIOS_WDG_Clear(); #endif /* PIOS_INCLUDE_LSM303 && PIOS_INCLUDE_I2C*/ #if defined(PIOS_INCLUDE_GPIO) PIOS_GPIO_Init(); #endif //FlyingF3 shield specific hw init uint8_t flyingf3_shield; uint32_t internal_adc_id; HwFlyingF3ShieldGet(&flyingf3_shield); switch (flyingf3_shield) { case HWFLYINGF3_SHIELD_RCFLYER: #if defined(PIOS_INCLUDE_ADC) //Sanity check, this is to ensure that no one changes the adc_pins array without changing the defines PIOS_Assert(internal_adc_cfg_rcflyer_shield.adc_pins[PIOS_ADC_RCFLYER_SHIELD_BARO_PIN].pin == GPIO_Pin_3); PIOS_Assert(internal_adc_cfg_rcflyer_shield.adc_pins[PIOS_ADC_RCFLYER_SHIELD_BAT_VOLTAGE_PIN].pin == GPIO_Pin_4); if (PIOS_INTERNAL_ADC_Init(&internal_adc_id, &internal_adc_cfg_rcflyer_shield) < 0) PIOS_Assert(0); PIOS_ADC_Init(&pios_internal_adc_id, &pios_internal_adc_driver, internal_adc_id); #endif #if defined(PIOS_INCLUDE_SPI) if (PIOS_SPI_Init(&pios_spi_2_id, &pios_spi_2_rcflyer_internal_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_SPI_Init(&pios_spi_3_id, &pios_spi_3_rcflyer_external_cfg)) { PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_MS5611_SPI) if (PIOS_MS5611_SPI_Init(pios_spi_2_id, 1, &pios_ms5611_cfg) != 0) { PIOS_Assert(0); } #endif /* PIOS_INCLUDE_MS5611_SPI */ #endif /* PIOS_INCLUDE_SPI */ break; case HWFLYINGF3_SHIELD_CHEBUZZ: #if defined(PIOS_INCLUDE_I2C) && defined(PIOS_INCLUDE_MS5611) if (PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_external_id) != 0) { PIOS_Assert(0); } #endif /* PIOS_INCLUDE_I2C && PIOS_INCLUDE_MS5611 */ #if defined(PIOS_INCLUDE_SPI) if (PIOS_SPI_Init(&pios_spi_2_id, &pios_spi_2_chebuzz_external_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_SPI_Init(&pios_spi_3_id, &pios_spi_3_chebuzz_internal_cfg)) { PIOS_DEBUG_Assert(0); } #endif /* PIOS_INCLUDE_SPI */ break; case HWFLYINGF3_SHIELD_BMP085: #if defined(PIOS_INCLUDE_BMP085) && defined(PIOS_INCLUDE_I2C) if (PIOS_BMP085_Init(&pios_bmp085_cfg, pios_i2c_external_id) != 0) panic(5); if (PIOS_BMP085_Test() != 0) panic(5); #endif /* PIOS_INCLUDE_BMP085 && PIOS_INCLUDE_I2C */ break; case HWFLYINGF3_SHIELD_NONE: break; default: PIOS_Assert(0); break; } /* Make sure we have at least one telemetry link configured or else fail initialization */ PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); }
/** * PIOS_Board_Init() */ void PIOS_Board_Init(void) { /* Delay system */ PIOS_DELAY_Init(); /* Initialize UAVObject libraries */ // EventDispatcherInitialize(); // UAVObjInitialize(); // HwSettingsInitialize(); // ManualControlSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* Initialize the alarms library */ // AlarmsInitialize(); /* Initialize the task monitor library */ // TaskMonitorInitialize(); #if 0 // XXX this is all very wrong now #if defined(PIOS_INCLUDE_SBUS) { uint32_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) { PIOS_Assert(0); } uint32_t pios_sbus_id; if (PIOS_SBUS_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_SBUS */ #if defined(PIOS_INCLUDE_SPEKTRUM) { uint32_t pios_usart_spektrum_id; if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_main_cfg)) { PIOS_Assert(0); } uint32_t pios_spektrum_id; if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_main_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, 0)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_SPEKTRUM */ #endif #if defined(PIOS_COM_AUX) uint32_t pios_usart_aux_id; if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) { PIOS_DEBUG_Assert(0); } static uint8_t rx_buffer[128]; static uint8_t tx_buffer[128]; if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id, rx_buffer, 128, tx_buffer, 128)) { PIOS_Assert(0); } #endif PIOS_COM_SendFormattedString(PIOS_COM_AUX, "PX2IO starting...\r\n"); /* Bring up the I2C slave interface */ PIOS_I2C_Slave_Init(pios_i2c_slave_id, &i2c_slave_cfg); PIOS_Servo_Init(); // PIOS_ADC_INIT(); PIOS_GPIO_Init(); /* Configure the selected receiver */ /* XXX we don't have these settings until we have config from FMU ... */ #if defined(PIOS_INCLUDE_PPM) #if !defined(PIOS_INCLUDE_RTC) # error PPM requires RTC #endif PIOS_PPM_Init(); uint32_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) { PIOS_Assert(0); } for (uint8_t i = 0; i < PIOS_PPM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); i++) { pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_ppm_rcvr_id; pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; pios_rcvr_max_channel++; } #endif /* PIOS_INCLUDE_PPM */ #if defined(PIOS_INCLUDE_SPEKTRUM) if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SPEKTRUM || hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_SPEKTRUM) { uint32_t pios_spektrum_rcvr_id; if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, 0)) { PIOS_Assert(0); } for (uint8_t i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); i++) { pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_spektrum_rcvr_id; pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; pios_rcvr_max_channel++; } } #endif /* PIOS_INCLUDE_SPEKTRUM */ #if defined(PIOS_INCLUDE_SBUS) if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SBUS) { uint32_t pios_sbus_rcvr_id; if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, 0)) { PIOS_Assert(0); } for (uint8_t i = 0; i < SBUS_NUMBER_OF_CHANNELS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); i++) { pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_sbus_rcvr_id; pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; pios_rcvr_max_channel++; } } #endif /* PIOS_INCLUDE_SBUS */ //PIOS_WDG_Init(); }