/** * Update system alarms */ static void updateSystemAlarms() { SystemStatsData stats; UAVObjStats objStats; EventStats evStats; SystemStatsGet(&stats); // Check heap if (stats.HeapRemaining < HEAP_LIMIT_CRITICAL) { AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL); } else if (stats.HeapRemaining < HEAP_LIMIT_WARNING) { AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY); } // Check CPU load if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) { AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL); } else if (stats.CPULoad > CPULOAD_LIMIT_WARNING) { AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD); } // Check for stack overflow if (stackOverflow == 1) { AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL); } else { AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW); } #if defined(PIOS_INCLUDE_SDCARD) // Check for SD card if (PIOS_SDCARD_IsMounted() == 0) { AlarmsSet(SYSTEMALARMS_ALARM_SDCARD, SYSTEMALARMS_ALARM_ERROR); } else { AlarmsClear(SYSTEMALARMS_ALARM_SDCARD); } #endif // Check for event errors UAVObjGetStats(&objStats); EventGetStats(&evStats); UAVObjClearStats(); EventClearStats(); if (objStats.eventErrors > 0 || evStats.eventErrors > 0) { AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM); } }
/** * Module thread, should not return. */ static void AttitudeTask(void *parameters) { uint8_t init = 0; AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE); // Keep flash CS pin high while talking accel PIOS_FLASH_DISABLE; PIOS_ADXL345_Init(); // Main task loop while (1) { if(xTaskGetTickCount() < 10000) { // For first 5 seconds use accels to get gyro bias accelKp = 1; // Decrease the rate of gyro learning during init accelKi = .5 / (1 + xTaskGetTickCount() / 5000); } else if (init == 0) { settingsUpdatedCb(AttitudeSettingsHandle()); init = 1; } PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); AttitudeRawData attitudeRaw; AttitudeRawGet(&attitudeRaw); updateSensors(&attitudeRaw); updateAttitude(&attitudeRaw); AttitudeRawSet(&attitudeRaw); } }
/** * Module starting */ int32_t ManualControlStart() { // Run this initially to make sure the configuration is checked configuration_check(); // Whenever the configuration changes, make sure it is safe to fly SystemSettingsConnectCallback(configurationUpdatedCb); ManualControlSettingsConnectCallback(configurationUpdatedCb); ManualControlCommandConnectCallback(commandUpdatedCb); // clear alarms AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); SettingsUpdatedCb(NULL); // Make sure unarmed on power up armHandler(true, frameType); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES takeOffLocationHandlerInit(); #endif // Start main task PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); return 0; }
/** * Clear all alarms */ void AlarmsClearAll() { uint32_t n; for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) { AlarmsClear(n); } }
/** * Module task */ static void stabilizationTask(void* parameters) { portTickType lastSysTime; portTickType thisSysTime; UAVObjEvent ev; QuadMotorsDesiredData actuatorDesired; FlightStatusData flightStatus; //SettingsUpdatedCb((UAVObjEvent *) NULL); // Main task loop lastSysTime = xTaskGetTickCount(); while(1) { PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); continue; } // Check how long since last update thisSysTime = xTaskGetTickCount(); if(thisSysTime > lastSysTime) // reuse dt in case of wraparound dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; lastSysTime = thisSysTime; FlightStatusGet(&flightStatus); QuadMotorsDesiredGet(&actuatorDesired); //if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) //{ // ZERO MOTORS //} //else //{ //} //ActuatorSettingsData settings; //ActuatorSettingsGet(&settings); //PIOS_SetMKSpeed(settings.ChannelAddr[mixer_channel],value); PIOS_SetMKSpeed(0,actuatorDesired.motorFront_NW); PIOS_SetMKSpeed(1,actuatorDesired.motorRight_NE); PIOS_SetMKSpeed(2,actuatorDesired.motorBack_SE); PIOS_SetMKSpeed(3,actuatorDesired.motorLeft_SW); // Clear alarms AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } }
/** * Module thread, should not return. */ static void AttitudeTask(void *parameters) { uint8_t init = 0; AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); PIOS_ADC_Config((PIOS_ADC_RATE / 1000.0f) * UPDATE_RATE); // Keep flash CS pin high while talking accel PIOS_FLASH_DISABLE; PIOS_ADXL345_Init(); // Force settings update to make sure rotation loaded settingsUpdatedCb(AttitudeSettingsHandle()); // Main task loop while (1) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if((xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { // For first 7 seconds use accels to get gyro bias accelKp = 1; accelKi = 0.9; yawBiasRate = 0.23; init = 0; } else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { accelKp = 1; accelKi = 0.9; yawBiasRate = 0.23; init = 0; } else if (init == 0) { // Reload settings (all the rates) AttitudeSettingsAccelKiGet(&accelKi); AttitudeSettingsAccelKpGet(&accelKp); AttitudeSettingsYawBiasRateGet(&yawBiasRate); init = 1; } PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); AttitudeRawData attitudeRaw; AttitudeRawGet(&attitudeRaw); updateSensors(&attitudeRaw); updateAttitude(&attitudeRaw); AttitudeRawSet(&attitudeRaw); } }
/** * Module thread, should not return. */ static void AttitudeTask(void *parameters) { bool first_run = true; uint32_t last_algorithm; AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); // Force settings update to make sure rotation loaded settingsUpdatedCb(NULL); // Wait for all the sensors be to read vTaskDelay(100); // Invalidate previous algorithm to trigger a first run last_algorithm = 0xfffffff; // Main task loop while (1) { int32_t ret_val = -1; if (last_algorithm != revoSettings.FusionAlgorithm) { last_algorithm = revoSettings.FusionAlgorithm; first_run = true; } // This function blocks on data queue switch (revoSettings.FusionAlgorithm ) { case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: ret_val = updateAttitudeComplementary(first_run); break; case REVOSETTINGS_FUSIONALGORITHM_INSOUTDOOR: ret_val = updateAttitudeINSGPS(first_run, true); break; case REVOSETTINGS_FUSIONALGORITHM_INSINDOOR: ret_val = updateAttitudeINSGPS(first_run, false); break; default: AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_CRITICAL); break; } if(ret_val == 0) first_run = false; PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); } }
/** * Module thread, should not return. */ static void ahrscommsTask(void* parameters) { portTickType lastSysTime; AhrsStatusData data; AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL); /*Until AHRS connects, assume it doesn't know home */ AhrsStatusGet(&data); data.HomeSet = AHRSSTATUS_HOMESET_FALSE; //data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE; data.AlgorithmSet = AHRSSTATUS_CALIBRATIONSET_FALSE; AhrsStatusSet(&data); // Main task loop while (1) { AHRSSettingsData settings; AHRSSettingsGet(&settings); AhrsSendObjects(); AhrsCommStatus stat = AhrsGetStatus(); if(stat.linkOk) { AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS); }else { AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING); } AhrsStatusData sData; AhrsStatusGet(&sData); sData.LinkRunning = stat.linkOk; sData.AhrsKickstarts = stat.remote.kickStarts; sData.AhrsCrcErrors = stat.remote.crcErrors; sData.AhrsRetries = stat.remote.retries; sData.AhrsInvalidPackets = stat.remote.invalidPacket; sData.OpCrcErrors = stat.local.crcErrors; sData.OpRetries = stat.local.retries; sData.OpInvalidPackets = stat.local.invalidPacket; AhrsStatusSet(&sData); /* Wait for the next update interval */ vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS ); } }
/** * @brief Main module task */ static void actuatorTask(void* parameters) { // UAVObjEvent ev; portTickType lastSysTime; ActuatorCommandData command; ActuatorSettingsData settings; // Set servo update frequency (done only on start-up) ActuatorSettingsGet(&settings); PIOS_Servo_SetHz(settings.ChannelUpdateFreq[0], settings.ChannelUpdateFreq[1]); // Go to the neutral (failsafe) values until an ActuatorDesired update is received setFailsafe(); // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { ActuatorCommandGet(&command); ActuatorSettingsGet(&settings); if ( RunMixers(&command, &settings) == -1 ) { AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); } else { AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); } // Update output object ActuatorCommandSet(&command); // Update in case read only (eg. during servo configuration) ActuatorCommandGet(&command); // Update servo outputs for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { PIOS_Servo_Set( n, command.Channel[n] ); } // Wait until next update vTaskDelayUntil(&lastSysTime, settings.UpdatePeriod / portTICK_RATE_MS ); } }
/* stub: module has no module thread */ int32_t GeofenceStart(void) { if (geofenceSettings == NULL) { return -1; } // Schedule periodic task to check position UAVObjEvent ev = { .obj = PositionActualHandle(), .instId = 0, .event = 0, }; EventPeriodicCallbackCreate(&ev, checkPosition, SAMPLE_PERIOD_MS); return 0; } MODULE_INITCALL(GeofenceInitialize, GeofenceStart); /** * Periodic callback that processes changes in position and * sets the alarm. */ static void checkPosition(UAVObjEvent* ev, void *ctx, void *obj, int len) { (void) ev; (void) ctx; (void) obj; (void) len; if (PositionActualHandle()) { PositionActualData positionActual; PositionActualGet(&positionActual); const float distance2 = powf(positionActual.North, 2) + powf(positionActual.East, 2); // ErrorRadius is squared when it is fetched, so this is correct if (distance2 > geofenceSettings->ErrorRadius) { AlarmsSet(SYSTEMALARMS_ALARM_GEOFENCE, SYSTEMALARMS_ALARM_ERROR); } else if (distance2 > geofenceSettings->WarningRadius) { AlarmsSet(SYSTEMALARMS_ALARM_GEOFENCE, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_GEOFENCE); } } }
/** * Module thread, should not return. */ static void ahrscommsTask(void *parameters) { portTickType lastSysTime; AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_CRITICAL); // Main task loop while (1) { PIOS_WDG_UpdateFlag(PIOS_WDG_AHRS); AhrsCommStatus stat; AhrsSendObjects(); AhrsGetStatus(&stat); if (stat.linkOk) { AlarmsClear(SYSTEMALARMS_ALARM_AHRSCOMMS); } else { AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING); } InsStatusData sData; InsStatusGet(&sData); sData.LinkRunning = stat.linkOk; sData.AhrsKickstarts = stat.remote.kickStarts; sData.AhrsCrcErrors = stat.remote.crcErrors; sData.AhrsRetries = stat.remote.retries; sData.AhrsInvalidPackets = stat.remote.invalidPacket; sData.OpCrcErrors = stat.local.crcErrors; sData.OpRetries = stat.local.retries; sData.OpInvalidPackets = stat.local.invalidPacket; InsStatusSet(&sData); /* Wait for the next update interval */ vTaskDelayUntil(&lastSysTime, 2 / portTICK_RATE_MS); } }
void PIOS_Board_Init(void) { check_bor(); const struct pios_board_info * bdinfo = &pios_board_info_blob; // Make sure all the PWM outputs are low const struct pios_servo_cfg * servo_cfg = get_servo_cfg(bdinfo->board_rev); const struct pios_tim_channel * channels = servo_cfg->channels; uint8_t num_channels = servo_cfg->num_channels; for (int i = 0; i < num_channels; i++) { GPIO_Init(channels[i].pin.gpio, (GPIO_InitTypeDef*) &channels[i].pin.init); } /* Delay system */ PIOS_DELAY_Init(); #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 defined(PIOS_INCLUDE_FLASH_JEDEC) if (get_external_flash(bdinfo->board_rev)) { if (PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_telem_flash_id, 1, &flash_m25p_cfg) != 0) panic(1); } #endif /* PIOS_INCLUDE_FLASH_JEDEC */ 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 */ if (PIOS_FLASHFS_Logfs_Init(&pios_uavo_settings_fs_id, get_flashfs_settings_cfg(bdinfo->board_rev), FLASH_PARTITION_LABEL_SETTINGS)) panic(1); #if defined(PIOS_INCLUDE_FLASH_JEDEC) if (get_external_flash(bdinfo->board_rev)) { if (PIOS_FLASHFS_Logfs_Init(&pios_waypoints_settings_fs_id, &flashfs_waypoints_cfg, FLASH_PARTITION_LABEL_WAYPOINTS) != 0) panic(1); } #endif /* PIOS_INCLUDE_FLASH_JEDEC */ #if defined(ERASE_FLASH) PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); #endif #endif /* PIOS_INCLUDE_FLASH */ /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); HwSparky2Initialize(); 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 /* Initialize the alarms library */ AlarmsInitialize(); /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_3_cfg); PIOS_TIM_InitClock(&tim_5_cfg); PIOS_TIM_InitClock(&tim_8_cfg); PIOS_TIM_InitClock(&tim_9_cfg); PIOS_TIM_InitClock(&tim_12_cfg); NVIC_InitTypeDef tim_8_up_irq = { .NVIC_IRQChannel = TIM8_UP_TIM13_IRQn, .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, .NVIC_IRQChannelSubPriority = 0, .NVIC_IRQChannelCmd = ENABLE, }; NVIC_Init(&tim_8_up_irq); /* 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 */ HwSparky2SetDefaults(HwSparky2Handle(), 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 */ HwSparky2USB_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 = HWSPARKY2_USB_VCPPORT_DISABLED; } PIOS_HAL_ConfigureCDC(hw_usb_vcpport, pios_usb_id, &pios_usb_cdc_cfg); #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwSparky2USB_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 = HWSPARKY2_USB_HIDPORT_DISABLED; } PIOS_HAL_ConfigureHID(hw_usb_hidport, pios_usb_id, &pios_usb_hid_cfg); #endif /* PIOS_INCLUDE_USB_HID */ if (usb_hid_present || usb_cdc_present) { PIOS_USBHOOK_Activate(); } #endif /* PIOS_INCLUDE_USB */ /* Configure IO ports */ HwSparky2DSMxModeOptions hw_DSMxMode; HwSparky2DSMxModeGet(&hw_DSMxMode); /* Configure main USART port */ uint8_t hw_mainport; HwSparky2MainPortGet(&hw_mainport); PIOS_HAL_ConfigurePort(hw_mainport, &pios_usart_main_cfg, &pios_usart_com_driver, NULL, NULL, NULL, PIOS_LED_ALARM, &pios_usart_dsm_hsum_main_cfg, &pios_dsm_main_cfg, hw_DSMxMode, NULL, NULL, false); /* Configure FlexiPort */ uint8_t hw_flexiport; HwSparky2FlexiPortGet(&hw_flexiport); PIOS_HAL_ConfigurePort(hw_flexiport, &pios_usart_flexi_cfg, &pios_usart_com_driver, &pios_i2c_flexiport_adapter_id, &pios_i2c_flexiport_adapter_cfg, NULL, PIOS_LED_ALARM, &pios_usart_dsm_hsum_flexi_cfg, &pios_dsm_flexi_cfg, hw_DSMxMode, NULL, NULL, false); #if defined(PIOS_INCLUDE_RFM22B) HwSparky2Data hwSparky2; HwSparky2Get(&hwSparky2); const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); const struct pios_openlrs_cfg *openlrs_cfg = PIOS_BOARD_HW_DEFS_GetOpenLRSCfg(bdinfo->board_rev); PIOS_HAL_ConfigureRFM22B(hwSparky2.Radio, bdinfo->board_type, bdinfo->board_rev, hwSparky2.MaxRfPower, hwSparky2.MaxRfSpeed, openlrs_cfg, rfm22b_cfg, hwSparky2.MinChannel, hwSparky2.MaxChannel, hwSparky2.CoordID, 1); #endif /* PIOS_INCLUDE_RFM22B */ /* Configure the receiver port*/ uint8_t hw_rcvrport; HwSparky2RcvrPortGet(&hw_rcvrport); if (bdinfo->board_rev != BRUSHEDSPARKY_V0_2 && hw_DSMxMode >= HWSPARKY2_DSMXMODE_BIND3PULSES) { hw_DSMxMode = HWSPARKY2_DSMXMODE_AUTODETECT; /* Do not try to bind through XOR */ } PIOS_HAL_ConfigurePort(hw_rcvrport, NULL, /* XXX TODO: fix as part of DSM refactor */ &pios_usart_com_driver, NULL, NULL, &pios_ppm_cfg, PIOS_LED_ALARM, &pios_usart_dsm_hsum_rcvr_cfg, &pios_dsm_rcvr_cfg, hw_DSMxMode, get_sbus_rcvr_cfg(bdinfo->board_rev), &pios_sbus_cfg, get_sbus_toggle(bdinfo->board_rev)); #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 /* Set up the servo outputs */ PIOS_Servo_Init(&pios_servo_cfg); #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif if (PIOS_I2C_Init(&pios_i2c_mag_pressure_adapter_id, &pios_i2c_mag_pressure_adapter_cfg)) { PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_CAN) if(get_use_can(bdinfo->board_rev)) { if (PIOS_CAN_Init(&pios_can_id, &pios_can_cfg) != 0) panic(6); uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_CAN_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_CAN_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_can_id, &pios_can_com_driver, pios_can_id, rx_buffer, PIOS_COM_CAN_RX_BUF_LEN, tx_buffer, PIOS_COM_CAN_TX_BUF_LEN)) panic(6); /* pios_com_bridge_id = pios_com_can_id; */ } #endif PIOS_DELAY_WaitmS(50); PIOS_SENSORS_Init(); #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); // configure the pullup for PA8 (inhibit pullups from current/sonar shared pin) GPIO_Init(pios_current_sonar_pin.gpio, &pios_current_sonar_pin.init); #endif #if defined(PIOS_INCLUDE_MS5611) if (PIOS_MS5611_Init(&pios_ms5611_cfg, pios_i2c_mag_pressure_adapter_id) != 0) panic(4); if (PIOS_MS5611_Test() != 0) panic(4); #endif uint8_t Magnetometer; HwSparky2MagnetometerGet(&Magnetometer); if (Magnetometer != HWSPARKY2_MAGNETOMETER_INTERNAL) pios_mpu9250_cfg.use_magnetometer = false; #if defined(PIOS_INCLUDE_MPU9250_SPI) if (PIOS_MPU9250_SPI_Init(pios_spi_gyro_id, 0, &pios_mpu9250_cfg) != 0) panic(2); // To be safe map from UAVO enum to driver enum uint8_t hw_gyro_range; HwSparky2GyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWSPARKY2_GYRORANGE_250: PIOS_MPU9250_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWSPARKY2_GYRORANGE_500: PIOS_MPU9250_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWSPARKY2_GYRORANGE_1000: PIOS_MPU9250_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWSPARKY2_GYRORANGE_2000: PIOS_MPU9250_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwSparky2AccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWSPARKY2_ACCELRANGE_2G: PIOS_MPU9250_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWSPARKY2_ACCELRANGE_4G: PIOS_MPU9250_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWSPARKY2_ACCELRANGE_8G: PIOS_MPU9250_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWSPARKY2_ACCELRANGE_16G: PIOS_MPU9250_SetAccelRange(PIOS_MPU60X0_ACCEL_16G); break; } // the filter has to be set before rate else divisor calculation will fail uint8_t hw_mpu9250_dlpf; HwSparky2MPU9250GyroLPFGet(&hw_mpu9250_dlpf); enum pios_mpu9250_gyro_filter mpu9250_gyro_lpf = \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250GYROLPF_184) ? PIOS_MPU9250_GYRO_LOWPASS_184_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250GYROLPF_92) ? PIOS_MPU9250_GYRO_LOWPASS_92_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250GYROLPF_41) ? PIOS_MPU9250_GYRO_LOWPASS_41_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250GYROLPF_20) ? PIOS_MPU9250_GYRO_LOWPASS_20_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250GYROLPF_10) ? PIOS_MPU9250_GYRO_LOWPASS_10_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250GYROLPF_5) ? PIOS_MPU9250_GYRO_LOWPASS_5_HZ : \ pios_mpu9250_cfg.default_gyro_filter; PIOS_MPU9250_SetGyroLPF(mpu9250_gyro_lpf); HwSparky2MPU9250AccelLPFGet(&hw_mpu9250_dlpf); enum pios_mpu9250_accel_filter mpu9250_accel_lpf = \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_460) ? PIOS_MPU9250_ACCEL_LOWPASS_460_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_184) ? PIOS_MPU9250_ACCEL_LOWPASS_184_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_92) ? PIOS_MPU9250_ACCEL_LOWPASS_92_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_41) ? PIOS_MPU9250_ACCEL_LOWPASS_41_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_20) ? PIOS_MPU9250_ACCEL_LOWPASS_20_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_10) ? PIOS_MPU9250_ACCEL_LOWPASS_10_HZ : \ (hw_mpu9250_dlpf == HWSPARKY2_MPU9250ACCELLPF_5) ? PIOS_MPU9250_ACCEL_LOWPASS_5_HZ : \ pios_mpu9250_cfg.default_accel_filter; PIOS_MPU9250_SetAccelLPF(mpu9250_accel_lpf); uint8_t hw_mpu9250_samplerate; HwSparky2MPU9250RateGet(&hw_mpu9250_samplerate); uint16_t mpu9250_samplerate = \ (hw_mpu9250_samplerate == HWSPARKY2_MPU9250RATE_200) ? 200 : \ (hw_mpu9250_samplerate == HWSPARKY2_MPU9250RATE_250) ? 250 : \ (hw_mpu9250_samplerate == HWSPARKY2_MPU9250RATE_333) ? 333 : \ (hw_mpu9250_samplerate == HWSPARKY2_MPU9250RATE_500) ? 500 : \ (hw_mpu9250_samplerate == HWSPARKY2_MPU9250RATE_1000) ? 1000 : \ pios_mpu9250_cfg.default_samplerate; PIOS_MPU9250_SetSampleRate(mpu9250_samplerate); #endif /* PIOS_INCLUDE_MPU9250_SPI */ PIOS_WDG_Clear(); #if defined(PIOS_INCLUDE_HMC5883) { uint8_t Magnetometer; HwSparky2MagnetometerGet(&Magnetometer); if (Magnetometer == HWSPARKY2_MAGNETOMETER_EXTERNALI2CFLEXIPORT) { if (PIOS_HMC5883_Init(pios_i2c_flexiport_adapter_id, &pios_hmc5883_external_cfg) != 0) panic(6); if (PIOS_HMC5883_Test() != 0) panic(6); } else if (Magnetometer == HWSPARKY2_MAGNETOMETER_EXTERNALAUXI2C) { if (PIOS_HMC5883_Init(pios_i2c_mag_pressure_adapter_id, &pios_hmc5883_external_cfg) != 0) panic(6); if (PIOS_HMC5883_Test() != 0) panic(6); } if (Magnetometer != HWSPARKY2_MAGNETOMETER_INTERNAL) { // setup sensor orientation uint8_t ExtMagOrientation; HwSparky2ExtMagOrientationGet(&ExtMagOrientation); enum pios_hmc5883_orientation hmc5883_orientation = \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_TOP0DEGCW) ? PIOS_HMC5883_TOP_0DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_TOP90DEGCW) ? PIOS_HMC5883_TOP_90DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_TOP180DEGCW) ? PIOS_HMC5883_TOP_180DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_TOP270DEGCW) ? PIOS_HMC5883_TOP_270DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_BOTTOM0DEGCW) ? PIOS_HMC5883_BOTTOM_0DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_BOTTOM90DEGCW) ? PIOS_HMC5883_BOTTOM_90DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_BOTTOM180DEGCW) ? PIOS_HMC5883_BOTTOM_180DEG : \ (ExtMagOrientation == HWSPARKY2_EXTMAGORIENTATION_BOTTOM270DEGCW) ? PIOS_HMC5883_BOTTOM_270DEG : \ pios_hmc5883_external_cfg.Default_Orientation; PIOS_HMC5883_SetOrientation(hmc5883_orientation); } } #endif /* PIOS_INCLUDE_HMC5883 */ #if defined(PIOS_INCLUDE_FLASH) && defined(PIOS_INCLUDE_FLASH_JEDEC) if (get_external_flash(bdinfo->board_rev)) { if ( PIOS_STREAMFS_Init(&streamfs_id, &streamfs_settings, FLASH_PARTITION_LABEL_LOG) != 0) panic(8); const uint32_t LOG_BUF_LEN = 256; uint8_t *log_rx_buffer = PIOS_malloc(LOG_BUF_LEN); uint8_t *log_tx_buffer = PIOS_malloc(LOG_BUF_LEN); if (PIOS_COM_Init(&pios_com_logging_id, &pios_streamfs_com_driver, streamfs_id, log_rx_buffer, LOG_BUF_LEN, log_tx_buffer, LOG_BUF_LEN) != 0) panic(9); } #endif /* PIOS_INCLUDE_FLASH */ switch (bdinfo->board_rev) { case BRUSHEDSPARKY_V0_2: { HwSparky2VTX_ChOptions channel; HwSparky2VTX_ChGet(&channel); set_vtx_channel(channel); } break; } }
/** * Module thread, should not return. */ static void groundPathFollowerTask(void *parameters) { SystemSettingsData systemSettings; FlightStatusData flightStatus; uint32_t lastUpdateTime; GroundPathFollowerSettingsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); GroundPathFollowerSettingsGet(&guidanceSettings); PathDesiredGet(&pathDesired); // Main task loop lastUpdateTime = PIOS_Thread_Systime(); while (1) { // Conditions when this runs: // 1. Must have GROUND type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path SystemSettingsGet(&systemSettings); if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE) ) { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING); PIOS_Thread_Sleep(1000); continue; } // Continue collecting data if not enough time PIOS_Thread_Sleep_Until(&lastUpdateTime, guidanceSettings.UpdatePeriod); // Convert the accels into the NED frame updateNedAccel(); FlightStatusGet(&flightStatus); // Check the combinations of flightmode and pathdesired mode switch(flightStatus.FlightMode) { /* This combination of RETURNTOHOME and HOLDPOSITION looks strange but * is correct. RETURNTOHOME mode uses HOLDPOSITION with the position * set to home */ case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateGroundDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateGroundDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT || pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateGroundDesiredAttitude(); } else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) { updatePathVelocity(); updateGroundDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; default: // Be cleaner and get rid of global variables northVelIntegral = 0; eastVelIntegral = 0; northPosIntegral = 0; eastPosIntegral = 0; // Track throttle before engaging this mode. Cheap system ident StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); throttleOffset = stabDesired.Throttle; break; } AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); } }
/** * Update system alarms */ static void updateSystemAlarms() { SystemStatsData stats; UAVObjStats objStats; EventStats evStats; SystemStatsGet(&stats); // Check heap, IRQ stack and malloc failures if (PIOS_heap_malloc_failed_p() || (stats.HeapRemaining < HEAP_LIMIT_CRITICAL) #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) || (stats.IRQStackRemaining < IRQSTACK_LIMIT_CRITICAL) #endif ) { AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL); } else if ( (stats.HeapRemaining < HEAP_LIMIT_WARNING) #if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK) || (stats.IRQStackRemaining < IRQSTACK_LIMIT_WARNING) #endif ) { AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY); } // Check CPU load if (stats.CPULoad > CPULOAD_LIMIT_CRITICAL) { AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_CRITICAL); } else if (stats.CPULoad > CPULOAD_LIMIT_WARNING) { AlarmsSet(SYSTEMALARMS_ALARM_CPUOVERLOAD, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_CPUOVERLOAD); } // Check for stack overflow if (stackOverflow) { AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL); } else { AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW); } // Check for event errors UAVObjGetStats(&objStats); EventGetStats(&evStats); UAVObjClearStats(); EventClearStats(); if (objStats.eventCallbackErrors > 0 || objStats.eventQueueErrors > 0 || evStats.eventErrors > 0) { AlarmsSet(SYSTEMALARMS_ALARM_EVENTSYSTEM, SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_EVENTSYSTEM); } if (objStats.lastCallbackErrorID || objStats.lastQueueErrorID || evStats.lastErrorID) { SystemStatsData sysStats; SystemStatsGet(&sysStats); sysStats.EventSystemWarningID = evStats.lastErrorID; sysStats.ObjectManagerCallbackID = objStats.lastCallbackErrorID; sysStats.ObjectManagerQueueID = objStats.lastQueueErrorID; SystemStatsSet(&sysStats); } }
/** * Module thread, should not return. */ static void guidanceTask(void *parameters) { SystemSettingsData systemSettings; GuidanceSettingsData guidanceSettings; ManualControlCommandData manualControl; portTickType thisTime; portTickType lastUpdateTime; UAVObjEvent ev; float accel[3] = {0,0,0}; uint32_t accel_accum = 0; float q[4]; float Rbe[3][3]; float accel_ned[3]; // Main task loop lastUpdateTime = xTaskGetTickCount(); while (1) { GuidanceSettingsGet(&guidanceSettings); // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(queue, &ev, guidanceSettings.UpdatePeriod / portTICK_RATE_MS) != pdTRUE ) { AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING); } else { AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); } // Collect downsampled attitude data AttitudeRawData attitudeRaw; AttitudeRawGet(&attitudeRaw); accel[0] += attitudeRaw.accels[0]; accel[1] += attitudeRaw.accels[1]; accel[2] += attitudeRaw.accels[2]; accel_accum++; // Continue collecting data if not enough time thisTime = xTaskGetTickCount(); if( (thisTime - lastUpdateTime) < (guidanceSettings.UpdatePeriod / portTICK_RATE_MS) ) continue; lastUpdateTime = xTaskGetTickCount(); accel[0] /= accel_accum; accel[1] /= accel_accum; accel[2] /= accel_accum; //rotate avg accels into earth frame and store it AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); q[0]=attitudeActual.q1; q[1]=attitudeActual.q2; q[2]=attitudeActual.q3; q[3]=attitudeActual.q4; Quaternion2R(q, Rbe); for (uint8_t i=0; i<3; i++){ accel_ned[i]=0; for (uint8_t j=0; j<3; j++) accel_ned[i] += Rbe[j][i]*accel[j]; } accel_ned[2] += 9.81; NedAccelData accelData; NedAccelGet(&accelData); // Convert from m/s to cm/s accelData.North = accel_ned[0] * 100; accelData.East = accel_ned[1] * 100; accelData.Down = accel_ned[2] * 100; NedAccelSet(&accelData); ManualControlCommandGet(&manualControl); SystemSettingsGet(&systemSettings); GuidanceSettingsGet(&guidanceSettings); if ((manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO) && ((systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) || (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) || (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) || (systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) )) { if(positionHoldLast == 0) { /* When enter position hold mode save current position */ PositionDesiredData positionDesired; PositionActualData positionActual; PositionDesiredGet(&positionDesired); PositionActualGet(&positionActual); positionDesired.North = positionActual.North; positionDesired.East = positionActual.East; PositionDesiredSet(&positionDesired); positionHoldLast = 1; } if(guidanceSettings.GuidanceMode == GUIDANCESETTINGS_GUIDANCEMODE_DUAL_LOOP) updateVtolDesiredVelocity(); else manualSetDesiredVelocity(); updateVtolDesiredAttitude(); } else { // Be cleaner and get rid of global variables northIntegral = 0; eastIntegral = 0; downIntegral = 0; positionHoldLast = 0; } accel[0] = accel[1] = accel[2] = 0; accel_accum = 0; } }
/** * Module task */ static void manualControlTask(void *parameters) { ManualControlSettingsData settings; StabilizationSettingsData stabSettings; ManualControlCommandData cmd; ActuatorDesiredData actuator; AttitudeDesiredData attitude; RateDesiredData rate; portTickType lastSysTime; float flightMode; uint8_t disconnected_count = 0; uint8_t connected_count = 0; enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED; // Make sure unarmed on power up ManualControlCommandGet(&cmd); cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; ManualControlCommandSet(&cmd); armState = ARM_STATE_DISARMED; // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; // Wait until next update vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); // Read settings ManualControlSettingsGet(&settings); StabilizationSettingsGet(&stabSettings); if (ManualControlCommandReadOnly(&cmd)) { FlightTelemetryStatsData flightTelemStats; FlightTelemetryStatsGet(&flightTelemStats); if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { /* trying to fly via GCS and lost connection. fall back to transmitter */ UAVObjMetadata metadata; UAVObjGetMetadata(&cmd, &metadata); metadata.access = ACCESS_READWRITE; UAVObjSetMetadata(&cmd, &metadata); } } if (!ManualControlCommandReadOnly(&cmd)) { // Check settings, if error raise alarm if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); continue; } // Read channel values in us // TODO: settings.InputMode is currently ignored because PIOS will not allow runtime // selection of PWM and PPM. The configuration is currently done at compile time in // the pios_config.h file. for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { #if defined(PIOS_INCLUDE_PWM) cmd.Channel[n] = PIOS_PWM_Get(n); #elif defined(PIOS_INCLUDE_PPM) cmd.Channel[n] = PIOS_PPM_Get(n); #elif defined(PIOS_INCLUDE_SPEKTRUM) cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); #endif scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); } // Scale channels to -1 -> +1 range cmd.Roll = scaledChannel[settings.Roll]; cmd.Pitch = scaledChannel[settings.Pitch]; cmd.Yaw = scaledChannel[settings.Yaw]; cmd.Throttle = scaledChannel[settings.Throttle]; flightMode = scaledChannel[settings.FlightMode]; if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) cmd.Accessory1 = scaledChannel[settings.Accessory1]; else cmd.Accessory1 = 0; if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) cmd.Accessory2 = scaledChannel[settings.Accessory2]; else cmd.Accessory2 = 0; if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) cmd.Accessory3 = scaledChannel[settings.Accessory3]; else cmd.Accessory3 = 0; if (flightMode < -FLIGHT_MODE_LIMIT) { // Position 1 for(int i = 0; i < 3; i++) { cmd.StabilizationSettings[i] = settings.Pos1StabilizationSettings[i]; // See assumptions1 } cmd.FlightMode = settings.Pos1FlightMode; // See assumptions2 } else if (flightMode > FLIGHT_MODE_LIMIT) { // Position 3 for(int i = 0; i < 3; i++) { cmd.StabilizationSettings[i] = settings.Pos3StabilizationSettings[i]; // See assumptions5 } cmd.FlightMode = settings.Pos3FlightMode; // See assumptions6 } else { // Position 2 for(int i = 0; i < 3; i++) { cmd.StabilizationSettings[i] = settings.Pos2StabilizationSettings[i]; // See assumptions3 } cmd.FlightMode = settings.Pos2FlightMode; // See assumptions4 } // Update the ManualControlCommand object ManualControlCommandSet(&cmd); // This seems silly to set then get, but the reason is if the GCS is // the control input, the set command will be blocked by the read only // setting and the get command will pull the right values from telemetry } else ManualControlCommandGet(&cmd); /* Under GCS control */ // Implement hysteresis loop on connection status // Must check both Max and Min in case they reversed if (!ManualControlCommandReadOnly(&cmd) && cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET && cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) { if (disconnected_count++ > 10) { connection_state = DISCONNECTED; connected_count = 0; disconnected_count = 0; } else disconnected_count++; } else { if (connected_count++ > 10) { connection_state = CONNECTED; connected_count = 0; disconnected_count = 0; } else connected_count++; } if (connection_state == DISCONNECTED) { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; cmd.Throttle = -1; // Shut down engine with no control cmd.Roll = 0; cmd.Yaw = 0; cmd.Pitch = 0; //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); ManualControlCommandSet(&cmd); } else { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); ManualControlCommandSet(&cmd); } // Arming and Disarming mechanism if (cmd.Throttle < 0) { // Throttle is low, in this condition the arming state could change uint8_t newCmdArmed = cmd.Armed; static portTickType armedDisarmStart; // Look for state changes and write in newArmState if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_NONE) { // No channel assigned to arming -> arm immediately when throttle is low newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; } else { float armStickLevel; uint8_t channel = settings.Arming/2; // 0=Channel1, 1=Channel1_Rev, 2=Channel2, .... bool reverse = (settings.Arming%2)==1; bool manualArm = false; bool manualDisarm = false; if (connection_state == CONNECTED) { // Should use RC input only if RX is connected armStickLevel = scaledChannel[channel]; if (reverse) armStickLevel =-armStickLevel; if (armStickLevel <= -0.90) manualArm = true; else if (armStickLevel >= +0.90) manualDisarm = true; } switch(armState) { case ARM_STATE_DISARMED: newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE; if (manualArm) { armedDisarmStart = lastSysTime; armState = ARM_STATE_ARMING_MANUAL; } break; case ARM_STATE_ARMING_MANUAL: if (manualArm) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_ARMED; } else armState = ARM_STATE_DISARMED; break; case ARM_STATE_ARMED: // When we get here, the throttle is low, // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_TIMEOUT; newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; break; case ARM_STATE_DISARMING_TIMEOUT: // We get here when armed while throttle low, even when the arming timeout is not enabled if (settings.ArmedTimeout != 0) if (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings.ArmedTimeout) armState = ARM_STATE_DISARMED; // Switch to disarming due to manual control when needed if (manualDisarm) { armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_MANUAL; } break; case ARM_STATE_DISARMING_MANUAL: if (manualDisarm) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_DISARMED; } else armState = ARM_STATE_ARMED; break; } } // Update cmd object when needed if (newCmdArmed != cmd.Armed) { cmd.Armed = newCmdArmed; ManualControlCommandSet(&cmd); } } else { // The throttle is not low, in case we where arming or disarming, abort switch(armState) { case ARM_STATE_DISARMING_MANUAL: case ARM_STATE_DISARMING_TIMEOUT: armState = ARM_STATE_ARMED; break; case ARM_STATE_ARMING_MANUAL: armState = ARM_STATE_DISARMED; break; default: // Nothing needs to be done in the other states break; } } // End of arming/disarming // Depending on the mode update the Stabilization or Actuator objects if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) { actuator.Roll = cmd.Roll; actuator.Pitch = cmd.Pitch; actuator.Yaw = cmd.Yaw; actuator.Throttle = cmd.Throttle; ActuatorDesiredSet(&actuator); } else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) { attitude.Roll = cmd.Roll * stabSettings.RollMax; attitude.Pitch = cmd.Pitch * stabSettings.PitchMax; attitude.Yaw = fmod(cmd.Yaw * 180.0, 360); attitude.Throttle = (cmd.Throttle < 0) ? -1 : cmd.Throttle; rate.Roll = cmd.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; rate.Pitch = cmd.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; rate.Yaw = cmd.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; AttitudeDesiredSet(&attitude); RateDesiredSet(&rate); } } }
/** * Module task */ static void stabilizationTask(void* parameters) { portTickType lastSysTime; portTickType thisSysTime; UAVObjEvent ev; ActuatorDesiredData actuatorDesired; StabilizationDesiredData stabDesired; RateDesiredData rateDesired; AttitudeActualData attitudeActual; AttitudeRawData attitudeRaw; SystemSettingsData systemSettings; FlightStatusData flightStatus; SettingsUpdatedCb((UAVObjEvent *) NULL); // Main task loop lastSysTime = xTaskGetTickCount(); ZeroPids(); while(1) { PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); continue; } // Check how long since last update thisSysTime = xTaskGetTickCount(); if(thisSysTime > lastSysTime) // reuse dt in case of wraparound dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; lastSysTime = thisSysTime; FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); AttitudeActualGet(&attitudeActual); AttitudeRawGet(&attitudeRaw); RateDesiredGet(&rateDesired); SystemSettingsGet(&systemSettings); #if defined(PIOS_QUATERNION_STABILIZATION) // Quaternion calculation of error in each axis. Uses more memory. float rpy_desired[3]; float q_desired[4]; float q_error[4]; float local_error[3]; // Essentially zero errors for anything in rate or none if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) rpy_desired[0] = stabDesired.Roll; else rpy_desired[0] = attitudeActual.Roll; if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) rpy_desired[1] = stabDesired.Pitch; else rpy_desired[1] = attitudeActual.Pitch; if(stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) rpy_desired[2] = stabDesired.Yaw; else rpy_desired[2] = attitudeActual.Yaw; RPY2Quaternion(rpy_desired, q_desired); quat_inverse(q_desired); quat_mult(q_desired, &attitudeActual.q1, q_error); quat_inverse(q_error); Quaternion2RPY(q_error, local_error); #else // Simpler algorithm for CC, less memory float local_error[3] = {stabDesired.Roll - attitudeActual.Roll, stabDesired.Pitch - attitudeActual.Pitch, stabDesired.Yaw - attitudeActual.Yaw}; local_error[2] = fmod(local_error[2] + 180, 360) - 180; #endif for(uint8_t i = 0; i < MAX_AXES; i++) { gyro_filtered[i] = gyro_filtered[i] * gyro_alpha + attitudeRaw.gyros[i] * (1 - gyro_alpha); } float *attitudeDesiredAxis = &stabDesired.Roll; float *actuatorDesiredAxis = &actuatorDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; //Calculate desired rate for(uint8_t i=0; i< MAX_AXES; i++) { switch(stabDesired.StabilizationMode[i]) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: rateDesiredAxis[i] = attitudeDesiredAxis[i]; axis_lock_accum[i] = 0; break; case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: { float weak_leveling = local_error[i] * weak_leveling_kp; if(weak_leveling > weak_leveling_max) weak_leveling = weak_leveling_max; if(weak_leveling < -weak_leveling_max) weak_leveling = -weak_leveling_max; rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; axis_lock_accum[i] = 0; break; } case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i]); axis_lock_accum[i] = 0; break; case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: if(fabs(attitudeDesiredAxis[i]) > max_axislock_rate) { // While getting strong commands act like rate mode rateDesiredAxis[i] = attitudeDesiredAxis[i]; axis_lock_accum[i] = 0; } else { // For weaker commands or no command simply attitude lock (almost) on no gyro change axis_lock_accum[i] += (attitudeDesiredAxis[i] - gyro_filtered[i]) * dT; if(axis_lock_accum[i] > max_axis_lock) axis_lock_accum[i] = max_axis_lock; else if(axis_lock_accum[i] < -max_axis_lock) axis_lock_accum[i] = -max_axis_lock; rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i]); } break; } } uint8_t shouldUpdate = 1; RateDesiredSet(&rateDesired); ActuatorDesiredGet(&actuatorDesired); //Calculate desired command for(int8_t ct=0; ct< MAX_AXES; ct++) { if(rateDesiredAxis[ct] > settings.MaximumRate[ct]) rateDesiredAxis[ct] = settings.MaximumRate[ct]; else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct]) rateDesiredAxis[ct] = -settings.MaximumRate[ct]; switch(stabDesired.StabilizationMode[ct]) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: { float command = ApplyPid(&pids[PID_RATE_ROLL + ct], rateDesiredAxis[ct] - gyro_filtered[ct]); actuatorDesiredAxis[ct] = bound(command); break; } case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: switch (ct) { case ROLL: actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); shouldUpdate = 1; break; case PITCH: actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); shouldUpdate = 1; break; case YAW: actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); shouldUpdate = 1; break; } break; } } // Save dT actuatorDesired.UpdateTime = dT * 1000; if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) == FLIGHTMODE_MANUAL) shouldUpdate = 0; if(shouldUpdate) { actuatorDesired.Throttle = stabDesired.Throttle; if(dT > 15) actuatorDesired.NumLongUpdates++; ActuatorDesiredSet(&actuatorDesired); } if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || (lowThrottleZeroIntegral && stabDesired.Throttle < 0) || !shouldUpdate) { ZeroPids(); } // Clear alarms AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } }
static void onTimer(UAVObjEvent* ev) { static portTickType lastSysTime; static bool firstRun = true; static FlightBatteryStateData flightBatteryData; if (firstRun) { #ifdef ENABLE_DEBUG_MSG PIOS_COM_ChangeBaud(DEBUG_PORT, 57600); #endif lastSysTime = xTaskGetTickCount(); //FlightBatteryStateGet(&flightBatteryData); firstRun = false; } AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR); portTickType thisSysTime; FlightBatterySettingsData batterySettings; static float dT = SAMPLE_PERIOD_MS / 1000; float Bob; float energyRemaining; // Check how long since last update thisSysTime = xTaskGetTickCount(); if(thisSysTime > lastSysTime) // reuse dt in case of wraparound dT = (float)(thisSysTime - lastSysTime) / (float)(portTICK_RATE_MS * 1000.0f); //lastSysTime = thisSysTime; FlightBatterySettingsGet(&batterySettings); //calculate the battery parameters flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(2)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts flightBatteryData.Current = ((float)PIOS_ADC_PinGet(1)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; //in Amps Bob =dT; // FIXME: something funky happens if I don't do this... Andrew flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * 1000.0 * dT / 3600.0) ;//in mAh if (flightBatteryData.Current > flightBatteryData.PeakCurrent)flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps flightBatteryData.AvgCurrent=(flightBatteryData.AvgCurrent*0.8)+(flightBatteryData.Current*0.2); //in Amps //sanity checks if (flightBatteryData.AvgCurrent<0)flightBatteryData.AvgCurrent=0.0; if (flightBatteryData.PeakCurrent<0)flightBatteryData.PeakCurrent=0.0; if (flightBatteryData.ConsumedEnergy<0)flightBatteryData.ConsumedEnergy=0.0; energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh flightBatteryData.EstimatedFlightTime = ((energyRemaining / (flightBatteryData.AvgCurrent*1000.0))*3600.0);//in Sec //generate alarms where needed... if ((flightBatteryData.Voltage<=0)&&(flightBatteryData.Current<=0)) { AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR); } else { if (flightBatteryData.EstimatedFlightTime < 30) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL); else if (flightBatteryData.EstimatedFlightTime < 60) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING); else AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME); // FIXME: should make the battery voltage detection dependent on battery type. if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); else AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); } lastSysTime = thisSysTime; FlightBatteryStateSet(&flightBatteryData); }
/** * Main task. It does not return. */ static void batteryTask(void * parameters) { const float dT = SAMPLE_PERIOD_MS / 1000.0f; settingsUpdatedCb(NULL); // Main task loop portTickType lastSysTime; lastSysTime = xTaskGetTickCount(); while (true) { vTaskDelayUntil(&lastSysTime, MS2TICKS(SAMPLE_PERIOD_MS)); FlightBatteryStateData flightBatteryData; FlightBatterySettingsData batterySettings; float energyRemaining; if (battery_settings_updated) { battery_settings_updated = false; FlightBatterySettingsGet(&batterySettings); voltageADCPin = batterySettings.VoltagePin; if (voltageADCPin == FLIGHTBATTERYSETTINGS_VOLTAGEPIN_NONE) voltageADCPin = -1; currentADCPin = batterySettings.CurrentPin; if (currentADCPin == FLIGHTBATTERYSETTINGS_CURRENTPIN_NONE) currentADCPin = -1; } //calculate the battery parameters if (voltageADCPin >= 0) { flightBatteryData.Voltage = ((float) PIOS_ADC_GetChannelVolt(voltageADCPin)) / batterySettings.SensorCalibrationFactor[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONFACTOR_VOLTAGE] * 1000.0f + batterySettings.SensorCalibrationOffset[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONOFFSET_VOLTAGE]; //in Volts } else { flightBatteryData.Voltage = 0; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC } if (currentADCPin >= 0) { flightBatteryData.Current = ((float) PIOS_ADC_GetChannelVolt(currentADCPin)) / batterySettings.SensorCalibrationFactor[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONFACTOR_CURRENT] * 1000.0f + batterySettings.SensorCalibrationOffset[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONOFFSET_CURRENT]; //in Amps if (flightBatteryData.Current > flightBatteryData.PeakCurrent) flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps } else { //If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm flightBatteryData.Current = -1; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC } flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f); //in mAh //Apply a 2 second rise time low-pass filter to average the current float alpha = 1.0f - dT / (dT + 2.0f); flightBatteryData.AvgCurrent = alpha * flightBatteryData.AvgCurrent + (1 - alpha) * flightBatteryData.Current; //in Amps energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh if (flightBatteryData.AvgCurrent > 0) flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent * 1000.0f)) * 3600.0f; //in Sec else flightBatteryData.EstimatedFlightTime = 9999; //generate alarms where needed... if ((flightBatteryData.Voltage <= 0) && (flightBatteryData.Current <= 0)) { //FIXME: There's no guarantee that a floating ADC will give 0. So this // check might fail, even when there's nothing attached. AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR); } else { // FIXME: should make the timer alarms user configurable if (flightBatteryData.EstimatedFlightTime < 30) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL); else if (flightBatteryData.EstimatedFlightTime < 120) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING); else AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME); // FIXME: should make the battery voltage detection dependent on battery type. /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/ if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); else AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); } FlightBatteryStateSet(&flightBatteryData); } }
static void gpsTask(void *parameters) { portTickType xDelay = 100 / portTICK_RATE_MS; uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; GPSPositionData GpsData; #ifdef ENABLE_GPS_BINARY_GTOP GTOP_BIN_init(); #else uint8_t rx_count = 0; bool start_flag = false; bool found_cr = false; int32_t gpsRxOverflow = 0; #endif #ifdef FULL_COLD_RESTART // tell the GPS to do a FULL COLD restart PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK104*37\r\n"); timeOfLastCommandMs = timeNowMs; while (timeNowMs - timeOfLastCommandMs < 300) // delay for 300ms to let the GPS sort itself out { vTaskDelay(xDelay); // Block task until next update timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; } #endif #ifdef DISABLE_GPS_TRESHOLD PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK397,0*23\r\n"); #endif #ifdef ENABLE_GPS_BINARY_GTOP // switch to GTOP binary mode PIOS_COM_SendStringNonBlocking(gpsPort ,"$PGCMD,21,1*6F\r\n"); #endif #ifdef ENABLE_GPS_ONESENTENCE_GTOP // switch to single sentence mode PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,2*6C\r\n"); #endif #ifdef ENABLE_GPS_NMEA // switch to NMEA mode PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,3*6D\r\n"); #endif numUpdates = 0; numChecksumErrors = 0; numParsingErrors = 0; timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; // Loop forever while (1) { #ifdef ENABLE_GPS_BINARY_GTOP // GTOP BINARY GPS mode while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0) { int res = GTOP_BIN_update_position(PIOS_COM_ReceiveBuffer(gpsPort), &numChecksumErrors, &numParsingErrors); if (res >= 0) { numUpdates++; timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; } } #else // NMEA or SINGLE-SENTENCE GPS mode // This blocks the task until there is something on the buffer while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0) { char c = PIOS_COM_ReceiveBuffer(gpsPort); // detect start while acquiring stream if (!start_flag && (c == '$')) { start_flag = true; found_cr = false; rx_count = 0; } else if (!start_flag) continue; if (rx_count >= sizeof(gps_rx_buffer)) { // The buffer is already full and we haven't found a valid NMEA sentence. // Flush the buffer and note the overflow event. gpsRxOverflow++; start_flag = false; found_cr = false; rx_count = 0; } else { gps_rx_buffer[rx_count] = c; rx_count++; } // look for ending '\r\n' sequence if (!found_cr && (c == '\r') ) found_cr = true; else if (found_cr && (c != '\n') ) found_cr = false; // false end flag else if (found_cr && (c == '\n') ) { // The NMEA functions require a zero-terminated string // As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n gps_rx_buffer[rx_count-2] = 0; // prepare to parse next sentence start_flag = false; found_cr = false; rx_count = 0; // Our rxBuffer must look like this now: // [0] = '$' // ... = zero or more bytes of sentence payload // [end_pos - 1] = '\r' // [end_pos] = '\n' // // Prepare to consume the sentence from the buffer // Validate the checksum over the sentence if (!NMEA_checksum(&gps_rx_buffer[1])) { // Invalid checksum. May indicate dropped characters on Rx. PIOS_DEBUG_PinHigh(2); ++numChecksumErrors; PIOS_DEBUG_PinLow(2); } else { // Valid checksum, use this packet to update the GPS position if (!NMEA_update_position(&gps_rx_buffer[1])) { PIOS_DEBUG_PinHigh(2); ++numParsingErrors; PIOS_DEBUG_PinLow(2); } else ++numUpdates; timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; } } } #endif // Check for GPS timeout timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) { // we have not received any valid GPS sentences for a while. // either the GPS is not plugged in or a hardware problem or the GPS has locked up. GPSPositionGet(&GpsData); GpsData.Status = GPSPOSITION_STATUS_NOGPS; GPSPositionSet(&GpsData); AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); if ((timeNowMs - timeOfLastCommandMs) >= GPS_COMMAND_RESEND_TIMEOUT_MS) { // resend the command .. just incase the gps has only just been plugged in or the gps did not get our last command timeOfLastCommandMs = timeNowMs; #ifdef ENABLE_GPS_BINARY_GTOP GTOP_BIN_init(); // switch to binary mode PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,1*6F\r\n"); #endif #ifdef ENABLE_GPS_ONESENTENCE_GTOP // switch to single sentence mode PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,2*6C\r\n"); #endif #ifdef ENABLE_GPS_NMEA // switch to NMEA mode PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,3*6D\r\n"); #endif #ifdef DISABLE_GPS_TRESHOLD PIOS_COM_SendStringNonBlocking(gpsPort,"$PMTK397,0*23\r\n"); #endif } } else { // we appear to be receiving GPS sentences OK, we've had an update HomeLocationData home; HomeLocationGet(&home); GPSPositionGet(&GpsData); if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE)) setHomeLocation(&GpsData); //criteria for GPS-OK taken from this post... //http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220 if ((GpsData.PDOP < 3.5) && (GpsData.Satellites >= 7)) AlarmsClear(SYSTEMALARMS_ALARM_GPS); else if (GpsData.Status == GPSPOSITION_STATUS_FIX3D) AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING); else AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); } // Block task until next update vTaskDelay(xDelay); } }
/** * Module task */ static void manualControlTask(void *parameters) { ManualControlSettingsData settings; StabilizationSettingsData stabSettings; ManualControlCommandData cmd; ActuatorDesiredData actuator; AttitudeDesiredData attitude; portTickType lastSysTime; portTickType armedDisarmStart = 0; float flightMode; uint8_t disconnected_count = 0; uint8_t connected_count = 0; enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED; // Make sure unarmed on power up ManualControlCommandGet(&cmd); cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; ManualControlCommandSet(&cmd); // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { // Wait until next update vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); // Read settings ManualControlSettingsGet(&settings); StabilizationSettingsGet(&stabSettings); if (!ManualControlCommandReadOnly(&cmd)) { // Check settings, if error raise alarm if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); continue; } // Read channel values in us // TODO: settings.InputMode is currently ignored because PIOS will not allow runtime // selection of PWM and PPM. The configuration is currently done at compile time in // the pios_config.h file. for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { #if defined(PIOS_INCLUDE_PWM) cmd.Channel[n] = PIOS_PWM_Get(n); #elif defined(PIOS_INCLUDE_PPM) cmd.Channel[n] = PIOS_PPM_Get(n); #elif defined(PIOS_INCLUDE_SPEKTRUM) cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); #endif } // Calculate roll command in range +1 to -1 cmd.Roll = scaleChannel(cmd.Channel[settings.Roll], settings.ChannelMax[settings.Roll], settings.ChannelMin[settings.Roll], settings.ChannelNeutral[settings.Roll]); // Calculate pitch command in range +1 to -1 cmd.Pitch = scaleChannel(cmd.Channel[settings.Pitch], settings.ChannelMax[settings.Pitch], settings.ChannelMin[settings.Pitch], settings.ChannelNeutral[settings.Pitch]); // Calculate yaw command in range +1 to -1 cmd.Yaw = scaleChannel(cmd.Channel[settings.Yaw], settings.ChannelMax[settings.Yaw], settings.ChannelMin[settings.Yaw], settings.ChannelNeutral[settings.Yaw]); // Calculate throttle command in range +1 to -1 cmd.Throttle = scaleChannel(cmd.Channel[settings.Throttle], settings.ChannelMax[settings.Throttle], settings.ChannelMin[settings.Throttle], settings.ChannelNeutral[settings.Throttle]); if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) cmd.Accessory1 = scaleChannel(cmd.Channel[settings.Accessory1], settings.ChannelMax[settings.Accessory1], settings.ChannelMin[settings.Accessory1], settings.ChannelNeutral[settings.Accessory1]); else cmd.Accessory1 = 0; if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) cmd.Accessory2 = scaleChannel(cmd.Channel[settings.Accessory2], settings.ChannelMax[settings.Accessory2], settings.ChannelMin[settings.Accessory2], settings.ChannelNeutral[settings.Accessory2]); else cmd.Accessory2 = 0; if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) cmd.Accessory3 = scaleChannel(cmd.Channel[settings.Accessory3], settings.ChannelMax[settings.Accessory3], settings.ChannelMin[settings.Accessory3], settings.ChannelNeutral[settings.Accessory3]); else cmd.Accessory3 = 0; // Update flight mode flightMode = scaleChannel(cmd.Channel[settings.FlightMode], settings.ChannelMax[settings.FlightMode], settings.ChannelMin[settings.FlightMode], settings.ChannelNeutral[settings.FlightMode]); if (flightMode < -FLIGHT_MODE_LIMIT) { // Position 1 for(int i = 0; i < 3; i++) { if(settings.Pos1StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE) cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE; else if(settings.Pos1StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE) cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE; else if(settings.Pos1StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE) cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE; } if(settings.Pos1FlightMode == MANUALCONTROLSETTINGS_POS1FLIGHTMODE_MANUAL) cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL; else if(settings.Pos1FlightMode == MANUALCONTROLSETTINGS_POS1FLIGHTMODE_STABILIZED) cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED; else if(settings.Pos1FlightMode == MANUALCONTROLSETTINGS_POS1FLIGHTMODE_AUTO) cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; } else if (flightMode > FLIGHT_MODE_LIMIT) { // Position 3 for(int i = 0; i < 3; i++) { if(settings.Pos3StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_NONE) cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE; else if(settings.Pos3StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_RATE) cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE; else if(settings.Pos3StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_ATTITUDE) cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE; } if(settings.Pos3FlightMode == MANUALCONTROLSETTINGS_POS3FLIGHTMODE_MANUAL) cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL; else if(settings.Pos3FlightMode == MANUALCONTROLSETTINGS_POS3FLIGHTMODE_STABILIZED) cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED; else if(settings.Pos3FlightMode == MANUALCONTROLSETTINGS_POS3FLIGHTMODE_AUTO) cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; } else { // Position 2 for(int i = 0; i < 3; i++) { if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_NONE) cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE; else if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_RATE) cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE; else if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_ATTITUDE) cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE; } if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_MANUAL) cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL; else if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_STABILIZED) cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED; else if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_AUTO) cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; } // Update the ManualControlCommand object ManualControlCommandSet(&cmd); // This seems silly to set then get, but the reason is if the GCS is // the control input, the set command will be blocked by the read only // setting and the get command will pull the right values from telemetry } else ManualControlCommandGet(&cmd); /* Under GCS control */ // Implement hysteresis loop on connection status // Must check both Max and Min in case they reversed if (!ManualControlCommandReadOnly(&cmd) && cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET && cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) { if (disconnected_count++ > 10) { connection_state = DISCONNECTED; connected_count = 0; disconnected_count = 0; } else disconnected_count++; } else { if (connected_count++ > 10) { connection_state = CONNECTED; connected_count = 0; disconnected_count = 0; } else connected_count++; } if (connection_state == DISCONNECTED) { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; cmd.Throttle = -1; // Shut down engine with no control cmd.Roll = 0; cmd.Yaw = 0; cmd.Pitch = 0; //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); ManualControlCommandSet(&cmd); } else { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); ManualControlCommandSet(&cmd); } /* Look for arm or disarm signal */ if ((cmd.Throttle <= 0.05) && (cmd.Roll <= -0.95)) { if ((armedDisarmStart == 0) || (lastSysTime < armedDisarmStart)) // store when started, deal with rollover armedDisarmStart = lastSysTime; else if ((lastSysTime - armedDisarmStart) > (1000 * portTICK_RATE_MS)) cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE; } else if ((cmd.Throttle <= 0.05) && (cmd.Roll >= 0.95)) { if ((armedDisarmStart == 0) || (lastSysTime < armedDisarmStart)) // store when started, deal with rollover armedDisarmStart = lastSysTime; else if ((lastSysTime - armedDisarmStart) > (1000 * portTICK_RATE_MS)) cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; } else { armedDisarmStart = 0; } // Depending on the mode update the Stabilization or Actuator objects if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) { actuator.Roll = cmd.Roll; actuator.Pitch = cmd.Pitch; actuator.Yaw = cmd.Yaw; actuator.Throttle = cmd.Throttle; ActuatorDesiredSet(&actuator); } else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) { attitude.Roll = cmd.Roll * stabSettings.RollMax; attitude.Pitch = cmd.Pitch * stabSettings.PitchMax; attitude.Yaw = fmod(cmd.Yaw * 180.0, 360); attitude.Throttle = (cmd.Throttle < 0) ? -1 : cmd.Throttle; AttitudeDesiredSet(&attitude); } if (cmd.Accessory3 < -.5) { //TODO: Make what happens here depend on GCS AHRSSettingsData attitudeSettings; AHRSSettingsGet(&attitudeSettings); // Hard coding a maximum bias of 15 for now... maybe mistake attitudeSettings.PitchBias = cmd.Accessory1 * 15; attitudeSettings.RollBias = cmd.Accessory2 * 15; AHRSSettingsSet(&attitudeSettings); } } }
/** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware * called from System/openpilot.c */ 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) PIOS_LED_Init(&pios_led_cfg); #endif /* PIOS_INCLUDE_LED */ #if defined(PIOS_INCLUDE_FLASH) /* Inititialize all flash drivers */ 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); #endif /* PIOS_INCLUDE_FLASH */ /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); /* Initialize the alarms library. Reads RCC reset flags */ AlarmsInitialize(); PIOS_RESET_Clear(); // Clear the RCC reset flags after use. /* Initialize the hardware UAVOs */ HwDiscoveryF4Initialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ PIOS_RTC_Init(&pios_rtc_main_cfg); #endif #ifndef ERASE_FLASH /* Initialize watchdog as early as possible to catch faults during init */ #ifndef DEBUG //PIOS_WDG_Init(); #endif #endif /* Check for repeated boot failures */ 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 */ HwDiscoveryF4SetDefaults(HwDiscoveryF4Handle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } #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 */ HwDiscoveryF4USB_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 = HWDISCOVERYF4_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 HWDISCOVERYF4_USB_VCPPORT_DISABLED: break; case HWDISCOVERYF4_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 HWDISCOVERYF4_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 HWDISCOVERYF4_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; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwDiscoveryF4USB_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 = HWDISCOVERYF4_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 HWDISCOVERYF4_USB_HIDPORT_DISABLED: break; case HWDISCOVERYF4_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 the main IO port */ uint8_t hw_mainport; HwDiscoveryF4MainPortGet(&hw_mainport); switch (hw_mainport) { case HWDISCOVERYF4_MAINPORT_DISABLED: break; case HWDISCOVERYF4_MAINPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_cfg, PIOS_COM_TELEM_RF_RX_BUF_LEN, PIOS_COM_TELEM_RF_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_telem_rf_id); #endif /* PIOS_INCLUDE_TELEMETRY_RF */ case HWDISCOVERYF4_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) && defined(PIOS_INCLUDE_USART) && defined(PIOS_INCLUDE_COM) PIOS_Board_configure_com(&pios_usart3_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; } #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 */ #if defined(PIOS_INCLUDE_GPIO) PIOS_GPIO_Init(); #endif /* Make sure we have at least one telemetry link configured or else fail initialization */ PIOS_Assert(pios_com_telem_usb_id); }
/** * Module thread, should not return. */ static void pathfollowerTask(void *parameters) { SystemSettingsData systemSettings; FlightStatusData flightStatus; uint32_t lastUpdateTime; AirspeedActualConnectCallback(airspeedActualUpdatedCb); FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb); FixedWingAirspeedsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); // Force update of all the settings SettingsUpdatedCb(NULL); FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings); path_desired_updated = false; PathDesiredGet(&pathDesired); PathDesiredConnectCallback(pathDesiredUpdated); // Main task loop lastUpdateTime = PIOS_Thread_Systime(); while (1) { // Conditions when this runs: // 1. Must have FixedWing type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path SystemSettingsGet(&systemSettings); if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) ) { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL); PIOS_Thread_Sleep(1000); continue; } // Continue collecting data if not enough time PIOS_Thread_Sleep_Until(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod); static uint8_t last_flight_mode; FlightStatusGet(&flightStatus); PathStatusGet(&pathStatus); PositionActualData positionActual; static enum {FW_FOLLOWER_IDLE, FW_FOLLOWER_RUNNING, FW_FOLLOWER_ERR} state = FW_FOLLOWER_IDLE; // Check whether an update to the path desired occured and we should // process it. This makes sure that the follower alarm state is // updated. bool process_path_desired_update = (last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER || last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && path_desired_updated; path_desired_updated = false; // Process most of these when the flight mode changes // except when in path following mode in which case // each iteration must make sure this has the latest // PathDesired if (flightStatus.FlightMode != last_flight_mode || process_path_desired_update) { last_flight_mode = flightStatus.FlightMode; switch(flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: state = FW_FOLLOWER_RUNNING; PositionActualGet(&positionActual); pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT; pathDesired.Start[0] = positionActual.North; pathDesired.Start[1] = positionActual.East; pathDesired.Start[2] = positionActual.Down; pathDesired.End[0] = 0; pathDesired.End[1] = 0; pathDesired.End[2] = -30.0f; pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius; pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed; pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed; PathDesiredSet(&pathDesired); break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: state = FW_FOLLOWER_RUNNING; PositionActualGet(&positionActual); pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT; pathDesired.Start[0] = positionActual.North; pathDesired.Start[1] = positionActual.East; pathDesired.Start[2] = positionActual.Down; pathDesired.End[0] = positionActual.North; pathDesired.End[1] = positionActual.East; pathDesired.End[2] = positionActual.Down; pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius; pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed; pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed; PathDesiredSet(&pathDesired); break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL: state = FW_FOLLOWER_RUNNING; PathDesiredGet(&pathDesired); switch(pathDesired.Mode) { case PATHDESIRED_MODE_ENDPOINT: case PATHDESIRED_MODE_VECTOR: case PATHDESIRED_MODE_CIRCLERIGHT: case PATHDESIRED_MODE_CIRCLELEFT: break; default: state = FW_FOLLOWER_ERR; pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; PathStatusSet(&pathStatus); AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL); break; } break; default: state = FW_FOLLOWER_IDLE; break; } } switch(state) { case FW_FOLLOWER_RUNNING: { updatePathVelocity(); uint8_t result = updateFixedDesiredAttitude(); if (result) { AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING); } PathStatusSet(&pathStatus); break; } case FW_FOLLOWER_IDLE: // Be cleaner and get rid of global variables northVelIntegral = 0; eastVelIntegral = 0; downVelIntegral = 0; bearingIntegral = 0; speedIntegral = 0; accelIntegral = 0; powerIntegral = 0; airspeedErrorInt = 0; AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); break; case FW_FOLLOWER_ERR: default: // Leave alarms set above break; } } }
/** * Module task */ static void manualControlTask(void *parameters) { ManualControlSettingsData settings; ManualControlCommandData cmd; portTickType lastSysTime; float flightMode = 0; uint8_t disconnected_count = 0; uint8_t connected_count = 0; enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED; // Make sure unarmed on power up ManualControlCommandGet(&cmd); cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; ManualControlCommandSet(&cmd); armState = ARM_STATE_DISARMED; // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; // Wait until next update vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); // Read settings ManualControlSettingsGet(&settings); if (ManualControlCommandReadOnly(&cmd)) { FlightTelemetryStatsData flightTelemStats; FlightTelemetryStatsGet(&flightTelemStats); if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { /* trying to fly via GCS and lost connection. fall back to transmitter */ UAVObjMetadata metadata; UAVObjGetMetadata(&cmd, &metadata); metadata.access = ACCESS_READWRITE; UAVObjSetMetadata(&cmd, &metadata); } } if (!ManualControlCommandReadOnly(&cmd)) { // Check settings, if error raise alarm if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); continue; } // Read channel values in us // TODO: settings.InputMode is currently ignored because PIOS will not allow runtime // selection of PWM and PPM. The configuration is currently done at compile time in // the pios_config.h file. for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { #if defined(PIOS_INCLUDE_PWM) cmd.Channel[n] = PIOS_PWM_Get(n); #elif defined(PIOS_INCLUDE_PPM) cmd.Channel[n] = PIOS_PPM_Get(n); #elif defined(PIOS_INCLUDE_SPEKTRUM) cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); #endif scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n], 0); } // Scale channels to -1 -> +1 range cmd.Roll = scaledChannel[settings.Roll]; cmd.Pitch = scaledChannel[settings.Pitch]; cmd.Yaw = scaledChannel[settings.Yaw]; cmd.Throttle = scaledChannel[settings.Throttle]; flightMode = scaledChannel[settings.FlightMode]; if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) cmd.Accessory1 = scaledChannel[settings.Accessory1]; else cmd.Accessory1 = 0; if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) cmd.Accessory2 = scaledChannel[settings.Accessory2]; else cmd.Accessory2 = 0; if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) cmd.Accessory3 = scaledChannel[settings.Accessory3]; else cmd.Accessory3 = 0; // Note here the code is ass if (flightMode < -FLIGHT_MODE_LIMIT) cmd.FlightMode = settings.FlightModePosition[0]; else if (flightMode > FLIGHT_MODE_LIMIT) cmd.FlightMode = settings.FlightModePosition[2]; else cmd.FlightMode = settings.FlightModePosition[1]; // Update the ManualControlCommand object ManualControlCommandSet(&cmd); // This seems silly to set then get, but the reason is if the GCS is // the control input, the set command will be blocked by the read only // setting and the get command will pull the right values from telemetry } else ManualControlCommandGet(&cmd); /* Under GCS control */ // Implement hysteresis loop on connection status // Must check both Max and Min in case they reversed if (!ManualControlCommandReadOnly(&cmd) && cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET && cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) { if (disconnected_count++ > 10) { connection_state = DISCONNECTED; connected_count = 0; disconnected_count = 0; } else disconnected_count++; } else { if (connected_count++ > 10) { connection_state = CONNECTED; connected_count = 0; disconnected_count = 0; } else connected_count++; } if (connection_state == DISCONNECTED) { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; cmd.Throttle = -1; // Shut down engine with no control cmd.Roll = 0; cmd.Yaw = 0; cmd.Pitch = 0; //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); ManualControlCommandSet(&cmd); } else { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); ManualControlCommandSet(&cmd); } // // Arming and Disarming mechanism // // Look for state changes and write in newArmState uint8_t newCmdArmed = cmd.Armed; // By default, keep the arming state the same if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { // In this configuration we always disarm newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE; } else { // In all other cases, we will not change the arm state when disconnected if (connection_state == CONNECTED) { if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { // In this configuration, we go into armed state as soon as the throttle is low, never disarm if (cmd.Throttle < 0) { newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; } } else { // When the configuration is not "Always armed" and no "Always disarmed", // the state will not be changed when the throttle is not low if (cmd.Throttle < 0) { static portTickType armedDisarmStart; float armingInputLevel = 0; // Calc channel see assumptions7 switch ( (settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { case ARMING_CHANNEL_ROLL: armingInputLevel = cmd.Roll; break; case ARMING_CHANNEL_PITCH: armingInputLevel = cmd.Pitch; break; case ARMING_CHANNEL_YAW: armingInputLevel = cmd.Yaw; break; } bool manualArm = false; bool manualDisarm = false; if (connection_state == CONNECTED) { // Should use RC input only if RX is connected if (armingInputLevel <= -0.90) manualArm = true; else if (armingInputLevel >= +0.90) manualDisarm = true; } // Swap arm-disarming see assumptions8 if ((settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) { bool temp = manualArm; manualArm = manualDisarm; manualDisarm = temp; } switch(armState) { case ARM_STATE_DISARMED: newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE; if (manualArm) { if (okToArm()) // only allow arming if it's OK too { armedDisarmStart = lastSysTime; armState = ARM_STATE_ARMING_MANUAL; } } break; case ARM_STATE_ARMING_MANUAL: if (manualArm) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_ARMED; } else armState = ARM_STATE_DISARMED; break; case ARM_STATE_ARMED: // When we get here, the throttle is low, // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_TIMEOUT; newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; break; case ARM_STATE_DISARMING_TIMEOUT: // We get here when armed while throttle low, even when the arming timeout is not enabled if (settings.ArmedTimeout != 0) if (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings.ArmedTimeout) armState = ARM_STATE_DISARMED; // Switch to disarming due to manual control when needed if (manualDisarm) { armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_MANUAL; } break; case ARM_STATE_DISARMING_MANUAL: if (manualDisarm) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_DISARMED; } else armState = ARM_STATE_ARMED; break; } // End Switch } else { // The throttle is not low, in case we where arming or disarming, abort switch(armState) { case ARM_STATE_DISARMING_MANUAL: case ARM_STATE_DISARMING_TIMEOUT: armState = ARM_STATE_ARMED; break; case ARM_STATE_ARMING_MANUAL: armState = ARM_STATE_DISARMED; break; default: // Nothing needs to be done in the other states break; } } } } } // Update cmd object when needed if (newCmdArmed != cmd.Armed) { cmd.Armed = newCmdArmed; ManualControlCommandSet(&cmd); } // // End of arming/disarming // // Depending on the mode update the Stabilization or Actuator objects switch(PARSE_FLIGHT_MODE(cmd.FlightMode)) { case FLIGHTMODE_UNDEFINED: // This reflects a bug in the code architecture! AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); break; case FLIGHTMODE_MANUAL: updateActuatorDesired(&cmd); break; case FLIGHTMODE_STABILIZED: updateStabilizationDesired(&cmd, &settings); break; case FLIGHTMODE_GUIDANCE: // TODO: Implement break; } } }
/** * Module thread, should not return. */ static void vtolPathFollowerTask(void *parameters) { SystemSettingsData systemSettings; FlightStatusData flightStatus; portTickType lastUpdateTime; VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); VtolPathFollowerSettingsGet(&guidanceSettings); PathDesiredGet(&pathDesired); // Main task loop lastUpdateTime = xTaskGetTickCount(); while (1) { // Conditions when this runs: // 1. Must have VTOL type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path SystemSettingsGet(&systemSettings); if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) ) { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING); vTaskDelay(1000); continue; } // Continue collecting data if not enough time vTaskDelayUntil(&lastUpdateTime, MS2TICKS(guidanceSettings.UpdatePeriod)); // Convert the accels into the NED frame updateNedAccel(); FlightStatusGet(&flightStatus); // Check the combinations of flightmode and pathdesired mode switch(flightStatus.FlightMode) { /* This combination of RETURNTOHOME and HOLDPOSITION looks strange but * is correct. RETURNTOHOME mode uses HOLDPOSITION with the position * set to home */ case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateVtolDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateVtolDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT || pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateVtolDesiredAttitude(); } else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) { updatePathVelocity(); updateVtolDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; default: for (uint32_t i = 0; i < VTOL_PID_NUM; i++) pid_zero(&vtol_pids[i]); // Track throttle before engaging this mode. Cheap system ident StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); throttleOffset = stabDesired.Throttle; break; } AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); } }
static void onTimer(UAVObjEvent* ev) { static FlightBatteryStateData flightBatteryData; FlightBatterySettingsData batterySettings; FlightBatterySettingsGet(&batterySettings); static float dT = SAMPLE_PERIOD_MS / 1000.0f; float energyRemaining; //calculate the battery parameters if (voltageADCPin >=0) { flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(voltageADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts } else { flightBatteryData.Voltage=1234; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC } if (currentADCPin >=0) { flightBatteryData.Current = ((float)PIOS_ADC_PinGet(currentADCPin)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; //in Amps if (flightBatteryData.Current > flightBatteryData.PeakCurrent) flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps } else { //If there's no current measurement, we still need to assign one. Make it negative, so it can never trigger an alarm flightBatteryData.Current=-0.1234f; //Dummy placeholder value. This is in case we get another source of battery current which is not from the ADC } flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * dT * 1000.0f / 3600.0f) ;//in mAh //Apply a 2 second rise time low-pass filter to average the current float alpha = 1.0f-dT/(dT+2.0f); flightBatteryData.AvgCurrent=alpha*flightBatteryData.AvgCurrent+(1-alpha)*flightBatteryData.Current; //in Amps /*The motor could regenerate power. Or we could have solar cells. In short, is there any likelihood of measuring negative current? If it's a bad current reading we want to check, then it makes sense to saturate at max and min values, because a misreading could as easily be very large, as negative. The simple sign check doesn't catch this.*/ // //sanity checks // if (flightBatteryData.AvgCurrent<0) flightBatteryData.AvgCurrent=0.0f; // if (flightBatteryData.PeakCurrent<0) flightBatteryData.PeakCurrent=0.0f; // if (flightBatteryData.ConsumedEnergy<0) flightBatteryData.ConsumedEnergy=0.0f; energyRemaining = batterySettings.Capacity - flightBatteryData.ConsumedEnergy; // in mAh if (flightBatteryData.AvgCurrent > 0) flightBatteryData.EstimatedFlightTime = (energyRemaining / (flightBatteryData.AvgCurrent*1000.0f))*3600.0f;//in Sec else flightBatteryData.EstimatedFlightTime = 9999; //generate alarms where needed... if ((flightBatteryData.Voltage<=0) && (flightBatteryData.Current<=0)) { //FIXME: There's no guarantee that a floating ADC will give 0. So this // check might fail, even when there's nothing attached. AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_ERROR); } else { // FIXME: should make the timer alarms user configurable if (flightBatteryData.EstimatedFlightTime < 30) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_CRITICAL); else if (flightBatteryData.EstimatedFlightTime < 120) AlarmsSet(SYSTEMALARMS_ALARM_FLIGHTTIME, SYSTEMALARMS_ALARM_WARNING); else AlarmsClear(SYSTEMALARMS_ALARM_FLIGHTTIME); // FIXME: should make the battery voltage detection dependent on battery type. /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/ if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_ALARM]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); else if (flightBatteryData.Voltage < batterySettings.VoltageThresholds[FLIGHTBATTERYSETTINGS_VOLTAGETHRESHOLDS_WARNING]) AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); else AlarmsClear(SYSTEMALARMS_ALARM_BATTERY); } FlightBatteryStateSet(&flightBatteryData); }
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) { /* 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 */ #if defined(PIOS_INCLUDE_SPI) /* Set up the SPI interface to the serial flash */ switch(bdinfo->board_rev) { case BOARD_REVISION_CC: if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc)) { PIOS_Assert(0); } break; case BOARD_REVISION_CC3D: if (PIOS_SPI_Init(&pios_spi_flash_accel_id, &pios_spi_flash_accel_cfg_cc3d)) { PIOS_Assert(0); } break; default: PIOS_Assert(0); } #endif switch(bdinfo->board_rev) { case BOARD_REVISION_CC: PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_flash_accel_id, 1, &flash_w25x_cfg); break; case BOARD_REVISION_CC3D: PIOS_Flash_Jedec_Init(&pios_external_flash_id, pios_spi_flash_accel_id, 0, &flash_m25p_cfg); break; default: PIOS_DEBUG_Assert(0); } 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); /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); /* Initialize the alarms library */ AlarmsInitialize(); HwCopterControlInitialize(); ModuleSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ PIOS_RTC_Init(&pios_rtc_main_cfg); #endif #ifndef ERASE_FLASH /* Initialize watchdog as early as possible to catch faults during init */ PIOS_WDG_Init(); #endif /* Check for repeated boot failures */ 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 configuration to defaults */ HwCopterControlSetDefaults(HwCopterControlHandle(), 0); ModuleSettingsSetDefaults(ModuleSettingsHandle(),0); AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL); } /* Initialize the task monitor library */ TaskMonitorInitialize(); /* Set up pulse timers */ PIOS_TIM_InitClock(&tim_1_cfg); PIOS_TIM_InitClock(&tim_2_cfg); PIOS_TIM_InitClock(&tim_3_cfg); PIOS_TIM_InitClock(&tim_4_cfg); #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; switch(bdinfo->board_rev) { case BOARD_REVISION_CC: PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc); break; case BOARD_REVISION_CC3D: PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg_cc3d); break; default: PIOS_Assert(0); } #if defined(PIOS_INCLUDE_USB_CDC) uint8_t hw_usb_vcpport; /* Configure the USB VCP port */ HwCopterControlUSB_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 = HWCOPTERCONTROL_USB_VCPPORT_DISABLED; } switch (hw_usb_vcpport) { case HWCOPTERCONTROL_USB_VCPPORT_DISABLED: break; case HWCOPTERCONTROL_USB_VCPPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { 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); } 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 HWCOPTERCONTROL_USB_VCPPORT_COMBRIDGE: #if defined(PIOS_INCLUDE_COM) { 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); } 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 HWCOPTERCONTROL_USB_VCPPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { 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); } 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; } #endif /* PIOS_INCLUDE_USB_CDC */ #if defined(PIOS_INCLUDE_USB_HID) /* Configure the usb HID port */ uint8_t hw_usb_hidport; HwCopterControlUSB_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 = HWCOPTERCONTROL_USB_HIDPORT_DISABLED; } switch (hw_usb_hidport) { case HWCOPTERCONTROL_USB_HIDPORT_DISABLED: break; case HWCOPTERCONTROL_USB_HIDPORT_USBTELEMETRY: #if defined(PIOS_INCLUDE_COM) { 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); } 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; case HWCOPTERCONTROL_USB_HIDPORT_RCTRANSMITTER: #if defined(PIOS_INCLUDE_USB_RCTX) { if (PIOS_USB_RCTX_Init(&pios_usb_rctx_id, &pios_usb_rctx_cfg, pios_usb_id)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_USB_RCTX */ break; } #endif /* PIOS_INCLUDE_USB_HID */ #endif /* PIOS_INCLUDE_USB */ /* Configure the main IO port */ HwCopterControlDSMxModeOptions hw_DSMxMode; HwCopterControlDSMxModeGet(&hw_DSMxMode); uint8_t hw_mainport; HwCopterControlMainPortGet(&hw_mainport); switch (hw_mainport) { case HWCOPTERCONTROL_MAINPORT_DISABLED: break; case HWCOPTERCONTROL_MAINPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWCOPTERCONTROL_MAINPORT_SBUS: #if defined(PIOS_INCLUDE_SBUS) { uintptr_t pios_usart_sbus_id; if (PIOS_USART_Init(&pios_usart_sbus_id, &pios_usart_sbus_main_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 HWCOPTERCONTROL_MAINPORT_GPS: #if defined(PIOS_INCLUDE_GPS) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_GPS_RX_BUF_LEN); PIOS_Assert(rx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, NULL, 0)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_GPS */ break; case HWCOPTERCONTROL_MAINPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { uintptr_t pios_usart_dsm_id; if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_hsum_main_cfg)) { PIOS_Assert(0); } if (hw_DSMxMode >= HWCOPTERCONTROL_DSMXMODE_BIND3PULSES) { hw_DSMxMode = HWCOPTERCONTROL_DSMXMODE_AUTODETECT; /* Do not try to bind through XOR */ } uintptr_t pios_dsm_id; if (PIOS_DSM_Init(&pios_dsm_id, &pios_dsm_main_cfg, &pios_usart_com_driver, pios_usart_dsm_id, hw_DSMxMode)) { PIOS_Assert(0); } uintptr_t pios_dsm_rcvr_id; if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSM] = pios_dsm_rcvr_id; } #endif /* PIOS_INCLUDE_DSM */ break; case HWCOPTERCONTROL_MAINPORT_HOTTSUMD: case HWCOPTERCONTROL_MAINPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; proto = (hw_mainport == HWCOPTERCONTROL_MAINPORT_HOTTSUMD) ? PIOS_HSUM_PROTO_SUMD : PIOS_HSUM_PROTO_SUMH; uintptr_t pios_usart_hsum_id; if (PIOS_USART_Init(&pios_usart_hsum_id, &pios_usart_dsm_hsum_main_cfg)) { PIOS_Assert(0); } uintptr_t pios_hsum_id; if (PIOS_HSUM_Init(&pios_hsum_id, &pios_usart_com_driver, pios_usart_hsum_id, proto)) { PIOS_Assert(0); } uintptr_t pios_hsum_rcvr_id; if (PIOS_RCVR_Init(&pios_hsum_rcvr_id, &pios_hsum_rcvr_driver, pios_hsum_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM] = pios_hsum_rcvr_id; } #endif /* PIOS_INCLUDE_HSUM */ break; case HWCOPTERCONTROL_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } 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_usart_com_driver, pios_usart_generic_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 HWCOPTERCONTROL_MAINPORT_COMBRIDGE: { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_RX_BUF_LEN); PIOS_Assert(rx_buffer); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_BRIDGE_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } break; case HWCOPTERCONTROL_MAINPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_MAVLINK) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_MAVLINK_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_MAVLINK_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWCOPTERCONTROL_MAINPORT_MAVLINKTX_GPS_RX: #if defined(PIOS_INCLUDE_GPS) #if defined(PIOS_INCLUDE_MAVLINK) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_GPS_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_MAVLINK_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, tx_buffer, PIOS_COM_MAVLINK_TX_BUF_LEN)) { PIOS_Assert(0); } pios_com_mavlink_id = pios_com_gps_id; } #endif /* PIOS_INCLUDE_MAVLINK */ #endif /* PIOS_INCLUDE_GPS */ break; case HWCOPTERCONTROL_MAINPORT_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_frsky_sensor_hub_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_FRSKYSENSORHUB */ break; case HWCOPTERCONTROL_MAINPORT_LIGHTTELEMETRYTX: { #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_main_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_lighttelemetry_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN)) { PIOS_Assert(0); } #endif } break; } /* Configure the flexi port */ uint8_t hw_flexiport; HwCopterControlFlexiPortGet(&hw_flexiport); switch (hw_flexiport) { case HWCOPTERCONTROL_FLEXIPORT_DISABLED: break; case HWCOPTERCONTROL_FLEXIPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ break; case HWCOPTERCONTROL_FLEXIPORT_COMBRIDGE: { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } 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_bridge_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_BRIDGE_RX_BUF_LEN, tx_buffer, PIOS_COM_BRIDGE_TX_BUF_LEN)) { PIOS_Assert(0); } } break; case HWCOPTERCONTROL_FLEXIPORT_GPS: #if defined(PIOS_INCLUDE_GPS) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_GPS_RX_BUF_LEN); PIOS_Assert(rx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_generic_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, NULL, 0)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_GPS */ break; case HWCOPTERCONTROL_FLEXIPORT_DSM: #if defined(PIOS_INCLUDE_DSM) { uintptr_t pios_usart_dsm_id; if (PIOS_USART_Init(&pios_usart_dsm_id, &pios_usart_dsm_hsum_flexi_cfg)) { PIOS_Assert(0); } uintptr_t pios_dsm_id; if (PIOS_DSM_Init(&pios_dsm_id, &pios_dsm_flexi_cfg, &pios_usart_com_driver, pios_usart_dsm_id, hw_DSMxMode)) { PIOS_Assert(0); } uintptr_t pios_dsm_rcvr_id; if (PIOS_RCVR_Init(&pios_dsm_rcvr_id, &pios_dsm_rcvr_driver, pios_dsm_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_DSM] = pios_dsm_rcvr_id; } #endif /* PIOS_INCLUDE_DSM */ break; case HWCOPTERCONTROL_FLEXIPORT_HOTTSUMD: case HWCOPTERCONTROL_FLEXIPORT_HOTTSUMH: #if defined(PIOS_INCLUDE_HSUM) { enum pios_hsum_proto proto; proto = (hw_flexiport == HWCOPTERCONTROL_FLEXIPORT_HOTTSUMD) ? PIOS_HSUM_PROTO_SUMD : PIOS_HSUM_PROTO_SUMH; uintptr_t pios_usart_hsum_id; if (PIOS_USART_Init(&pios_usart_hsum_id, &pios_usart_dsm_hsum_flexi_cfg)) { PIOS_Assert(0); } uintptr_t pios_hsum_id; if (PIOS_HSUM_Init(&pios_hsum_id, &pios_usart_com_driver, pios_usart_hsum_id, proto)) { PIOS_Assert(0); } uintptr_t pios_hsum_rcvr_id; if (PIOS_RCVR_Init(&pios_hsum_rcvr_id, &pios_hsum_rcvr_driver, pios_hsum_id)) { PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTSUM] = pios_hsum_rcvr_id; } #endif /* PIOS_INCLUDE_HSUM */ break; case HWCOPTERCONTROL_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } 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_usart_com_driver, pios_usart_generic_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 HWCOPTERCONTROL_FLEXIPORT_I2C: #if defined(PIOS_INCLUDE_I2C) { if (PIOS_I2C_Init(&pios_i2c_flexi_adapter_id, &pios_i2c_flexi_adapter_cfg)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_I2C */ #if defined(PIOS_INCLUDE_PCF8591) { uintptr_t pcf8591_adc_id; if(PIOS_PCF8591_ADC_Init(&pcf8591_adc_id, &pios_8591_cfg) < 0) PIOS_Assert(0); PIOS_ADC_Init(&pios_pcf8591_adc_id, &pios_pcf8591_adc_driver, pcf8591_adc_id); } #endif break; case HWCOPTERCONTROL_FLEXIPORT_MAVLINKTX: #if defined(PIOS_INCLUDE_MAVLINK) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_MAVLINK_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_mavlink_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_MAVLINK_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_MAVLINK */ break; case HWCOPTERCONTROL_FLEXIPORT_LIGHTTELEMETRYTX: { #if defined(PIOS_INCLUDE_LIGHTTELEMETRY) uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_lighttelemetry_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_LIGHTTELEMETRY_TX_BUF_LEN)) { PIOS_Assert(0); } #endif case HWCOPTERCONTROL_FLEXIPORT_FRSKYSENSORHUB: #if defined(PIOS_INCLUDE_FRSKY_SENSOR_HUB) { uintptr_t pios_usart_generic_id; if (PIOS_USART_Init(&pios_usart_generic_id, &pios_usart_generic_flexi_cfg)) { PIOS_Assert(0); } uint8_t * tx_buffer = (uint8_t *) PIOS_malloc(PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_frsky_sensor_hub_id, &pios_usart_com_driver, pios_usart_generic_id, NULL, 0, tx_buffer, PIOS_COM_FRSKYSENSORHUB_TX_BUF_LEN)) { PIOS_Assert(0); } } #endif /* PIOS_INCLUDE_FRSKYSENSORHUB */ break; } break; } /* Configure the rcvr port */ uint8_t hw_rcvrport; HwCopterControlRcvrPortGet(&hw_rcvrport); switch (hw_rcvrport) { case HWCOPTERCONTROL_RCVRPORT_DISABLED: break; case HWCOPTERCONTROL_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) { 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 HWCOPTERCONTROL_RCVRPORT_PPM: case HWCOPTERCONTROL_RCVRPORT_PPMONPIN8: case HWCOPTERCONTROL_RCVRPORT_PPMOUTPUTS: #if defined(PIOS_INCLUDE_PPM) { uintptr_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, (hw_rcvrport == HWCOPTERCONTROL_RCVRPORT_PPMONPIN8) ? &pios_ppm_pin8_cfg : &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 */ break; case HWCOPTERCONTROL_RCVRPORT_PPMPWM: /* This is a combination of PPM and PWM inputs */ #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 */ #if defined(PIOS_INCLUDE_PWM) { uintptr_t pios_pwm_id; PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_with_ppm_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; } #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 */ /* Remap AFIO pin for PB4 (Servo 5 Out)*/ GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); #ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS switch (hw_rcvrport) { case HWCOPTERCONTROL_RCVRPORT_DISABLED: case HWCOPTERCONTROL_RCVRPORT_PWM: case HWCOPTERCONTROL_RCVRPORT_PPM: case HWCOPTERCONTROL_RCVRPORT_PPMONPIN8: case HWCOPTERCONTROL_RCVRPORT_PPMPWM: PIOS_Servo_Init(&pios_servo_cfg); break; case HWCOPTERCONTROL_RCVRPORT_PPMOUTPUTS: case HWCOPTERCONTROL_RCVRPORT_OUTPUTS: PIOS_Servo_Init(&pios_servo_rcvr_cfg); break; } #else PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); #endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */ PIOS_SENSORS_Init(); switch(bdinfo->board_rev) { case BOARD_REVISION_CC: // Revision 1 with invensense gyros, start the ADC #if defined(PIOS_INCLUDE_ADC) { uint32_t internal_adc_id; PIOS_INTERNAL_ADC_Init(&internal_adc_id, &internal_adc_cfg); PIOS_ADC_Init(&pios_internal_adc_id, &pios_internal_adc_driver, internal_adc_id); } #endif #if defined(PIOS_INCLUDE_ADXL345) PIOS_ADXL345_Init(pios_spi_flash_accel_id, 0); #endif break; case BOARD_REVISION_CC3D: // Revision 2 with L3GD20 gyros, start a SPI interface and connect to it GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); #if defined(PIOS_INCLUDE_MPU6000) // Set up the SPI interface to the serial flash if (PIOS_SPI_Init(&pios_spi_gyro_id, &pios_spi_gyro_cfg)) { PIOS_Assert(0); } PIOS_MPU6000_Init(pios_spi_gyro_id,0,&pios_mpu6000_cfg); init_test = PIOS_MPU6000_Test(); uint8_t hw_gyro_range; HwCopterControlGyroRangeGet(&hw_gyro_range); switch(hw_gyro_range) { case HWCOPTERCONTROL_GYRORANGE_250: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_250_DEG); break; case HWCOPTERCONTROL_GYRORANGE_500: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_500_DEG); break; case HWCOPTERCONTROL_GYRORANGE_1000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_1000_DEG); break; case HWCOPTERCONTROL_GYRORANGE_2000: PIOS_MPU6000_SetGyroRange(PIOS_MPU60X0_SCALE_2000_DEG); break; } uint8_t hw_accel_range; HwCopterControlAccelRangeGet(&hw_accel_range); switch(hw_accel_range) { case HWCOPTERCONTROL_ACCELRANGE_2G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_2G); break; case HWCOPTERCONTROL_ACCELRANGE_4G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_4G); break; case HWCOPTERCONTROL_ACCELRANGE_8G: PIOS_MPU6000_SetAccelRange(PIOS_MPU60X0_ACCEL_8G); break; case HWCOPTERCONTROL_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; HwCopterControlMPU6000DLPFGet(&hw_mpu6000_dlpf); enum pios_mpu60x0_filter mpu6000_dlpf = \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_256) ? PIOS_MPU60X0_LOWPASS_256_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_188) ? PIOS_MPU60X0_LOWPASS_188_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_98) ? PIOS_MPU60X0_LOWPASS_98_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_42) ? PIOS_MPU60X0_LOWPASS_42_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_20) ? PIOS_MPU60X0_LOWPASS_20_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_10) ? PIOS_MPU60X0_LOWPASS_10_HZ : \ (hw_mpu6000_dlpf == HWCOPTERCONTROL_MPU6000DLPF_5) ? PIOS_MPU60X0_LOWPASS_5_HZ : \ pios_mpu6000_cfg.default_filter; PIOS_MPU6000_SetLPF(mpu6000_dlpf); uint8_t hw_mpu6000_samplerate; HwCopterControlMPU6000RateGet(&hw_mpu6000_samplerate); uint16_t mpu6000_samplerate = \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_200) ? 200 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_333) ? 333 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_500) ? 500 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_666) ? 666 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_1000) ? 1000 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_2000) ? 2000 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_4000) ? 4000 : \ (hw_mpu6000_samplerate == HWCOPTERCONTROL_MPU6000RATE_8000) ? 8000 : \ pios_mpu6000_cfg.default_samplerate; PIOS_MPU6000_SetSampleRate(mpu6000_samplerate); #endif /* PIOS_INCLUDE_MPU6000 */ break; default: PIOS_Assert(0); } PIOS_GPIO_Init(); /* Make sure we have at least one telemetry link configured or else fail initialization */ PIOS_Assert(pios_com_telem_rf_id || pios_com_telem_usb_id); }
/** * Run a preflight check over the hardware configuration * and currently active modules */ int32_t configuration_check() { int32_t severity = SYSTEMALARMS_ALARM_OK; SystemAlarmsExtendedAlarmStatusOptions alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE; uint8_t alarmsubstatus = 0; // Get board type const struct pios_board_info *bdinfo = &pios_board_info_blob; bool coptercontrol = bdinfo->board_type == 0x04; // Classify airframe type bool multirotor = true; uint8_t airframe_type; SystemSettingsAirframeTypeGet(&airframe_type); switch (airframe_type) { case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX: case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP: case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA: case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO: case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX: case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV: case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP: case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX: case SYSTEMSETTINGS_AIRFRAMETYPE_TRI: case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX: multirotor = true; break; default: multirotor = false; } // For each available flight mode position sanity check the available // modes uint8_t num_modes; uint8_t modes[MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM]; ManualControlSettingsFlightModeNumberGet(&num_modes); ManualControlSettingsFlightModePositionGet(modes); for (uint32_t i = 0; i < num_modes; i++) { switch (modes[i]) { case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL: if (multirotor) { severity = SYSTEMALARMS_ALARM_ERROR; } break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1: severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity; break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2: severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity; break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3: severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity; break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) { severity = SYSTEMALARMS_ALARM_ERROR; } break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD: case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } // TODO: put check equivalent to TASK_MONITOR_IsRunning // here as soon as available for delayed callbacks break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold severity = SYSTEMALARMS_ALARM_ERROR; } break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports Position Hold severity = SYSTEMALARMS_ALARM_ERROR; } break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports AutoLand Mode severity = SYSTEMALARMS_ALARM_ERROR; } break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports POI Mode severity = SYSTEMALARMS_ALARM_ERROR; } break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } else { // Revo supports PathPlanner and that must be OK or we are not sane // PathPlan alarm is uninitialized if not running // PathPlan alarm is warning or error if the flightplan is invalid SystemAlarmsAlarmData alarms; SystemAlarmsAlarmGet(&alarms); if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) { severity = SYSTEMALARMS_ALARM_ERROR; } } break; case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports ReturnToBase severity = SYSTEMALARMS_ALARM_ERROR; } break; default: // Uncovered modes are automatically an error severity = SYSTEMALARMS_ALARM_ERROR; } // mark the first encountered erroneous setting in status and substatus if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) { alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE; alarmsubstatus = i; } } // TODO: Check on a multirotor no axis supports "None" if (severity != SYSTEMALARMS_ALARM_OK) { ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus); } else { AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION); } return 0; }
static void SensorsTask(void *parameters) { portTickType lastSysTime; uint32_t accel_samples = 0; uint32_t gyro_samples = 0; int32_t accel_accum[3] = {0, 0, 0}; int32_t gyro_accum[3] = {0,0,0}; float gyro_scaling = 0; float accel_scaling = 0; static int32_t timeval; AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); UAVObjEvent ev; settingsUpdatedCb(&ev); const struct pios_board_info * bdinfo = &pios_board_info_blob; switch(bdinfo->board_rev) { case 0x01: #if defined(PIOS_INCLUDE_L3GD20) gyro_test = PIOS_L3GD20_Test(); #endif #if defined(PIOS_INCLUDE_BMA180) accel_test = PIOS_BMA180_Test(); #endif break; case 0x02: #if defined(PIOS_INCLUDE_MPU6000) gyro_test = PIOS_MPU6000_Test(); accel_test = gyro_test; #endif break; default: PIOS_DEBUG_Assert(0); } #if defined(PIOS_INCLUDE_HMC5883) mag_test = PIOS_HMC5883_Test(); #else mag_test = 0; #endif if(accel_test < 0 || gyro_test < 0 || mag_test < 0) { AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); while(1) { PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); vTaskDelay(10); } } // Main task loop lastSysTime = xTaskGetTickCount(); bool error = false; uint32_t mag_update_time = PIOS_DELAY_GetRaw(); while (1) { // TODO: add timeouts to the sensor reads and set an error if the fail sensor_dt_us = PIOS_DELAY_DiffuS(timeval); timeval = PIOS_DELAY_GetRaw(); if (error) { PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); lastSysTime = xTaskGetTickCount(); vTaskDelayUntil(&lastSysTime, SENSOR_PERIOD / portTICK_RATE_MS); AlarmsSet(SYSTEMALARMS_ALARM_SENSORS, SYSTEMALARMS_ALARM_CRITICAL); error = false; } else { AlarmsClear(SYSTEMALARMS_ALARM_SENSORS); } for (int i = 0; i < 3; i++) { accel_accum[i] = 0; gyro_accum[i] = 0; } accel_samples = 0; gyro_samples = 0; AccelsData accelsData; GyrosData gyrosData; switch(bdinfo->board_rev) { case 0x01: // L3GD20 + BMA180 board #if defined(PIOS_INCLUDE_BMA180) { struct pios_bma180_data accel; int32_t read_good; int32_t count; count = 0; while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0 && !error) error = ((xTaskGetTickCount() - lastSysTime) > SENSOR_PERIOD) ? true : error; if (error) { // Unfortunately if the BMA180 ever misses getting read, then it will not // trigger more interrupts. In this case we must force a read to kickstarts // it. struct pios_bma180_data data; PIOS_BMA180_ReadAccels(&data); continue; } while(read_good == 0) { count++; accel_accum[1] += accel.x; accel_accum[0] += accel.y; accel_accum[2] -= accel.z; read_good = PIOS_BMA180_ReadFifo(&accel); } accel_samples = count; accel_scaling = PIOS_BMA180_GetScale(); // Get temp from last reading accelsData.temperature = 25.0f + ((float) accel.temperature - 2.0f) / 2.0f; } #endif #if defined(PIOS_INCLUDE_L3GD20) { struct pios_l3gd20_data gyro; gyro_samples = 0; xQueueHandle gyro_queue = PIOS_L3GD20_GetQueue(); if(xQueueReceive(gyro_queue, (void *) &gyro, 4) == errQUEUE_EMPTY) { error = true; continue; } gyro_samples = 1; gyro_accum[1] += gyro.gyro_x; gyro_accum[0] += gyro.gyro_y; gyro_accum[2] -= gyro.gyro_z; gyro_scaling = PIOS_L3GD20_GetScale(); // Get temp from last reading gyrosData.temperature = gyro.temperature; } #endif break; case 0x02: // MPU6000 board case 0x03: // MPU6000 board #if defined(PIOS_INCLUDE_MPU6000) { struct pios_mpu6000_data mpu6000_data; xQueueHandle queue = PIOS_MPU6000_GetQueue(); while(xQueueReceive(queue, (void *) &mpu6000_data, gyro_samples == 0 ? 10 : 0) != errQUEUE_EMPTY) { gyro_accum[0] += mpu6000_data.gyro_x; gyro_accum[1] += mpu6000_data.gyro_y; gyro_accum[2] += mpu6000_data.gyro_z; accel_accum[0] += mpu6000_data.accel_x; accel_accum[1] += mpu6000_data.accel_y; accel_accum[2] += mpu6000_data.accel_z; gyro_samples ++; accel_samples ++; } if (gyro_samples == 0) { PIOS_MPU6000_ReadGyros(&mpu6000_data); error = true; continue; } gyro_scaling = PIOS_MPU6000_GetScale(); accel_scaling = PIOS_MPU6000_GetAccelScale(); gyrosData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; accelsData.temperature = 35.0f + ((float) mpu6000_data.temperature + 512.0f) / 340.0f; } #endif /* PIOS_INCLUDE_MPU6000 */ break; default: PIOS_DEBUG_Assert(0); } // Scale the accels float accels[3] = {(float) accel_accum[0] / accel_samples, (float) accel_accum[1] / accel_samples, (float) accel_accum[2] / accel_samples}; float accels_out[3] = {accels[0] * accel_scaling * accel_scale[0] - accel_bias[0], accels[1] * accel_scaling * accel_scale[1] - accel_bias[1], accels[2] * accel_scaling * accel_scale[2] - accel_bias[2]}; if (rotate) { rot_mult(R, accels_out, accels); accelsData.x = accels[0]; accelsData.y = accels[1]; accelsData.z = accels[2]; } else { accelsData.x = accels_out[0]; accelsData.y = accels_out[1]; accelsData.z = accels_out[2]; } AccelsSet(&accelsData); // Scale the gyros float gyros[3] = {(float) gyro_accum[0] / gyro_samples, (float) gyro_accum[1] / gyro_samples, (float) gyro_accum[2] / gyro_samples}; float gyros_out[3] = {gyros[0] * gyro_scaling, gyros[1] * gyro_scaling, gyros[2] * gyro_scaling}; if (rotate) { rot_mult(R, gyros_out, gyros); gyrosData.x = gyros[0]; gyrosData.y = gyros[1]; gyrosData.z = gyros[2]; } else { gyrosData.x = gyros_out[0]; gyrosData.y = gyros_out[1]; gyrosData.z = gyros_out[2]; } if (bias_correct_gyro) { // Apply bias correction to the gyros from the state estimator GyrosBiasData gyrosBias; GyrosBiasGet(&gyrosBias); gyrosData.x -= gyrosBias.x; gyrosData.y -= gyrosBias.y; gyrosData.z -= gyrosBias.z; } GyrosSet(&gyrosData); // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) #if defined(PIOS_INCLUDE_HMC5883) MagnetometerData mag; if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { int16_t values[3]; PIOS_HMC5883_ReadMag(values); float mags[3] = {-(float) values[1] * mag_scale[0] - mag_bias[0], -(float) values[0] * mag_scale[1] - mag_bias[1], -(float) values[2] * mag_scale[2] - mag_bias[2]}; if (rotate) { float mag_out[3]; rot_mult(R, mags, mag_out); mag.x = mag_out[0]; mag.y = mag_out[1]; mag.z = mag_out[2]; } else { mag.x = mags[0]; mag.y = mags[1]; mag.z = mags[2]; } // Correct for mag bias and update if the rate is non zero if(cal.MagBiasNullingRate > 0) magOffsetEstimation(&mag); MagnetometerSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); } #endif PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); lastSysTime = xTaskGetTickCount(); } }