/** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ void PIOS_Board_Init(void) { /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); PIOS_LED_On(LED1); /* Delay system */ PIOS_DELAY_Init(); /* Communication system */ #if !defined(PIOS_ENABLE_DEBUG_PINS) #if defined(PIOS_INCLUDE_COM) if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id)) { PIOS_DEBUG_Assert(0); } #endif /* PIOS_INCLUDE_COM */ #endif /* IAP System Setup */ PIOS_IAP_Init(); /* ADC system */ PIOS_ADC_Init(); extern uint8_t adc_oversampling; PIOS_ADC_Config(adc_oversampling); extern void adc_callback(float *); PIOS_ADC_SetCallback(adc_callback); /* ADC buffer */ extern t_fifo_buffer adc_fifo_buffer; fifoBuf_init(&adc_fifo_buffer, adc_fifo_buf, sizeof(adc_fifo_buf)); /* Setup the Accelerometer FS (Full-Scale) GPIO */ PIOS_GPIO_Enable(0); SET_ACCEL_6G; #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) /* Magnetic sensor system */ if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) { PIOS_DEBUG_Assert(0); } PIOS_HMC5843_Init(); #endif #if defined(PIOS_INCLUDE_SPI) #include "ahrs_spi_comm.h" AhrsInitComms(); /* Set up the SPI interface to the OP board */ if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) { PIOS_DEBUG_Assert(0); } AhrsConnect(pios_spi_op_id); #endif }
/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t AHRSCommsInitialize(void) { AhrsInitComms(); // Start main task xTaskCreate(ahrscommsTask, (signed char*)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); return 0; }
/** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ void PIOS_Board_Init(void) { /* Remap AFIO pin */ //GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); /* Debug services */ PIOS_DEBUG_Init(); /* Delay system */ PIOS_DELAY_Init(); #if defined(PIOS_INCLUDE_SPI) /* Set up the SPI interface to the SD card */ if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) { PIOS_Assert(0); } /* Enable and mount the SDCard */ PIOS_SDCARD_Init(pios_spi_sdcard_id); PIOS_SDCARD_MountFS(0); #endif /* PIOS_INCLUDE_SPI */ /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); UAVObjectsInitializeAll(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* Initialize the alarms library */ AlarmsInitialize(); /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Prepare the AHRS Comms upper layer protocol */ AhrsInitComms(); /* Set up the SPI interface to the AHRS */ if (PIOS_SPI_Init(&pios_spi_ahrs_id, &pios_spi_ahrs_cfg)) { PIOS_Assert(0); } /* Bind the AHRS comms layer to the AHRS SPI link */ AhrsConnect(pios_spi_ahrs_id); /* Initialize the PiOS library */ #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_TELEMETRY_RF) uint32_t pios_usart_telem_rf_id; if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) { PIOS_Assert(0); } if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id)) { PIOS_Assert(0); } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ #if defined(PIOS_INCLUDE_GPS) uint32_t pios_usart_gps_id; if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { PIOS_Assert(0); } if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id)) { PIOS_Assert(0); } #endif /* PIOS_INCLUDE_GPS */ #endif PIOS_Servo_Init(); PIOS_ADC_Init(); PIOS_GPIO_Init(); #if defined(PIOS_INCLUDE_SPEKTRUM) #if (PIOS_SPEKTRUM_NUM_INPUTS > PIOS_RCVR_MAX_DEVS) #error More receiver inputs than available devices #endif /* SPEKTRUM init must come before comms */ PIOS_SPEKTRUM_Init(&pios_spektrum_cfg, false); uint32_t pios_usart_spektrum_id; if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) { PIOS_Assert(0); } for (uint8_t i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS && i < PIOS_RCVR_MAX_DEVS; i++) { if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel], &pios_spektrum_rcvr_driver, i)) { pios_rcvr_max_channel++; } else { PIOS_Assert(0); } } #endif #if defined(PIOS_INCLUDE_PWM) #if (PIOS_PWM_NUM_INPUTS > PIOS_RCVR_MAX_DEVS) #error More receiver inputs than available devices #endif PIOS_PWM_Init(); for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS && i < PIOS_RCVR_MAX_DEVS; i++) { if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel], &pios_pwm_rcvr_driver, i)) { pios_rcvr_max_channel++; } else { PIOS_Assert(0); } } #endif #if defined(PIOS_INCLUDE_PPM) #if (PIOS_PPM_NUM_INPUTS > PIOS_RCVR_MAX_DEVS) #error More receiver inputs than available devices #endif PIOS_PPM_Init(); for (uint8_t i = 0; i < PIOS_PPM_NUM_INPUTS && i < PIOS_RCVR_MAX_DEVS; i++) { if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel], &pios_ppm_rcvr_driver, i)) { pios_rcvr_max_channel++; } else { PIOS_Assert(0); } } #endif #if defined(PIOS_INCLUDE_USB_HID) PIOS_USB_HID_Init(0); #if defined(PIOS_INCLUDE_COM) if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_com_driver, 0)) { PIOS_Assert(0); } #endif /* PIOS_INCLUDE_COM */ #endif /* PIOS_INCLUDE_USB_HID */ #if defined(PIOS_INCLUDE_I2C) if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) { PIOS_Assert(0); } #endif /* PIOS_INCLUDE_I2C */ PIOS_IAP_Init(); PIOS_WDG_Init(); }
/** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ void PIOS_Board_Init(void) { /* Remap AFIO pin */ //GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); #ifdef PIOS_DEBUG_ENABLE_DEBUG_PINS PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */ /* Delay system */ PIOS_DELAY_Init(); #if defined(PIOS_INCLUDE_SPI) /* Set up the SPI interface to the SD card */ if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) { PIOS_Assert(0); } /* Enable and mount the SDCard */ PIOS_SDCARD_Init(pios_spi_sdcard_id); PIOS_SDCARD_MountFS(0); #endif /* PIOS_INCLUDE_SPI */ /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); HwSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* 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_3_cfg); PIOS_TIM_InitClock(&tim_5_cfg); PIOS_TIM_InitClock(&tim_4_cfg); PIOS_TIM_InitClock(&tim_8_cfg); /* Prepare the AHRS Comms upper layer protocol */ AhrsInitComms(); /* Set up the SPI interface to the AHRS */ if (PIOS_SPI_Init(&pios_spi_ahrs_id, &pios_spi_ahrs_cfg)) { PIOS_Assert(0); } /* Bind the AHRS comms layer to the AHRS SPI link */ AhrsConnect(pios_spi_ahrs_id); /* Configure the main IO port */ uint8_t hwsettings_op_mainport; HwSettingsOP_MainPortGet(&hwsettings_op_mainport); switch (hwsettings_op_mainport) { case HWSETTINGS_OP_MAINPORT_DISABLED: break; case HWSETTINGS_OP_MAINPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) { uint32_t pios_usart_telem_rf_id; if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_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_telem_rf_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; } /* Configure the flexi port */ uint8_t hwsettings_op_flexiport; HwSettingsOP_FlexiPortGet(&hwsettings_op_flexiport); switch (hwsettings_op_flexiport) { case HWSETTINGS_OP_FLEXIPORT_DISABLED: break; case HWSETTINGS_OP_FLEXIPORT_GPS: #if defined(PIOS_INCLUDE_GPS) { uint32_t pios_usart_gps_id; if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { PIOS_Assert(0); } uint8_t * 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_gps_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, NULL, 0)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_GPS */ break; } #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS PIOS_Servo_Init(&pios_servo_cfg); #endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */ PIOS_ADC_Init(); PIOS_GPIO_Init(); /* Configure the rcvr port */ uint8_t hwsettings_rcvrport; HwSettingsOP_RcvrPortGet(&hwsettings_rcvrport); switch (hwsettings_rcvrport) { case HWSETTINGS_OP_RCVRPORT_DISABLED: break; case HWSETTINGS_OP_RCVRPORT_DEBUG: /* Not supported yet */ break; case HWSETTINGS_OP_RCVRPORT_DSM2: case HWSETTINGS_OP_RCVRPORT_DSMX10BIT: case HWSETTINGS_OP_RCVRPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) { enum pios_dsm_proto proto; switch (hwsettings_rcvrport) { case HWSETTINGS_OP_RCVRPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWSETTINGS_OP_RCVRPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWSETTINGS_OP_RCVRPORT_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_cfg)) { PIOS_Assert(0); } uint32_t pios_dsm_id; if (PIOS_DSM_Init(&pios_dsm_id, &pios_dsm_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 break; case HWSETTINGS_OP_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_OP_RCVRPORT_PPM: #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; } #if defined(PIOS_INCLUDE_USB_HID) uint32_t pios_usb_hid_id; PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_main_cfg); #if defined(PIOS_INCLUDE_COM) uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_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 */ #endif /* PIOS_INCLUDE_USB_HID */ #if defined(PIOS_INCLUDE_I2C) if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) { PIOS_Assert(0); } #endif /* PIOS_INCLUDE_I2C */ PIOS_IAP_Init(); PIOS_WDG_Init(); }
/** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ void PIOS_Board_Init(void) { /* Remap AFIO pin */ //GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); #ifdef PIOS_DEBUG_ENABLE_DEBUG_PINS PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */ /* Delay system */ PIOS_DELAY_Init(); #if defined(PIOS_INCLUDE_SPI) /* Set up the SPI interface to the SD card */ if (PIOS_SPI_Init(&pios_spi_sdcard_id, &pios_spi_sdcard_cfg)) { PIOS_Assert(0); } /* Enable and mount the SDCard */ PIOS_SDCARD_Init(pios_spi_sdcard_id); PIOS_SDCARD_MountFS(0); #endif /* PIOS_INCLUDE_SPI */ /* 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 #if defined(PIOS_INCLUDE_LED) PIOS_LED_Init(&pios_led_cfg); #endif /* PIOS_INCLUDE_LED */ HwSettingsInitialize(); PIOS_WDG_Init(); /* Initialize the alarms library */ AlarmsInitialize(); PIOS_IAP_Init(); uint16_t boot_count = PIOS_IAP_ReadBootCount(); if (boot_count < 3) { PIOS_IAP_WriteBootCount(++boot_count); AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); } else { /* Too many failed boot attempts, force hwsettings to defaults */ HwSettingsSetDefaults(HwSettingsHandle(), 0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_3_cfg); PIOS_TIM_InitClock(&tim_5_cfg); PIOS_TIM_InitClock(&tim_4_cfg); PIOS_TIM_InitClock(&tim_8_cfg); /* Prepare the AHRS Comms upper layer protocol */ AhrsInitComms(); /* Set up the SPI interface to the AHRS */ if (PIOS_SPI_Init(&pios_spi_ahrs_id, &pios_spi_ahrs_cfg)) { PIOS_Assert(0); } /* Bind the AHRS comms layer to the AHRS SPI link */ AhrsConnect(pios_spi_ahrs_id); #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; uint8_t hwsettings_usb_devicetype; HwSettingsUSB_DeviceTypeGet(&hwsettings_usb_devicetype); switch (hwsettings_usb_devicetype) { case HWSETTINGS_USB_DEVICETYPE_HIDONLY: if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; break; case HWSETTINGS_USB_DEVICETYPE_HIDVCP: if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; break; case HWSETTINGS_USB_DEVICETYPE_VCPONLY: break; default: PIOS_Assert(0); } uint32_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_CDC) /* Configure the USB VCP port */ uint8_t hwsettings_usb_vcpport; HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED; } switch (hwsettings_usb_vcpport) { case HWSETTINGS_USB_VCPPORT_DISABLED: break; case HWSETTINGS_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint32_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWSETTINGS_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { uint32_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hwsettings_usb_hidport; HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED; } switch (hwsettings_usb_hidport) { case HWSETTINGS_USB_HIDPORT_DISABLED: break; case HWSETTINGS_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint32_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_HID */ #endif /* PIOS_INCLUDE_USB */ /* Configure the main IO port */ uint8_t hwsettings_op_mainport; HwSettingsOP_MainPortGet(&hwsettings_op_mainport); switch (hwsettings_op_mainport) { case HWSETTINGS_OP_MAINPORT_DISABLED: break; case HWSETTINGS_OP_MAINPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) { uint32_t pios_usart_telem_rf_id; if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_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_telem_rf_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; } /* Configure the flexi port */ uint8_t hwsettings_op_flexiport; HwSettingsOP_FlexiPortGet(&hwsettings_op_flexiport); switch (hwsettings_op_flexiport) { case HWSETTINGS_OP_FLEXIPORT_DISABLED: break; case HWSETTINGS_OP_FLEXIPORT_GPS: #if defined(PIOS_INCLUDE_GPS) { uint32_t pios_usart_gps_id; if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { PIOS_Assert(0); } uint8_t * 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_gps_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, NULL, 0)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_GPS */ break; } #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS PIOS_Servo_Init(&pios_servo_cfg); #endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */ PIOS_ADC_Init(); PIOS_GPIO_Init(); /* Configure the rcvr port */ uint8_t hwsettings_rcvrport; HwSettingsOP_RcvrPortGet(&hwsettings_rcvrport); switch (hwsettings_rcvrport) { case HWSETTINGS_OP_RCVRPORT_DISABLED: break; case HWSETTINGS_OP_RCVRPORT_DEBUG: /* Not supported yet */ break; case HWSETTINGS_OP_RCVRPORT_DSM2: case HWSETTINGS_OP_RCVRPORT_DSMX10BIT: case HWSETTINGS_OP_RCVRPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) { enum pios_dsm_proto proto; switch (hwsettings_rcvrport) { case HWSETTINGS_OP_RCVRPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWSETTINGS_OP_RCVRPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWSETTINGS_OP_RCVRPORT_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_cfg)) { PIOS_Assert(0); } uint32_t pios_dsm_id; if (PIOS_DSM_Init(&pios_dsm_id, &pios_dsm_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 break; case HWSETTINGS_OP_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_OP_RCVRPORT_PPM: #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; } #if defined(PIOS_INCLUDE_I2C) if (PIOS_I2C_Init(&pios_i2c_main_adapter_id, &pios_i2c_main_adapter_cfg)) { PIOS_Assert(0); } #endif /* PIOS_INCLUDE_I2C */ /* 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); }
int main() { float gyro[3], accel[3], mag[3]; float vel[3] = { 0, 0, 0 }; /* Normaly we get/set UAVObjects but this one only needs to be set. We will never expect to get this from another module*/ AttitudeActualData attitude_actual; AHRSSettingsData ahrs_settings; /* Brings up System using CMSIS functions, enables the LEDs. */ PIOS_SYS_Init(); /* Delay system */ PIOS_DELAY_Init(); /* Communication system */ PIOS_COM_Init(); /* ADC system */ AHRS_ADC_Config( adc_oversampling ); /* Setup the Accelerometer FS (Full-Scale) GPIO */ PIOS_GPIO_Enable( 0 ); SET_ACCEL_2G; #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) /* Magnetic sensor system */ PIOS_I2C_Init(); PIOS_HMC5843_Init(); // Get 3 ID bytes strcpy(( char * )mag_data.id, "ZZZ" ); PIOS_HMC5843_ReadID( mag_data.id ); #endif /* SPI link to master */ // PIOS_SPI_Init(); AhrsInitComms(); AHRSCalibrationConnectCallback( calibration_callback ); GPSPositionConnectCallback( gps_callback ); ahrs_state = AHRS_IDLE; while( !AhrsLinkReady() ) { AhrsPoll(); while( ahrs_state != AHRS_DATA_READY ) ; ahrs_state = AHRS_PROCESSING; downsample_data(); ahrs_state = AHRS_IDLE; if(( total_conversion_blocks % 50 ) == 0 ) PIOS_LED_Toggle( LED1 ); } AHRSSettingsGet(&ahrs_settings); /* Use simple averaging filter for now */ for( int i = 0; i < adc_oversampling; i++ ) fir_coeffs[i] = 1; fir_coeffs[adc_oversampling] = adc_oversampling; if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS) { // compute a data point and initialize INS downsample_data(); converge_insgps(); } #ifdef DUMP_RAW int previous_conversion; while( 1 ) { AhrsPoll(); int result; uint8_t framing[16] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }; while( ahrs_state != AHRS_DATA_READY ) ; ahrs_state = AHRS_PROCESSING; if( total_conversion_blocks != previous_conversion + 1 ) PIOS_LED_On( LED1 ); // not keeping up else PIOS_LED_Off( LED1 ); previous_conversion = total_conversion_blocks; downsample_data(); ahrs_state = AHRS_IDLE;; // Dump raw buffer result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 ); // framing header result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) ); // dump block number result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & valid_data_buffer[0], ADC_OVERSAMPLE * ADC_CONTINUOUS_CHANNELS * sizeof( valid_data_buffer[0] ) ); if( result == 0 ) PIOS_LED_Off( LED1 ); else { PIOS_LED_On( LED1 ); } } #endif timer_start(); /******************* Main EKF loop ****************************/ while( 1 ) { AhrsPoll(); AHRSCalibrationData calibration; AHRSCalibrationGet( &calibration ); BaroAltitudeData baro_altitude; BaroAltitudeGet( &baro_altitude ); GPSPositionData gps_position; GPSPositionGet( &gps_position ); AHRSSettingsGet(&ahrs_settings); // Alive signal if(( total_conversion_blocks % 100 ) == 0 ) PIOS_LED_Toggle( LED1 ); #if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C) // Get magnetic readings if( PIOS_HMC5843_NewDataAvailable() ) { PIOS_HMC5843_ReadMag( mag_data.raw.axis ); mag_data.updated = 1; } attitude_raw.magnetometers[0] = mag_data.raw.axis[0]; attitude_raw.magnetometers[2] = mag_data.raw.axis[1]; attitude_raw.magnetometers[2] = mag_data.raw.axis[2]; #endif // Delay for valid data counter_val = timer_count(); running_counts = counter_val - last_counter_idle_end; last_counter_idle_start = counter_val; while( ahrs_state != AHRS_DATA_READY ) ; counter_val = timer_count(); idle_counts = counter_val - last_counter_idle_start; last_counter_idle_end = counter_val; ahrs_state = AHRS_PROCESSING; downsample_data(); /***************** SEND BACK SOME RAW DATA ************************/ // Hacky - grab one sample from buffer to populate this. Need to send back // all raw data if it's happening accel_data.raw.x = valid_data_buffer[0]; accel_data.raw.y = valid_data_buffer[2]; accel_data.raw.z = valid_data_buffer[4]; gyro_data.raw.x = valid_data_buffer[1]; gyro_data.raw.y = valid_data_buffer[3]; gyro_data.raw.z = valid_data_buffer[5]; gyro_data.temp.xy = valid_data_buffer[6]; gyro_data.temp.z = valid_data_buffer[7]; if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_INSGPS) { /******************** INS ALGORITHM **************************/ // format data for INS algo gyro[0] = gyro_data.filtered.x; gyro[1] = gyro_data.filtered.y; gyro[2] = gyro_data.filtered.z; accel[0] = accel_data.filtered.x, accel[1] = accel_data.filtered.y, accel[2] = accel_data.filtered.z, // Note: The magnetometer driver returns registers X,Y,Z from the chip which are // (left, backward, up). Remapping to (forward, right, down). mag[0] = -( mag_data.raw.axis[1] - calibration.mag_bias[1] ); mag[1] = -( mag_data.raw.axis[0] - calibration.mag_bias[0] ); mag[2] = -( mag_data.raw.axis[2] - calibration.mag_bias[2] ); INSStatePrediction( gyro, accel, 1 / ( float )EKF_RATE ); INSCovariancePrediction( 1 / ( float )EKF_RATE ); if( gps_updated && gps_position.Status == GPSPOSITION_STATUS_FIX3D ) { // Compute velocity from Heading and groundspeed vel[0] = gps_position.Groundspeed * cos( gps_position.Heading * M_PI / 180 ); vel[1] = gps_position.Groundspeed * sin( gps_position.Heading * M_PI / 180 ); // Completely unprincipled way to make the position variance // increase as data quality decreases but keep it bounded // Variance becomes 40 m^2 and 40 (m/s)^2 when no gps INSSetPosVelVar( 0.004 ); HomeLocationData home; HomeLocationGet( &home ); float ned[3]; double lla[3] = {( double ) gps_position.Latitude / 1e7, ( double ) gps_position.Longitude / 1e7, ( double )( gps_position.GeoidSeparation + gps_position.Altitude )}; // convert from cm back to meters double ecef[3] = {( double )( home.ECEF[0] / 100 ), ( double )( home.ECEF[1] / 100 ), ( double )( home.ECEF[2] / 100 )}; LLA2Base( lla, ecef, ( float( * )[3] ) home.RNE, ned ); if( gps_updated ) { //FIXME: Is this correct? //TOOD: add check for altitude updates FullCorrection( mag, ned, vel, baro_altitude.Altitude ); gps_updated = false; } else { GpsBaroCorrection( ned, vel, baro_altitude.Altitude ); } gps_updated = false; mag_data.updated = 0; } else if( gps_position.Status == GPSPOSITION_STATUS_FIX3D && mag_data.updated == 1 ) { MagCorrection( mag ); // only trust mags if outdoors mag_data.updated = 0; } else { // Indoors, update with zero position and velocity and high covariance INSSetPosVelVar( 0.1 ); vel[0] = 0; vel[1] = 0; vel[2] = 0; VelBaroCorrection( vel, baro_altitude.Altitude ); // MagVelBaroCorrection(mag,vel,altitude_data.altitude); // only trust mags if outdoors } attitude_actual.q1 = Nav.q[0]; attitude_actual.q2 = Nav.q[1]; attitude_actual.q3 = Nav.q[2]; attitude_actual.q4 = Nav.q[3]; } else if( ahrs_settings.Algorithm == AHRSSETTINGS_ALGORITHM_SIMPLE ) { float q[4]; float rpy[3]; /***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/ /* Very simple computation of the heading and attitude from accel. */ rpy[2] = atan2(( mag_data.raw.axis[0] ), ( -1 * mag_data.raw.axis[1] ) ) * 180 / M_PI; rpy[1] = atan2( accel_data.filtered.x, accel_data.filtered.z ) * 180 / M_PI; rpy[0] = atan2( accel_data.filtered.y, accel_data.filtered.z ) * 180 / M_PI; RPY2Quaternion( rpy, q ); attitude_actual.q1 = q[0]; attitude_actual.q2 = q[1]; attitude_actual.q3 = q[2]; attitude_actual.q4 = q[3]; } ahrs_state = AHRS_IDLE; #ifdef DUMP_FRIENDLY PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "b: %d\r\n", total_conversion_blocks ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "a: %d %d %d\r\n", ( int16_t )( accel_data.filtered.x * 1000 ), ( int16_t )( accel_data.filtered.y * 1000 ), ( int16_t )( accel_data.filtered.z * 1000 ) ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "g: %d %d %d\r\n", ( int16_t )( gyro_data.filtered.x * 1000 ), ( int16_t )( gyro_data.filtered.y * 1000 ), ( int16_t )( gyro_data.filtered.z * 1000 ) ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "m: %d %d %d\r\n", mag_data.raw.axis[0], mag_data.raw.axis[1], mag_data.raw.axis[2] ); PIOS_COM_SendFormattedStringNonBlocking( PIOS_COM_AUX, "q: %d %d %d %d\r\n", ( int16_t )( Nav.q[0] * 1000 ), ( int16_t )( Nav.q[1] * 1000 ), ( int16_t )( Nav.q[2] * 1000 ), ( int16_t )( Nav.q[3] * 1000 ) ); #endif #ifdef DUMP_EKF uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 }; extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances extern float K[NUMX][NUMV]; // feedback gain matrix // Dump raw buffer int8_t result; result = PIOS_COM_SendBuffer( PIOS_COM_AUX, &framing[0], 16 ); // framing header result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & total_conversion_blocks, sizeof( total_conversion_blocks ) ); // dump block number result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & mag_data, sizeof( mag_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & gps_data, sizeof( gps_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & accel_data, sizeof( accel_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & gyro_data, sizeof( gyro_data ) ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & Q, sizeof( float ) * NUMX * NUMX ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & K, sizeof( float ) * NUMX * NUMV ); result += PIOS_COM_SendBuffer( PIOS_COM_AUX, ( uint8_t * ) & X, sizeof( float ) * NUMX * NUMX ); if( result == 0 ) PIOS_LED_Off( LED1 ); else { PIOS_LED_On( LED1 ); } #endif AttitudeActualSet( &attitude_actual ); /*FIXME: This is dangerous. There is no locking for UAVObjects so it could stomp all over the airspeed/climb rate etc. This used to be done in the OP module which was bad. Having ~4ms latency for the round trip makes it worse here. */ PositionActualData pos; PositionActualGet( &pos ); for( int ct = 0; ct < 3; ct++ ) { pos.NED[ct] = Nav.Pos[ct]; pos.Vel[ct] = Nav.Vel[ct]; } PositionActualSet( &pos ); static bool was_calibration = false; AhrsStatusData status; AhrsStatusGet( &status ); if( was_calibration != status.CalibrationSet ) { was_calibration = status.CalibrationSet; if( status.CalibrationSet ) { calibrate_sensors(); AhrsStatusGet( &status ); status.CalibrationSet = true; } } status.CPULoad = (( float )running_counts / ( float )( idle_counts + running_counts ) ) * 100; status.IdleTimePerCyle = idle_counts / ( TIMER_RATE / 10000 ); status.RunningTimePerCyle = running_counts / ( TIMER_RATE / 10000 ); status.DroppedUpdates = ekf_too_slow; AhrsStatusSet( &status ); } return 0; }