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

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotSystem OpenPilot System
   4  * @{
   5  * @addtogroup OpenPilotCore OpenPilot Core
   6  * @{
   7  *
   8  * @file       pios_board.c
   9  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  10  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  11  * @brief      Defines board specific static initializers for hardware for the OpenPilot board.
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  *****************************************************************************/
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  */
  30
  31 #include "inc/openpilot.h"
  32 #include <pios_board_info.h>
  33 #include <pios_ppm_out.h>
  34 #include <oplinksettings.h>
  35 #include <oplinkreceiver.h>
  36 #include <pios_openlrs.h>
  37 #include <taskinfo.h>
  38 #ifdef PIOS_INCLUDE_SERVO
  39 #include <pios_servo.h>
  40 #endif
  41 #include <pios_board_io.h>
  42
  43 /*
  44  * Pull in the board-specific static HW definitions.
  45  * Including .c files is a bit ugly but this allows all of
  46  * the HW definitions to be const and static to limit their
  47  * scope.
  48  *
  49  * NOTE: THIS IS THE ONLY PLACE THAT SHOULD EVER INCLUDE THIS FILE
  50  */
  51 #include "../board_hw_defs.c"
  52
  53 uint32_t pios_com_hid_id     = 0;
  54 // uint32_t pios_com_vcp_id     = 0; /* this is provided by pios_board_io.c */
  55 uint32_t pios_com_main_id    = 0;
  56 uint32_t pios_com_flexi_id   = 0;
  57 uint32_t pios_com_gcs_id     = 0;
  58 uint32_t pios_com_gcs_out_id = 0;
  59 #if defined(PIOS_INCLUDE_PPM_OUT)
  60 uint32_t pios_ppm_out_id     = 0;
  61 bool ppm_rssi = false;
  62 #endif
  63 #if defined(PIOS_INCLUDE_RFM22B)
  64 #include <pios_rfm22b_com.h>
  65 uint32_t pios_com_pri_radio_out_id = 0;
  66 uint32_t pios_com_aux_radio_out_id = 0;
  67 #endif
  68
  69 uintptr_t pios_uavo_settings_fs_id;
  70 uintptr_t pios_user_fs_id  = 0;
  71
  72 static uint8_t servo_count = 0;
  73
  74 // Forward definitions
  75 static void PIOS_Board_PPM_callback(uint32_t context, const int16_t *channels);
  76
  77 /**
  78  * PIOS_Board_Init()
  79  * initializes all the core subsystems on this specific hardware
  80  * called from System/openpilot.c
  81  */
  82 void PIOS_Board_Init(void)
  83 {
  84     /* Delay system */
  85     PIOS_DELAY_Init();
  86
  87 #ifdef PIOS_INCLUDE_FLASH_LOGFS_SETTINGS
  88     uintptr_t flash_id;
  89     PIOS_Flash_Internal_Init(&flash_id, &flash_internal_cfg);
  90     PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_internal_cfg, &pios_internal_flash_driver, flash_id);
  91 #endif
  92
  93     /* Initialize the task monitor */
  94     if (PIOS_TASK_MONITOR_Initialize(TASKINFO_RUNNING_NUMELEM)) {
  95         PIOS_Assert(0);
  96     }
  97
  98     /* Initialize the delayed callback library */
  99     PIOS_CALLBACKSCHEDULER_Initialize();
 100
 101     /* Initialize UAVObject libraries */
 102     EventDispatcherInitialize();
 103     UAVObjInitialize();
 104     SETTINGS_INITIALISE_ALL;
 105
 106     /* Set up the SPI interface to the rfm22b */
 107     if (PIOS_SPI_Init(&pios_spi_rfm22b_id, &pios_spi_rfm22b_cfg)) {
 108         PIOS_DEBUG_Assert(0);
 109     }
 110
 111 #ifdef PIOS_INCLUDE_WDG
 112     /* Initialize watchdog as early as possible to catch faults during init */
 113     PIOS_WDG_Init();
 114 #endif /* PIOS_INCLUDE_WDG */
 115
 116 #if defined(PIOS_INCLUDE_RTC)
 117     /* Initialize the real-time clock and its associated tick */
 118     PIOS_RTC_Init(&pios_rtc_main_cfg);
 119 #endif /* PIOS_INCLUDE_RTC */
 120
 121 #if defined(PIOS_INCLUDE_LED)
 122     PIOS_LED_Init(&pios_led_cfg);
 123 #endif /* PIOS_INCLUDE_LED */
 124
 125     /* IAP System Setup */
 126     PIOS_IAP_Init();
 127     // check for safe mode commands from gcs
 128     if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 &&
 129         PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 &&
 130         PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) {
 131         PIOS_FLASHFS_Format(pios_uavo_settings_fs_id);
 132         PIOS_IAP_WriteBootCmd(0, 0);
 133         PIOS_IAP_WriteBootCmd(1, 0);
 134         PIOS_IAP_WriteBootCmd(2, 0);
 135     }
 136
 137     OPLinkReceiverInitialize();
 138
 139     /* Retrieve the settings object. */
 140     OPLinkSettingsData oplinkSettings;
 141     OPLinkSettingsGet(&oplinkSettings);
 142
 143     /* Determine the modem protocols */
 144     bool is_coordinator = (oplinkSettings.Protocol == OPLINKSETTINGS_PROTOCOL_OPLINKCOORDINATOR);
 145     bool ppm_only    = (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_CONTROL);
 146     bool ppm_mode    = ((oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_CONTROL) ||
 147                         (oplinkSettings.LinkType == OPLINKSETTINGS_LINKTYPE_DATAANDCONTROL));
 148     ppm_rssi = (oplinkSettings.PPMOutRSSI == OPLINKSETTINGS_PPMOUTRSSI_TRUE);
 149     bool servo_main  = false;
 150     bool servo_flexi = false;
 151
 152 #if defined(PIOS_INCLUDE_TIM)
 153     /* Set up pulse timers */
 154     PIOS_TIM_InitClock(&tim_1_cfg);
 155     PIOS_TIM_InitClock(&tim_2_cfg);
 156     PIOS_TIM_InitClock(&tim_3_cfg);
 157     PIOS_TIM_InitClock(&tim_4_cfg);
 158 #endif /* PIOS_INCLUDE_TIM */
 159
 160     /* Initialize board specific USB data */
 161     PIOS_USB_BOARD_DATA_Init();
 162
 163
 164 #if defined(PIOS_INCLUDE_USB_CDC)
 165     /* Flags to determine if various USB interfaces are advertised */
 166     bool usb_cdc_present = false;
 167
 168     if (PIOS_USB_DESC_HID_CDC_Init()) {
 169         PIOS_Assert(0);
 170     }
 171     usb_cdc_present = true;
 172 #else
 173     if (PIOS_USB_DESC_HID_ONLY_Init()) {
 174         PIOS_Assert(0);
 175     }
 176 #endif
 177
 178     /*Initialize the USB device */
 179     uint32_t pios_usb_id;
 180     PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg);
 181
 182     /* Configure the USB HID port */
 183     {
 184         uint32_t pios_usb_hid_id;
 185         if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) {
 186             PIOS_Assert(0);
 187         }
 188         uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN);
 189         uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN);
 190         PIOS_Assert(rx_buffer);
 191         PIOS_Assert(tx_buffer);
 192         if (PIOS_COM_Init(&pios_com_hid_id, &pios_usb_hid_com_driver, pios_usb_hid_id,
 193                           rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN,
 194                           tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) {
 195             PIOS_Assert(0);
 196         }
 197     }
 198
 199     /* Configure the USB virtual com port (VCP) */
 200 #if defined(PIOS_INCLUDE_USB_CDC)
 201     if (usb_cdc_present) {
 202         uint32_t pios_usb_cdc_id;
 203         if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) {
 204             PIOS_Assert(0);
 205         }
 206         uint8_t *rx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_VCP_RX_BUF_LEN);
 207         uint8_t *tx_buffer = (uint8_t *)pios_malloc(PIOS_COM_TELEM_VCP_TX_BUF_LEN);
 208         PIOS_Assert(rx_buffer);
 209         PIOS_Assert(tx_buffer);
 210         if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id,
 211                           rx_buffer, PIOS_COM_TELEM_VCP_RX_BUF_LEN,
 212                           tx_buffer, PIOS_COM_TELEM_VCP_TX_BUF_LEN)) {
 213             PIOS_Assert(0);
 214         }
 215     }
 216 #endif
 217
 218     // Configure the main port
 219     uint32_t mainComSpeed = 0;
 220     switch (oplinkSettings.MainComSpeed) {
 221     case OPLINKSETTINGS_MAINCOMSPEED_4800:
 222         mainComSpeed = 4800;
 223         break;
 224     case OPLINKSETTINGS_MAINCOMSPEED_9600:
 225         mainComSpeed = 9600;
 226         break;
 227     case OPLINKSETTINGS_MAINCOMSPEED_19200:
 228         mainComSpeed = 19200;
 229         break;
 230     case OPLINKSETTINGS_MAINCOMSPEED_38400:
 231         mainComSpeed = 38400;
 232         break;
 233     case OPLINKSETTINGS_MAINCOMSPEED_57600:
 234         mainComSpeed = 57600;
 235         break;
 236     case OPLINKSETTINGS_MAINCOMSPEED_115200:
 237         mainComSpeed = 115200;
 238         break;
 239     case OPLINKSETTINGS_MAINCOMSPEED_DISABLED:
 240         break;
 241     }
 242     switch (oplinkSettings.MainPort) {
 243     case OPLINKSETTINGS_MAINPORT_TELEMETRY:
 244     case OPLINKSETTINGS_MAINPORT_SERIAL:
 245 #ifndef PIOS_RFM22B_DEBUG_ON_TELEM
 246         PIOS_BOARD_IO_Configure_UART_COM(&pios_usart_main_cfg,
 247                                          PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN,
 248                                          &pios_com_main_id);
 249         PIOS_COM_ChangeBaud(pios_com_main_id, mainComSpeed);
 250 #endif /* !PIOS_RFM22B_DEBUG_ON_TELEM */
 251         break;
 252     case OPLINKSETTINGS_MAINPORT_PPM:
 253 #if defined(PIOS_INCLUDE_PPM)
 254         /* PPM input is configured on the coordinator modem and output on the remote modem. */
 255         if (is_coordinator) {
 256             PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_main_cfg);
 257         }
 258 #if defined(PIOS_INCLUDE_PPM_OUT)
 259         else {
 260             PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_main_ppm_out_cfg);
 261         }
 262 #endif /* PIOS_INCLUDE_PPM_OUT */
 263 #endif /* PIOS_INCLUDE_PPM */
 264         break;
 265     case OPLINKSETTINGS_MAINPORT_PWM:
 266         servo_main = true;
 267     default:
 268         break;
 269     }
 270
 271     // Configure the flexi port
 272     uint32_t flexiComSpeed = 0;
 273     switch (oplinkSettings.FlexiComSpeed) {
 274     case OPLINKSETTINGS_FLEXICOMSPEED_4800:
 275         flexiComSpeed = 4800;
 276         break;
 277     case OPLINKSETTINGS_FLEXICOMSPEED_9600:
 278         flexiComSpeed = 9600;
 279         break;
 280     case OPLINKSETTINGS_FLEXICOMSPEED_19200:
 281         flexiComSpeed = 19200;
 282         break;
 283     case OPLINKSETTINGS_FLEXICOMSPEED_38400:
 284         flexiComSpeed = 38400;
 285         break;
 286     case OPLINKSETTINGS_FLEXICOMSPEED_57600:
 287         flexiComSpeed = 57600;
 288         break;
 289     case OPLINKSETTINGS_FLEXICOMSPEED_115200:
 290         flexiComSpeed = 115200;
 291         break;
 292     case OPLINKSETTINGS_FLEXICOMSPEED_DISABLED:
 293         break;
 294     }
 295     switch (oplinkSettings.FlexiPort) {
 296     case OPLINKSETTINGS_FLEXIPORT_TELEMETRY:
 297     case OPLINKSETTINGS_FLEXIPORT_SERIAL:
 298 #ifndef PIOS_RFM22B_DEBUG_ON_TELEM
 299         PIOS_BOARD_IO_Configure_UART_COM(&pios_usart_flexi_cfg,
 300                                          PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN,
 301                                          &pios_com_flexi_id);
 302         PIOS_COM_ChangeBaud(pios_com_flexi_id, flexiComSpeed);
 303 #endif /* !PIOS_RFM22B_DEBUG_ON_TELEM */
 304         break;
 305     case OPLINKSETTINGS_FLEXIPORT_PPM:
 306 #if defined(PIOS_INCLUDE_PPM)
 307         /* PPM input is configured on the coordinator modem and output on the remote modem. */
 308         if (is_coordinator) {
 309             PIOS_BOARD_IO_Configure_PPM_RCVR(&pios_ppm_flexi_cfg);
 310         }
 311 #if defined(PIOS_INCLUDE_PPM_OUT)
 312         else {
 313             PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_flexi_ppm_out_cfg);
 314         }
 315 #endif /* PIOS_INCLUDE_PPM_OUT */
 316 #endif /* PIOS_INCLUDE_PPM */
 317         break;
 318     case OPLINKSETTINGS_FLEXIPORT_PWM:
 319         servo_flexi = true;
 320     default:
 321         break;
 322     }
 323
 324     /* Configure the PWM servo outputs. */
 325 #if defined(PIOS_INCLUDE_SERVO)
 326     if (servo_main) {
 327         if (servo_flexi) {
 328             servo_count = 4;
 329             PIOS_Servo_Init(&pios_servo_main_flexi_cfg);
 330         } else {
 331             servo_count = 2;
 332             PIOS_Servo_Init(&pios_servo_main_cfg);
 333         }
 334     } else if (servo_flexi) {
 335         servo_count = 2;
 336         PIOS_Servo_Init(&pios_servo_flexi_cfg);
 337     }
 338
 339     // Set bank modes and activate output. We need to do this here because oplm does not run Actuator module.
 340     PIOS_Servo_SetBankMode(0, PIOS_SERVO_BANK_MODE_PWM);
 341     PIOS_Servo_SetBankMode(1, PIOS_SERVO_BANK_MODE_PWM);
 342     PIOS_Servo_SetActive(0b11);
 343 #endif
 344
 345     PIOS_BOARD_IO_Configure_RFM22B();
 346
 347     /* Set the PPM callback if we should be receiving PPM. */
 348     if (ppm_mode || (ppm_only && !is_coordinator)) {
 349         PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback, 0);
 350     }
 351
 352     // Configure the primary radio stream destination.
 353     switch (oplinkSettings.RadioPriStream) {
 354     case OPLINKSETTINGS_RADIOPRISTREAM_DISABLED:
 355         break;
 356     case OPLINKSETTINGS_RADIOPRISTREAM_HID:
 357         // HID is always connected to GCS
 358         pios_com_gcs_id     = pios_com_hid_id;
 359         pios_com_gcs_out_id = pios_com_pri_radio_id;
 360         break;
 361     case OPLINKSETTINGS_RADIOPRISTREAM_MAIN:
 362         // Is the main port configured for telemetry (GCS)?
 363         if (oplinkSettings.MainPort == OPLINKSETTINGS_MAINPORT_TELEMETRY) {
 364             pios_com_gcs_id     = pios_com_main_id;
 365             pios_com_gcs_out_id = pios_com_pri_radio_id;
 366         } else {
 367             PIOS_COM_LinkComPair(pios_com_pri_radio_id, pios_com_main_id, false, false);
 368         }
 369         break;
 370     case OPLINKSETTINGS_RADIOPRISTREAM_FLEXI:
 371         // Is the flexi port configured for telemetry (GCS)?
 372         if (oplinkSettings.FlexiPort == OPLINKSETTINGS_FLEXIPORT_TELEMETRY) {
 373             pios_com_gcs_id     = pios_com_flexi_id;
 374             pios_com_gcs_out_id = pios_com_pri_radio_id;
 375         } else {
 376             PIOS_COM_LinkComPair(pios_com_pri_radio_id, pios_com_flexi_id, false, false);
 377         }
 378         break;
 379     case OPLINKSETTINGS_RADIOPRISTREAM_VCP:
 380         // VCP is never connected to GCS
 381         PIOS_COM_LinkComPair(pios_com_pri_radio_id, pios_com_vcp_id, false, false);
 382         break;
 383     }
 384
 385     // Configure the Auxiliary radio stream destination.
 386     switch (oplinkSettings.RadioAuxStream) {
 387     case OPLINKSETTINGS_RADIOAUXSTREAM_DISABLED:
 388         break;
 389     case OPLINKSETTINGS_RADIOAUXSTREAM_HID:
 390         // HID is always connected to GCS
 391         pios_com_gcs_id     = pios_com_hid_id;
 392         pios_com_gcs_out_id = pios_com_aux_radio_id;
 393         break;
 394     case OPLINKSETTINGS_RADIOAUXSTREAM_MAIN:
 395         // Is the main port configured for telemetry (GCS)?
 396         if (oplinkSettings.MainPort == OPLINKSETTINGS_MAINPORT_TELEMETRY) {
 397             pios_com_gcs_id     = pios_com_main_id;
 398             pios_com_gcs_out_id = pios_com_aux_radio_id;
 399         } else {
 400             PIOS_COM_LinkComPair(pios_com_aux_radio_id, pios_com_main_id, false, false);
 401         }
 402         break;
 403     case OPLINKSETTINGS_RADIOAUXSTREAM_FLEXI:
 404         // Is the flexi port configured for telemetry (GCS)?
 405         if (oplinkSettings.FlexiPort == OPLINKSETTINGS_FLEXIPORT_TELEMETRY) {
 406             pios_com_gcs_id     = pios_com_flexi_id;
 407             pios_com_gcs_out_id = pios_com_aux_radio_id;
 408         } else {
 409             PIOS_COM_LinkComPair(pios_com_aux_radio_id, pios_com_flexi_id, false, false);
 410         }
 411         break;
 412     case OPLINKSETTINGS_RADIOAUXSTREAM_VCP:
 413         // VCP is never connected to GCS
 414         PIOS_COM_LinkComPair(pios_com_aux_radio_id, pios_com_vcp_id, false, false);
 415         break;
 416     }
 417
 418     // Configure the VCP COM bridge
 419     switch (oplinkSettings.VCPBridge) {
 420     case OPLINKSETTINGS_VCPBRIDGE_MAIN:
 421         PIOS_COM_LinkComPair(pios_com_vcp_id, pios_com_main_id, true, true);
 422         break;
 423     case OPLINKSETTINGS_VCPBRIDGE_FLEXI:
 424         PIOS_COM_LinkComPair(pios_com_vcp_id, pios_com_flexi_id, true, true);
 425         break;
 426     case OPLINKSETTINGS_VCPBRIDGE_DISABLED:
 427         break;
 428     }
 429
 430     /* Remap AFIO pin */
 431     GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE);
 432
 433 #ifdef PIOS_INCLUDE_ADC
 434     PIOS_ADC_Init();
 435 #endif
 436 }
 437
 438 static void PIOS_Board_PPM_callback(__attribute__((unused)) uint32_t context, const int16_t *channels)
 439 {
 440 #if defined(PIOS_INCLUDE_PPM) && defined(PIOS_INCLUDE_PPM_OUT)
 441     if (pios_ppm_out_id) {
 442         for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) {
 443             if ((channels[i] != PIOS_RCVR_INVALID) && (channels[i] != PIOS_RCVR_TIMEOUT)) {
 444                 PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, channels[i]);
 445             }
 446         }
 447         // Rssi channel output is added after RC channels
 448         // Output Rssi from 1000µs to 2000µs (-127dBm to -16dBm range)
 449         if (ppm_rssi) {
 450             int8_t rssi;
 451             int16_t ppm_value;
 452             OPLinkStatusRSSIGet(&rssi);
 453             ppm_value = 1000 + ((rssi + 127) * 9);
 454             PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, RFM22B_PPM_NUM_CHANNELS, ppm_value);
 455         }
 456     } else {
 457         // If there is no PPM output defined, try sending the control outputs as a UAVObject.
 458         OPLinkReceiverData recv_data;
 459         for (uint8_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; ++i) {
 460             if (i < RFM22B_PPM_NUM_CHANNELS) {
 461                 recv_data.Channel[i] = channels[i];
 462             } else {
 463                 recv_data.Channel[i] = PIOS_RCVR_TIMEOUT;
 464             }
 465         }
 466         // Update the RSSI and quality fields.
 467         int8_t rssi;
 468         OPLinkStatusRSSIGet(&rssi);
 469         recv_data.RSSI = rssi;
 470         uint16_t quality;
 471         OPLinkStatusLinkQualityGet(&quality);
 472         // Link quality is 0-128, so scale it down to 0-100
 473         recv_data.LinkQuality = quality * 100 / 128;
 474         OPLinkReceiverSet(&recv_data);
 475     }
 476 #if defined(PIOS_INCLUDE_SERVO)
 477     for (uint8_t i = 0; i < servo_count; ++i) {
 478         uint16_t val = ((channels[i] == PIOS_RCVR_INVALID) || (channels[i] == PIOS_RCVR_TIMEOUT)) ? 0 : channels[i];
 479         PIOS_Servo_Set(i, val);
 480     }
 481 #endif /* PIOS_INCLUDE_SERVO */
 482 #endif /* PIOS_INCLUDE_PPM && PIOS_INCLUDE_PPM_OUT */
 483 }
 484
 485 /**
 486  * @}
 487  */