/** * @brief Initialize the MPU6000 3-axis gyro sensor * \return none * \param[in] PIOS_MPU6000_ConfigTypeDef struct to be used to configure sensor. * */ static void PIOS_MPU6000_Config(struct pios_mpu6000_cfg const *cfg) { PIOS_MPU6000_Test(); // Reset chip while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, PIOS_MPU6000_PWRMGMT_IMU_RST) != 0) { ; } PIOS_DELAY_WaitmS(50); // Reset chip and fifo while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, PIOS_MPU6000_USERCTL_GYRO_RST | PIOS_MPU6000_USERCTL_SIG_COND | PIOS_MPU6000_USERCTL_FIFO_RST) != 0) { ; } // Wait for reset to finish while (PIOS_MPU6000_GetReg(PIOS_MPU6000_USER_CTRL_REG) & (PIOS_MPU6000_USERCTL_GYRO_RST | PIOS_MPU6000_USERCTL_SIG_COND | PIOS_MPU6000_USERCTL_FIFO_RST)) { ; } PIOS_DELAY_WaitmS(10); // Power management configuration while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) { ; } // Interrupt configuration while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) { ; } // Interrupt configuration while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) { ; } // FIFO storage #if defined(PIOS_MPU6000_ACCEL) while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store | PIOS_MPU6000_ACCEL_OUT) != 0) { ; } #else while (PIOS_MPU6000_SetReg(PIOS_MPU6000_FIFO_EN_REG, cfg->Fifo_store) != 0) { ; } #endif PIOS_MPU6000_ConfigureRanges(cfg->gyro_range, cfg->accel_range, cfg->filter); // Interrupt configuration while (PIOS_MPU6000_SetReg(PIOS_MPU6000_USER_CTRL_REG, cfg->User_ctl) != 0) { ; } // Interrupt configuration while (PIOS_MPU6000_SetReg(PIOS_MPU6000_PWR_MGMT_REG, cfg->Pwr_mgmt_clk) != 0) { ; } // Interrupt configuration while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_CFG_REG, cfg->interrupt_cfg) != 0) { ; } // Interrupt configuration while (PIOS_MPU6000_SetReg(PIOS_MPU6000_INT_EN_REG, cfg->interrupt_en) != 0) { ; } if ((PIOS_MPU6000_GetReg(PIOS_MPU6000_INT_EN_REG)) != cfg->interrupt_en) { return; } mpu6000_configured = true; }
void PIOS_Board_Init(void) { /* Delay system */ PIOS_DELAY_Init(); const struct pios_board_info * bdinfo = &pios_board_info_blob; #if defined(PIOS_INCLUDE_LED) const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); PIOS_Assert(led_cfg); PIOS_LED_Init(led_cfg); #endif /* PIOS_INCLUDE_LED */ /* Set up the SPI interface to the gyro/acelerometer */ if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } /* Set up the SPI interface to the flash and rfm22b */ if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_FLASH) /* Connect flash to the approrpiate interface and configure it */ uintptr_t flash_id; if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1, &flash_m25p_cfg) != 0) panic(1); uintptr_t fs_id; if (PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id) != 0) panic(1); #endif /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); HwFreedomInitialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* Initialize the alarms library */ AlarmsInitialize(); /* Initialize the task monitor library */ TaskMonitorInitialize(); // /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_3_cfg); /* IAP System Setup */ PIOS_IAP_Init(); uint16_t boot_count = PIOS_IAP_ReadBootCount(); if (boot_count < 3) { PIOS_IAP_WriteBootCount(++boot_count); AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); } else { /* Too many failed boot attempts, force hw config to defaults */ HwFreedomSetDefaults(HwFreedomHandle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; #endif uint32_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwFreedomUSB_VCPPortGet(&hw_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hw_usb_vcpport = HWFREEDOM_USB_VCPPORT_DISABLED; } switch (hw_usb_vcpport) { case HWFREEDOM_USB_VCPPORT_DISABLED: break; case HWFREEDOM_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_telem_usb_id); #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_vcp_id); #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uint32_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwFreedomUSB_HIDPortGet(&hw_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hw_usb_hidport = HWFREEDOM_USB_HIDPORT_DISABLED; } switch (hw_usb_hidport) { case HWFREEDOM_USB_HIDPORT_DISABLED: break; case HWFREEDOM_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint32_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_HID */ if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } #endif /* PIOS_INCLUDE_USB */ /* Configure IO ports */ uint8_t hw_DSMxBind; HwFreedomDSMxBindGet(&hw_DSMxBind); /* Configure FlexiPort */ uint8_t hw_mainport; HwFreedomMainPortGet(&hw_mainport); switch (hw_mainport) { case HWFREEDOM_MAINPORT_DISABLED: break; case HWFREEDOM_MAINPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; break; case HWFREEDOM_MAINPORT_GPS: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_MAINPORT_DSM2: case HWFREEDOM_MAINPORT_DSMX10BIT: case HWFREEDOM_MAINPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hw_mainport) { case HWFREEDOM_MAINPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWFREEDOM_MAINPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWFREEDOM_MAINPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hw_DSMxBind); } break; case HWFREEDOM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_MAINPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; } /* hw_freedom_mainport */ /* Configure flexi USART port */ uint8_t hw_flexiport; HwFreedomFlexiPortGet(&hw_flexiport); switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_DISABLED: break; case HWFREEDOM_FLEXIPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWFREEDOM_FLEXIPORT_GPS: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_FLEXIPORT_DSM2: case HWFREEDOM_FLEXIPORT_DSMX10BIT: case HWFREEDOM_FLEXIPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWFREEDOM_FLEXIPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWFREEDOM_FLEXIPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,&hw_DSMxBind); } break; case HWFREEDOM_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_I2C */ case HWFREEDOM_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_FLEXIPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; } /* hw_freedom_flexiport */ /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) uint8_t hwsettings_radioport; HwFreedomRadioPortGet(&hwsettings_radioport); switch (hwsettings_radioport) { case HWFREEDOM_RADIOPORT_DISABLED: break; case HWFREEDOM_RADIOPORT_TELEMETRY: { const struct pios_board_info * bdinfo = &pios_board_info_blob; const struct pios_rfm22b_cfg *pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) { PIOS_Assert(0); } uint8_t *rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { PIOS_Assert(0); } break; } } #endif /* PIOS_INCLUDE_RFM22B */ /* Configure input receiver USART port */ uint8_t hw_rcvrport; HwFreedomRcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_DISABLED: break; case HWFREEDOM_RCVRPORT_PPM: { uint32_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); uint32_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } break; case HWFREEDOM_RCVRPORT_DSM2: case HWFREEDOM_RCVRPORT_DSMX10BIT: case HWFREEDOM_RCVRPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWFREEDOM_RCVRPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWFREEDOM_RCVRPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_rcvr_cfg, &pios_dsm_rcvr_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,&hw_DSMxBind); } break; case HWFREEDOM_RCVRPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) { uint32_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_rcvr_cfg)) { PIOS_Assert(0); } uint32_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } uint32_t pios_sbus_rcvr_id; if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; } #endif break; } if (hw_rcvrport != HWFREEDOM_RCVRPORT_SBUS) { GPIO_Init(pios_sbus_cfg.inv.gpio, (GPIO_InitTypeDef*)&pios_sbus_cfg.inv.init); GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); } #if defined(PIOS_OVERO_SPI) /* Set up the SPI based PIOS_COM interface to the overo */ { bool overo_enabled = false; #ifdef MODULE_OveroSync_BUILTIN overo_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_OVEROSYNC] == MODULESETTINGS_ADMINSTATE_ENABLED) { overo_enabled = true; } else { overo_enabled = false; } #endif if (overo_enabled) { if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { PIOS_DEBUG_Assert(0); } const uint32_t PACKET_SIZE = 1024; uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_overo_id, &pios_overo_com_driver, pios_overo_id, rx_buffer, PACKET_SIZE, tx_buffer, PACKET_SIZE)) { PIOS_Assert(0); } } } #endif #if defined(PIOS_INCLUDE_GCSRCVR) GCSReceiverInitialize(); uint32_t pios_gcsrcvr_id; PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); uint32_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS uint8_t hw_output_port; HwFreedomOutputGet(&hw_output_port); switch (hw_output_port) { case HWFREEDOM_OUTPUT_DISABLED: break; case HWFREEDOM_OUTPUT_PWM: /* Set up the servo outputs */ PIOS_Servo_Init(&pios_servo_cfg); break; default: break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif PIOS_DELAY_WaitmS(200); PIOS_SENSORS_Init(); #if defined(PIOS_INCLUDE_MPU6000) if (PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg) != 0) panic(2); if (PIOS_MPU6000_Test() != 0) panic(2); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwFreedomGyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWFREEDOM_GYRORANGE_250: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWFREEDOM_GYRORANGE_500: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWFREEDOM_GYRORANGE_1000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWFREEDOM_GYRORANGE_2000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwFreedomAccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWFREEDOM_ACCELRANGE_2G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWFREEDOM_ACCELRANGE_4G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWFREEDOM_ACCELRANGE_8G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWFREEDOM_ACCELRANGE_16G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_16G); break; } uint8_t hw_mpu6000_rate; HwFreedomMPU6000RateGet(&hw_mpu6000_rate); uint16_t hw_mpu6000_divisor = \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_500) ? 15 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_666) ? 11 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_1000) ? 7 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_2000) ? 3 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_4000) ? 1 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_8000) ? 0 : \ 15; PIOS_MPU6000_SetDivisor(hw_mpu6000_divisor); uint8_t hw_dlpf; HwFreedomMPU6000DLPFGet(&hw_dlpf); uint16_t hw_mpu6000_dlpf = \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ PIOS_MPU60X0_LOWPASS_256_HZ; PIOS_MPU6000_SetLPF(hw_mpu6000_dlpf); #endif if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_mag_pressure_adapter_id) != 0) panic(5); PIOS_DELAY_WaitmS(50); #if defined(PIOS_INCLUDE_ADC) PIOS_ADC_Init(&pios_adc_cfg); #endif PIOS_LED_On(0); #if defined(PIOS_INCLUDE_HMC5883) if (PIOS_HMC5883_Init(pios_i2c_mag_pressure_adapter_id, &pios_hmc5883_cfg) != 0) panic(3); #endif PIOS_LED_On(1); #if defined(PIOS_INCLUDE_MS5611) if (PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id) != 0) panic(4); #endif PIOS_LED_On(2); }
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 */ #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 */ AlarmsInitialize(); 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_I2C) if (PIOS_I2C_Init (&pios_i2c_internal_adapter_id, &pios_i2c_internal_adapter_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_internal_adapter_id) != 0) panic(5); #endif #if defined(PIOS_INCLUDE_SPI) if (PIOS_SPI_Init(&pios_spi_flash_id, &pios_spi_flash_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_SPI_Init (&pios_spi_gyro_accel_id, &pios_spi_gyro_accel_cfg)) { PIOS_Assert(0); } #endif #if defined(PIOS_INCLUDE_FLASH) /* Inititialize all flash drivers */ if (PIOS_Flash_Jedec_Init (&pios_external_flash_id, pios_spi_flash_id, 0, &flash_mx25_cfg) != 0) panic(1); if (PIOS_Flash_Internal_Init (&pios_internal_flash_id, &flash_internal_cfg) != 0) panic(1); /* Register the partition table */ const struct pios_flash_partition *flash_partition_table; uint32_t num_partitions; flash_partition_table = PIOS_BOARD_HW_DEFS_GetPartitionTable(bdinfo->board_rev, &num_partitions); PIOS_FLASH_register_partition_table(flash_partition_table, num_partitions); /* Mount all filesystems */ if (PIOS_FLASHFS_Logfs_Init (&pios_uavo_settings_fs_id, &flashfs_settings_cfg, FLASH_PARTITION_LABEL_SETTINGS) != 0) panic(1); if (PIOS_FLASHFS_Logfs_Init (&pios_waypoints_settings_fs_id, &flashfs_waypoints_cfg, FLASH_PARTITION_LABEL_WAYPOINTS) != 0) panic(1); #if defined(ERASE_FLASH) PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); #endif #endif /* PIOS_INCLUDE_FLASH */ /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Initialize UAVObject libraries */ UAVObjInitialize(); /* Initialize the alarms library. Reads RCC reset flags */ AlarmsInitialize(); PIOS_RESET_Clear(); // Clear the RCC reset flags after use. /* Initialize the hardware UAVOs */ HwColibriInitialize(); 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 */ //Timers used for inputs (1, 2, 5, 8) PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_5_cfg); PIOS_TIM_InitClock(&tim_8_cfg); // Timers used for outputs (3, 10, 11, 12) PIOS_TIM_InitClock(&tim_3_cfg); PIOS_TIM_InitClock(&tim_10_cfg); PIOS_TIM_InitClock(&tim_11_cfg); PIOS_TIM_InitClock(&tim_12_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 */ HwColibriSetDefaults(HwColibriHandle(), 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 */ HwColibriUSB_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 = HWCOLIBRI_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 HWCOLIBRI_USB_VCPPORT_DISABLED: break; case HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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; HwColibriUSB_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 = HWCOLIBRI_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 HWCOLIBRI_USB_HIDPORT_DISABLED: break; case HWCOLIBRI_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 */ HwColibriDSMxModeOptions hw_DSMxMode; HwColibriDSMxModeGet(&hw_DSMxMode); /* init sensor queue registration */ PIOS_SENSORS_Init(); /* UART1 Port */ uint8_t hw_uart1; HwColibriUart1Get(&hw_uart1); switch (hw_uart1) { case HWCOLIBRI_UART1_DISABLED: break; case HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_UART1_I2C: #if defined(PIOS_INCLUDE_I2C) if (PIOS_I2C_Init (&pios_i2c_usart1_adapter_id, &pios_i2c_usart1_adapter_cfg)) { PIOS_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_usart1_adapter_id) != 0) panic(6); #if defined(PIOS_INCLUDE_HMC5883) { uint8_t Magnetometer; HwColibriMagnetometerGet(&Magnetometer); if (Magnetometer == HWCOLIBRI_MAGNETOMETER_EXTERNALI2CUART1) { // init sensor if (PIOS_HMC5883_Init (pios_i2c_usart1_adapter_id, &pios_hmc5883_external_cfg) != 0) panic(8); if (PIOS_HMC5883_Test() != 0) panic(8); } } #endif /* PIOS_INCLUDE_HMC5883 */ #endif /* PIOS_INCLUDE_I2C */ break; case HWCOLIBRI_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 HWCOLIBRI_UART1_HOTTSUMD: case HWCOLIBRI_UART1_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_uart1) { case HWCOLIBRI_UART1_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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; HwColibriUart2Get(&hw_uart2); switch (hw_uart2) { case HWCOLIBRI_UART2_DISABLED: break; case HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_UART2_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_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 HWCOLIBRI_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 HWCOLIBRI_UART2_HOTTSUMD: case HWCOLIBRI_UART2_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_uart2) { case HWCOLIBRI_UART2_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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; } if (hw_uart2 != HWCOLIBRI_UART2_SBUS) { GPIO_Init(pios_usart2_sbus_aux_cfg.inv.gpio, (GPIO_InitTypeDef*)&pios_usart2_sbus_aux_cfg.inv.init); GPIO_WriteBit(pios_usart2_sbus_aux_cfg.inv.gpio, pios_usart2_sbus_aux_cfg.inv.init.GPIO_Pin, pios_usart2_sbus_aux_cfg.gpio_inv_disable); } /* UART3 Port */ uint8_t hw_uart3; HwColibriUart3Get(&hw_uart3); switch (hw_uart3) { case HWCOLIBRI_UART3_DISABLED: break; case HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_UART3_I2C: #if defined(PIOS_INCLUDE_I2C) if (PIOS_I2C_Init (&pios_i2c_usart3_adapter_id, &pios_i2c_usart3_adapter_cfg)) { PIOS_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_usart3_adapter_id) != 0) panic(7); #if defined(PIOS_INCLUDE_HMC5883) { uint8_t Magnetometer; HwColibriMagnetometerGet(&Magnetometer); if (Magnetometer == HWCOLIBRI_MAGNETOMETER_EXTERNALI2CUART3) { // init sensor if (PIOS_HMC5883_Init (pios_i2c_usart3_adapter_id, &pios_hmc5883_external_cfg) != 0) panic(9); if (PIOS_HMC5883_Test() != 0) panic(9); } } #endif /* PIOS_INCLUDE_HMC5883 */ #endif /* PIOS_INCLUDE_I2C */ break; case HWCOLIBRI_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 HWCOLIBRI_UART3_HOTTSUMD: case HWCOLIBRI_UART3_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_uart3) { case HWCOLIBRI_UART3_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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; } /* UART4 Port */ uint8_t hw_uart4; HwColibriUart4Get(&hw_uart4); switch (hw_uart4) { case HWCOLIBRI_UART4_DISABLED: break; case HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_UART4_HOTTSUMD: case HWCOLIBRI_UART4_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_uart4) { case HWCOLIBRI_UART4_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_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 HWCOLIBRI_UART4_PICOC: #if defined(PIOS_INCLUDE_PICOC) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart4_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; } /* Configure the rcvr port */ uint8_t hw_rcvrport; HwColibriRcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWCOLIBRI_RCVRPORT_DISABLED: break; case HWCOLIBRI_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 HWCOLIBRI_RCVRPORT_PWMADC: #if defined(PIOS_INCLUDE_PWM) { uintptr_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_adc_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 HWCOLIBRI_RCVRPORT_PPM: case HWCOLIBRI_RCVRPORT_PPMADC: case HWCOLIBRI_RCVRPORT_PPMOUTPUTS: case HWCOLIBRI_RCVRPORT_PPMOUTPUTSADC: #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 HWCOLIBRI_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; case HWCOLIBRI_RCVRPORT_PPMPWMADC: /* This is a combination of PPM and PWM inputs with IN6 and IN7 free for adc */ #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_with_adc_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 HWCOLIBRI_RCVRPORT_DISABLED: case HWCOLIBRI_RCVRPORT_PWM: case HWCOLIBRI_RCVRPORT_PWMADC: case HWCOLIBRI_RCVRPORT_PPM: case HWCOLIBRI_RCVRPORT_PPMADC: case HWCOLIBRI_RCVRPORT_PPMPWM: case HWCOLIBRI_RCVRPORT_PPMPWMADC: /* Set up the servo outputs */ #ifdef PIOS_INCLUDE_SERVO PIOS_Servo_Init(&pios_servo_cfg); #endif break; case HWCOLIBRI_RCVRPORT_PPMOUTPUTS: case HWCOLIBRI_RCVRPORT_OUTPUTS: #ifdef PIOS_INCLUDE_SERVO PIOS_Servo_Init(&pios_servo_with_rcvr_cfg); #endif break; case HWCOLIBRI_RCVRPORT_PPMOUTPUTSADC: case HWCOLIBRI_RCVRPORT_OUTPUTSADC: #ifdef PIOS_INCLUDE_SERVO PIOS_Servo_Init(&pios_servo_with_rcvr_with_adc_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_MPU6000) if (PIOS_MPU6000_Init(pios_spi_gyro_accel_id, 0, &pios_mpu6000_cfg) != 0) panic(2); if (PIOS_MPU6000_Test() != 0) panic(2); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwColibriGyroRangeGet(&hw_gyro_range); switch (hw_gyro_range) { case HWCOLIBRI_GYRORANGE_250: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWCOLIBRI_GYRORANGE_500: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWCOLIBRI_GYRORANGE_1000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWCOLIBRI_GYRORANGE_2000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwColibriAccelRangeGet(&hw_accel_range); switch (hw_accel_range) { case HWCOLIBRI_ACCELRANGE_2G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWCOLIBRI_ACCELRANGE_4G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWCOLIBRI_ACCELRANGE_8G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWCOLIBRI_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; HwColibriMPU6000DLPFGet(&hw_mpu6000_dlpf); enum pios_mpu60x0_filter mpu6000_dlpf = (hw_mpu6000_dlpf == HWCOLIBRI_MPU6000DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : (hw_mpu6000_dlpf == HWCOLIBRI_MPU6000DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : (hw_mpu6000_dlpf == HWCOLIBRI_MPU6000DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : (hw_mpu6000_dlpf == HWCOLIBRI_MPU6000DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : (hw_mpu6000_dlpf == HWCOLIBRI_MPU6000DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : (hw_mpu6000_dlpf == HWCOLIBRI_MPU6000DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : (hw_mpu6000_dlpf == HWCOLIBRI_MPU6000DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : pios_mpu6000_cfg.default_filter; PIOS_MPU6000_SetLPF(mpu6000_dlpf); uint8_t hw_mpu6000_samplerate; HwColibriMPU6000RateGet(&hw_mpu6000_samplerate); uint16_t mpu6000_samplerate = (hw_mpu6000_samplerate == HWCOLIBRI_MPU6000RATE_200) ? 200 : (hw_mpu6000_samplerate == HWCOLIBRI_MPU6000RATE_333) ? 333 : (hw_mpu6000_samplerate == HWCOLIBRI_MPU6000RATE_500) ? 500 : (hw_mpu6000_samplerate == HWCOLIBRI_MPU6000RATE_666) ? 666 : (hw_mpu6000_samplerate == HWCOLIBRI_MPU6000RATE_1000) ? 1000 : (hw_mpu6000_samplerate == HWCOLIBRI_MPU6000RATE_2000) ? 2000 : (hw_mpu6000_samplerate == HWCOLIBRI_MPU6000RATE_4000) ? 4000 : (hw_mpu6000_samplerate == HWCOLIBRI_MPU6000RATE_8000) ? 8000 : pios_mpu6000_cfg.default_samplerate; PIOS_MPU6000_SetSampleRate(mpu6000_samplerate); #endif #if defined(PIOS_INCLUDE_I2C) #if defined(PIOS_INCLUDE_HMC5883) { uint8_t Magnetometer; HwColibriMagnetometerGet(&Magnetometer); if (Magnetometer == HWCOLIBRI_MAGNETOMETER_INTERNAL) { if (PIOS_HMC5883_Init (pios_i2c_internal_adapter_id, &pios_hmc5883_internal_cfg) != 0) panic(3); if (PIOS_HMC5883_Test() != 0) panic(3); } if (Magnetometer == HWCOLIBRI_MAGNETOMETER_EXTERNALI2CUART1 || Magnetometer == HWCOLIBRI_MAGNETOMETER_EXTERNALI2CUART3) { // setup sensor orientation uint8_t ExtMagOrientation; HwColibriExtMagOrientationGet(&ExtMagOrientation); enum pios_hmc5883_orientation hmc5883_orientation = (ExtMagOrientation == HWCOLIBRI_EXTMAGORIENTATION_TOP0DEGCW) ? PIOS_HMC5883_TOP_0DEG : (ExtMagOrientation == HWCOLIBRI_EXTMAGORIENTATION_TOP90DEGCW) ? PIOS_HMC5883_TOP_90DEG : (ExtMagOrientation == HWCOLIBRI_EXTMAGORIENTATION_TOP180DEGCW) ? PIOS_HMC5883_TOP_180DEG : (ExtMagOrientation == HWCOLIBRI_EXTMAGORIENTATION_TOP270DEGCW) ? PIOS_HMC5883_TOP_270DEG : (ExtMagOrientation == HWCOLIBRI_EXTMAGORIENTATION_BOTTOM0DEGCW) ? PIOS_HMC5883_BOTTOM_0DEG : (ExtMagOrientation == HWCOLIBRI_EXTMAGORIENTATION_BOTTOM90DEGCW) ? PIOS_HMC5883_BOTTOM_90DEG : (ExtMagOrientation == HWCOLIBRI_EXTMAGORIENTATION_BOTTOM180DEGCW) ? PIOS_HMC5883_BOTTOM_180DEG : (ExtMagOrientation == HWCOLIBRI_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_internal_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) if (hw_rcvrport == HWCOLIBRI_RCVRPORT_PWMADC || hw_rcvrport == HWCOLIBRI_RCVRPORT_PPMADC || hw_rcvrport == HWCOLIBRI_RCVRPORT_PPMPWMADC || hw_rcvrport == HWCOLIBRI_RCVRPORT_OUTPUTSADC || hw_rcvrport == HWCOLIBRI_RCVRPORT_PPMOUTPUTSADC) { uint32_t internal_adc_id; PIOS_INTERNAL_ADC_Init(&internal_adc_id, &pios_adc_cfg); PIOS_ADC_Init(&pios_internal_adc_id, &pios_internal_adc_driver, internal_adc_id); } #endif #if defined(PIOS_INCLUDE_FLASH) if ( PIOS_STREAMFS_Init(&streamfs_id, &streamfs_settings, FLASH_PARTITION_LABEL_LOG) != 0) panic(8); const uint32_t LOG_BUF_LEN = 256; uint8_t *log_rx_buffer = PIOS_malloc(LOG_BUF_LEN); uint8_t *log_tx_buffer = PIOS_malloc(LOG_BUF_LEN); if (PIOS_COM_Init(&pios_com_spiflash_logging_id, &pios_streamfs_com_driver, streamfs_id, log_rx_buffer, LOG_BUF_LEN, log_tx_buffer, LOG_BUF_LEN) != 0) panic(9); #endif /* PIOS_INCLUDE_FLASH */ //Set battery input pin to floating as long as it is unused GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15; GPIO_Init(GPIOC, &GPIO_InitStructure); GPIO_ResetBits(GPIOC, GPIO_Pin_15); //Set buzzer output to low as long as it is unused GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; GPIO_Init(GPIOA, &GPIO_InitStructure); GPIO_ResetBits(GPIOA, GPIO_Pin_4); /* 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 */ /* Set up the SPI interface to the gyro/acelerometer */ if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } /* Set up the SPI interface to the flash and rfm22b */ if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_FLASH) /* Inititialize all flash drivers */ if (PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_telem_flash_id, 1, &flash_m25p_cfg) != 0) panic(1); if (PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg) != 0) panic(1); /* Register the partition table */ const struct pios_flash_partition * flash_partition_table; uint32_t num_partitions; flash_partition_table = PIOS_BOARD_HW_DEFS_GetPartitionTable(bdinfo->board_rev, &num_partitions); PIOS_FLASH_register_partition_table(flash_partition_table, num_partitions); /* Mount all filesystems */ if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_settings_cfg, FLASH_PARTITION_LABEL_SETTINGS) != 0) panic(1); if (PIOS_FLASHFS_Logfs_Init(&pios_waypoints_settings_fs_id, &flashfs_waypoints_cfg, FLASH_PARTITION_LABEL_WAYPOINTS) != 0) panic(1); #if defined(ERASE_FLASH) PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); #endif #endif /* PIOS_INCLUDE_FLASH */ /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); /* Initialize the alarms library */ AlarmsInitialize(); HwFreedomInitialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) PIOS_RTC_Init(&pios_rtc_main_cfg); #endif #ifndef ERASE_FLASH /* Initialize watchdog as early as possible to catch faults during init * but do it only if there is no debugger connected */ if ((CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) == 0) { PIOS_WDG_Init(); } #endif // /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_3_cfg); /* IAP System Setup */ PIOS_IAP_Init(); uint16_t boot_count = PIOS_IAP_ReadBootCount(); if (boot_count < 3) { PIOS_IAP_WriteBootCount(++boot_count); AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); } else { /* Too many failed boot attempts, force hw config to defaults */ HwFreedomSetDefaults(HwFreedomHandle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; #endif uintptr_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwFreedomUSB_VCPPortGet(&hw_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hw_usb_vcpport = HWFREEDOM_USB_VCPPORT_DISABLED; } uintptr_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } switch (hw_usb_vcpport) { case HWFREEDOM_USB_VCPPORT_DISABLED: break; case HWFREEDOM_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_PICOC: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_PICOC_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_PICOC_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_picoc_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_PICOC_RX_BUF_LEN, tx_buffer, PIOS_COM_PICOC_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwFreedomUSB_HIDPortGet(&hw_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hw_usb_hidport = HWFREEDOM_USB_HIDPORT_DISABLED; } uintptr_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } switch (hw_usb_hidport) { case HWFREEDOM_USB_HIDPORT_DISABLED: break; case HWFREEDOM_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_HID */ if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } #endif /* PIOS_INCLUDE_USB */ /* Configure IO ports */ uint8_t hw_DSMxBind; HwFreedomDSMxBindGet(&hw_DSMxBind); /* Configure FlexiPort */ uint8_t hw_mainport; HwFreedomMainPortGet(&hw_mainport); switch (hw_mainport) { case HWFREEDOM_MAINPORT_DISABLED: break; case HWFREEDOM_MAINPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWFREEDOM_MAINPORT_GPS: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_RX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_MAINPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { PIOS_Board_configure_dsm(&pios_usart_dsm_hsum_main_cfg, &pios_dsm_main_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFREEDOM_MAINPORT_HOTTSUMD: case HWFREEDOM_MAINPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_mainport) { case HWFREEDOM_MAINPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFREEDOM_MAINPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_main_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFREEDOM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_MAINPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; case HWFREEDOM_MAINPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWFREEDOM_MAINPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_GPS) #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); pios_com_mavlink_id = pios_com_gps_id; #endif /* PIOS_INCLUDE_MAVLINK */ #endif /* PIOS_INCLUDE_GPS */ break; case HWFREEDOM_MAINPORT_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); #endif /* PIOS_INCLUDE_HOTT */ break; case HWFREEDOM_MAINPORT_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sensor_hub_id); #endif /* PIOS_INCLUDE_FRSKY_SENSOR_HUB */ break; case HWFREEDOM_MAINPORT_LIGHTTELEMETRYTX: #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_lighttelemetry_id); #endif break; case HWFREEDOM_MAINPORT_PICOC: #if defined(PIOS_INCLUDE_PICOC) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_PICOC_RX_BUF_LEN, PIOS_COM_PICOC_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_picoc_id); #endif /* PIOS_INCLUDE_PICOC */ break; case HWFREEDOM_MAINPORT_FRSKYSPORTTELEMETRY: #if defined(PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_FRSKYSPORT_RX_BUF_LEN, PIOS_COM_FRSKYSPORT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sport_id); #endif /* PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY */ break; } /* hw_freedom_mainport */ /* Configure flexi USART port */ uint8_t hw_flexiport; HwFreedomFlexiPortGet(&hw_flexiport); switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_DISABLED: break; case HWFREEDOM_FLEXIPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWFREEDOM_FLEXIPORT_GPS: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_RX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_FLEXIPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_hsum_flexi_cfg, &pios_dsm_flexi_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFREEDOM_FLEXIPORT_HOTTSUMD: case HWFREEDOM_FLEXIPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFREEDOM_FLEXIPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_flexi_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFREEDOM_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_I2C */ break; case HWFREEDOM_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_FLEXIPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; case HWFREEDOM_FLEXIPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWFREEDOM_FLEXIPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_GPS) #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); pios_com_mavlink_id = pios_com_gps_id; #endif /* PIOS_INCLUDE_MAVLINK */ #endif /* PIOS_INCLUDE_GPS */ break; case HWFREEDOM_FLEXIPORT_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); #endif /* PIOS_INCLUDE_HOTT */ break; case HWFREEDOM_FLEXIPORT_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sensor_hub_id); #endif /* PIOS_INCLUDE_FRSKY_SENSOR_HUB */ break; case HWFREEDOM_FLEXIPORT_LIGHTTELEMETRYTX: #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_lighttelemetry_id); #endif break; case HWFREEDOM_FLEXIPORT_PICOC: #if defined(PIOS_INCLUDE_PICOC) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_PICOC_RX_BUF_LEN, PIOS_COM_PICOC_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_picoc_id); #endif /* PIOS_INCLUDE_PICOC */ break; case HWFREEDOM_FLEXIPORT_FRSKYSPORTTELEMETRY: #if defined(PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_FRSKYSPORT_RX_BUF_LEN, PIOS_COM_FRSKYSPORT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sport_id); #endif /* PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY */ break; } /* hw_freedom_flexiport */ /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) RFM22BStatusInitialize(); RFM22BStatusCreateInstance(); // Initialize out status object. RFM22BStatusData rfm22bstatus; RFM22BStatusGet(&rfm22bstatus); RFM22BStatusInstSet(1,&rfm22bstatus); HwFreedomData hwFreedom; HwFreedomGet(&hwFreedom); // Initialize out status object. rfm22bstatus.BoardType = bdinfo->board_type; rfm22bstatus.BoardRevision = bdinfo->board_rev; if (hwFreedom.Radio == HWFREEDOM_RADIO_DISABLED || hwFreedom.MaxRfPower == HWFREEDOM_MAXRFPOWER_0) { // When radio disabled, it is ok for init to fail. This allows boards without populating // this component. const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg) == 0) { PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); rfm22bstatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); rfm22bstatus.BoardRevision = PIOS_RFM22B_ModuleVersion(pios_rfm22b_id); } else { pios_rfm22b_id = 0; } rfm22bstatus.LinkState = RFM22BSTATUS_LINKSTATE_DISABLED; } else { // always allow receiving PPM when radio is on bool ppm_mode = hwFreedom.Radio == HWFREEDOM_RADIO_TELEMPPM || hwFreedom.Radio == HWFREEDOM_RADIO_PPM; bool ppm_only = hwFreedom.Radio == HWFREEDOM_RADIO_PPM; bool is_oneway = hwFreedom.Radio == HWFREEDOM_RADIO_PPM; // Sparky2 can only receive PPM only /* Configure the RFM22B device. */ const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { panic(6); } rfm22bstatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); rfm22bstatus.BoardRevision = PIOS_RFM22B_ModuleVersion(pios_rfm22b_id); /* Configure the radio com interface */ uint8_t *rx_buffer = (uint8_t *)PIOS_malloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *)PIOS_malloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { panic(6); } /* Set Telemetry to use RFM22b if no other telemetry is configured (USB always overrides anyway) */ if (!pios_com_telem_rf_id) { pios_com_telem_rf_id = pios_com_rf_id; } rfm22bstatus.LinkState = RFM22BSTATUS_LINKSTATE_ENABLED; enum rfm22b_datarate datarate = RFM22_datarate_64000; switch (hwFreedom.MaxRfSpeed) { case HWFREEDOM_MAXRFSPEED_9600: datarate = RFM22_datarate_9600; break; case HWFREEDOM_MAXRFSPEED_19200: datarate = RFM22_datarate_19200; break; case HWFREEDOM_MAXRFSPEED_32000: datarate = RFM22_datarate_32000; break; case HWFREEDOM_MAXRFSPEED_64000: datarate = RFM22_datarate_64000; break; case HWFREEDOM_MAXRFSPEED_100000: datarate = RFM22_datarate_100000; break; case HWFREEDOM_MAXRFSPEED_192000: datarate = RFM22_datarate_192000; break; } /* Set the radio configuration parameters. */ PIOS_RFM22B_Config(pios_rfm22b_id, datarate, hwFreedom.MinChannel, hwFreedom.MaxChannel, hwFreedom.CoordID, is_oneway, ppm_mode, ppm_only); /* Set the modem Tx poer level */ switch (hwFreedom.MaxRfPower) { case HWFREEDOM_MAXRFPOWER_125: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); break; case HWFREEDOM_MAXRFPOWER_16: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); break; case HWFREEDOM_MAXRFPOWER_316: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); break; case HWFREEDOM_MAXRFPOWER_63: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); break; case HWFREEDOM_MAXRFPOWER_126: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); break; case HWFREEDOM_MAXRFPOWER_25: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); break; case HWFREEDOM_MAXRFPOWER_50: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); break; case HWFREEDOM_MAXRFPOWER_100: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); break; default: // do nothing break; } /* Reinitialize the modem. */ PIOS_RFM22B_Reinit(pios_rfm22b_id); #if defined(PIOS_INCLUDE_RFM22B_RCVR) { uintptr_t pios_rfm22brcvr_id; PIOS_RFM22B_Rcvr_Init(&pios_rfm22brcvr_id, pios_rfm22b_id); uintptr_t pios_rfm22brcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_rfm22brcvr_rcvr_id, &pios_rfm22b_rcvr_driver, pios_rfm22brcvr_id)) { panic(6); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_RFM22B] = pios_rfm22brcvr_rcvr_id; } } RFM22BStatusInstSet(1,&rfm22bstatus); #endif /* PIOS_INCLUDE_RFM22B_RCVR */ #endif /* PIOS_INCLUDE_RFM22B */ PIOS_WDG_Clear(); /* Configure input receiver USART port */ uint8_t hw_rcvrport; HwFreedomRcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_DISABLED: break; case HWFREEDOM_RCVRPORT_PPM: { uintptr_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); uintptr_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } break; case HWFREEDOM_RCVRPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_hsum_rcvr_cfg, &pios_dsm_rcvr_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMRCVRPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFREEDOM_RCVRPORT_HOTTSUMD: case HWFREEDOM_RCVRPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFREEDOM_RCVRPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_rcvr_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFREEDOM_RCVRPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_rcvr_cfg)) { PIOS_Assert(0); } uintptr_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } uintptr_t pios_sbus_rcvr_id; if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; } #endif break; } if (hw_rcvrport != HWFREEDOM_RCVRPORT_SBUS) { GPIO_Init(pios_sbus_cfg.inv.gpio, (GPIO_InitTypeDef*)&pios_sbus_cfg.inv.init); GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); } #if defined(PIOS_OVERO_SPI) /* Set up the SPI based PIOS_COM interface to the overo */ { bool overo_enabled = false; #ifdef MODULE_OveroSync_BUILTIN overo_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_OVEROSYNC] == MODULESETTINGS_ADMINSTATE_ENABLED) { overo_enabled = true; } else { overo_enabled = false; } #endif if (overo_enabled) { if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { PIOS_DEBUG_Assert(0); } const uint32_t PACKET_SIZE = 1024; uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PACKET_SIZE); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PACKET_SIZE); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_overo_id, &pios_overo_com_driver, pios_overo_id, rx_buffer, PACKET_SIZE, tx_buffer, PACKET_SIZE)) { PIOS_Assert(0); } } } #endif #if defined(PIOS_INCLUDE_GCSRCVR) GCSReceiverInitialize(); uintptr_t pios_gcsrcvr_id; PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); uintptr_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS uint8_t hw_output_port; HwFreedomOutputGet(&hw_output_port); switch (hw_output_port) { case HWFREEDOM_OUTPUT_DISABLED: break; case HWFREEDOM_OUTPUT_PWM: /* Set up the servo outputs */ PIOS_Servo_Init(&pios_servo_cfg); break; default: break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif PIOS_DELAY_WaitmS(200); PIOS_SENSORS_Init(); #if defined(PIOS_INCLUDE_MPU6000) if (PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg) != 0) panic(2); if (PIOS_MPU6000_Test() != 0) panic(2); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwFreedomGyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWFREEDOM_GYRORANGE_250: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWFREEDOM_GYRORANGE_500: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWFREEDOM_GYRORANGE_1000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWFREEDOM_GYRORANGE_2000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwFreedomAccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWFREEDOM_ACCELRANGE_2G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWFREEDOM_ACCELRANGE_4G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWFREEDOM_ACCELRANGE_8G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWFREEDOM_ACCELRANGE_16G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_16G); break; } // the filter has to be set before rate else divisor calculation will fail uint8_t hw_mpu6000_dlpf; HwFreedomMPU6000DLPFGet(&hw_mpu6000_dlpf); enum pios_mpu60x0_filter mpu6000_dlpf = \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ pios_mpu6000_cfg.default_filter; PIOS_MPU6000_SetLPF(mpu6000_dlpf); uint8_t hw_mpu6000_samplerate; HwFreedomMPU6000RateGet(&hw_mpu6000_samplerate); uint16_t mpu6000_samplerate = \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_200) ? 200 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_333) ? 333 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_500) ? 500 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_666) ? 666 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_1000) ? 1000 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_2000) ? 2000 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_4000) ? 4000 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_8000) ? 8000 : \ pios_mpu6000_cfg.default_samplerate; PIOS_MPU6000_SetSampleRate(mpu6000_samplerate); #endif if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_mag_pressure_adapter_id) != 0) panic(5); PIOS_DELAY_WaitmS(50); #if defined(PIOS_INCLUDE_ADC) uint32_t internal_adc_id; PIOS_INTERNAL_ADC_Init(&internal_adc_id, &pios_adc_cfg); PIOS_ADC_Init(&pios_internal_adc_id, &pios_internal_adc_driver, internal_adc_id); #endif PIOS_LED_On(0); #if defined(PIOS_INCLUDE_HMC5883) if (PIOS_HMC5883_Init(pios_i2c_mag_pressure_adapter_id, &pios_hmc5883_cfg) != 0) panic(3); #endif PIOS_LED_On(1); #if defined(PIOS_INCLUDE_MS5611) if (PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id) != 0) panic(4); #endif PIOS_LED_On(2); }
void PIOS_Board_Init(void) { /* Delay system */ PIOS_DELAY_Init(); const struct pios_board_info * bdinfo = &pios_board_info_blob; #if defined(PIOS_INCLUDE_LED) const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); PIOS_Assert(led_cfg); PIOS_LED_Init(led_cfg); #endif /* PIOS_INCLUDE_LED */ #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); }