Пример #1
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);

	}
}
Пример #2
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);

	}
}
Пример #3
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;
		}
	}
}
/**
 * fixed wing autopilot:
 * straight forward:
 * 1. update path velocity for limited motion crafts
 * 2. update attitude according to default fixed wing pathfollower algorithm
 */
uint8_t FixedWingFlyController::updateAutoPilotFixedWing()
{
    updatePathVelocity(fixedWingSettings->CourseFeedForward, true);
    return updateFixedDesiredAttitude();
}