flight/targets/boards/revolution/firmware/pios_board.c

   1 /**
   2  ******************************************************************************
   3  * @file       pios_board.c
   4  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
   5  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
   6  *             PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012
   7  * @addtogroup OpenPilotSystem OpenPilot System
   8  * @{
   9  * @addtogroup OpenPilotCore OpenPilot Core
  10  * @{
  11  * @brief Defines board specific static initializers for hardware for the revomini board.
  12  *****************************************************************************/
  13 /*
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License as published by
  16  * the Free Software Foundation; either version 3 of the License, or
  17  * (at your option) any later version.
  18  *
  19  * This program is distributed in the hope that it will be useful, but
  20  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  21  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  22  * for more details.
  23  *
  24  * You should have received a copy of the GNU General Public License along
  25  * with this program; if not, write to the Free Software Foundation, Inc.,
  26  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  27  */
  28
  29 #include "inc/openpilot.h"
  30 #include <pios_board_info.h>
  31 #include <uavobjectsinit.h>
  32 #include <hwsettings.h>
  33 #include <manualcontrolsettings.h>
  34 #include <oplinksettings.h>
  35 #include <oplinkstatus.h>
  36 #include <oplinkreceiver.h>
  37 #include <pios_oplinkrcvr_priv.h>
  38 #include <pios_openlrs.h>
  39 #include <pios_openlrs_rcvr_priv.h>
  40 #include <taskinfo.h>
  41 #include <pios_ws2811.h>
  42 #include <auxmagsettings.h>
  43
  44 #ifdef PIOS_INCLUDE_INSTRUMENTATION
  45 #include <pios_instrumentation.h>
  46 #endif
  47
  48 /*
  49  * Pull in the board-specific static HW definitions.
  50  * Including .c files is a bit ugly but this allows all of
  51  * the HW definitions to be const and static to limit their
  52  * scope.
  53  *
  54  * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
  55  */
  56 #include "../board_hw_defs.c"
  57
  58 /**
  59  * Sensor configurations
  60  */
  61 #if defined(PIOS_INCLUDE_ADC)
  62 #include "pios_adc_priv.h"
  63 void PIOS_ADC_DMC_irq_handler(void);
  64 void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler")));
  65
  66 struct pios_adc_cfg pios_adc_cfg = {
  67     .adc_dev = ADC1,
  68     .dma     = {
  69         .irq                                       = {
  70             .flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4),
  71             .init  = {
  72                 .NVIC_IRQChannel    = DMA2_Stream4_IRQn,
  73                 .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
  74                 .NVIC_IRQChannelSubPriority        = 0,
  75                 .NVIC_IRQChannelCmd = ENABLE,
  76             },
  77         },
  78         .rx                                        = {
  79             .channel = DMA2_Stream4,
  80             .init    = {
  81                 .DMA_Channel                       = DMA_Channel_0,
  82                 .DMA_PeripheralBaseAddr            = (uint32_t)&ADC1->DR
  83             },
  84         }
  85     },
  86     .half_flag = DMA_IT_HTIF4,
  87     .full_flag = DMA_IT_TCIF4,
  88 };
  89
  90 void PIOS_ADC_DMC_irq_handler(void)
  91 {
  92     /* Call into the generic code to handle the IRQ for this specific device */
  93     PIOS_ADC_DMA_Handler();
  94 }
  95 #endif /* if defined(PIOS_INCLUDE_ADC) */
  96
  97 #if defined(PIOS_INCLUDE_HMC5X83)
  98 #include "pios_hmc5x83.h"
  99 pios_hmc5x83_dev_t onboard_mag  = 0;
 100 pios_hmc5x83_dev_t external_mag = 0;
 101
 102 #ifdef PIOS_HMC5X83_HAS_GPIOS
 103 bool pios_board_internal_mag_handler()
 104 {
 105     return PIOS_HMC5x83_IRQHandler(onboard_mag);
 106 }
 107
 108 static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = {
 109     .vector = pios_board_internal_mag_handler,
 110     .line   = EXTI_Line7,
 111     .pin    = {
 112         .gpio = GPIOB,
 113         .init = {
 114             .GPIO_Pin   = GPIO_Pin_7,
 115             .GPIO_Speed = GPIO_Speed_100MHz,
 116             .GPIO_Mode  = GPIO_Mode_IN,
 117             .GPIO_OType = GPIO_OType_OD,
 118             .GPIO_PuPd  = GPIO_PuPd_NOPULL,
 119         },
 120     },
 121     .irq                                       = {
 122         .init                                  = {
 123             .NVIC_IRQChannel    = EXTI9_5_IRQn,
 124             .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
 125             .NVIC_IRQChannelSubPriority        = 0,
 126             .NVIC_IRQChannelCmd = ENABLE,
 127         },
 128     },
 129     .exti                                      = {
 130         .init                                  = {
 131             .EXTI_Line    = EXTI_Line7, // matches above GPIO pin
 132             .EXTI_Mode    = EXTI_Mode_Interrupt,
 133             .EXTI_Trigger = EXTI_Trigger_Rising,
 134             .EXTI_LineCmd = ENABLE,
 135         },
 136     },
 137 };
 138 #endif /* PIOS_HMC5X83_HAS_GPIOS */
 139
 140 static const struct pios_hmc5x83_cfg pios_hmc5x83_internal_cfg = {
 141 #ifdef PIOS_HMC5X83_HAS_GPIOS
 142     .exti_cfg  = &pios_exti_hmc5x83_cfg,
 143 #endif
 144     .M_ODR     = PIOS_HMC5x83_ODR_75,
 145     .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
 146     .Gain   = PIOS_HMC5x83_GAIN_1_9,
 147     .Mode   = PIOS_HMC5x83_MODE_CONTINUOUS,
 148     .TempCompensation = false,
 149     .Driver = &PIOS_HMC5x83_I2C_DRIVER,
 150     .Orientation      = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP,
 151 };
 152
 153 static const struct pios_hmc5x83_cfg pios_hmc5x83_external_cfg = {
 154 #ifdef PIOS_HMC5X83_HAS_GPIOS
 155     .exti_cfg  = NULL,
 156 #endif
 157     .M_ODR     = PIOS_HMC5x83_ODR_75, // if you change this for auxmag, change AUX_MAG_SKIP in sensors.c
 158     .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL,
 159     .Gain   = PIOS_HMC5x83_GAIN_1_9,
 160     .Mode   = PIOS_HMC5x83_MODE_CONTINUOUS,
 161     .TempCompensation = false,
 162     .Driver = &PIOS_HMC5x83_I2C_DRIVER,
 163     .Orientation      = PIOS_HMC5X83_ORIENTATION_EAST_NORTH_UP, // ENU for GPSV9, WND for typical I2C mag
 164 };
 165 #endif /* PIOS_INCLUDE_HMC5X83 */
 166
 167 /**
 168  * Configuration for the MS5611 chip
 169  */
 170 #if defined(PIOS_INCLUDE_MS5611)
 171 #include "pios_ms5611.h"
 172 static const struct pios_ms5611_cfg pios_ms5611_cfg = {
 173     .oversampling = MS5611_OSR_4096,
 174 };
 175 #endif /* PIOS_INCLUDE_MS5611 */
 176
 177
 178 /**
 179  * Configuration for the MPU6000 chip
 180  */
 181 #if defined(PIOS_INCLUDE_MPU6000)
 182 #include "pios_mpu6000.h"
 183 #include "pios_mpu6000_config.h"
 184 static const struct pios_exti_cfg pios_exti_mpu6000_cfg __exti_config = {
 185     .vector = PIOS_MPU6000_IRQHandler,
 186     .line   = EXTI_Line4,
 187     .pin    = {
 188         .gpio = GPIOC,
 189         .init = {
 190             .GPIO_Pin   = GPIO_Pin_4,
 191             .GPIO_Speed = GPIO_Speed_100MHz,
 192             .GPIO_Mode  = GPIO_Mode_IN,
 193             .GPIO_OType = GPIO_OType_OD,
 194             .GPIO_PuPd  = GPIO_PuPd_NOPULL,
 195         },
 196     },
 197     .irq                                       = {
 198         .init                                  = {
 199             .NVIC_IRQChannel    = EXTI4_IRQn,
 200             .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
 201             .NVIC_IRQChannelSubPriority        = 0,
 202             .NVIC_IRQChannelCmd = ENABLE,
 203         },
 204     },
 205     .exti                                      = {
 206         .init                                  = {
 207             .EXTI_Line    = EXTI_Line4, // matches above GPIO pin
 208             .EXTI_Mode    = EXTI_Mode_Interrupt,
 209             .EXTI_Trigger = EXTI_Trigger_Rising,
 210             .EXTI_LineCmd = ENABLE,
 211         },
 212     },
 213 };
 214
 215 static const struct pios_mpu6000_cfg pios_mpu6000_cfg = {
 216     .exti_cfg   = &pios_exti_mpu6000_cfg,
 217     .Fifo_store = PIOS_MPU6000_FIFO_TEMP_OUT | PIOS_MPU6000_FIFO_GYRO_X_OUT | PIOS_MPU6000_FIFO_GYRO_Y_OUT | PIOS_MPU6000_FIFO_GYRO_Z_OUT,
 218     // Clock at 8 khz
 219     .Smpl_rate_div_no_dlp = 0,
 220     // with dlp on output rate is 1000Hz
 221     .Smpl_rate_div_dlp    = 0,
 222     .interrupt_cfg  = PIOS_MPU6000_INT_CLR_ANYRD,
 223     .interrupt_en   = PIOS_MPU6000_INTEN_DATA_RDY,
 224     .User_ctl             = PIOS_MPU6000_USERCTL_DIS_I2C,
 225     .Pwr_mgmt_clk   = PIOS_MPU6000_PWRMGMT_PLL_X_CLK,
 226     .accel_range    = PIOS_MPU6000_ACCEL_8G,
 227     .gyro_range     = PIOS_MPU6000_SCALE_2000_DEG,
 228     .filter               = PIOS_MPU6000_LOWPASS_256_HZ,
 229     .orientation    = PIOS_MPU6000_TOP_180DEG,
 230     .fast_prescaler = PIOS_SPI_PRESCALER_4,
 231     .std_prescaler  = PIOS_SPI_PRESCALER_64,
 232     .max_downsample = 20,
 233 };
 234 #endif /* PIOS_INCLUDE_MPU6000 */
 235
 236 /* One slot per selectable receiver group.
 237  *  eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS
 238  * NOTE: No slot in this map for NONE.
 239  */
 240 uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE];
 241
 242 #define PIOS_COM_TELEM_RF_RX_BUF_LEN     512
 243 #define PIOS_COM_TELEM_RF_TX_BUF_LEN     512
 244
 245 #define PIOS_COM_GPS_RX_BUF_LEN          128
 246 #define PIOS_COM_GPS_TX_BUF_LEN          32
 247
 248 #define PIOS_COM_TELEM_USB_RX_BUF_LEN    65
 249 #define PIOS_COM_TELEM_USB_TX_BUF_LEN    65
 250
 251 #define PIOS_COM_BRIDGE_RX_BUF_LEN       65
 252 #define PIOS_COM_BRIDGE_TX_BUF_LEN       12
 253
 254 #define PIOS_COM_HOTT_RX_BUF_LEN         512
 255 #define PIOS_COM_HOTT_TX_BUF_LEN         512
 256
 257 #define PIOS_COM_RFM22B_RF_RX_BUF_LEN    512
 258 #define PIOS_COM_RFM22B_RF_TX_BUF_LEN    512
 259
 260 #define PIOS_COM_HKOSD_RX_BUF_LEN        22
 261 #define PIOS_COM_HKOSD_TX_BUF_LEN        22
 262
 263 #define PIOS_COM_MSP_TX_BUF_LEN          128
 264 #define PIOS_COM_MSP_RX_BUF_LEN          64
 265 #define PIOS_COM_MAVLINK_TX_BUF_LEN      128
 266 #define PIOS_COM_MAVLINK_RX_BUF_LEN      128
 267
 268 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
 269 #define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40
 270 uint32_t pios_com_debug_id;
 271 #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
 272
 273 uint32_t pios_com_gps_id       = 0;
 274 uint32_t pios_com_telem_usb_id = 0;
 275 uint32_t pios_com_telem_rf_id  = 0;
 276 uint32_t pios_com_rf_id        = 0;
 277 uint32_t pios_com_bridge_id    = 0;
 278 uint32_t pios_com_hott_id      = 0;
 279 uint32_t pios_com_overo_id     = 0;
 280 uint32_t pios_com_hkosd_id     = 0;
 281 uint32_t pios_com_vcp_id       = 0;
 282 uint32_t pios_com_msp_id       = 0;
 283 uint32_t pios_com_mavlink_id   = 0;
 284
 285 #if defined(PIOS_INCLUDE_RFM22B)
 286 uint32_t pios_rfm22b_id        = 0;
 287 #include <pios_rfm22b_com.h>
 288 #endif
 289
 290 uintptr_t pios_uavo_settings_fs_id;
 291 uintptr_t pios_user_fs_id;
 292
 293 /*
 294  * Setup a com port based on the passed cfg, driver and buffer sizes.
 295  * tx size = 0 make the port rx only
 296  * rx size = 0 make the port tx only
 297  * having both tx and rx size = 0 is not valid and will fail further down in PIOS_COM_Init()
 298  */
 299 static void PIOS_Board_configure_com(const struct pios_usart_cfg *usart_port_cfg, uint16_t rx_buf_len, uint16_t tx_buf_len,
 300                                      const struct pios_com_driver *com_driver, uint32_t *pios_com_id)
 301 {
 302     uint32_t pios_usart_id;
 303
 304     if (PIOS_USART_Init(&pios_usart_id, usart_port_cfg)) {
 305         PIOS_Assert(0);
 306     }
 307
 308     uint8_t *rx_buffer = 0, *tx_buffer = 0;
 309
 310     if (rx_buf_len > 0) {
 311         rx_buffer = (uint8_t *)pios_malloc(rx_buf_len);
 312         PIOS_Assert(rx_buffer);
 313     }
 314
 315     if (tx_buf_len > 0) {
 316         tx_buffer = (uint8_t *)pios_malloc(tx_buf_len);
 317         PIOS_Assert(tx_buffer);
 318     }
 319
 320     if (PIOS_COM_Init(pios_com_id, com_driver, pios_usart_id,
 321                       rx_buffer, rx_buf_len,
 322                       tx_buffer, tx_buf_len)) {
 323         PIOS_Assert(0);
 324     }
 325 }
 326
 327 static void PIOS_Board_configure_dsm(const struct pios_usart_cfg *pios_usart_dsm_cfg, const struct pios_dsm_cfg *pios_dsm_cfg,
 328                                      const struct pios_com_driver *usart_com_driver,
 329                                      ManualControlSettingsChannelGroupsOptions channelgroup, uint8_t *bind)
 330 {
 331     uint32_t pios_usart_dsm_id;
 332
 333     if (PIOS_USART_Init(&pios_usart_dsm_id, pios_usart_dsm_cfg)) {
 334         PIOS_Assert(0);
 335     }
 336
 337     uint32_t pios_dsm_id;
 338     if (PIOS_DSM_Init(&pios_dsm_id, pios_dsm_cfg, usart_com_driver,
 339                       pios_usart_dsm_id, *bind)) {
 340         PIOS_Assert(0);
 341     }
 342
 343     uint32_t pios_dsm_rcvr_id;
 344     if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) {
 345         PIOS_Assert(0);
 346     }
 347     pios_rcvr_group_map[channelgroup] = pios_dsm_rcvr_id;
 348 }
 349
 350 static void PIOS_Board_configure_ibus(const struct pios_usart_cfg *usart_cfg)
 351 {
 352     uint32_t pios_usart_ibus_id;
 353
 354     if (PIOS_USART_Init(&pios_usart_ibus_id, usart_cfg)) {
 355         PIOS_Assert(0);
 356     }
 357
 358     uint32_t pios_ibus_id;
 359     if (PIOS_IBUS_Init(&pios_ibus_id, &pios_usart_com_driver, pios_usart_ibus_id)) {
 360         PIOS_Assert(0);
 361     }
 362
 363     uint32_t pios_ibus_rcvr_id;
 364     if (PIOS_RCVR_Init(&pios_ibus_rcvr_id, &pios_ibus_rcvr_driver, pios_ibus_id)) {
 365         PIOS_Assert(0);
 366     }
 367
 368     pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_IBUS] = pios_ibus_rcvr_id;
 369 }
 370
 371 static void PIOS_Board_configure_pwm(const struct pios_pwm_cfg *pwm_cfg)
 372 {
 373     /* Set up the receiver port.  Later this should be optional */
 374     uint32_t pios_pwm_id;
 375
 376     PIOS_PWM_Init(&pios_pwm_id, pwm_cfg);
 377
 378     uint32_t pios_pwm_rcvr_id;
 379     if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) {
 380         PIOS_Assert(0);
 381     }
 382     pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id;
 383 }
 384
 385 static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg)
 386 {
 387     uint32_t pios_ppm_id;
 388
 389     PIOS_PPM_Init(&pios_ppm_id, ppm_cfg);
 390
 391     uint32_t pios_ppm_rcvr_id;
 392     if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) {
 393         PIOS_Assert(0);
 394     }
 395     pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id;
 396 }
 397
 398 /**
 399  * PIOS_Board_Init()
 400  * initializes all the core subsystems on this specific hardware
 401  * called from System/openpilot.c
 402  */
 403
 404 #include <pios_board_info.h>
 405
 406 void PIOS_Board_Init(void)
 407 {
 408     const struct pios_board_info *bdinfo = &pios_board_info_blob;
 409
 410 #if defined(PIOS_INCLUDE_LED)
 411     const struct pios_gpio_cfg *led_cfg  = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
 412     PIOS_Assert(led_cfg);
 413     PIOS_LED_Init(led_cfg);
 414 #endif /* PIOS_INCLUDE_LED */
 415
 416 #ifdef PIOS_INCLUDE_INSTRUMENTATION
 417     PIOS_Instrumentation_Init(PIOS_INSTRUMENTATION_MAX_COUNTERS);
 418 #endif
 419
 420     /* Set up the SPI interface to the gyro/acelerometer */
 421     if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) {
 422         PIOS_DEBUG_Assert(0);
 423     }
 424
 425     /* Set up the SPI interface to the flash and rfm22b */
 426     if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) {
 427         PIOS_DEBUG_Assert(0);
 428     }
 429
 430 #if defined(PIOS_INCLUDE_FLASH)
 431     /* Connect flash to the appropriate interface and configure it */
 432     uintptr_t flash_id;
 433
 434     // Initialize the external USER flash
 435     if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1)) {
 436         PIOS_DEBUG_Assert(0);
 437     }
 438
 439     if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_external_system_cfg, &pios_jedec_flash_driver, flash_id)) {
 440         PIOS_DEBUG_Assert(0);
 441     }
 442 #endif /* if defined(PIOS_INCLUDE_FLASH) */
 443
 444 #if defined(PIOS_INCLUDE_RTC)
 445     PIOS_RTC_Init(&pios_rtc_main_cfg);
 446 #endif
 447
 448     /* IAP System Setup */
 449     PIOS_IAP_Init();
 450     // check for safe mode commands from gcs
 451     if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 &&
 452         PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 &&
 453         PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) {
 454         PIOS_FLASHFS_Format(pios_uavo_settings_fs_id);
 455         PIOS_IAP_WriteBootCmd(0, 0);
 456         PIOS_IAP_WriteBootCmd(1, 0);
 457         PIOS_IAP_WriteBootCmd(2, 0);
 458     }
 459
 460 #ifdef PIOS_INCLUDE_WDG
 461     PIOS_WDG_Init();
 462 #endif
 463
 464     /* Initialize the task monitor */
 465     if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
 466         PIOS_Assert(0);
 467     }
 468
 469     /* Initialize the delayed callback library */
 470     PIOS_CALLBACKSCHEDULER_Initialize();
 471
 472     /* Initialize UAVObject libraries */
 473     EventDispatcherInitialize();
 474     UAVObjInitialize();
 475     HwSettingsInitialize();
 476 #if defined(PIOS_INCLUDE_RFM22B)
 477     OPLinkSettingsInitialize();
 478     OPLinkStatusInitialize();
 479 #endif /* PIOS_INCLUDE_RFM22B */
 480
 481     /* Initialize the alarms library */
 482     AlarmsInitialize();
 483
 484     /* Set up pulse timers */
 485     PIOS_TIM_InitClock(&tim_1_cfg);
 486     PIOS_TIM_InitClock(&tim_3_cfg);
 487     PIOS_TIM_InitClock(&tim_4_cfg);
 488     PIOS_TIM_InitClock(&tim_5_cfg);
 489     PIOS_TIM_InitClock(&tim_8_cfg);
 490     PIOS_TIM_InitClock(&tim_9_cfg);
 491     PIOS_TIM_InitClock(&tim_10_cfg);
 492     PIOS_TIM_InitClock(&tim_11_cfg);
 493     PIOS_TIM_InitClock(&tim_12_cfg);
 494
 495     uint16_t boot_count = PIOS_IAP_ReadBootCount();
 496     if (boot_count < 3) {
 497         PIOS_IAP_WriteBootCount(++boot_count);
 498         AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT);
 499     } else {
 500         /* Too many failed boot attempts, force hwsettings to defaults */
 501         HwSettingsSetDefaults(HwSettingsHandle(), 0);
 502         AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);
 503     }
 504
 505     /* Configure IO ports */
 506     uint8_t hwsettings_DSMxBind;
 507     HwSettingsDSMxBindGet(&hwsettings_DSMxBind);
 508
 509     /* Configure FlexiPort */
 510     uint8_t hwsettings_flexiport;
 511     HwSettingsRM_FlexiPortGet(&hwsettings_flexiport);
 512     switch (hwsettings_flexiport) {
 513     case HWSETTINGS_RM_FLEXIPORT_DISABLED:
 514         break;
 515     case HWSETTINGS_RM_FLEXIPORT_TELEMETRY:
 516         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);
 517         break;
 518     case HWSETTINGS_RM_FLEXIPORT_I2C:
 519 #if defined(PIOS_INCLUDE_I2C)
 520         if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) {
 521             PIOS_Assert(0);
 522         }
 523         PIOS_DELAY_WaitmS(50); // this was after the other PIOS_I2C_Init(), so I copied it here too
 524 #ifdef PIOS_INCLUDE_WDG
 525         // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
 526         // this is not in a loop, so it is safe
 527         // leave this here even if PIOS_INCLUDE_HMC5X83 is undefined
 528         // to avoid making something else fail when HMC5X83 is removed
 529         PIOS_WDG_Clear();
 530 #endif /* PIOS_INCLUDE_WDG */
 531 #if defined(PIOS_INCLUDE_HMC5X83)
 532         // get auxmag type
 533         AuxMagSettingsTypeOptions option;
 534         AuxMagSettingsInitialize();
 535         AuxMagSettingsTypeGet(&option);
 536         // if the aux mag type is FlexiPort then set it up
 537         if (option == AUXMAGSETTINGS_TYPE_FLEXI) {
 538             // attach the 5x83 mag to the previously inited I2C2
 539             external_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_external_cfg, pios_i2c_flexiport_adapter_id, 0);
 540 #ifdef PIOS_INCLUDE_WDG
 541             // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
 542             // this is not in a loop, so it is safe
 543             PIOS_WDG_Clear();
 544 #endif /* PIOS_INCLUDE_WDG */
 545             // add this sensor to the sensor task's list
 546             // be careful that you don't register a slow, unimportant sensor after registering the fastest sensor
 547             // and before registering some other fast and important sensor
 548             // as that would cause delay and time jitter for the second fast sensor
 549             PIOS_HMC5x83_Register(external_mag, PIOS_SENSORS_TYPE_3AXIS_AUXMAG);
 550             // mag alarm is cleared later, so use I2C
 551             AlarmsSet(SYSTEMALARMS_ALARM_I2C, (external_mag) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_WARNING);
 552         }
 553 #endif /* PIOS_INCLUDE_HMC5X83 */
 554 #endif /* PIOS_INCLUDE_I2C */
 555         break;
 556     case HWSETTINGS_RM_FLEXIPORT_GPS:
 557         PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id);
 558         break;
 559     case HWSETTINGS_RM_FLEXIPORT_DSM:
 560         // TODO: Define the various Channelgroup for Revo dsm inputs and handle here
 561         PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg,
 562                                  &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT, &hwsettings_DSMxBind);
 563         break;
 564     case HWSETTINGS_RM_FLEXIPORT_DEBUGCONSOLE:
 565 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
 566         {
 567             PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id);
 568         }
 569 #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
 570         break;
 571     case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE:
 572         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);
 573         break;
 574     case HWSETTINGS_RM_FLEXIPORT_MSP:
 575         PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id);
 576         break;
 577     case HWSETTINGS_RM_FLEXIPORT_MAVLINK:
 578         PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id);
 579         break;
 580     case HWSETTINGS_RM_FLEXIPORT_OSDHK:
 581         PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
 582         break;
 583
 584     case HWSETTINGS_RM_FLEXIPORT_SRXL:
 585 #if defined(PIOS_INCLUDE_SRXL)
 586         {
 587             uint32_t pios_usart_srxl_id;
 588             if (PIOS_USART_Init(&pios_usart_srxl_id, &pios_usart_srxl_flexi_cfg)) {
 589                 PIOS_Assert(0);
 590             }
 591
 592             uint32_t pios_srxl_id;
 593             if (PIOS_SRXL_Init(&pios_srxl_id, &pios_usart_com_driver, pios_usart_srxl_id)) {
 594                 PIOS_Assert(0);
 595             }
 596
 597             uint32_t pios_srxl_rcvr_id;
 598             if (PIOS_RCVR_Init(&pios_srxl_rcvr_id, &pios_srxl_rcvr_driver, pios_srxl_id)) {
 599                 PIOS_Assert(0);
 600             }
 601             pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL] = pios_srxl_rcvr_id;
 602         }
 603 #endif /* PIOS_INCLUDE_SRXL */
 604         break;
 605
 606     case HWSETTINGS_RM_FLEXIPORT_IBUS:
 607 #if defined(PIOS_INCLUDE_IBUS)
 608         PIOS_Board_configure_ibus(&pios_usart_ibus_flexi_cfg);
 609 #endif /* PIOS_INCLUDE_IBUS */
 610         break;
 611
 612     case HWSETTINGS_RM_FLEXIPORT_HOTTSUMD:
 613     case HWSETTINGS_RM_FLEXIPORT_HOTTSUMH:
 614 #if defined(PIOS_INCLUDE_HOTT)
 615         {
 616             uint32_t pios_usart_hott_id;
 617             if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_flexi_cfg)) {
 618                 PIOS_Assert(0);
 619             }
 620
 621             uint32_t pios_hott_id;
 622             if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id,
 623                                hwsettings_flexiport == HWSETTINGS_RM_FLEXIPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) {
 624                 PIOS_Assert(0);
 625             }
 626
 627             uint32_t pios_hott_rcvr_id;
 628             if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) {
 629                 PIOS_Assert(0);
 630             }
 631             pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT] = pios_hott_rcvr_id;
 632         }
 633 #endif /* PIOS_INCLUDE_HOTT */
 634         break;
 635
 636     case HWSETTINGS_RM_FLEXIPORT_EXBUS:
 637 #if defined(PIOS_INCLUDE_EXBUS)
 638         {
 639             uint32_t pios_usart_exbus_id;
 640             if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_flexi_cfg)) {
 641                 PIOS_Assert(0);
 642             }
 643
 644             uint32_t pios_exbus_id;
 645             if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) {
 646                 PIOS_Assert(0);
 647             }
 648
 649             uint32_t pios_exbus_rcvr_id;
 650             if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) {
 651                 PIOS_Assert(0);
 652             }
 653             pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS] = pios_exbus_rcvr_id;
 654         }
 655 #endif /* PIOS_INCLUDE_EXBUS */
 656         break;
 657     } /* hwsettings_rm_flexiport */
 658
 659     /* Moved this here to allow binding on flexiport */
 660 #if defined(PIOS_INCLUDE_FLASH)
 661     if (PIOS_FLASHFS_Logfs_Init(&pios_user_fs_id, &flashfs_external_user_cfg, &pios_jedec_flash_driver, flash_id)) {
 662         PIOS_DEBUG_Assert(0);
 663     }
 664 #endif /* if defined(PIOS_INCLUDE_FLASH) */
 665
 666 #if defined(PIOS_INCLUDE_USB)
 667     /* Initialize board specific USB data */
 668     PIOS_USB_BOARD_DATA_Init();
 669
 670     /* Flags to determine if various USB interfaces are advertised */
 671     bool usb_hid_present = false;
 672     bool usb_cdc_present = false;
 673
 674 #if defined(PIOS_INCLUDE_USB_CDC)
 675     if (PIOS_USB_DESC_HID_CDC_Init()) {
 676         PIOS_Assert(0);
 677     }
 678     usb_hid_present = true;
 679     usb_cdc_present = true;
 680 #else
 681     if (PIOS_USB_DESC_HID_ONLY_Init()) {
 682         PIOS_Assert(0);
 683     }
 684     usb_hid_present = true;
 685 #endif
 686
 687     uint32_t pios_usb_id;
 688     PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev));
 689
 690 #if defined(PIOS_INCLUDE_USB_CDC)
 691
 692     uint8_t hwsettings_usb_vcpport;
 693     /* Configure the USB VCP port */
 694     HwSettingsUSB_VCPPortGet(&hwsettings_usb_vcpport);
 695
 696     if (!usb_cdc_present) {
 697         /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */
 698         hwsettings_usb_vcpport = HWSETTINGS_USB_VCPPORT_DISABLED;
 699     }
 700     uint32_t pios_usb_cdc_id;
 701     if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) {
 702         PIOS_Assert(0);
 703     }
 704
 705     uint32_t pios_usb_hid_id;
 706     if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
 707         PIOS_Assert(0);
 708     }
 709
 710     switch (hwsettings_usb_vcpport) {
 711     case HWSETTINGS_USB_VCPPORT_DISABLED:
 712         break;
 713     case HWSETTINGS_USB_VCPPORT_USBTELEMETRY:
 714 #if defined(PIOS_INCLUDE_COM)
 715         {
 716             uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
 717             uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
 718             PIOS_Assert(rx_buffer);
 719             PIOS_Assert(tx_buffer);
 720             if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
 721                               rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
 722                               tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
 723                 PIOS_Assert(0);
 724             }
 725         }
 726 #endif /* PIOS_INCLUDE_COM */
 727         break;
 728     case HWSETTINGS_USB_VCPPORT_COMBRIDGE:
 729 #if defined(PIOS_INCLUDE_COM)
 730         {
 731             uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN);
 732             uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN);
 733             PIOS_Assert(rx_buffer);
 734             PIOS_Assert(tx_buffer);
 735             if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
 736                               rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN,
 737                               tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) {
 738                 PIOS_Assert(0);
 739             }
 740         }
 741 #endif /* PIOS_INCLUDE_COM */
 742         break;
 743     case HWSETTINGS_USB_VCPPORT_DEBUGCONSOLE:
 744 #if defined(PIOS_INCLUDE_COM)
 745 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
 746         {
 747             uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN);
 748             PIOS_Assert(tx_buffer);
 749             if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
 750                               NULL, 0,
 751                               tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) {
 752                 PIOS_Assert(0);
 753             }
 754         }
 755 #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
 756 #endif /* PIOS_INCLUDE_COM */
 757
 758         break;
 759     }
 760 #endif /* PIOS_INCLUDE_USB_CDC */
 761
 762 #if defined(PIOS_INCLUDE_USB_HID)
 763     /* Configure the usb HID port */
 764     uint8_t hwsettings_usb_hidport;
 765     HwSettingsUSB_HIDPortGet(&hwsettings_usb_hidport);
 766
 767     if (!usb_hid_present) {
 768         /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */
 769         hwsettings_usb_hidport = HWSETTINGS_USB_HIDPORT_DISABLED;
 770     }
 771
 772     switch (hwsettings_usb_hidport) {
 773     case HWSETTINGS_USB_HIDPORT_DISABLED:
 774         break;
 775     case HWSETTINGS_USB_HIDPORT_USBTELEMETRY:
 776 #if defined(PIOS_INCLUDE_COM)
 777         {
 778             uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
 779             uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
 780             PIOS_Assert(rx_buffer);
 781             PIOS_Assert(tx_buffer);
 782             if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id,
 783                               rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
 784                               tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
 785                 PIOS_Assert(0);
 786             }
 787         }
 788 #endif /* PIOS_INCLUDE_COM */
 789         break;
 790     }
 791
 792 #endif /* PIOS_INCLUDE_USB_HID */
 793
 794     if (usb_hid_present || usb_cdc_present) {
 795         PIOS_USBHOOK_Activate();
 796     }
 797 #endif /* PIOS_INCLUDE_USB */
 798
 799     /* Configure main USART port */
 800     uint8_t hwsettings_mainport;
 801     HwSettingsRM_MainPortGet(&hwsettings_mainport);
 802     switch (hwsettings_mainport) {
 803     case HWSETTINGS_RM_MAINPORT_DISABLED:
 804         break;
 805     case HWSETTINGS_RM_MAINPORT_TELEMETRY:
 806         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);
 807         break;
 808     case HWSETTINGS_RM_MAINPORT_GPS:
 809         PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id);
 810         break;
 811     case HWSETTINGS_RM_MAINPORT_SBUS:
 812 #if defined(PIOS_INCLUDE_SBUS)
 813         {
 814             uint32_t pios_usart_sbus_id;
 815             if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_cfg)) {
 816                 PIOS_Assert(0);
 817             }
 818
 819             uint32_t pios_sbus_id;
 820             if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) {
 821                 PIOS_Assert(0);
 822             }
 823
 824             uint32_t pios_sbus_rcvr_id;
 825             if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) {
 826                 PIOS_Assert(0);
 827             }
 828             pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id;
 829         }
 830 #endif
 831         break;
 832     case HWSETTINGS_RM_MAINPORT_DSM:
 833         // Force binding to zero on the main port
 834         hwsettings_DSMxBind = 0;
 835
 836         // TODO: Define the various Channelgroup for Revo dsm inputs and handle here
 837         PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg,
 838                                  &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind);
 839         break;
 840     case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE:
 841 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
 842         {
 843             PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id);
 844         }
 845 #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
 846         break;
 847     case HWSETTINGS_RM_MAINPORT_COMBRIDGE:
 848         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);
 849         break;
 850     case HWSETTINGS_RM_MAINPORT_MSP:
 851         PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id);
 852         break;
 853     case HWSETTINGS_RM_MAINPORT_MAVLINK:
 854         PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id);
 855         break;
 856     case HWSETTINGS_RM_MAINPORT_OSDHK:
 857         PIOS_Board_configure_com(&pios_usart_hkosd_main_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
 858         break;
 859     } /*        hwsettings_rm_mainport */
 860
 861     if (hwsettings_mainport != HWSETTINGS_RM_MAINPORT_SBUS) {
 862         GPIO_Init(pios_sbus_cfg.inv.gpio, &pios_sbus_cfg.inv.init);
 863         GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable);
 864     }
 865
 866     /* Initialize the RFM22B radio COM device. */
 867 #if defined(PIOS_INCLUDE_RFM22B)
 868
 869     /* Fetch the OPLinkSettings object. */
 870     OPLinkSettingsData oplinkSettings;
 871     OPLinkSettingsGet(&oplinkSettings);
 872
 873     // Initialize out status object.
 874     OPLinkStatusData oplinkStatus;
 875     OPLinkStatusGet(&oplinkStatus);
 876     oplinkStatus.BoardType     = bdinfo->board_type;
 877     PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM);
 878     PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial);
 879     oplinkStatus.BoardRevision = bdinfo->board_rev;
 880
 881     /* Is the radio turned on? */
 882     bool is_coordinator = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPLINKCOORDINATOR);
 883     bool openlrs    = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPENLRS);
 884     bool data_mode  = ((oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATA) ||
 885                        (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL));
 886     bool ppm_mode   = ((oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_CONTROL) ||
 887                        (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL));
 888     bool is_enabled = ((oplinkSettings.Protocol != OPLINKSETTINGS_PROTOCOL_DISABLED) &&
 889                        ((oplinkSettings.MaxRFPower != OPLINKSETTINGS_MAXRFPOWER_0) || openlrs));
 890     if (is_enabled) {
 891         if (openlrs) {
 892 #if defined(PIOS_INCLUDE_OPENLRS_RCVR)
 893             const struct pios_openlrs_cfg *openlrs_cfg = PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(bdinfo->board_rev);
 894             uint32_t openlrs_id;
 895
 896             PIOS_OpenLRS_Init(&openlrs_id, PIOS_RFM22_SPI_PORT, 0, openlrs_cfg);
 897             {
 898                 uint32_t openlrsrcvr_id;
 899                 PIOS_OpenLRS_Rcvr_Init(&openlrsrcvr_id, openlrs_id);
 900                 uint32_t openlrsrcvr_rcvr_id;
 901                 if (PIOS_RCVR_Init(&openlrsrcvr_rcvr_id, &pios_openlrs_rcvr_driver, openlrsrcvr_id)) {
 902                     PIOS_Assert(0);
 903                 }
 904                 pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPENLRS] = openlrsrcvr_rcvr_id;
 905             }
 906 #endif /* PIOS_INCLUDE_OPENLRS_RCVR */
 907         } else {
 908             /* Configure the RFM22B device. */
 909             const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev);
 910
 911             if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg, oplinkSettings.RFBand)) {
 912                 PIOS_Assert(0);
 913             }
 914
 915             /* Configure the radio com interface */
 916             uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN);
 917             uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN);
 918             PIOS_Assert(rx_buffer);
 919             PIOS_Assert(tx_buffer);
 920             if (PIOS_COM_Init(&pios_com_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id,
 921                               rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN,
 922                               tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) {
 923                 PIOS_Assert(0);
 924             }
 925
 926             oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED;
 927
 928             // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex.
 929             enum rfm22b_datarate datarate = RFM22_datarate_64000;
 930             switch (oplinkSettings.ComSpeed) {
 931             case OPLINKSETTINGS_COMSPEED_4800:
 932                 datarate = RFM22_datarate_9600;
 933                 break;
 934             case OPLINKSETTINGS_COMSPEED_9600:
 935                 datarate = RFM22_datarate_19200;
 936                 break;
 937             case OPLINKSETTINGS_COMSPEED_19200:
 938                 datarate = RFM22_datarate_32000;
 939                 break;
 940             case OPLINKSETTINGS_COMSPEED_38400:
 941                 datarate = RFM22_datarate_64000;
 942                 break;
 943             case OPLINKSETTINGS_COMSPEED_57600:
 944                 datarate = RFM22_datarate_100000;
 945                 break;
 946             case OPLINKSETTINGS_COMSPEED_115200:
 947                 datarate = RFM22_datarate_192000;
 948                 break;
 949             }
 950
 951             /* Set the radio configuration parameters. */
 952             PIOS_RFM22B_SetDeviceID(pios_rfm22b_id, oplinkSettings.CustomDeviceID);
 953             PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID);
 954             PIOS_RFM22B_SetXtalCap(pios_rfm22b_id, oplinkSettings.RFXtalCap);
 955             PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, is_coordinator, data_mode, ppm_mode);
 956
 957             /* Set the modem Tx power level */
 958             switch (oplinkSettings.MaxRFPower) {
 959             case OPLINKSETTINGS_MAXRFPOWER_125:
 960                 PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0);
 961                 break;
 962             case OPLINKSETTINGS_MAXRFPOWER_16:
 963                 PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1);
 964                 break;
 965             case OPLINKSETTINGS_MAXRFPOWER_316:
 966                 PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2);
 967                 break;
 968             case OPLINKSETTINGS_MAXRFPOWER_63:
 969                 PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3);
 970                 break;
 971             case OPLINKSETTINGS_MAXRFPOWER_126:
 972                 PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4);
 973                 break;
 974             case OPLINKSETTINGS_MAXRFPOWER_25:
 975                 PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5);
 976                 break;
 977             case OPLINKSETTINGS_MAXRFPOWER_50:
 978                 PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6);
 979                 break;
 980             case OPLINKSETTINGS_MAXRFPOWER_100:
 981                 PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7);
 982                 break;
 983             default:
 984                 // do nothing
 985                 break;
 986             }
 987
 988             /* Reinitialize the modem. */
 989             PIOS_RFM22B_Reinit(pios_rfm22b_id);
 990
 991             uint8_t hwsettings_radioaux;
 992             HwSettingsRadioAuxStreamGet(&hwsettings_radioaux);
 993             // TODO: this is in preparation for full mavlink support and is used by LP-368
 994             uint16_t mavlink_rx_size = PIOS_COM_MAVLINK_RX_BUF_LEN;
 995
 996             switch (hwsettings_radioaux) {
 997             case HWSETTINGS_RADIOAUXSTREAM_DEBUGCONSOLE:
 998             case HWSETTINGS_RADIOAUXSTREAM_DISABLED:
 999                 break;
1000             case HWSETTINGS_RADIOAUXSTREAM_MAVLINK:
1001             {
1002                 uint8_t *auxrx_buffer = 0;
1003                 if (mavlink_rx_size) {
1004                     auxrx_buffer = (uint8_t *)pios_malloc(mavlink_rx_size);
1005                 }
1006                 uint8_t *auxtx_buffer = (uint8_t *)pios_malloc(PIOS_COM_MAVLINK_TX_BUF_LEN);
1007                 PIOS_Assert(auxrx_buffer);
1008                 PIOS_Assert(auxtx_buffer);
1009                 if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_rfm22b_aux_com_driver, pios_rfm22b_id,
1010                                   auxrx_buffer, mavlink_rx_size,
1011                                   auxtx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) {
1012                     PIOS_Assert(0);
1013                 }
1014             }
1015             }
1016         }
1017     } else {
1018         oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED;
1019     }
1020
1021     OPLinkStatusSet(&oplinkStatus);
1022 #endif /* PIOS_INCLUDE_RFM22B */
1023
1024 #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM)
1025     const struct pios_servo_cfg *pios_servo_cfg;
1026     // default to servo outputs only
1027     pios_servo_cfg = &pios_servo_cfg_out;
1028 #endif
1029
1030     /* Configure the receiver port*/
1031     uint8_t hwsettings_rcvrport;
1032     HwSettingsRM_RcvrPortGet(&hwsettings_rcvrport);
1033
1034     // Configure rcvrport PPM/PWM/OUTPUTS
1035     switch (hwsettings_rcvrport) {
1036     case HWSETTINGS_RM_RCVRPORT_PWM:
1037 #if defined(PIOS_INCLUDE_PWM)
1038         /* Set up the receiver port.  Later this should be optional */
1039         PIOS_Board_configure_pwm(&pios_pwm_cfg);
1040 #endif /* PIOS_INCLUDE_PWM */
1041         break;
1042     case HWSETTINGS_RM_RCVRPORT_PPM:
1043     case HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS:
1044     case HWSETTINGS_RM_RCVRPORT_PPMPWM:
1045     case HWSETTINGS_RM_RCVRPORT_PPMTELEMETRY:
1046     case HWSETTINGS_RM_RCVRPORT_PPMDEBUGCONSOLE:
1047     case HWSETTINGS_RM_RCVRPORT_PPMCOMBRIDGE:
1048     case HWSETTINGS_RM_RCVRPORT_PPMMSP:
1049     case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK:
1050     case HWSETTINGS_RM_RCVRPORT_PPMGPS:
1051 #if defined(PIOS_INCLUDE_PPM)
1052         PIOS_Board_configure_ppm(&pios_ppm_cfg);
1053
1054         if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS) {
1055             // configure servo outputs and the remaining 5 inputs as outputs
1056             pios_servo_cfg = &pios_servo_cfg_out_in_ppm;
1057         }
1058
1059         // enable pwm on the remaining channels
1060         if (hwsettings_rcvrport == HWSETTINGS_RM_RCVRPORT_PPMPWM) {
1061             PIOS_Board_configure_pwm(&pios_pwm_ppm_cfg);
1062         }
1063
1064         break;
1065 #endif /* PIOS_INCLUDE_PPM */
1066     case HWSETTINGS_RM_RCVRPORT_OUTPUTS:
1067         // configure only the servo outputs
1068         pios_servo_cfg = &pios_servo_cfg_out_in;
1069         break;
1070     }
1071
1072     // Configure rcvrport usart
1073     switch (hwsettings_rcvrport) {
1074     case HWSETTINGS_RM_RCVRPORT_TELEMETRY:
1075     case HWSETTINGS_RM_RCVRPORT_PPMTELEMETRY:
1076         PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id);
1077         break;
1078     case HWSETTINGS_RM_RCVRPORT_DEBUGCONSOLE:
1079     case HWSETTINGS_RM_RCVRPORT_PPMDEBUGCONSOLE:
1080 #if defined(PIOS_INCLUDE_DEBUG_CONSOLE)
1081         PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id);
1082 #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */
1083         break;
1084     case HWSETTINGS_RM_RCVRPORT_COMBRIDGE:
1085     case HWSETTINGS_RM_RCVRPORT_PPMCOMBRIDGE:
1086         PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id);
1087         break;
1088     case HWSETTINGS_RM_RCVRPORT_MSP:
1089     case HWSETTINGS_RM_RCVRPORT_PPMMSP:
1090         PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_MSP_RX_BUF_LEN, PIOS_COM_MSP_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_msp_id);
1091         break;
1092     case HWSETTINGS_RM_RCVRPORT_MAVLINK:
1093     case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK:
1094         PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id);
1095         break;
1096     case HWSETTINGS_RM_RCVRPORT_GPS:
1097     case HWSETTINGS_RM_RCVRPORT_PPMGPS:
1098         PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id);
1099         break;
1100     case HWSETTINGS_RM_RCVRPORT_IBUS:
1101 #if defined(PIOS_INCLUDE_IBUS)
1102         PIOS_Board_configure_ibus(&pios_usart_ibus_rcvr_cfg);
1103 #endif /* PIOS_INCLUDE_IBUS */
1104         break;
1105     }
1106
1107 #if defined(PIOS_INCLUDE_GCSRCVR)
1108     GCSReceiverInitialize();
1109     uint32_t pios_gcsrcvr_id;
1110     PIOS_GCSRCVR_Init(&pios_gcsrcvr_id);
1111     uint32_t pios_gcsrcvr_rcvr_id;
1112     if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) {
1113         PIOS_Assert(0);
1114     }
1115     pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id;
1116 #endif /* PIOS_INCLUDE_GCSRCVR */
1117
1118 #if defined(PIOS_INCLUDE_OPLINKRCVR)
1119     {
1120         OPLinkReceiverInitialize();
1121         uint32_t pios_oplinkrcvr_id;
1122         PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id, pios_rfm22b_id);
1123         uint32_t pios_oplinkrcvr_rcvr_id;
1124         if (PIOS_RCVR_Init(&pios_oplinkrcvr_rcvr_id, &pios_oplinkrcvr_rcvr_driver, pios_oplinkrcvr_id)) {
1125             PIOS_Assert(0);
1126         }
1127         pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_oplinkrcvr_rcvr_id;
1128     }
1129 #endif /* PIOS_INCLUDE_OPLINKRCVR */
1130
1131 #ifndef PIOS_ENABLE_DEBUG_PINS
1132     // pios_servo_cfg points to the correct configuration based on input port settings
1133     PIOS_Servo_Init(pios_servo_cfg);
1134 #else
1135     PIOS_DEBUG_Init(pios_tim_servoport_all_pins, NELEMENTS(pios_tim_servoport_all_pins));
1136 #endif
1137
1138     // Disable GPIO_A8 Pullup to prevent wrong results on battery voltage readout
1139     GPIO_InitTypeDef gpioA8 = {
1140         .GPIO_Speed = GPIO_Speed_2MHz,
1141         .GPIO_Mode  = GPIO_Mode_IN,
1142         .GPIO_PuPd  = GPIO_PuPd_NOPULL,
1143         .GPIO_Pin   = GPIO_Pin_8,
1144         .GPIO_OType = GPIO_OType_OD,
1145     };
1146     GPIO_Init(GPIOA, &gpioA8);
1147
1148     // init I2C1 for use with the internal mag and baro
1149     if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) {
1150         PIOS_DEBUG_Assert(0);
1151     }
1152     PIOS_DELAY_WaitmS(50);
1153
1154 #if defined(PIOS_INCLUDE_ADC)
1155     PIOS_ADC_Init(&pios_adc_cfg);
1156 #endif
1157
1158 #if defined(PIOS_INCLUDE_MPU6000)
1159     PIOS_MPU6000_Init(pios_spi_gyro_id, 0, &pios_mpu6000_cfg);
1160     PIOS_MPU6000_CONFIG_Configure();
1161     PIOS_MPU6000_Register();
1162 #endif
1163
1164 #ifdef PIOS_INCLUDE_WDG
1165     // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
1166     // this is not in a loop, so it is safe
1167     // leave this here even if PIOS_INCLUDE_HMC5X83 is undefined
1168     // to avoid making something else fail when HMC5X83 is removed
1169     PIOS_WDG_Clear();
1170 #endif /* PIOS_INCLUDE_WDG */
1171
1172 #if defined(PIOS_INCLUDE_HMC5X83)
1173     // attach the 5x83 mag to the previously inited I2C1
1174     onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_internal_cfg, pios_i2c_mag_pressure_adapter_id, 0);
1175 #ifdef PIOS_INCLUDE_WDG
1176     // give HMC5x83 on I2C some extra time to allow for reset, etc. if needed
1177     // this is not in a loop, so it is safe
1178     PIOS_WDG_Clear();
1179 #endif /* PIOS_INCLUDE_WDG */
1180     // add this sensor to the sensor task's list
1181     PIOS_HMC5x83_Register(onboard_mag, PIOS_SENSORS_TYPE_3AXIS_MAG);
1182 #endif
1183
1184 #if defined(PIOS_INCLUDE_MS5611)
1185     PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id);
1186     PIOS_MS5611_Register();
1187 #endif
1188
1189     #ifdef PIOS_INCLUDE_WS2811
1190 #include <pios_ws2811.h>
1191     HwSettingsWS2811LED_OutOptions ws2811_pin_settings;
1192     HwSettingsWS2811LED_OutGet(&ws2811_pin_settings);
1193
1194     if (ws2811_pin_settings != HWSETTINGS_WS2811LED_OUT_DISABLED && ws2811_pin_settings < NELEMENTS(pios_ws2811_pin_cfg)) {
1195         PIOS_WS2811_Init(&pios_ws2811_cfg, &pios_ws2811_pin_cfg[ws2811_pin_settings]);
1196     }
1197 #endif // PIOS_INCLUDE_WS2811
1198 #ifdef PIOS_INCLUDE_ADC
1199     {
1200         uint8_t adc_config[HWSETTINGS_ADCROUTING_NUMELEM];
1201         HwSettingsADCRoutingArrayGet(adc_config);
1202         for (uint32_t i = 0; i < HWSETTINGS_ADCROUTING_NUMELEM; i++) {
1203             if (adc_config[i] != HWSETTINGS_ADCROUTING_DISABLED) {
1204                 PIOS_ADC_PinSetup(i);
1205             }
1206         }
1207     }
1208 #endif // PIOS_INCLUDE_ADC
1209
1210     DEBUG_PRINTF(2, "Board complete\r\n");
1211 }
1212
1213 /**
1214  * @}
1215  * @}
1216  */