/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t AutotuneInitialize(void) { // Create a queue, connect to manual control command and flightstatus #ifdef MODULE_Autotune_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_AUTOTUNE] == MODULESETTINGS_ADMINSTATE_ENABLED) module_enabled = true; else module_enabled = false; #endif if (module_enabled) { SystemIdentInitialize(); at_queue = circ_queue_new(sizeof(struct at_queued_data), AT_QUEUE_NUMELEM); if (!at_queue) module_enabled = false; } return 0; }
/** * Module initialization */ int32_t PathPlannerInitialize() { taskHandle = NULL; #ifdef MODULE_PathPlanner_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_PATHPLANNER] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif if(module_enabled) { PathPlannerSettingsInitialize(); WaypointInitialize(); WaypointActiveInitialize(); // Create object queue queue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); return 0; } return -1; }
/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t GeofenceInitialize(void) { bool module_enabled = false; #ifdef MODULE_Geofence_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_GEOFENCE] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif if (module_enabled) { GeoFenceSettingsInitialize(); // allocate and initialize the static data storage only if module is enabled geofenceSettings = (GeoFenceSettingsData *) PIOS_malloc(sizeof(GeoFenceSettingsData)); if (geofenceSettings == NULL) { module_enabled = false; return -1; } GeoFenceSettingsConnectCallback(settingsUpdated); settingsUpdated(NULL, NULL, NULL, 0); return 0; } return -1; }
/** * Initialise the Logging module * \return -1 if initialisation failed * \return 0 on success */ int32_t LoggingInitialize(void) { // TODO: make selectable logging_com_id = PIOS_COM_LOGGING; #ifdef MODULE_Logging_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_LOGGING] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif if (!logging_com_id) module_enabled = false; if (!module_enabled) return -1; LoggingStatsInitialize(); LoggingSettingsInitialize(); // Initialise UAVTalk uavTalkCon = UAVTalkInitialize(&send_data); return 0; }
/** * Initialise the UAVORelay module * \return -1 if initialisation failed * \return 0 on success */ int32_t UAVORelayInitialize(void) { // TODO: make selectable uavorelay_com_id = pios_com_can_id; #ifdef MODULE_UAVORelay_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_UAVORELAY] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif if (!uavorelay_com_id) module_enabled = false; if (!module_enabled) return -1; // Create object queues queue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); // Initialise UAVTalk uavTalkCon = UAVTalkInitialize(&send_data); CameraDesiredInitialize(); return 0; }
/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ static int32_t MagBaroInitialize() { #ifdef MODULE_MagBaro_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_MAGBARO] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif if (module_enabled) { MagnetometerInitialize(); BaroAltitudeInitialize(); // init down-sampling data alt_ds_temp = 0; alt_ds_pres = 0; alt_ds_count = 0; } return 0; }
/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t FixedWingPathFollowerInitialize() { #ifdef MODULE_FixedWingPathFollower_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_FIXEDWINGPATHFOLLOWER] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif if (module_enabled) { FixedWingPathFollowerSettingsInitialize(); FixedWingAirspeedsInitialize(); FixedWingPathFollowerStatusInitialize(); PathDesiredInitialize(); PathStatusInitialize(); VelocityDesiredInitialize(); AirspeedActualInitialize(); } return 0; }
/** * Initialise the module * \return -1 if initialisation failed * \return 0 on success */ static int32_t uavoMavlinkBridgeInitialize(void) { mavlink_port = PIOS_COM_MAVLINK; uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (mavlink_port && (module_state[MODULESETTINGS_ADMINSTATE_UAVOMAVLINKBRIDGE] == MODULESETTINGS_ADMINSTATE_ENABLED)) { updateSettings(); mav_msg = PIOS_malloc(sizeof(*mav_msg)); stream_ticks = PIOS_malloc_no_dma(MAXSTREAMS); if (mav_msg && stream_ticks) { for (int x = 0; x < MAXSTREAMS; ++x) { stream_ticks[x] = (TASK_RATE_HZ / mav_rates[x]); } module_enabled = true; } } return 0; }
/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t VtolPathFollowerInitialize() { #ifdef MODULE_VtolPathFollower_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_VTOLPATHFOLLOWER] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif if (!module_enabled) { return -1; } VtolPathFollowerSettingsInitialize(); PathStatusInitialize(); NedAccelInitialize(); PathDesiredInitialize(); VelocityDesiredInitialize(); return 0; }
/** * Initialise the module * \return -1 if initialisation failed * \return 0 on success */ static int32_t comUsbBridgeInitialize(void) { // TODO: Get from settings object usart_port = PIOS_COM_BRIDGE; vcp_port = PIOS_COM_VCP; /* Only run the module if we have a VCP port and a selected USART port */ if (!usart_port || !vcp_port) { module_enabled = false; return 0; } #ifdef MODULE_ComUsbBridge_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_COMUSBBRIDGE] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif if (module_enabled) { updateSettings(); } return 0; }
/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t AltitudeHoldInitialize() { #ifdef MODULE_AltitudeHold_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_ALTITUDEHOLD] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif if(module_enabled) { AltitudeHoldSettingsInitialize(); AltitudeHoldDesiredInitialize(); // Create object queue queue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); return 0; } return -1; }
/** * Initialise the overo sync module * \return -1 if initialisation failed * \return 0 on success */ int32_t OveroSyncInitialize(void) { #ifdef MODULE_OveroSync_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_OVEROSYNC] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif if (!module_enabled) return -1; // Create object queues queue = PIOS_Queue_Create(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); OveroSyncStatsInitialize(); // Initialise UAVTalk uavTalkCon = UAVTalkInitialize(&pack_data); return 0; }
/** * Initialise the gps module * \return -1 if initialisation failed * \return 0 on success */ int32_t GPSInitialize(void) { gpsPort = PIOS_COM_GPS; uint8_t gpsProtocol; #ifdef MODULE_GPS_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_GPS] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif // These things are only conditional on small F1 targets. // Expected to be always present otherwise. #ifdef SMALLF1 if (gpsPort && module_enabled) { #endif GPSPositionInitialize(); GPSVelocityInitialize(); #if !defined(PIOS_GPS_MINIMAL) GPSTimeInitialize(); GPSSatellitesInitialize(); HomeLocationInitialize(); UBloxInfoInitialize(); #endif #if defined(PIOS_GPS_PROVIDES_AIRSPEED) AirspeedActualInitialize(); #endif updateSettings(); #ifdef SMALLF1 } #endif if (gpsPort && module_enabled) { ModuleSettingsGPSDataProtocolGet(&gpsProtocol); switch (gpsProtocol) { case MODULESETTINGS_GPSDATAPROTOCOL_NMEA: gps_rx_buffer = PIOS_malloc(NMEA_MAX_PACKET_LENGTH); break; case MODULESETTINGS_GPSDATAPROTOCOL_UBX: gps_rx_buffer = PIOS_malloc(sizeof(struct UBXPacket)); break; default: gps_rx_buffer = NULL; } PIOS_Assert(gps_rx_buffer); return 0; } return -1; }
//! Check if RTH is possible and safe static bool check_rth_preconditions_met() { // If transmitter throttle was low before failsafe kicked in // RTH is not safe, because pilot probably just forgot to disarm // before powering transmitter off if (throttle_low_before_engage) return false; // No RTH when disarmed if (!armed_when_enabled) return false; // Only VTOLs are currently RTH capable uint8_t airframe_type; SystemSettingsAirframeTypeGet(&airframe_type); if (airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX && airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP && airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA && airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO && airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX && airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV && airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP && airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX && airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_TRI && airframe_type != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) return false; uint8_t module_admin_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_admin_state); // VTOLPathFollower module must be enabled if (module_admin_state[MODULESETTINGS_ADMINSTATE_VTOLPATHFOLLOWER] != MODULESETTINGS_ADMINSTATE_ENABLED) return false; // GPS module must be enabled if (module_admin_state[MODULESETTINGS_ADMINSTATE_GPS] != MODULESETTINGS_ADMINSTATE_ENABLED) return false; // check path follower alarm if (AlarmsGet(SYSTEMALARMS_ALARM_PATHFOLLOWER) >= SYSTEMALARMS_ALARM_WARNING) return false; // attitude filter must be set to complementary or INSOutdoor and navigation filter must be INS StateEstimationData stateEstimation; StateEstimationGet(&stateEstimation); if (stateEstimation.AttitudeFilter == STATEESTIMATION_ATTITUDEFILTER_INSINDOOR || stateEstimation.NavigationFilter != STATEESTIMATION_NAVIGATIONFILTER_INS) return false; return true; }
/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t BatteryInitialize(void) { #ifdef MODULE_Battery_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_BATTERY] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; return 0; } #endif FlightBatterySettingsInitialize(); FlightBatteryStateInitialize(); return 0; }
/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t AutotuneInitialize(void) { // Create a queue, connect to manual control command and flightstatus #ifdef MODULE_Autotune_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_AUTOTUNE] == MODULESETTINGS_ADMINSTATE_ENABLED) module_enabled = true; else module_enabled = false; #endif if (module_enabled) { RelayTuningSettingsInitialize(); RelayTuningInitialize(); } return 0; }
/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t AirspeedInitialize() { #ifdef MODULE_Airspeed_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_AIRSPEED] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif if (!module_enabled) return -1; #ifdef BARO_AIRSPEED_PRESENT ADCRoutingInitialize(); uint8_t adc_channel_map[ADCROUTING_CHANNELMAP_NUMELEM]; ADCRoutingChannelMapGet(adc_channel_map); //Determine if the barometric airspeed sensor is routed to an ADC pin for (int i = 0; i < ADCROUTING_CHANNELMAP_NUMELEM; i++) { if (adc_channel_map[i] == ADCROUTING_CHANNELMAP_ANALOGAIRSPEED) { airspeedADCPin = i; } } #endif BaroAirspeedInitialize(); AirspeedActualInitialize(); AirspeedSettingsInitialize(); AirspeedSettingsConnectCallback(AirspeedSettingsUpdatedCb); return 0; }
/** * Initialise the Logging module * \return -1 if initialisation failed * \return 0 on success */ int32_t LoggingInitialize(void) { if (PIOS_COM_OPENLOG) { logging_com_id = PIOS_COM_OPENLOG; destination_spi_flash = false; } else if (PIOS_COM_SPIFLASH) { logging_com_id = PIOS_COM_SPIFLASH; destination_spi_flash = true; } #ifdef MODULE_Logging_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_LOGGING] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif if (!logging_com_id) module_enabled = false; if (!module_enabled) return -1; LoggingStatsInitialize(); LoggingSettingsInitialize(); // Initialise UAVTalk uavTalkCon = UAVTalkInitialize(&send_data); return 0; }
void PIOS_Board_Init(void) { /* Delay system */ PIOS_DELAY_Init(); const struct pios_board_info * bdinfo = &pios_board_info_blob; #if defined(PIOS_INCLUDE_LED) const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); PIOS_Assert(led_cfg); PIOS_LED_Init(led_cfg); #endif /* PIOS_INCLUDE_LED */ /* Set up the SPI interface to the gyro/acelerometer */ if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } /* Set up the SPI interface to the flash and rfm22b */ if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_FLASH) /* Connect flash to the approrpiate interface and configure it */ uintptr_t flash_id; if (PIOS_Flash_Jedec_Init(&flash_id, pios_spi_telem_flash_id, 1, &flash_m25p_cfg) != 0) panic(1); uintptr_t fs_id; if (PIOS_FLASHFS_Logfs_Init(&fs_id, &flashfs_m25p_cfg, &pios_jedec_flash_driver, flash_id) != 0) panic(1); #endif /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); HwFreedomInitialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* Initialize the alarms library */ AlarmsInitialize(); /* Initialize the task monitor library */ TaskMonitorInitialize(); // /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_3_cfg); /* IAP System Setup */ PIOS_IAP_Init(); uint16_t boot_count = PIOS_IAP_ReadBootCount(); if (boot_count < 3) { PIOS_IAP_WriteBootCount(++boot_count); AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); } else { /* Too many failed boot attempts, force hw config to defaults */ HwFreedomSetDefaults(HwFreedomHandle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; #endif uint32_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwFreedomUSB_VCPPortGet(&hw_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hw_usb_vcpport = HWFREEDOM_USB_VCPPORT_DISABLED; } switch (hw_usb_vcpport) { case HWFREEDOM_USB_VCPPORT_DISABLED: break; case HWFREEDOM_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_TELEM_USB_RX_BUF_LEN, PIOS_COM_TELEM_USB_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_telem_usb_id); #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usb_cdc_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usb_cdc_com_driver, &pios_com_vcp_id); #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uint32_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwFreedomUSB_HIDPortGet(&hw_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hw_usb_hidport = HWFREEDOM_USB_HIDPORT_DISABLED; } switch (hw_usb_hidport) { case HWFREEDOM_USB_HIDPORT_DISABLED: break; case HWFREEDOM_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint32_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_HID */ if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } #endif /* PIOS_INCLUDE_USB */ /* Configure IO ports */ uint8_t hw_DSMxBind; HwFreedomDSMxBindGet(&hw_DSMxBind); /* Configure FlexiPort */ uint8_t hw_mainport; HwFreedomMainPortGet(&hw_mainport); switch (hw_mainport) { case HWFREEDOM_MAINPORT_DISABLED: break; case HWFREEDOM_MAINPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; break; case HWFREEDOM_MAINPORT_GPS: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_MAINPORT_DSM2: case HWFREEDOM_MAINPORT_DSMX10BIT: case HWFREEDOM_MAINPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hw_mainport) { case HWFREEDOM_MAINPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWFREEDOM_MAINPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWFREEDOM_MAINPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hw_DSMxBind); } break; case HWFREEDOM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_MAINPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; } /* hw_freedom_mainport */ /* Configure flexi USART port */ uint8_t hw_flexiport; HwFreedomFlexiPortGet(&hw_flexiport); switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_DISABLED: break; case HWFREEDOM_FLEXIPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWFREEDOM_FLEXIPORT_GPS: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, -1, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_FLEXIPORT_DSM2: case HWFREEDOM_FLEXIPORT_DSMX10BIT: case HWFREEDOM_FLEXIPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWFREEDOM_FLEXIPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWFREEDOM_FLEXIPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_flexi_cfg, &pios_dsm_flexi_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,&hw_DSMxBind); } break; case HWFREEDOM_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_I2C */ case HWFREEDOM_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_FLEXIPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; } /* hw_freedom_flexiport */ /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) uint8_t hwsettings_radioport; HwFreedomRadioPortGet(&hwsettings_radioport); switch (hwsettings_radioport) { case HWFREEDOM_RADIOPORT_DISABLED: break; case HWFREEDOM_RADIOPORT_TELEMETRY: { const struct pios_board_info * bdinfo = &pios_board_info_blob; const struct pios_rfm22b_cfg *pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) { PIOS_Assert(0); } uint8_t *rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { PIOS_Assert(0); } break; } } #endif /* PIOS_INCLUDE_RFM22B */ /* Configure input receiver USART port */ uint8_t hw_rcvrport; HwFreedomRcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_DISABLED: break; case HWFREEDOM_RCVRPORT_PPM: { uint32_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); uint32_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } break; case HWFREEDOM_RCVRPORT_DSM2: case HWFREEDOM_RCVRPORT_DSMX10BIT: case HWFREEDOM_RCVRPORT_DSMX11BIT: { enum pios_dsm_proto proto; switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWFREEDOM_RCVRPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWFREEDOM_RCVRPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_rcvr_cfg, &pios_dsm_rcvr_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,&hw_DSMxBind); } break; case HWFREEDOM_RCVRPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) { uint32_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_rcvr_cfg)) { PIOS_Assert(0); } uint32_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } uint32_t pios_sbus_rcvr_id; if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; } #endif break; } if (hw_rcvrport != HWFREEDOM_RCVRPORT_SBUS) { GPIO_Init(pios_sbus_cfg.inv.gpio, (GPIO_InitTypeDef*)&pios_sbus_cfg.inv.init); GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); } #if defined(PIOS_OVERO_SPI) /* Set up the SPI based PIOS_COM interface to the overo */ { bool overo_enabled = false; #ifdef MODULE_OveroSync_BUILTIN overo_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_OVEROSYNC] == MODULESETTINGS_ADMINSTATE_ENABLED) { overo_enabled = true; } else { overo_enabled = false; } #endif if (overo_enabled) { if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { PIOS_DEBUG_Assert(0); } const uint32_t PACKET_SIZE = 1024; uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_overo_id, &pios_overo_com_driver, pios_overo_id, rx_buffer, PACKET_SIZE, tx_buffer, PACKET_SIZE)) { PIOS_Assert(0); } } } #endif #if defined(PIOS_INCLUDE_GCSRCVR) GCSReceiverInitialize(); uint32_t pios_gcsrcvr_id; PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); uint32_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS uint8_t hw_output_port; HwFreedomOutputGet(&hw_output_port); switch (hw_output_port) { case HWFREEDOM_OUTPUT_DISABLED: break; case HWFREEDOM_OUTPUT_PWM: /* Set up the servo outputs */ PIOS_Servo_Init(&pios_servo_cfg); break; default: break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif PIOS_DELAY_WaitmS(200); PIOS_SENSORS_Init(); #if defined(PIOS_INCLUDE_MPU6000) if (PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg) != 0) panic(2); if (PIOS_MPU6000_Test() != 0) panic(2); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwFreedomGyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWFREEDOM_GYRORANGE_250: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWFREEDOM_GYRORANGE_500: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWFREEDOM_GYRORANGE_1000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWFREEDOM_GYRORANGE_2000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwFreedomAccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWFREEDOM_ACCELRANGE_2G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWFREEDOM_ACCELRANGE_4G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWFREEDOM_ACCELRANGE_8G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWFREEDOM_ACCELRANGE_16G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_16G); break; } uint8_t hw_mpu6000_rate; HwFreedomMPU6000RateGet(&hw_mpu6000_rate); uint16_t hw_mpu6000_divisor = \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_500) ? 15 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_666) ? 11 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_1000) ? 7 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_2000) ? 3 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_4000) ? 1 : \ (hw_mpu6000_rate == HWFREEDOM_MPU6000RATE_8000) ? 0 : \ 15; PIOS_MPU6000_SetDivisor(hw_mpu6000_divisor); uint8_t hw_dlpf; HwFreedomMPU6000DLPFGet(&hw_dlpf); uint16_t hw_mpu6000_dlpf = \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_dlpf == HWFREEDOM_MPU6000DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ PIOS_MPU60X0_LOWPASS_256_HZ; PIOS_MPU6000_SetLPF(hw_mpu6000_dlpf); #endif if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_mag_pressure_adapter_id) != 0) panic(5); PIOS_DELAY_WaitmS(50); #if defined(PIOS_INCLUDE_ADC) PIOS_ADC_Init(&pios_adc_cfg); #endif PIOS_LED_On(0); #if defined(PIOS_INCLUDE_HMC5883) if (PIOS_HMC5883_Init(pios_i2c_mag_pressure_adapter_id, &pios_hmc5883_cfg) != 0) panic(3); #endif PIOS_LED_On(1); #if defined(PIOS_INCLUDE_MS5611) if (PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id) != 0) panic(4); #endif PIOS_LED_On(2); }
void PIOS_Board_Init(void) { const struct pios_board_info * bdinfo = &pios_board_info_blob; /* Delay system */ PIOS_DELAY_Init(); PIOS_LED_Init(&pios_led_cfg); /* Set up the SPI interface to the accelerometer*/ if (PIOS_SPI_Init(&pios_spi_accel_id, &pios_spi_accel_cfg)) { PIOS_DEBUG_Assert(0); } /* Set up the SPI interface to the gyro */ if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } #if !defined(PIOS_FLASH_ON_ACCEL) /* Set up the SPI interface to the flash */ if (PIOS_SPI_Init(&pios_spi_flash_id, &pios_spi_flash_cfg)) { PIOS_DEBUG_Assert(0); } /* Inititialize all flash drivers */ PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_flash_id, 0, &flash_m25p_cfg); #else /* Inititialize all flash drivers */ PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_accel_id, 1, &flash_m25p_cfg); #endif PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg); /* Register the partition table */ const struct pios_flash_partition * flash_partition_table; uint32_t num_partitions; flash_partition_table = PIOS_BOARD_HW_DEFS_GetPartitionTable(bdinfo->board_rev, &num_partitions); PIOS_FLASH_register_partition_table(flash_partition_table, num_partitions); /* Mount all filesystems */ PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_settings_cfg, FLASH_PARTITION_LABEL_SETTINGS); PIOS_FLASHFS_Logfs_Init(&pios_waypoints_settings_fs_id, &flashfs_waypoints_cfg, FLASH_PARTITION_LABEL_WAYPOINTS); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); HwRevolutionInitialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* Initialize the alarms library */ AlarmsInitialize(); /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_3_cfg); PIOS_TIM_InitClock(&tim_4_cfg); PIOS_TIM_InitClock(&tim_5_cfg); PIOS_TIM_InitClock(&tim_9_cfg); PIOS_TIM_InitClock(&tim_10_cfg); PIOS_TIM_InitClock(&tim_11_cfg); /* IAP System Setup */ PIOS_IAP_Init(); uint16_t boot_count = PIOS_IAP_ReadBootCount(); if (boot_count < 3) { PIOS_IAP_WriteBootCount(++boot_count); AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); } else { /* Too many failed boot attempts, force hw config to defaults */ HwRevolutionSetDefaults(HwRevolutionHandle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } //PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; #endif uintptr_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwRevolutionUSB_VCPPortGet(&hw_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hw_usb_vcpport = HWREVOLUTION_USB_VCPPORT_DISABLED; } uintptr_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } switch (hw_usb_vcpport) { case HWREVOLUTION_USB_VCPPORT_DISABLED: break; case HWREVOLUTION_USB_VCPPORT_USBTELEMETRY: #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_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 HWREVOLUTION_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { 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; case HWREVOLUTION_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwRevolutionUSB_HIDPortGet(&hw_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hw_usb_hidport = HWREVOLUTION_USB_HIDPORT_DISABLED; } uintptr_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } switch (hw_usb_hidport) { case HWREVOLUTION_USB_HIDPORT_DISABLED: break; case HWREVOLUTION_USB_HIDPORT_USBTELEMETRY: #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_hid_com_driver, pios_usb_hid_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_HID */ if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } #endif /* PIOS_INCLUDE_USB */ /* Configure IO ports */ uint8_t hw_DSMxBind; HwRevolutionDSMxBindGet(&hw_DSMxBind); /* Configure Telemetry port */ uint8_t hw_telemetryport; HwRevolutionTelemetryPortGet(&hw_telemetryport); switch (hw_telemetryport){ case HWREVOLUTION_TELEMETRYPORT_DISABLED: break; case HWREVOLUTION_TELEMETRYPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWREVOLUTION_TELEMETRYPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) PIOS_Board_configure_com(&pios_usart_telem_cfg, 0, PIOS_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWREVOLUTION_TELEMETRYPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_telem_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; } /* hw_telemetryport */ /* Configure GPS port */ uint8_t hw_gpsport; HwRevolutionGPSPortGet(&hw_gpsport); switch (hw_gpsport){ case HWREVOLUTION_GPSPORT_DISABLED: break; case HWREVOLUTION_GPSPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWREVOLUTION_GPSPORT_GPS: PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_GPS_RX_BUF_LEN, 0, &pios_usart_com_driver, &pios_com_gps_id); break; case HWREVOLUTION_GPSPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) PIOS_Board_configure_com(&pios_usart_gps_cfg, 0, PIOS_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWREVOLUTION_GPSPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_gps_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; }/* hw_gpsport */ /* Configure AUXPort */ uint8_t hw_auxport; HwRevolutionAuxPortGet(&hw_auxport); switch (hw_auxport) { case HWREVOLUTION_AUXPORT_DISABLED: break; case HWREVOLUTION_AUXPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWREVOLUTION_AUXPORT_DSM2: case HWREVOLUTION_AUXPORT_DSMX10BIT: case HWREVOLUTION_AUXPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) { enum pios_dsm_proto proto; switch (hw_auxport) { case HWREVOLUTION_AUXPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWREVOLUTION_AUXPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWREVOLUTION_AUXPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_hsum_aux_cfg, &pios_dsm_aux_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWREVOLUTION_AUXPORT_HOTTSUMD: case HWREVOLUTION_AUXPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_auxport) { case HWREVOLUTION_AUXPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWREVOLUTION_AUXPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_aux_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWREVOLUTION_AUXPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_aux_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWREVOLUTION_AUXPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) && defined(PIOS_INCLUDE_GPS) PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); pios_com_mavlink_id = pios_com_gps_id; #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWREVOLUTION_AUXPORT_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); #endif /* PIOS_INCLUDE_HOTT */ break; case HWREVOLUTION_AUXPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) PIOS_Board_configure_com(&pios_usart_aux_cfg, 0, PIOS_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWREVOLUTION_AUXPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_aux_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; } /* hw_auxport */ /* Configure AUXSbusPort */ //TODO: ensure that the serial invertion pin is setted correctly uint8_t hw_auxsbusport; HwRevolutionAuxSBusPortGet(&hw_auxsbusport); switch (hw_auxsbusport) { case HWREVOLUTION_AUXSBUSPORT_DISABLED: break; case HWREVOLUTION_AUXSBUSPORT_SBUS: #ifdef PIOS_INCLUDE_SBUS { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_auxsbus_cfg)) { PIOS_Assert(0); } uintptr_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } uintptr_t pios_sbus_rcvr_id; if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; } #endif /* PIOS_INCLUDE_SBUS */ break; case HWREVOLUTION_AUXSBUSPORT_DSM2: case HWREVOLUTION_AUXSBUSPORT_DSMX10BIT: case HWREVOLUTION_AUXSBUSPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) { enum pios_dsm_proto proto; switch (hw_auxsbusport) { case HWREVOLUTION_AUXSBUSPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWREVOLUTION_AUXSBUSPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWREVOLUTION_AUXSBUSPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_hsum_auxsbus_cfg, &pios_dsm_auxsbus_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWREVOLUTION_AUXSBUSPORT_HOTTSUMD: case HWREVOLUTION_AUXSBUSPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_auxsbusport) { case HWREVOLUTION_AUXSBUSPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWREVOLUTION_AUXSBUSPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_auxsbus_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWREVOLUTION_AUXSBUSPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWREVOLUTION_AUXSBUSPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) && defined(PIOS_INCLUDE_GPS) PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); pios_com_mavlink_id = pios_com_gps_id; #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWREVOLUTION_AUXSBUSPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, 0, PIOS_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWREVOLUTION_AUXSBUSPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; case HWREVOLUTION_AUXSBUSPORT_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_auxsbus_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); #endif /* PIOS_INCLUDE_HOTT */ break; } /* hw_auxport */ /* Configure FlexiPort */ uint8_t hw_flexiport; HwRevolutionFlexiPortGet(&hw_flexiport); switch (hw_flexiport) { case HWREVOLUTION_FLEXIPORT_DISABLED: break; case HWREVOLUTION_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_I2C */ break; case HWREVOLUTION_FLEXIPORT_DSM2: case HWREVOLUTION_FLEXIPORT_DSMX10BIT: case HWREVOLUTION_FLEXIPORT_DSMX11BIT: #if defined(PIOS_INCLUDE_DSM) { enum pios_dsm_proto proto; switch (hw_flexiport) { case HWREVOLUTION_FLEXIPORT_DSM2: proto = PIOS_DSM_PROTO_DSM2; break; case HWREVOLUTION_FLEXIPORT_DSMX10BIT: proto = PIOS_DSM_PROTO_DSMX10BIT; break; case HWREVOLUTION_FLEXIPORT_DSMX11BIT: proto = PIOS_DSM_PROTO_DSMX11BIT; break; default: PIOS_Assert(0); break; } //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_hsum_flexi_cfg, &pios_dsm_flexi_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWREVOLUTION_FLEXIPORT_HOTTSUMD: case HWREVOLUTION_FLEXIPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_flexiport) { case HWREVOLUTION_FLEXIPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWREVOLUTION_FLEXIPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_flexi_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWREVOLUTION_FLEXIPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWREVOLUTION_FLEXIPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) && defined(PIOS_INCLUDE_MAVLINK) && defined(PIOS_INCLUDE_GPS) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); pios_com_mavlink_id = pios_com_gps_id; #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWREVOLUTION_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWREVOLUTION_FLEXIPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; case HWREVOLUTION_FLEXIPORT_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); #endif /* PIOS_INCLUDE_HOTT */ break; } /* hw_flexiport */ /* Configure the receiver port*/ uint8_t hw_rcvrport; HwRevolutionRcvrPortGet(&hw_rcvrport); // switch (hw_rcvrport){ case HWREVOLUTION_RCVRPORT_DISABLED: break; case HWREVOLUTION_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) { /* Set up the receiver port. Later this should be optional */ uintptr_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); uintptr_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } #endif /* PIOS_INCLUDE_PWM */ break; case HWREVOLUTION_RCVRPORT_PPM: case HWREVOLUTION_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_PPM) { uintptr_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); uintptr_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } #endif /* PIOS_INCLUDE_PPM */ case HWREVOLUTION_RCVRPORT_OUTPUTS: break; } #if defined(PIOS_OVERO_SPI) /* Set up the SPI based PIOS_COM interface to the overo */ { bool overo_enabled = false; #ifdef MODULE_OveroSync_BUILTIN overo_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_OVEROSYNC] == MODULESETTINGS_ADMINSTATE_ENABLED) { overo_enabled = true; } else { overo_enabled = false; } #endif if (overo_enabled) { if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { PIOS_DEBUG_Assert(0); } const uint32_t PACKET_SIZE = 1024; uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PACKET_SIZE); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_overo_id, &pios_overo_com_driver, pios_overo_id, rx_buffer, PACKET_SIZE, tx_buffer, PACKET_SIZE)) { PIOS_Assert(0); } } } #endif #if defined(PIOS_INCLUDE_GCSRCVR) GCSReceiverInitialize(); uintptr_t pios_gcsrcvr_id; PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); uintptr_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS switch (hw_rcvrport) { case HWREVOLUTION_RCVRPORT_DISABLED: case HWREVOLUTION_RCVRPORT_PWM: case HWREVOLUTION_RCVRPORT_PPM: /* Set up the servo outputs */ PIOS_Servo_Init(&pios_servo_cfg); break; case HWREVOLUTION_RCVRPORT_PPMOUTPUTS: case HWREVOLUTION_RCVRPORT_OUTPUTS: //PIOS_Servo_Init(&pios_servo_rcvr_cfg); //TODO: Prepare the configurations on board_hw_defs and handle here: PIOS_Servo_Init(&pios_servo_cfg); break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif if (PIOS_I2C_Init(&pios_i2c_mag_adapter_id, &pios_i2c_mag_adapter_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_Init(&pios_i2c_pressure_adapter_id, &pios_i2c_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } PIOS_DELAY_WaitmS(50); PIOS_SENSORS_Init(); #if defined(PIOS_INCLUDE_ADC) uint32_t internal_adc_id; if(PIOS_INTERNAL_ADC_Init(&internal_adc_id, &pios_adc_cfg) < 0) PIOS_Assert(0); PIOS_ADC_Init(&pios_internal_adc_id, &pios_internal_adc_driver, internal_adc_id); #endif #if defined(PIOS_INCLUDE_HMC5883) PIOS_HMC5883_Init(PIOS_I2C_MAIN_ADAPTER, &pios_hmc5883_cfg); #endif #if defined(PIOS_INCLUDE_MS5611) PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_pressure_adapter_id); #endif switch(bdinfo->board_rev) { case 0x01: #if defined(PIOS_INCLUDE_L3GD20) PIOS_L3GD20_Init(pios_spi_gyro_id, 0, &pios_l3gd20_cfg); PIOS_Assert(PIOS_L3GD20_Test() == 0); #endif #if defined(PIOS_INCLUDE_BMA180) PIOS_BMA180_Init(pios_spi_accel_id, 0, &pios_bma180_cfg); PIOS_Assert(PIOS_BMA180_Test() == 0); #endif break; case 0x02: #if defined(PIOS_INCLUDE_MPU6000) PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwRevolutionGyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWREVOLUTION_GYRORANGE_250: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWREVOLUTION_GYRORANGE_500: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWREVOLUTION_GYRORANGE_1000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWREVOLUTION_GYRORANGE_2000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwRevolutionAccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWREVOLUTION_ACCELRANGE_2G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWREVOLUTION_ACCELRANGE_4G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWREVOLUTION_ACCELRANGE_8G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWREVOLUTION_ACCELRANGE_16G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_16G); break; } // the filter has to be set before rate else divisor calculation will fail uint8_t hw_mpu6000_dlpf; HwRevolutionMPU6000DLPFGet(&hw_mpu6000_dlpf); enum pios_mpu60x0_filter mpu6000_dlpf = \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_mpu6000_dlpf == HWREVOLUTION_MPU6000DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ pios_mpu6000_cfg.default_filter; PIOS_MPU6000_SetLPF(mpu6000_dlpf); uint8_t hw_mpu6000_samplerate; HwRevolutionMPU6000RateGet(&hw_mpu6000_samplerate); uint16_t mpu6000_samplerate = \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_200) ? 200 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_333) ? 333 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_500) ? 500 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_666) ? 666 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_1000) ? 1000 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_2000) ? 2000 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_4000) ? 4000 : \ (hw_mpu6000_samplerate == HWREVOLUTION_MPU6000RATE_8000) ? 8000 : \ pios_mpu6000_cfg.default_samplerate; PIOS_MPU6000_SetSampleRate(mpu6000_samplerate); #endif break; default: PIOS_DEBUG_Assert(0); } }
/** * Initialise the module, called on startup * \returns 0 on success or -1 if initialisation failed */ int32_t TxPIDInitialize(void) { bool module_enabled; #ifdef MODULE_TxPID_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_TXPID] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif if (module_enabled) { TxPIDSettingsInitialize(); AccessoryDesiredInitialize(); UAVObjEvent ev = { .obj = AccessoryDesiredHandle(), .instId = 0, .event = 0, }; EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS); #if (TELEMETRY_UPDATE_PERIOD_MS != 0) // Change StabilizationSettings update rate from OnChange to periodic // to prevent telemetry link flooding with frequent updates in case of // control channel jitter. // Warning: saving to flash with this code active will change the // StabilizationSettings update rate permanently. Use Metadata via // browser to reset to defaults (telemetryAcked=true, OnChange). UAVObjMetadata metadata; StabilizationSettingsInitialize(); StabilizationSettingsGetMetadata(&metadata); metadata.telemetryAcked = 0; metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC; metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS; StabilizationSettingsSetMetadata(&metadata); #endif return 0; } return -1; } MODULE_INITCALL(TxPIDInitialize, NULL); /** * Update PIDs callback function */ static void updatePIDs(UAVObjEvent* ev) { if (ev->obj != AccessoryDesiredHandle()) return; TxPIDSettingsData inst; TxPIDSettingsGet(&inst); if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) return; uint8_t armed; FlightStatusArmedGet(&armed); if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) && (armed == FLIGHTSTATUS_ARMED_DISARMED)) return; StabilizationSettingsData stab; StabilizationSettingsGet(&stab); #ifdef UAVOBJ_INIT_vtolpathfollowersettings VtolPathFollowerSettingsData vtolPathFollowerSettingsData; // Check to make sure the settings UAVObject has been instantiated if (VtolPathFollowerSettingsHandle()) { VtolPathFollowerSettingsGet(&vtolPathFollowerSettingsData); } bool vtolPathFollowerSettingsNeedsUpdate = false; #endif AccessoryDesiredData accessory; bool stabilizationSettingsNeedsUpdate = false; // Loop through every enabled instance for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { if (inst.PIDs[i] != TXPIDSETTINGS_PIDS_DISABLED) { float value; if (inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) { ManualControlCommandThrottleGet(&value); value = scale(value, inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN], inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX], inst.MinPID[i], inst.MaxPID[i]); } else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) { value = scale(accessory.AccessoryVal, -1.0, 1.0, inst.MinPID[i], inst.MaxPID[i]); } else { continue; } switch (inst.PIDs[i]) { case TXPIDSETTINGS_PIDS_ROLLRATEKP: stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); break; case TXPIDSETTINGS_PIDS_ROLLRATEKI: stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); break; case TXPIDSETTINGS_PIDS_ROLLRATEKD: stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); break; case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT: stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); break; case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP: stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); break; case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI: stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); break; case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT: stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); break; case TXPIDSETTINGS_PIDS_PITCHRATEKP: stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); break; case TXPIDSETTINGS_PIDS_PITCHRATEKI: stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); break; case TXPIDSETTINGS_PIDS_PITCHRATEKD: stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); break; case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT: stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); break; case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP: stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); break; case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI: stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); break; case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT: stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP: stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value); stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI: stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value); stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD: stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value); stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT: stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value); stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP: stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value); stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI: stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value); stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT: stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value); stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value); break; case TXPIDSETTINGS_PIDS_YAWRATEKP: stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value); break; case TXPIDSETTINGS_PIDS_YAWRATEKI: stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value); break; case TXPIDSETTINGS_PIDS_YAWRATEKD: stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value); break; case TXPIDSETTINGS_PIDS_YAWRATEILIMIT: stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value); break; case TXPIDSETTINGS_PIDS_YAWATTITUDEKP: stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value); break; case TXPIDSETTINGS_PIDS_YAWATTITUDEKI: stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value); break; case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT: stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value); break; case TXPIDSETTINGS_PIDS_GYROCUTOFF: stabilizationSettingsNeedsUpdate |= update(&stab.GyroCutoff, value); break; case TXPIDSETTINGS_PIDS_ROLLVBARSENSITIVITY: stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_ROLL], value); break; case TXPIDSETTINGS_PIDS_PITCHVBARSENSITIVITY: stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_PITCH], value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHVBARSENSITIVITY: stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_ROLL], value); stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_PITCH], value); break; case TXPIDSETTINGS_PIDS_YAWVBARSENSITIVITY: stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_YAW], value); break; case TXPIDSETTINGS_PIDS_ROLLVBARKP: stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KP], value); break; case TXPIDSETTINGS_PIDS_ROLLVBARKI: stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KI], value); break; case TXPIDSETTINGS_PIDS_ROLLVBARKD: stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KD], value); break; case TXPIDSETTINGS_PIDS_PITCHVBARKP: stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KP], value); break; case TXPIDSETTINGS_PIDS_PITCHVBARKI: stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KI], value); break; case TXPIDSETTINGS_PIDS_PITCHVBARKD: stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KD], value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKP: stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KP], value); stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KP], value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKI: stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KI], value); stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KI], value); break; case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKD: stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KD], value); stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KD], value); break; case TXPIDSETTINGS_PIDS_YAWVBARKP: stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KP], value); break; case TXPIDSETTINGS_PIDS_YAWVBARKI: stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KI], value); break; case TXPIDSETTINGS_PIDS_YAWVBARKD: stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KD], value); break; #ifdef UAVOBJ_INIT_vtolpathfollowersettings case TXPIDSETTINGS_PIDS_HORIZONTALPOSKP: vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP], value); break; case TXPIDSETTINGS_PIDS_HORIZONTALPOSKI: vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], value); break; case TXPIDSETTINGS_PIDS_HORIZONTALPOSILIMIT: vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], value); break; case TXPIDSETTINGS_PIDS_HORIZONTALVELKP: vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], value); break; case TXPIDSETTINGS_PIDS_HORIZONTALVELKI: vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], value); break; case TXPIDSETTINGS_PIDS_HORIZONTALVELKD: vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD], value); break; #endif /* UAVOBJ_INIT_vtolpathfollowersettings */ default: // Previously this would assert. But now the // object may be missing and it's not worth a // crash. break; } } } // Update UAVOs, if necessary if (stabilizationSettingsNeedsUpdate) { StabilizationSettingsSet(&stab); } #ifdef UAVOBJ_INIT_vtolpathfollowersettings if (vtolPathFollowerSettingsNeedsUpdate) { // Check to make sure the settings UAVObject has been instantiated if (VtolPathFollowerSettingsHandle()) { VtolPathFollowerSettingsSet(&vtolPathFollowerSettingsData); } } #endif } /** * Scales input val from [inMin..inMax] range to [outMin..outMax]. * If val is out of input range (inMin <= inMax), it will be bound. * (outMin > outMax) is ok, in that case output will be decreasing. * * \returns scaled value */ static float scale(float val, float inMin, float inMax, float outMin, float outMax) { // bound input value if (val > inMax) val = inMax; if (val < inMin) val = inMin; // normalize input value to [0..1] if (inMax <= inMin) val = 0.0; else val = (val - inMin) / (inMax - inMin); // update output bounds if (outMin > outMax) { float t = outMin; outMin = outMax; outMax = t; val = 1.0f - val; } return (outMax - outMin) * val + outMin; } /** * Updates var using val if needed. * \returns 1 if updated, 0 otherwise */ static bool update(float *var, float val) { if (*var != val) { *var = val; return true; } return false; }
void PIOS_Board_Init(void) { /* Delay system */ PIOS_DELAY_Init(); const struct pios_board_info * bdinfo = &pios_board_info_blob; #if defined(PIOS_INCLUDE_LED) const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev); PIOS_Assert(led_cfg); PIOS_LED_Init(led_cfg); #endif /* PIOS_INCLUDE_LED */ /* Set up the SPI interface to the gyro/acelerometer */ if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_DEBUG_Assert(0); } /* Set up the SPI interface to the flash and rfm22b */ if (PIOS_SPI_Init(&pios_spi_telem_flash_id, &pios_spi_telem_flash_cfg)) { PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_FLASH) /* Inititialize all flash drivers */ if (PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_telem_flash_id, 1, &flash_m25p_cfg) != 0) panic(1); if (PIOS_Flash_Internal_Init(&pios_internal_flash_id, &flash_internal_cfg) != 0) panic(1); /* Register the partition table */ const struct pios_flash_partition * flash_partition_table; uint32_t num_partitions; flash_partition_table = PIOS_BOARD_HW_DEFS_GetPartitionTable(bdinfo->board_rev, &num_partitions); PIOS_FLASH_register_partition_table(flash_partition_table, num_partitions); /* Mount all filesystems */ if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, &flashfs_settings_cfg, FLASH_PARTITION_LABEL_SETTINGS) != 0) panic(1); if (PIOS_FLASHFS_Logfs_Init(&pios_waypoints_settings_fs_id, &flashfs_waypoints_cfg, FLASH_PARTITION_LABEL_WAYPOINTS) != 0) panic(1); #if defined(ERASE_FLASH) PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); #endif #endif /* PIOS_INCLUDE_FLASH */ /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); /* Initialize the alarms library */ AlarmsInitialize(); HwFreedomInitialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) PIOS_RTC_Init(&pios_rtc_main_cfg); #endif #ifndef ERASE_FLASH /* Initialize watchdog as early as possible to catch faults during init * but do it only if there is no debugger connected */ if ((CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk) == 0) { PIOS_WDG_Init(); } #endif // /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_3_cfg); /* IAP System Setup */ PIOS_IAP_Init(); uint16_t boot_count = PIOS_IAP_ReadBootCount(); if (boot_count < 3) { PIOS_IAP_WriteBootCount(++boot_count); AlarmsClear(SYSTEMALARMS_ALARM_BOOTFAULT); } else { /* Too many failed boot attempts, force hw config to defaults */ HwFreedomSetDefaults(HwFreedomHandle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } PIOS_IAP_Init(); #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); /* Flags to determine if various USB interfaces are advertised */ bool usb_hid_present = false; bool usb_cdc_present = false; #if defined(PIOS_INCLUDE_USB_CDC) if (PIOS_USB_DESC_HID_CDC_Init()) { PIOS_Assert(0); } usb_hid_present = true; usb_cdc_present = true; #else if (PIOS_USB_DESC_HID_ONLY_Init()) { PIOS_Assert(0); } usb_hid_present = true; #endif uintptr_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, PIOS_BOARD_HW_DEFS_GetUsbCfg(bdinfo->board_rev)); #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwFreedomUSB_VCPPortGet(&hw_usb_vcpport); if (!usb_cdc_present) { /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ hw_usb_vcpport = HWFREEDOM_USB_VCPPORT_DISABLED; } uintptr_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } switch (hw_usb_vcpport) { case HWFREEDOM_USB_VCPPORT_DISABLED: break; case HWFREEDOM_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_debug_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, NULL, 0, tx_buffer, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #endif /* PIOS_INCLUDE_COM */ break; case HWFREEDOM_USB_VCPPORT_PICOC: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_PICOC_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_PICOC_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_picoc_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, rx_buffer, PIOS_COM_PICOC_RX_BUF_LEN, tx_buffer, PIOS_COM_PICOC_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwFreedomUSB_HIDPortGet(&hw_usb_hidport); if (!usb_hid_present) { /* Force HID port function to disabled if we haven't advertised HID in our USB descriptor */ hw_usb_hidport = HWFREEDOM_USB_HIDPORT_DISABLED; } uintptr_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } switch (hw_usb_hidport) { case HWFREEDOM_USB_HIDPORT_DISABLED: break; case HWFREEDOM_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_COM */ break; } #endif /* PIOS_INCLUDE_USB_HID */ if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } #endif /* PIOS_INCLUDE_USB */ /* Configure IO ports */ uint8_t hw_DSMxBind; HwFreedomDSMxBindGet(&hw_DSMxBind); /* Configure FlexiPort */ uint8_t hw_mainport; HwFreedomMainPortGet(&hw_mainport); switch (hw_mainport) { case HWFREEDOM_MAINPORT_DISABLED: break; case HWFREEDOM_MAINPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWFREEDOM_MAINPORT_GPS: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_RX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_MAINPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { PIOS_Board_configure_dsm(&pios_usart_dsm_hsum_main_cfg, &pios_dsm_main_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFREEDOM_MAINPORT_HOTTSUMD: case HWFREEDOM_MAINPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_mainport) { case HWFREEDOM_MAINPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFREEDOM_MAINPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_main_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFREEDOM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_MAINPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; case HWFREEDOM_MAINPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWFREEDOM_MAINPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_GPS) #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); pios_com_mavlink_id = pios_com_gps_id; #endif /* PIOS_INCLUDE_MAVLINK */ #endif /* PIOS_INCLUDE_GPS */ break; case HWFREEDOM_MAINPORT_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); #endif /* PIOS_INCLUDE_HOTT */ break; case HWFREEDOM_MAINPORT_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sensor_hub_id); #endif /* PIOS_INCLUDE_FRSKY_SENSOR_HUB */ break; case HWFREEDOM_MAINPORT_LIGHTTELEMETRYTX: #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_lighttelemetry_id); #endif break; case HWFREEDOM_MAINPORT_PICOC: #if defined(PIOS_INCLUDE_PICOC) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_PICOC_RX_BUF_LEN, PIOS_COM_PICOC_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_picoc_id); #endif /* PIOS_INCLUDE_PICOC */ break; case HWFREEDOM_MAINPORT_FRSKYSPORTTELEMETRY: #if defined(PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_FRSKYSPORT_RX_BUF_LEN, PIOS_COM_FRSKYSPORT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sport_id); #endif /* PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY */ break; } /* hw_freedom_mainport */ /* Configure flexi USART port */ uint8_t hw_flexiport; HwFreedomFlexiPortGet(&hw_flexiport); switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_DISABLED: break; case HWFREEDOM_FLEXIPORT_TELEMETRY: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); break; case HWFREEDOM_FLEXIPORT_GPS: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_RX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); break; case HWFREEDOM_FLEXIPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_hsum_flexi_cfg, &pios_dsm_flexi_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFREEDOM_FLEXIPORT_HOTTSUMD: case HWFREEDOM_FLEXIPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_flexiport) { case HWFREEDOM_FLEXIPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFREEDOM_FLEXIPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_flexi_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFREEDOM_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_I2C */ break; case HWFREEDOM_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; case HWFREEDOM_FLEXIPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; case HWFREEDOM_FLEXIPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_mavlink_id); #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWFREEDOM_FLEXIPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_GPS) #if defined(PIOS_INCLUDE_MAVLINK) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_MAVLINK_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); pios_com_mavlink_id = pios_com_gps_id; #endif /* PIOS_INCLUDE_MAVLINK */ #endif /* PIOS_INCLUDE_GPS */ break; case HWFREEDOM_FLEXIPORT_HOTTTELEMETRY: #if defined(PIOS_INCLUDE_HOTT) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); #endif /* PIOS_INCLUDE_HOTT */ break; case HWFREEDOM_FLEXIPORT_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sensor_hub_id); #endif /* PIOS_INCLUDE_FRSKY_SENSOR_HUB */ break; case HWFREEDOM_FLEXIPORT_LIGHTTELEMETRYTX: #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_lighttelemetry_id); #endif break; case HWFREEDOM_FLEXIPORT_PICOC: #if defined(PIOS_INCLUDE_PICOC) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_PICOC_RX_BUF_LEN, PIOS_COM_PICOC_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_picoc_id); #endif /* PIOS_INCLUDE_PICOC */ break; case HWFREEDOM_FLEXIPORT_FRSKYSPORTTELEMETRY: #if defined(PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_FRSKYSPORT_RX_BUF_LEN, PIOS_COM_FRSKYSPORT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_frsky_sport_id); #endif /* PIOS_INCLUDE_FRSKY_SPORT_TELEMETRY */ break; } /* hw_freedom_flexiport */ /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) RFM22BStatusInitialize(); RFM22BStatusCreateInstance(); // Initialize out status object. RFM22BStatusData rfm22bstatus; RFM22BStatusGet(&rfm22bstatus); RFM22BStatusInstSet(1,&rfm22bstatus); HwFreedomData hwFreedom; HwFreedomGet(&hwFreedom); // Initialize out status object. rfm22bstatus.BoardType = bdinfo->board_type; rfm22bstatus.BoardRevision = bdinfo->board_rev; if (hwFreedom.Radio == HWFREEDOM_RADIO_DISABLED || hwFreedom.MaxRfPower == HWFREEDOM_MAXRFPOWER_0) { // When radio disabled, it is ok for init to fail. This allows boards without populating // this component. const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg) == 0) { PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); rfm22bstatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); rfm22bstatus.BoardRevision = PIOS_RFM22B_ModuleVersion(pios_rfm22b_id); } else { pios_rfm22b_id = 0; } rfm22bstatus.LinkState = RFM22BSTATUS_LINKSTATE_DISABLED; } else { // always allow receiving PPM when radio is on bool ppm_mode = hwFreedom.Radio == HWFREEDOM_RADIO_TELEMPPM || hwFreedom.Radio == HWFREEDOM_RADIO_PPM; bool ppm_only = hwFreedom.Radio == HWFREEDOM_RADIO_PPM; bool is_oneway = hwFreedom.Radio == HWFREEDOM_RADIO_PPM; // Sparky2 can only receive PPM only /* Configure the RFM22B device. */ const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { panic(6); } rfm22bstatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); rfm22bstatus.BoardRevision = PIOS_RFM22B_ModuleVersion(pios_rfm22b_id); /* Configure the radio com interface */ uint8_t *rx_buffer = (uint8_t *)PIOS_malloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *)PIOS_malloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { panic(6); } /* Set Telemetry to use RFM22b if no other telemetry is configured (USB always overrides anyway) */ if (!pios_com_telem_rf_id) { pios_com_telem_rf_id = pios_com_rf_id; } rfm22bstatus.LinkState = RFM22BSTATUS_LINKSTATE_ENABLED; enum rfm22b_datarate datarate = RFM22_datarate_64000; switch (hwFreedom.MaxRfSpeed) { case HWFREEDOM_MAXRFSPEED_9600: datarate = RFM22_datarate_9600; break; case HWFREEDOM_MAXRFSPEED_19200: datarate = RFM22_datarate_19200; break; case HWFREEDOM_MAXRFSPEED_32000: datarate = RFM22_datarate_32000; break; case HWFREEDOM_MAXRFSPEED_64000: datarate = RFM22_datarate_64000; break; case HWFREEDOM_MAXRFSPEED_100000: datarate = RFM22_datarate_100000; break; case HWFREEDOM_MAXRFSPEED_192000: datarate = RFM22_datarate_192000; break; } /* Set the radio configuration parameters. */ PIOS_RFM22B_Config(pios_rfm22b_id, datarate, hwFreedom.MinChannel, hwFreedom.MaxChannel, hwFreedom.CoordID, is_oneway, ppm_mode, ppm_only); /* Set the modem Tx poer level */ switch (hwFreedom.MaxRfPower) { case HWFREEDOM_MAXRFPOWER_125: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); break; case HWFREEDOM_MAXRFPOWER_16: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); break; case HWFREEDOM_MAXRFPOWER_316: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); break; case HWFREEDOM_MAXRFPOWER_63: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); break; case HWFREEDOM_MAXRFPOWER_126: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); break; case HWFREEDOM_MAXRFPOWER_25: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); break; case HWFREEDOM_MAXRFPOWER_50: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); break; case HWFREEDOM_MAXRFPOWER_100: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); break; default: // do nothing break; } /* Reinitialize the modem. */ PIOS_RFM22B_Reinit(pios_rfm22b_id); #if defined(PIOS_INCLUDE_RFM22B_RCVR) { uintptr_t pios_rfm22brcvr_id; PIOS_RFM22B_Rcvr_Init(&pios_rfm22brcvr_id, pios_rfm22b_id); uintptr_t pios_rfm22brcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_rfm22brcvr_rcvr_id, &pios_rfm22b_rcvr_driver, pios_rfm22brcvr_id)) { panic(6); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_RFM22B] = pios_rfm22brcvr_rcvr_id; } } RFM22BStatusInstSet(1,&rfm22bstatus); #endif /* PIOS_INCLUDE_RFM22B_RCVR */ #endif /* PIOS_INCLUDE_RFM22B */ PIOS_WDG_Clear(); /* Configure input receiver USART port */ uint8_t hw_rcvrport; HwFreedomRcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_DISABLED: break; case HWFREEDOM_RCVRPORT_PPM: { uintptr_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); uintptr_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } break; case HWFREEDOM_RCVRPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { //TODO: Define the various Channelgroup for Revo dsm inputs and handle here PIOS_Board_configure_dsm(&pios_usart_dsm_hsum_rcvr_cfg, &pios_dsm_rcvr_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMRCVRPORT,&hw_DSMxBind); } #endif /* PIOS_INCLUDE_DSM */ break; case HWFREEDOM_RCVRPORT_HOTTSUMD: case HWFREEDOM_RCVRPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; switch (hw_rcvrport) { case HWFREEDOM_RCVRPORT_HOTTSUMD: proto = PIOS_HSUM_PROTO_SUMD; break; case HWFREEDOM_RCVRPORT_HOTTSUMH: proto = PIOS_HSUM_PROTO_SUMH; break; default: PIOS_Assert(0); break; } PIOS_Board_configure_hsum(&pios_usart_dsm_hsum_rcvr_cfg, &pios_usart_com_driver, &proto, MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM); } #endif /* PIOS_INCLUDE_HSUM */ break; case HWFREEDOM_RCVRPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_rcvr_cfg)) { PIOS_Assert(0); } uintptr_t pios_sbus_id; if (PIOS_SBus_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } uintptr_t pios_sbus_rcvr_id; if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; } #endif break; } if (hw_rcvrport != HWFREEDOM_RCVRPORT_SBUS) { GPIO_Init(pios_sbus_cfg.inv.gpio, (GPIO_InitTypeDef*)&pios_sbus_cfg.inv.init); GPIO_WriteBit(pios_sbus_cfg.inv.gpio, pios_sbus_cfg.inv.init.GPIO_Pin, pios_sbus_cfg.gpio_inv_disable); } #if defined(PIOS_OVERO_SPI) /* Set up the SPI based PIOS_COM interface to the overo */ { bool overo_enabled = false; #ifdef MODULE_OveroSync_BUILTIN overo_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_OVEROSYNC] == MODULESETTINGS_ADMINSTATE_ENABLED) { overo_enabled = true; } else { overo_enabled = false; } #endif if (overo_enabled) { if (PIOS_OVERO_Init(&pios_overo_id, &pios_overo_cfg)) { PIOS_DEBUG_Assert(0); } const uint32_t PACKET_SIZE = 1024; uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PACKET_SIZE); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PACKET_SIZE); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_overo_id, &pios_overo_com_driver, pios_overo_id, rx_buffer, PACKET_SIZE, tx_buffer, PACKET_SIZE)) { PIOS_Assert(0); } } } #endif #if defined(PIOS_INCLUDE_GCSRCVR) GCSReceiverInitialize(); uintptr_t pios_gcsrcvr_id; PIOS_GCSRCVR_Init(&pios_gcsrcvr_id); uintptr_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, pios_gcsrcvr_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS uint8_t hw_output_port; HwFreedomOutputGet(&hw_output_port); switch (hw_output_port) { case HWFREEDOM_OUTPUT_DISABLED: break; case HWFREEDOM_OUTPUT_PWM: /* Set up the servo outputs */ PIOS_Servo_Init(&pios_servo_cfg); break; default: break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif PIOS_DELAY_WaitmS(200); PIOS_SENSORS_Init(); #if defined(PIOS_INCLUDE_MPU6000) if (PIOS_MPU6000_Init(pios_spi_gyro_id,0, &pios_mpu6000_cfg) != 0) panic(2); if (PIOS_MPU6000_Test() != 0) panic(2); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwFreedomGyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWFREEDOM_GYRORANGE_250: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWFREEDOM_GYRORANGE_500: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWFREEDOM_GYRORANGE_1000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWFREEDOM_GYRORANGE_2000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwFreedomAccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWFREEDOM_ACCELRANGE_2G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWFREEDOM_ACCELRANGE_4G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWFREEDOM_ACCELRANGE_8G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWFREEDOM_ACCELRANGE_16G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_16G); break; } // the filter has to be set before rate else divisor calculation will fail uint8_t hw_mpu6000_dlpf; HwFreedomMPU6000DLPFGet(&hw_mpu6000_dlpf); enum pios_mpu60x0_filter mpu6000_dlpf = \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_mpu6000_dlpf == HWFREEDOM_MPU6000DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ pios_mpu6000_cfg.default_filter; PIOS_MPU6000_SetLPF(mpu6000_dlpf); uint8_t hw_mpu6000_samplerate; HwFreedomMPU6000RateGet(&hw_mpu6000_samplerate); uint16_t mpu6000_samplerate = \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_200) ? 200 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_333) ? 333 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_500) ? 500 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_666) ? 666 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_1000) ? 1000 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_2000) ? 2000 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_4000) ? 4000 : \ (hw_mpu6000_samplerate == HWFREEDOM_MPU6000RATE_8000) ? 8000 : \ pios_mpu6000_cfg.default_samplerate; PIOS_MPU6000_SetSampleRate(mpu6000_samplerate); #endif if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } if (PIOS_I2C_CheckClear(pios_i2c_mag_pressure_adapter_id) != 0) panic(5); PIOS_DELAY_WaitmS(50); #if defined(PIOS_INCLUDE_ADC) uint32_t internal_adc_id; PIOS_INTERNAL_ADC_Init(&internal_adc_id, &pios_adc_cfg); PIOS_ADC_Init(&pios_internal_adc_id, &pios_internal_adc_driver, internal_adc_id); #endif PIOS_LED_On(0); #if defined(PIOS_INCLUDE_HMC5883) if (PIOS_HMC5883_Init(pios_i2c_mag_pressure_adapter_id, &pios_hmc5883_cfg) != 0) panic(3); #endif PIOS_LED_On(1); #if defined(PIOS_INCLUDE_MS5611) if (PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id) != 0) panic(4); #endif PIOS_LED_On(2); }
/** * Initialise the gps module * \return -1 if initialisation failed * \return 0 on success */ int32_t GPSInitialize(void) { gpsPort = PIOS_COM_GPS; uint8_t gpsProtocol; #ifdef MODULE_GPS_BUILTIN module_enabled = true; #else uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); if (module_state[MODULESETTINGS_ADMINSTATE_GPS] == MODULESETTINGS_ADMINSTATE_ENABLED) { module_enabled = true; } else { module_enabled = false; } #endif #if defined(REVOLUTION) // These objects MUST be initialized for Revolution // because the rest of the system expects to just // attach to their queues GPSPositionInitialize(); GPSVelocityInitialize(); GPSTimeInitialize(); GPSSatellitesInitialize(); HomeLocationInitialize(); UBloxInfoInitialize(); updateSettings(); #else if (gpsPort && module_enabled) { GPSPositionInitialize(); GPSVelocityInitialize(); #if !defined(PIOS_GPS_MINIMAL) GPSTimeInitialize(); GPSSatellitesInitialize(); #endif #if defined(PIOS_GPS_PROVIDES_AIRSPEED) AirspeedActualInitialize(); #endif updateSettings(); } #endif if (gpsPort && module_enabled) { ModuleSettingsGPSDataProtocolGet(&gpsProtocol); switch (gpsProtocol) { case MODULESETTINGS_GPSDATAPROTOCOL_NMEA: gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); break; case MODULESETTINGS_GPSDATAPROTOCOL_UBX: gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket)); break; default: gps_rx_buffer = NULL; } PIOS_Assert(gps_rx_buffer); return 0; } return -1; }
/** * Correct attitude drift. Choose from any of the following algorithms */ void updateAttitudeDrift(AccelsData * accelsData, GyrosData * gyrosData, const float delT, GlobalAttitudeVariables *glblAtt, AttitudeSettingsData *attitudeSettings, SensorSettingsData *inertialSensorSettings) { float *gyros = &gyrosData->x; float *accels = &accelsData->x; float omegaCorrP[3]; if (attitudeSettings->FilterChoice == ATTITUDESETTINGS_FILTERCHOICE_CCC) { CottonComplementaryCorrection(accels, gyros, delT, glblAtt, omegaCorrP); } else if (attitudeSettings->FilterChoice == ATTITUDESETTINGS_FILTERCHOICE_PREMERLANI || attitudeSettings->FilterChoice == ATTITUDESETTINGS_FILTERCHOICE_PREMERLANI_GPS) { if (firstpass_flag) { uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM]; ModuleSettingsAdminStateGet(module_state); //Allocate memory for DCM drift globals drft = (struct GlobalDcmDriftVariables *) pvPortMalloc(sizeof (struct GlobalDcmDriftVariables)); memset(drft->GPSV_old, 0, sizeof(drft->GPSV_old)); memset(drft->omegaCorrI, 0, sizeof(drft->omegaCorrI)); memset(drft->accels_e_integrator, 0, sizeof(drft->accels_e_integrator)); // TODO: Expose these settings through UAVO drft->accelsKp = 1; drft->rollPitchKp = 20; drft->rollPitchKi = 1; drft->yawKp = 0; drft->yawKi = 0; drft->gyroCalibTau = 100; // Set flags if (module_state[MODULESETTINGS_ADMINSTATE_GPS] == MODULESETTINGS_ADMINSTATE_ENABLED && PIOS_COM_GPS) { GPSVelocityConnectCallback(GPSVelocityUpdatedCb); drft->gpsPresent_flag = true; drft->gpsVelocityDataConsumption_flag = GPS_CONSUMED; } else { drft->gpsPresent_flag = false; } #if defined (PIOS_INCLUDE_MAGNETOMETER) MagnetometerConnectCallback(MagnetometerUpdatedCb); #endif drft->magNewData_flag = false; drft->delT_between_GPS = 0; firstpass_flag = false; } // Apply arbitrary scaling to get into effective units drft->rollPitchKp = glblAtt->accelKp * 1000.0f; drft->rollPitchKi = glblAtt->accelKi * 10000.0f; // Convert quaternions into rotation matrix float Rbe[3][3]; Quaternion2R(glblAtt->q, Rbe); #if defined (PIOS_INCLUDE_GPS) if (attitudeSettings->FilterChoice == ATTITUDESETTINGS_FILTERCHOICE_PREMERLANI_GPS) { Premerlani_GPS(accels, gyros, Rbe, delT, true, glblAtt, omegaCorrP); } else if (attitudeSettings->FilterChoice == ATTITUDESETTINGS_FILTERCHOICE_PREMERLANI) #endif { Premerlani_DCM(accels, gyros, Rbe, delT, false, glblAtt, omegaCorrP); //<-- GAWD, I HATE HOW FUNCTION ARGUMENTS JUST PILE UP. IT LOOKS UNPROFESSIONAL TO MIX INPUTS AND OUTPUTS } } //Calibrate the gyroscopes. //TODO: but only calibrate when system is armed. if (0) { //<-- CURRENTLY DISABLE UNTIL TESTING CAN BE DONE. float normOmegaScalar = VectorMagnitude(gyros); calibrate_gyros_high_speed(gyros, omegaCorrP, normOmegaScalar, delT, inertialSensorSettings); } }