/**
* 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;
}
