/**
* Keep a running filtered version of the acceleration in the NED frame
*/
static void updateNedAccel()
{
float accel[3];
float q[4];
float Rbe[3][3];
float accel_ned[3];
// Collect downsampled attitude data
AccelsData accels;
AccelsGet(&accels);
accel[0] = accels.x;
accel[1] = accels.y;
accel[2] = accels.z;
//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] += GRAVITY;
NedAccelData accelData;
NedAccelGet(&accelData);
accelData.North = accel_ned[0];
accelData.East = accel_ned[1];
accelData.Down = accel_ned[2];
NedAccelSet(&accelData);
}
/**
* 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()
{
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
StabilizationSettingsData stabSettings;
float northError;
float northCommand;
float eastError;
float eastCommand;
float downError;
float downCommand;
VtolPathFollowerSettingsGet(&guidanceSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = velocityActual.North;
float eastVel = velocityActual.East;
float downVel = velocityActual.Down;
// Compute desired north command from velocity error
northError = velocityDesired.North - northVel;
northCommand = pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], northError,
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch, dT) + velocityDesired.North * guidanceSettings.VelocityFeedforward;
// Compute desired east command from velocity error
eastError = velocityDesired.East - eastVel;
eastCommand = pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], eastError,
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch, dT) + velocityDesired.East * guidanceSettings.VelocityFeedforward;
// Compute desired down command. Using NED accel as the damping term
downError = velocityDesired.Down - downVel;
// Negative is critical here since throttle is negative with down
downCommand = -pid_apply_antiwindup(&vtol_pids[DOWN_VELOCITY], downError, -1, 1, dT) +
nedAccel.Down * guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD];
// If this setting is zero then the throttle level available when enabled is used for hover:wq
float used_throttle_offset = (guidanceSettings.HoverThrottle == 0) ? throttleOffset : guidanceSettings.HoverThrottle;
stabDesired.Throttle = bound_min_max(downCommand + used_throttle_offset, 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.
// Notice the inputs are crudely bounded by the anti-winded but if both N and E were
// saturated and the craft were at 45 degrees that would result in a value greater than
// the limit, so apply limit again here.
stabDesired.Pitch = bound_min_max(-northCommand * cosf(attitudeActual.Yaw * DEG2RAD) +
-eastCommand * sinf(attitudeActual.Yaw * DEG2RAD),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
stabDesired.Roll = bound_min_max(-northCommand * sinf(attitudeActual.Yaw * DEG2RAD) +
eastCommand * cosf(attitudeActual.Yaw * DEG2RAD),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
if(guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Throttle = manualControl.Throttle;
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDEPLUS;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDEPLUS;
float yaw;
switch(guidanceSettings.YawMode) {
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_RATE:
/* This is awkward. This allows the transmitter to control the yaw while flying navigation */
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK:
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE:
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.YawMax * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI:
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
break;
}
StabilizationDesiredSet(&stabDesired);
}
/**
* 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);
}
/**
* 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 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 the desired acceleration based on the desired
* velocity and actual velocity
*/
static int32_t vtol_follower_control_accel(float dT)
{
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
AccelDesiredData accelDesired;
NedAccelData nedAccel;
float north_error, north_acceleration;
float east_error, east_acceleration;
float down_error;
static float last_north_velocity;
static float last_east_velocity;
NedAccelGet(&nedAccel);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
// Optionally compute the acceleration required component from a changing velocity desired
if (guidanceSettings.VelocityChangePrediction == VTOLPATHFOLLOWERSETTINGS_VELOCITYCHANGEPREDICTION_TRUE && dT > 0) {
north_acceleration = (velocityDesired.North - last_north_velocity) / dT;
east_acceleration = (velocityDesired.East - last_east_velocity) / dT;
last_north_velocity = velocityDesired.North;
last_east_velocity = velocityDesired.East;
} else {
north_acceleration = 0;
east_acceleration = 0;
}
// Convert the max angles into the maximum angle that would be requested
const float MAX_ACCELERATION = GRAVITY * sinf(guidanceSettings.MaxRollPitch * DEG2RAD);
// Compute desired north command from velocity error
north_error = velocityDesired.North - velocityActual.North;
north_acceleration += pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], north_error,
-MAX_ACCELERATION, MAX_ACCELERATION, dT) +
velocityDesired.North * guidanceSettings.VelocityFeedforward +
-nedAccel.North * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD];
// Compute desired east command from velocity error
east_error = velocityDesired.East - velocityActual.East;
east_acceleration += pid_apply_antiwindup(&vtol_pids[EAST_VELOCITY], east_error,
-MAX_ACCELERATION, MAX_ACCELERATION, dT) +
velocityDesired.East * guidanceSettings.VelocityFeedforward +
-nedAccel.East * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD];
accelDesired.North = north_acceleration;
accelDesired.East = east_acceleration;
// Note: vertical controller really isn't currently in units of acceleration and the anti-windup may
// not be appropriate. However, it is fine for now since it this output is just directly used on the
// output. To use this appropriately we need a model of throttle to acceleration.
// Compute desired down command. Using NED accel as the damping term
down_error = velocityDesired.Down - velocityActual.Down;
// Negative is critical here since throttle is negative with down
accelDesired.Down = -pid_apply_antiwindup(&vtol_pids[DOWN_VELOCITY], down_error, -1, 0, dT);
// Store the desired acceleration
AccelDesiredSet(&accelDesired);
return 0;
}
