/**
* Module thread, should not return.
*/
static void pathfollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
uint32_t lastUpdateTime;
AirspeedActualConnectCallback(airspeedActualUpdatedCb);
FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
FixedWingAirspeedsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
// Force update of all the settings
SettingsUpdatedCb(NULL);
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
path_desired_updated = false;
PathDesiredGet(&pathDesired);
PathDesiredConnectCallback(pathDesiredUpdated);
// Main task loop
lastUpdateTime = PIOS_Thread_Systime();
while (1) {
// Conditions when this runs:
// 1. Must have FixedWing type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) )
{
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL);
PIOS_Thread_Sleep(1000);
continue;
}
// Continue collecting data if not enough time
PIOS_Thread_Sleep_Until(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod);
static uint8_t last_flight_mode;
FlightStatusGet(&flightStatus);
PathStatusGet(&pathStatus);
PositionActualData positionActual;
static enum {FW_FOLLOWER_IDLE, FW_FOLLOWER_RUNNING, FW_FOLLOWER_ERR} state = FW_FOLLOWER_IDLE;
// Check whether an update to the path desired occured and we should
// process it. This makes sure that the follower alarm state is
// updated.
bool process_path_desired_update =
(last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ||
last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) &&
path_desired_updated;
path_desired_updated = false;
// Process most of these when the flight mode changes
// except when in path following mode in which case
// each iteration must make sure this has the latest
// PathDesired
if (flightStatus.FlightMode != last_flight_mode ||
process_path_desired_update) {
last_flight_mode = flightStatus.FlightMode;
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
state = FW_FOLLOWER_RUNNING;
PositionActualGet(&positionActual);
pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT;
pathDesired.Start[0] = positionActual.North;
pathDesired.Start[1] = positionActual.East;
pathDesired.Start[2] = positionActual.Down;
pathDesired.End[0] = 0;
pathDesired.End[1] = 0;
pathDesired.End[2] = -30.0f;
pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius;
pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed;
pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed;
PathDesiredSet(&pathDesired);
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
state = FW_FOLLOWER_RUNNING;
PositionActualGet(&positionActual);
pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT;
pathDesired.Start[0] = positionActual.North;
pathDesired.Start[1] = positionActual.East;
pathDesired.Start[2] = positionActual.Down;
pathDesired.End[0] = positionActual.North;
pathDesired.End[1] = positionActual.East;
pathDesired.End[2] = positionActual.Down;
pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius;
pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed;
pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed;
PathDesiredSet(&pathDesired);
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL:
state = FW_FOLLOWER_RUNNING;
PathDesiredGet(&pathDesired);
switch(pathDesired.Mode) {
case PATHDESIRED_MODE_ENDPOINT:
case PATHDESIRED_MODE_VECTOR:
case PATHDESIRED_MODE_CIRCLERIGHT:
case PATHDESIRED_MODE_CIRCLELEFT:
break;
default:
state = FW_FOLLOWER_ERR;
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
PathStatusSet(&pathStatus);
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL);
break;
}
break;
default:
state = FW_FOLLOWER_IDLE;
break;
}
}
switch(state) {
case FW_FOLLOWER_RUNNING:
{
updatePathVelocity();
uint8_t result = updateFixedDesiredAttitude();
if (result) {
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING);
}
PathStatusSet(&pathStatus);
break;
}
case FW_FOLLOWER_IDLE:
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
downVelIntegral = 0;
bearingIntegral = 0;
speedIntegral = 0;
accelIntegral = 0;
powerIntegral = 0;
airspeedErrorInt = 0;
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
break;
case FW_FOLLOWER_ERR:
default:
// Leave alarms set above
break;
}
}
}
/**
* Module 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);
}
}
/**
* 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);
}
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedActual which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
static void updateGroundDesiredAttitude()
{
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
GroundPathFollowerSettingsData guidanceSettings;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
SystemSettingsGet(&systemSettings);
GroundPathFollowerSettingsGet(&guidanceSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = velocityActual.North;
float eastVel = velocityActual.East;
// Calculate direction from velocityDesired and set stabDesired.Yaw
stabDesired.Yaw = atan2f( velocityDesired.East, velocityDesired.North ) * RAD2DEG;
// Calculate throttle and set stabDesired.Throttle
float velDesired = sqrtf(powf(velocityDesired.East,2) + powf(velocityDesired.North,2));
float velActual = sqrtf(powf(eastVel,2) + powf(northVel,2));
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
switch (guidanceSettings.ThrottleControl) {
case GROUNDPATHFOLLOWERSETTINGS_THROTTLECONTROL_MANUAL:
{
stabDesired.Throttle = manualControlData.Throttle;
break;
}
case GROUNDPATHFOLLOWERSETTINGS_THROTTLECONTROL_PROPORTIONAL:
{
float velRatio = velDesired / guidanceSettings.HorizontalVelMax;
stabDesired.Throttle = guidanceSettings.MaxThrottle * velRatio;
if (guidanceSettings.ManualOverride == GROUNDPATHFOLLOWERSETTINGS_MANUALOVERRIDE_TRUE) {
stabDesired.Throttle = stabDesired.Throttle * manualControlData.Throttle;
}
break;
}
case GROUNDPATHFOLLOWERSETTINGS_THROTTLECONTROL_AUTO:
{
float velError = velDesired - velActual;
stabDesired.Throttle = pid_apply(&ground_pids[VELOCITY], velError, dT) + velDesired * guidanceSettings.VelocityFeedforward;
if (guidanceSettings.ManualOverride == GROUNDPATHFOLLOWERSETTINGS_MANUALOVERRIDE_TRUE) {
stabDesired.Throttle = stabDesired.Throttle * manualControlData.Throttle;
}
break;
}
default:
{
PIOS_Assert(0);
break;
}
}
// Limit throttle as per settings
stabDesired.Throttle = bound_min_max(stabDesired.Throttle, 0, guidanceSettings.MaxThrottle);
// Set StabilizationDesired object
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
StabilizationDesiredSet(&stabDesired);
}
/**
* 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;
}
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedActual which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
static void updateVtolDesiredAttitude()
{
static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();;
float dT;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
AttitudeDesiredData attitudeDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
GuidanceSettingsData guidanceSettings;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
float northError;
float northCommand;
float eastError;
float eastCommand;
float downError;
float downCommand;
// Check how long since last update
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
SystemSettingsGet(&systemSettings);
GuidanceSettingsGet(&guidanceSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
AttitudeDesiredGet(&attitudeDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
attitudeDesired.Yaw = 0; // try and face north
// Compute desired north command
northError = velocityDesired.North - velocityActual.North;
northIntegral = bound(northIntegral + northError * dT,
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
northCommand = (northError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
northIntegral * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI] -
nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
// Compute desired east command
eastError = velocityDesired.East - velocityActual.East;
eastIntegral = bound(eastIntegral + eastError * dT,
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
eastCommand = (eastError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
eastIntegral * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI] -
nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
// Compute desired down command
downError = velocityDesired.Down - velocityActual.Down;
downIntegral = bound(downIntegral + downError * dT,
-guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT],
guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT]);
downCommand = (downError * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KP] +
downIntegral * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI] -
nedAccel.Down * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]);
attitudeDesired.Throttle = bound(downCommand, 0, 1);
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch
attitudeDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) +
-eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
attitudeDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) +
eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
if(guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
attitudeDesired.Throttle = manualControl.Throttle;
}
AttitudeDesiredSet(&attitudeDesired);
}
/**
* 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);
}
}
/**
* Module task
*/
static void stabilizationTask(void* parameters)
{
StabilizationSettingsData stabSettings;
ActuatorDesiredData actuatorDesired;
AttitudeDesiredData attitudeDesired;
AttitudeActualData attitudeActual;
ManualControlCommandData manualControl;
SystemSettingsData systemSettings;
portTickType lastSysTime;
float pitchErrorGlobal, pitchErrorLastGlobal;
float yawErrorGlobal, yawErrorLastGlobal;
float pitchError, pitchErrorLast;
float yawError, yawErrorLast;
float rollError, rollErrorLast;
float pitchDerivative;
float yawDerivative;
float rollDerivative;
float pitchIntegral, pitchIntegralLimit;
float yawIntegral, yawIntegralLimit;
float rollIntegral, rollIntegralLimit;
// Initialize
pitchIntegral = 0.0;
yawIntegral = 0.0;
rollIntegral = 0.0;
pitchErrorLastGlobal = 0.0;
yawErrorLastGlobal = 0.0;
rollErrorLast = 0.0;
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1)
{
// Read settings and other objects
StabilizationSettingsGet(&stabSettings);
SystemSettingsGet(&systemSettings);
ManualControlCommandGet(&manualControl);
AttitudeDesiredGet(&attitudeDesired);
AttitudeActualGet(&attitudeActual);
// For all three axis, calculate Error and ErrorLast - translating from global to local reference frame.
// global pitch error
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
{
pitchErrorGlobal = angleDifference(
bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch
);
}
else
{
// in AUTO mode, Stabilization is used to steer the plane freely,
// while Navigation dictates the flight path, including maneuvers outside stable limits.
pitchErrorGlobal = angleDifference(attitudeDesired.Pitch - attitudeActual.Pitch);
}
// global yaw error
if ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL || manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
{
// VTOLS consider yaw. AUTO mode considers YAW, too.
yawErrorGlobal = angleDifference(attitudeDesired.Yaw - attitudeActual.Yaw);
} else {
// FIXED WING STABILIZATION however does not.
yawErrorGlobal = 0;
}
// local pitch error
pitchError = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal;
// local roll error (no translation needed - always local)
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
{
rollError = angleDifference(
bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll
);
}
else
{
// in AUTO mode, Stabilization is used to steer the plane freely,
// while Navigation dictates the flight path, including maneuvers outside stable limits.
rollError = angleDifference(attitudeDesired.Roll - attitudeActual.Roll);
}
// local yaw error
yawError = cos(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal;
// for the derivative, the local last errors are needed. Therefore global lasts are translated into local lasts
// pitch last
pitchErrorLast = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal;
// yaw last
yawErrorLast = cos(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal;
// global last variables are no longer needed
pitchErrorLastGlobal = pitchErrorGlobal;
yawErrorLastGlobal = yawErrorGlobal;
// local Pitch stabilization control loop
pitchDerivative = pitchError - pitchErrorLast;
pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi;
pitchIntegral = bound(pitchIntegral+pitchError*stabSettings.UpdatePeriod, -pitchIntegralLimit, pitchIntegralLimit);
actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);
// local Roll stabilization control loop
rollDerivative = rollError - rollErrorLast;
rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi;
rollIntegral = bound(rollIntegral+rollError*stabSettings.UpdatePeriod, -rollIntegralLimit, rollIntegralLimit);
actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
rollErrorLast = rollError;
sdf;
// local Yaw stabilization control loop (only enabled on VTOL airframes)
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
{
yawDerivative = yawError - yawErrorLast;
yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
yawIntegral = bound(yawIntegral+yawError*stabSettings.UpdatePeriod, -yawIntegralLimit, yawIntegralLimit);
actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
}
else
{
actuatorDesired.Yaw = 0.0;
}
// Setup throttle
actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax);
// Write actuator desired (if not in manual mode)
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )
{
ActuatorDesiredSet(&actuatorDesired);
}
else
{
pitchIntegral = 0.0;
yawIntegral = 0.0;
rollIntegral = 0.0;
pitchErrorLastGlobal = 0.0;
yawErrorLastGlobal = 0.0;
rollErrorLast = 0.0;
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
// Wait until next update
vTaskDelayUntil(&lastSysTime, stabSettings.UpdatePeriod / portTICK_RATE_MS );
}
}
/**
* Compute desired attitude from the desired velocity for fixed wing craft
*/
uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
{
uint8_t result = 1;
const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
AirspeedStateData airspeedState;
SystemSettingsData systemSettings;
float groundspeedProjection;
float indicatedAirspeedState;
float indicatedAirspeedDesired;
float airspeedError;
float pitchCommand;
float descentspeedDesired;
float descentspeedError;
float powerCommand;
float airspeedVector[2];
float fluidMovement[2];
float courseComponent[2];
float courseError;
float courseCommand;
FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
VelocityStateGet(&velocityState);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
AirspeedStateGet(&airspeedState);
SystemSettingsGet(&systemSettings);
/**
* Compute speed error and course
*/
// missing sensors for airspeed-direction we have to assume within
// reasonable error that measured airspeed is actually the airspeed
// component in forward pointing direction
// airspeedVector is normalized
airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw);
airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw);
// current ground speed projected in forward direction
groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
// note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
// but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction
// than airspeed and gps sensors alone
indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
// fluidMovement is a vector describing the aproximate movement vector of
// the surrounding fluid in 2d space (aka wind vector)
fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
// calculate the movement vector we need to fly to reach velocityDesired -
// taking fluidMovement into account
courseComponent[0] = velocityDesired.North - fluidMovement[0];
courseComponent[1] = velocityDesired.East - fluidMovement[1];
indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
fixedWingSettings->HorizontalVelMin,
fixedWingSettings->HorizontalVelMax);
// if we could fly at arbitrary speeds, we'd just have to move towards the
// courseComponent vector as previously calculated and we'd be fine
// unfortunately however we are bound by min and max air speed limits, so
// we need to recalculate the correct course to meet at least the
// velocityDesired vector direction at our current speed
// this overwrites courseComponent
bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
// Error condition: wind speed too high, we can't go where we want anymore
fixedWingPathFollowerStatus.Errors.Wind = 0;
if ((!valid) &&
fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Wind = 1;
result = 0;
}
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
// Vertical speed error
descentspeedDesired = boundf(
velocityDesired.Down,
-fixedWingSettings->VerticalVelMax,
fixedWingSettings->VerticalVelMax);
descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: plane too slow or too fast
fixedWingPathFollowerStatus.Errors.Highspeed = 0;
fixedWingPathFollowerStatus.Errors.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) {
fixedWingPathFollowerStatus.Errors.Overspeed = 1;
result = 0;
}
if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) {
fixedWingPathFollowerStatus.Errors.Highspeed = 1;
result = 0;
}
if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
result = 0;
}
/**
* Compute desired thrust command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp,
-fixedWingSettings->AirspeedToPowerCrossFeed.Max,
fixedWingSettings->AirspeedToPowerCrossFeed.Max
);
// Compute final thrust response
powerCommand = pid_apply(&PIDpower, -descentspeedError, dT) +
speedErrorToPowerCommandComponent;
// Output internal state to telemetry
fixedWingPathFollowerStatus.Error.Power = descentspeedError;
fixedWingPathFollowerStatus.ErrorInt.Power = PIDpower.iAccumulator;
fixedWingPathFollowerStatus.Command.Power = powerCommand;
// set thrust
stabDesired.Thrust = boundf(fixedWingSettings->ThrustLimit.Neutral + powerCommand,
fixedWingSettings->ThrustLimit.Min,
fixedWingSettings->ThrustLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedWingPathFollowerStatus.Errors.Lowpower = 0;
if (fixedWingSettings->ThrustLimit.Neutral + powerCommand >= fixedWingSettings->ThrustLimit.Max && // thrust at maximum
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError > 0.0f && // we are too slow already
fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Lowpower = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
fixedWingPathFollowerStatus.Errors.Highpower = 0;
if (fixedWingSettings->ThrustLimit.Neutral + powerCommand <= fixedWingSettings->ThrustLimit.Min && // thrust at minimum
velocityState.Down < 0.0f && // we ARE going up
descentspeedDesired > 0.0f && // we WANT to go down
airspeedError < 0.0f && // we are too fast already
fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Highpower = 1;
result = 0;
}
/**
* Compute desired pitch command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp,
-fixedWingSettings->VerticalToPitchCrossFeed.Max,
fixedWingSettings->VerticalToPitchCrossFeed.Max
);
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
fixedWingPathFollowerStatus.Error.Speed = airspeedError;
fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator;
fixedWingPathFollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand,
fixedWingSettings->PitchLimit.Min,
fixedWingSettings->PitchLimit.Max);
// Error condition: high speed dive
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError < 0.0f && // we are too fast already
fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
result = 0;
}
/**
* Compute desired roll command
*/
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
// overlap calculation. Theres a dead zone behind the craft where the
// counter-yawing of some craft while rolling could render a desired right
// turn into a desired left turn. Making the turn direction based on
// current roll angle keeps the plane committed to a direction once chosen
if (courseError < -180.0f + (fixedWingSettings->ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll > 0.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f - (fixedWingSettings->ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll < 0.0f) {
courseError -= 360.0f;
}
courseCommand = pid_apply(&PIDcourse, courseError, dT);
fixedWingPathFollowerStatus.Error.Course = courseError;
fixedWingPathFollowerStatus.ErrorInt.Course = PIDcourse.iAccumulator;
fixedWingPathFollowerStatus.Command.Course = courseCommand;
stabDesired.Roll = boundf(fixedWingSettings->RollLimit.Neutral +
courseCommand,
fixedWingSettings->RollLimit.Min,
fixedWingSettings->RollLimit.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
/**
* Compute desired yaw command
*/
// TODO implement raw control mode for yaw and base on Accels.Y
stabDesired.Yaw = 0.0f;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
return result;
}
static void SensorsTask(void *parameters)
{
AlarmsClear(SYSTEMALARMS_ALARM_SENSORS);
// HomeLocationData homeLocation;
// HomeLocationGet(&homeLocation);
// homeLocation.Latitude = 0;
// homeLocation.Longitude = 0;
// homeLocation.Altitude = 0;
// homeLocation.Be[0] = 26000;
// homeLocation.Be[1] = 400;
// homeLocation.Be[2] = 40000;
// homeLocation.Set = HOMELOCATION_SET_TRUE;
// HomeLocationSet(&homeLocation);
PIOS_SENSORS_SetMaxGyro(500);
// Main task loop
while (1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS);
SystemSettingsData systemSettings;
SystemSettingsGet(&systemSettings);
switch(systemSettings.AirframeType) {
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING:
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON:
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL:
sensor_sim_type = MODEL_AIRPLANE;
break;
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
sensor_sim_type = MODEL_QUADCOPTER;
break;
case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR:
sensor_sim_type = MODEL_CAR;
break;
default:
sensor_sim_type = MODEL_AGNOSTIC;
}
static int i;
i++;
if (i % 5000 == 0) {
//float dT = PIOS_DELAY_DiffuS(last_time) / 10.0e6;
//fprintf(stderr, "Sensor relative timing: %f\n", dT);
}
sensors_count++;
switch(sensor_sim_type) {
case CONSTANT:
simulateConstant();
break;
case MODEL_AGNOSTIC:
simulateModelAgnostic();
break;
case MODEL_QUADCOPTER:
simulateModelQuadcopter();
break;
case MODEL_AIRPLANE:
simulateModelAirplane();
break;
case MODEL_CAR:
simulateModelCar();
}
vTaskDelay(MS2TICKS(2));
}
}
/**
* Module task
*/
static void stabilizationTask(void* parameters)
{
UAVObjEvent ev;
StabilizationSettingsData stabSettings;
ActuatorDesiredData actuatorDesired;
AttitudeDesiredData attitudeDesired;
AttitudeActualData attitudeActual;
ManualControlCommandData manualControl;
SystemSettingsData systemSettings;
portTickType lastSysTime;
portTickType thisSysTime;
float pitchErrorGlobal, pitchErrorLastGlobal;
float yawErrorGlobal, yawErrorLastGlobal;
float pitchError, pitchErrorLast;
float yawError, yawErrorLast;
float rollError, rollErrorLast;
float pitchDerivative;
float yawDerivative;
float rollDerivative;
float pitchIntegral, pitchIntegralLimit;
float yawIntegral, yawIntegralLimit;
float rollIntegral, rollIntegralLimit;
float yawPrevious;
float yawChange;
// Initialize
pitchIntegral = 0.0;
yawIntegral = 0.0;
rollIntegral = 0.0;
pitchErrorLastGlobal = 0.0;
yawErrorLastGlobal = 0.0;
rollErrorLast = 0.0;
yawPrevious = 0.0;
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1)
{
// Wait until the ActuatorDesired 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);
}
// 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;
// Read settings and other objects
StabilizationSettingsGet(&stabSettings);
SystemSettingsGet(&systemSettings);
ManualControlCommandGet(&manualControl);
AttitudeDesiredGet(&attitudeDesired);
AttitudeActualGet(&attitudeActual);
// For all three axis, calculate Error and ErrorLast - translating from global to local reference frame.
// global pitch error
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
{
pitchErrorGlobal = angleDifference(
bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch
);
}
else
{
// in AUTO mode, Stabilization is used to steer the plane freely,
// while Navigation dictates the flight path, including maneuvers outside stable limits.
pitchErrorGlobal = angleDifference(attitudeDesired.Pitch - attitudeActual.Pitch);
}
// global yaw error
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
{
// VTOLS consider yaw. AUTO mode considers YAW, too.
if(stabSettings.YawMode == STABILIZATIONSETTINGS_YAWMODE_RATE) { // rate stabilization on yaw
yawChange = (attitudeActual.Yaw - yawPrevious) / dT;
yawPrevious = attitudeActual.Yaw;
yawErrorGlobal = angleDifference(bound(attitudeDesired.Yaw, -stabSettings.YawMax, stabSettings.YawMax) - yawChange);
} else { // heading stabilization
yawError = angleDifference(attitudeDesired.Yaw - attitudeActual.Yaw);
}
} else {
// FIXED WING STABILIZATION however does not.
yawErrorGlobal = 0;
}
// local pitch error
pitchError = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal;
// local roll error (no translation needed - always local)
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
{
rollError = angleDifference(
bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll
);
}
else
{
// in AUTO mode, Stabilization is used to steer the plane freely,
// while Navigation dictates the flight path, including maneuvers outside stable limits.
rollError = angleDifference(attitudeDesired.Roll - attitudeActual.Roll);
}
// local yaw error
yawError = cos(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal;
// for the derivative, the local last errors are needed. Therefore global lasts are translated into local lasts
// pitch last
pitchErrorLast = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal;
// yaw last
yawErrorLast = cos(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal;
// global last variables are no longer needed
pitchErrorLastGlobal = pitchErrorGlobal;
yawErrorLastGlobal = yawErrorGlobal;
// local Pitch stabilization control loop
pitchDerivative = pitchError - pitchErrorLast;
pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi;
pitchIntegral = bound(pitchIntegral+pitchError*dT, -pitchIntegralLimit, pitchIntegralLimit);
actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);
// local Roll stabilization control loop
rollDerivative = rollError - rollErrorLast;
rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi;
rollIntegral = bound(rollIntegral+rollError*dT, -rollIntegralLimit, rollIntegralLimit);
actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
rollErrorLast = rollError;
// local Yaw stabilization control loop (only enabled on VTOL airframes)
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
{
yawDerivative = yawError - yawErrorLast;
yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
yawIntegral = bound(yawIntegral+yawError*dT, -yawIntegralLimit, yawIntegralLimit);
actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
}
else
{
actuatorDesired.Yaw = 0.0;
}
// Setup throttle
actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax);
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
// Write actuator desired (if not in manual mode)
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )
{
ActuatorDesiredSet(&actuatorDesired);
}
else
{
pitchIntegral = 0.0;
yawIntegral = 0.0;
rollIntegral = 0.0;
pitchErrorLastGlobal = 0.0;
yawErrorLastGlobal = 0.0;
rollErrorLast = 0.0;
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
