예제 #1
0
/**
 * 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);
	}
}
예제 #2
0
/**
 * 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); 	

	}
}
예제 #3
0
/**
 * 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;
}
예제 #4
0
파일: alarms.c 프로젝트: 1heinz/TauLabs
/**
 * Clear all alarms
 */
void AlarmsClearAll()
{
	uint32_t n;
    for (n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n)
    {
    	AlarmsClear(n);
    }
}
예제 #5
0
/**
 * 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);
	}
}
예제 #6
0
/**
 * 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);

	}
}
예제 #7
0
파일: attitude.c 프로젝트: mirasanti/vbrain
/**
 * 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);
	}
}
예제 #8
0
/**
 * 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 );

    }
}
예제 #9
0
/**
 * @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 );

	}
}
예제 #10
0
파일: geofence.c 프로젝트: Trex4Git/dRonin
/* 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);
		}
	}
}
예제 #11
0
/**
 * 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);

	}
}
예제 #12
0
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;
	}

}
예제 #13
0
/**
 * 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);

	}
}
예제 #14
0
파일: systemmod.c 프로젝트: CheBuzz/TauLabs
/**
 * 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);
	}
		
}
예제 #15
0
/**
 * 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;
	}
}
예제 #16
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);
		}
	}
}
예제 #17
0
/**
 * 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);
	}
}
예제 #18
0
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);
}
예제 #19
0
파일: battery.c 프로젝트: 1heinz/TauLabs
/**
 * 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);
	}
}
예제 #20
0
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);
	}
}
예제 #21
0
/**
 * 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);
		}
	}
}
예제 #22
0
/**
 * 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);
}
예제 #23
0
/**
 * 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;
		}
	}
}
예제 #24
0
/**
 * 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;
		}				
	}
}
예제 #25
0
/**
 * 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);

	}
}
예제 #26
0
파일: battery.c 프로젝트: mirasanti/vbrain
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);
}
예제 #27
0
파일: pios_board.c 프로젝트: Crash1/TauLabs
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);
}
예제 #28
0
void PIOS_Board_Init(void) {

	/* Delay system */
	PIOS_DELAY_Init();

	const struct pios_board_info * bdinfo = &pios_board_info_blob;

#if defined(PIOS_INCLUDE_LED)
	const struct pios_led_cfg * led_cfg = PIOS_BOARD_HW_DEFS_GetLedCfg(bdinfo->board_rev);
	PIOS_Assert(led_cfg);
	PIOS_LED_Init(led_cfg);
#endif	/* PIOS_INCLUDE_LED */

#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);
}
예제 #29
0
/**
 * 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;
}
예제 #30
0
파일: sensors.c 프로젝트: mirasanti/vbrain
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();
	}
}