/**
* 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 manualControlTask(void *parameters)
{
ManualControlSettingsData settings;
StabilizationSettingsData stabSettings;
ManualControlCommandData cmd;
ActuatorDesiredData actuator;
AttitudeDesiredData attitude;
RateDesiredData rate;
portTickType lastSysTime;
float flightMode;
uint8_t disconnected_count = 0;
uint8_t connected_count = 0;
enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED;
// Make sure unarmed on power up
ManualControlCommandGet(&cmd);
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
ManualControlCommandSet(&cmd);
armState = ARM_STATE_DISARMED;
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1) {
float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM];
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
// Read settings
ManualControlSettingsGet(&settings);
StabilizationSettingsGet(&stabSettings);
if (ManualControlCommandReadOnly(&cmd)) {
FlightTelemetryStatsData flightTelemStats;
FlightTelemetryStatsGet(&flightTelemStats);
if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata;
UAVObjGetMetadata(&cmd, &metadata);
metadata.access = ACCESS_READWRITE;
UAVObjSetMetadata(&cmd, &metadata);
}
}
if (!ManualControlCommandReadOnly(&cmd)) {
// Check settings, if error raise alarm
if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE ||
settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE ||
settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE ||
settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE ||
settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
continue;
}
// Read channel values in us
// TODO: settings.InputMode is currently ignored because PIOS will not allow runtime
// selection of PWM and PPM. The configuration is currently done at compile time in
// the pios_config.h file.
for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
#if defined(PIOS_INCLUDE_PWM)
cmd.Channel[n] = PIOS_PWM_Get(n);
#elif defined(PIOS_INCLUDE_PPM)
cmd.Channel[n] = PIOS_PPM_Get(n);
#elif defined(PIOS_INCLUDE_SPEKTRUM)
cmd.Channel[n] = PIOS_SPEKTRUM_Get(n);
#endif
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
}
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[settings.Roll];
cmd.Pitch = scaledChannel[settings.Pitch];
cmd.Yaw = scaledChannel[settings.Yaw];
cmd.Throttle = scaledChannel[settings.Throttle];
flightMode = scaledChannel[settings.FlightMode];
if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE)
cmd.Accessory1 = scaledChannel[settings.Accessory1];
else
cmd.Accessory1 = 0;
if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE)
cmd.Accessory2 = scaledChannel[settings.Accessory2];
else
cmd.Accessory2 = 0;
if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE)
cmd.Accessory3 = scaledChannel[settings.Accessory3];
else
cmd.Accessory3 = 0;
if (flightMode < -FLIGHT_MODE_LIMIT) {
// Position 1
for(int i = 0; i < 3; i++) {
cmd.StabilizationSettings[i] = settings.Pos1StabilizationSettings[i]; // See assumptions1
}
cmd.FlightMode = settings.Pos1FlightMode; // See assumptions2
} else if (flightMode > FLIGHT_MODE_LIMIT) {
// Position 3
for(int i = 0; i < 3; i++) {
cmd.StabilizationSettings[i] = settings.Pos3StabilizationSettings[i]; // See assumptions5
}
cmd.FlightMode = settings.Pos3FlightMode; // See assumptions6
} else {
// Position 2
for(int i = 0; i < 3; i++) {
cmd.StabilizationSettings[i] = settings.Pos2StabilizationSettings[i]; // See assumptions3
}
cmd.FlightMode = settings.Pos2FlightMode; // See assumptions4
}
// Update the ManualControlCommand object
ManualControlCommandSet(&cmd);
// This seems silly to set then get, but the reason is if the GCS is
// the control input, the set command will be blocked by the read only
// setting and the get command will pull the right values from telemetry
} else
ManualControlCommandGet(&cmd); /* Under GCS control */
// Implement hysteresis loop on connection status
// Must check both Max and Min in case they reversed
if (!ManualControlCommandReadOnly(&cmd) &&
cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET &&
cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) {
if (disconnected_count++ > 10) {
connection_state = DISCONNECTED;
connected_count = 0;
disconnected_count = 0;
} else
disconnected_count++;
} else {
if (connected_count++ > 10) {
connection_state = CONNECTED;
connected_count = 0;
disconnected_count = 0;
} else
connected_count++;
}
if (connection_state == DISCONNECTED) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0;
cmd.Yaw = 0;
cmd.Pitch = 0;
//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
ManualControlCommandSet(&cmd);
} else {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
ManualControlCommandSet(&cmd);
}
// Arming and Disarming mechanism
if (cmd.Throttle < 0) {
// Throttle is low, in this condition the arming state could change
uint8_t newCmdArmed = cmd.Armed;
static portTickType armedDisarmStart;
// Look for state changes and write in newArmState
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_NONE) {
// No channel assigned to arming -> arm immediately when throttle is low
newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE;
} else {
float armStickLevel;
uint8_t channel = settings.Arming/2; // 0=Channel1, 1=Channel1_Rev, 2=Channel2, ....
bool reverse = (settings.Arming%2)==1;
bool manualArm = false;
bool manualDisarm = false;
if (connection_state == CONNECTED) {
// Should use RC input only if RX is connected
armStickLevel = scaledChannel[channel];
if (reverse)
armStickLevel =-armStickLevel;
if (armStickLevel <= -0.90)
manualArm = true;
else if (armStickLevel >= +0.90)
manualDisarm = true;
}
switch(armState) {
case ARM_STATE_DISARMED:
newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE;
if (manualArm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
case ARM_STATE_ARMING_MANUAL:
if (manualArm) {
if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
armState = ARM_STATE_ARMED;
}
else
armState = ARM_STATE_DISARMED;
break;
case ARM_STATE_ARMED:
// When we get here, the throttle is low,
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_TIMEOUT;
newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE;
break;
case ARM_STATE_DISARMING_TIMEOUT:
// We get here when armed while throttle low, even when the arming timeout is not enabled
if (settings.ArmedTimeout != 0)
if (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings.ArmedTimeout)
armState = ARM_STATE_DISARMED;
// Switch to disarming due to manual control when needed
if (manualDisarm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_MANUAL;
}
break;
case ARM_STATE_DISARMING_MANUAL:
if (manualDisarm) {
if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
armState = ARM_STATE_DISARMED;
}
else
armState = ARM_STATE_ARMED;
break;
}
}
// Update cmd object when needed
if (newCmdArmed != cmd.Armed) {
cmd.Armed = newCmdArmed;
ManualControlCommandSet(&cmd);
}
} else {
// The throttle is not low, in case we where arming or disarming, abort
switch(armState) {
case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED;
break;
case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED;
break;
default:
// Nothing needs to be done in the other states
break;
}
}
// End of arming/disarming
// Depending on the mode update the Stabilization or Actuator objects
if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) {
actuator.Roll = cmd.Roll;
actuator.Pitch = cmd.Pitch;
actuator.Yaw = cmd.Yaw;
actuator.Throttle = cmd.Throttle;
ActuatorDesiredSet(&actuator);
} else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) {
attitude.Roll = cmd.Roll * stabSettings.RollMax;
attitude.Pitch = cmd.Pitch * stabSettings.PitchMax;
attitude.Yaw = fmod(cmd.Yaw * 180.0, 360);
attitude.Throttle = (cmd.Throttle < 0) ? -1 : cmd.Throttle;
rate.Roll = cmd.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL];
rate.Pitch = cmd.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH];
rate.Yaw = cmd.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
AttitudeDesiredSet(&attitude);
RateDesiredSet(&rate);
}
}
}
/**
* Module task
*/
static void stabilizationTask(void* parameters)
{
UAVObjEvent ev;
uint32_t timeval = PIOS_DELAY_GetRaw();
ActuatorDesiredData actuatorDesired;
StabilizationDesiredData stabDesired;
RateDesiredData rateDesired;
AttitudeActualData attitudeActual;
GyrosData gyrosData;
FlightStatusData flightStatus;
float *stabDesiredAxis = &stabDesired.Roll;
float *actuatorDesiredAxis = &actuatorDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
float horizonRateFraction = 0.0f;
// Force refresh of all settings immediately before entering main task loop
SettingsUpdatedCb((UAVObjEvent *) NULL);
// Settings for system identification
uint32_t iteration = 0;
const uint32_t SYSTEM_IDENT_PERIOD = 75;
uint32_t system_ident_timeval = PIOS_DELAY_GetRaw();
float dT_filtered = 0;
// Main task loop
zero_pids();
while(1) {
iteration++;
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if (PIOS_Queue_Receive(queue, &ev, FAILSAFE_TIMEOUT_MS) != true)
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
continue;
}
calculate_pids();
float dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f;
timeval = PIOS_DELAY_GetRaw();
// exponential moving averaging (EMA) of dT to reduce jitter; ~200points
// to have more or less equivalent noise reduction to a normal N point moving averaging: alpha = 2 / (N + 1)
// run it only at the beginning for the first samples, to reduce CPU load, and the value should converge to a constant value
if (iteration < 100) {
dT_filtered = dT;
} else if (iteration < 2000) {
dT_filtered = 0.01f * dT + (1.0f - 0.01f) * dT_filtered;
} else if (iteration == 2000) {
gyro_filter_updated = true;
}
if (gyro_filter_updated) {
if (settings.GyroCutoff < 1.0f) {
gyro_alpha = 0;
} else {
gyro_alpha = expf(-2.0f * (float)(M_PI) *
settings.GyroCutoff * dT_filtered);
}
// Compute time constant for vbar decay term
if (settings.VbarTau < 0.001f) {
vbar_decay = 0;
} else {
vbar_decay = expf(-dT_filtered / settings.VbarTau);
}
gyro_filter_updated = false;
}
FlightStatusGet(&flightStatus);
StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual);
GyrosGet(&gyrosData);
ActuatorDesiredGet(&actuatorDesired);
#if defined(RATEDESIRED_DIAGNOSTICS)
RateDesiredGet(&rateDesired);
#endif
struct TrimmedAttitudeSetpoint {
float Roll;
float Pitch;
float Yaw;
} trimmedAttitudeSetpoint;
// Mux in level trim values, and saturate the trimmed attitude setpoint.
trimmedAttitudeSetpoint.Roll = bound_min_max(
stabDesired.Roll + trimAngles.Roll,
-settings.RollMax + trimAngles.Roll,
settings.RollMax + trimAngles.Roll);
trimmedAttitudeSetpoint.Pitch = bound_min_max(
stabDesired.Pitch + trimAngles.Pitch,
-settings.PitchMax + trimAngles.Pitch,
settings.PitchMax + trimAngles.Pitch);
trimmedAttitudeSetpoint.Yaw = stabDesired.Yaw;
// For horizon mode we need to compute the desire attitude from an unscaled value and apply the
// trim offset. Also track the stick with the most deflection to choose rate blending.
horizonRateFraction = 0.0f;
if (stabDesired.StabilizationMode[ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) {
trimmedAttitudeSetpoint.Roll = bound_min_max(
stabDesired.Roll * settings.RollMax + trimAngles.Roll,
-settings.RollMax + trimAngles.Roll,
settings.RollMax + trimAngles.Roll);
horizonRateFraction = fabsf(stabDesired.Roll);
}
if (stabDesired.StabilizationMode[PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) {
trimmedAttitudeSetpoint.Pitch = bound_min_max(
stabDesired.Pitch * settings.PitchMax + trimAngles.Pitch,
-settings.PitchMax + trimAngles.Pitch,
settings.PitchMax + trimAngles.Pitch);
horizonRateFraction = MAX(horizonRateFraction, fabsf(stabDesired.Pitch));
}
if (stabDesired.StabilizationMode[YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) {
trimmedAttitudeSetpoint.Yaw = stabDesired.Yaw * settings.YawMax;
horizonRateFraction = MAX(horizonRateFraction, fabsf(stabDesired.Yaw));
}
// For weak leveling mode the attitude setpoint is the trim value (drifts back towards "0")
if (stabDesired.StabilizationMode[ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) {
trimmedAttitudeSetpoint.Roll = trimAngles.Roll;
}
if (stabDesired.StabilizationMode[PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) {
trimmedAttitudeSetpoint.Pitch = trimAngles.Pitch;
}
if (stabDesired.StabilizationMode[YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) {
trimmedAttitudeSetpoint.Yaw = 0;
}
// Note we divide by the maximum limit here so the fraction ranges from 0 to 1 depending on
// how much is requested.
horizonRateFraction = bound_sym(horizonRateFraction, HORIZON_MODE_MAX_BLEND) / HORIZON_MODE_MAX_BLEND;
// Calculate the errors in each axis. The local error is used in the following modes:
// ATTITUDE, HORIZON, WEAKLEVELING
float local_attitude_error[3];
local_attitude_error[0] = trimmedAttitudeSetpoint.Roll - attitudeActual.Roll;
local_attitude_error[1] = trimmedAttitudeSetpoint.Pitch - attitudeActual.Pitch;
local_attitude_error[2] = trimmedAttitudeSetpoint.Yaw - attitudeActual.Yaw;
// Wrap yaw error to [-180,180]
local_attitude_error[2] = circular_modulus_deg(local_attitude_error[2]);
static float gyro_filtered[3];
gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha);
gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha);
// A flag to track which stabilization mode each axis is in
static uint8_t previous_mode[MAX_AXES] = {255,255,255};
bool error = false;
//Run the selected stabilization algorithm on each axis:
for(uint8_t i=0; i< MAX_AXES; i++)
{
// Check whether this axis mode needs to be reinitialized
bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]);
// The unscaled input (-1,1)
float *raw_input = &stabDesired.Roll;
previous_mode[i] = stabDesired.StabilizationMode[i];
// Apply the selected control law
switch(stabDesired.StabilizationMode[i])
{
case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE:
if(reinit)
pids[PID_GROUP_RATE + i].iAccumulator = 0;
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ACROPLUS:
// this implementation is based on the Openpilot/Librepilot Acro+ flightmode
// and our existing rate & MWRate flightmodes
if(reinit)
pids[PID_GROUP_RATE + i].iAccumulator = 0;
// The factor for gyro suppression / mixing raw stick input into the output; scaled by raw stick input
float factor = fabsf(raw_input[i]) * settings.AcroInsanityFactor / 100;
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound_sym(raw_input[i] * settings.ManualRate[i], settings.ManualRate[i]);
// Zero integral for aggressive maneuvers, like it is done for MWRate
if ((i < 2 && fabsf(gyro_filtered[i]) > 150.0f) ||
(i == 0 && fabsf(raw_input[i]) > 0.2f)) {
pids[PID_GROUP_RATE + i].iAccumulator = 0;
pids[PID_GROUP_RATE + i].i = 0;
}
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = factor * raw_input[i] + (1.0f - factor) * actuatorDesiredAxis[i];
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i], 1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE:
if(reinit) {
pids[PID_GROUP_ATT + i].iAccumulator = 0;
pids[PID_GROUP_RATE + i].iAccumulator = 0;
}
// Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT);
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
// Store for debugging output
rateDesiredAxis[i] = stabDesiredAxis[i];
// Run a virtual flybar stabilization algorithm on this axis
stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &pids[PID_GROUP_VBAR + i], &settings);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING:
{
if (reinit)
pids[PID_GROUP_RATE + i].iAccumulator = 0;
float weak_leveling = local_attitude_error[i] * weak_leveling_kp;
weak_leveling = bound_sym(weak_leveling, weak_leveling_max);
// Compute desired rate as input biased towards leveling
rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling;
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
break;
}
case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK:
if (reinit)
pids[PID_GROUP_RATE + i].iAccumulator = 0;
if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) {
// While getting strong commands act like rate mode
rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]);
// Reset accumulator
axis_lock_accum[i] = 0;
} else {
// For weaker commands or no command simply lock (almost) on no gyro change
axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT;
axis_lock_accum[i] = bound_sym(axis_lock_accum[i], max_axis_lock);
// Compute the inner loop
float tmpRateDesired = pid_apply(&pids[PID_GROUP_ATT + i], axis_lock_accum[i], dT);
rateDesiredAxis[i] = bound_sym(tmpRateDesired, settings.MaximumRate[i]);
}
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON:
if(reinit) {
pids[PID_GROUP_RATE + i].iAccumulator = 0;
}
// Do not allow outer loop integral to wind up in this mode since the controller
// is often disengaged.
pids[PID_GROUP_ATT + i].iAccumulator = 0;
// Compute the outer loop for the attitude control
float rateDesiredAttitude = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT);
// Compute the desire rate for a rate control
float rateDesiredRate = raw_input[i] * settings.ManualRate[i];
// Blend from one rate to another. The maximum of all stick positions is used for the
// amount so that when one axis goes completely to rate the other one does too. This
// prevents doing flips while one axis tries to stay in attitude mode.
rateDesiredAxis[i] = rateDesiredAttitude * (1.0f-horizonRateFraction) + rateDesiredRate * horizonRateFraction;
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.ManualRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_MWRATE:
{
if(reinit) {
pids[PID_GROUP_MWR + i].iAccumulator = 0;
}
/*
Conversion from MultiWii PID settings to our units.
Kp = Kp_mw * 4 / 80 / 500
Kd = Kd_mw * looptime * 1e-6 * 4 * 3 / 32 / 500
Ki = Ki_mw * 4 / 125 / 64 / (looptime * 1e-6) / 500
These values will just be approximate and should help
you get started.
*/
// The unscaled input (-1,1) - note in MW this is from (-500,500)
float *raw_input = &stabDesired.Roll;
// dynamic PIDs are scaled both by throttle and stick position
float scale = (i == 0 || i == 1) ? mwrate_settings.RollPitchRate : mwrate_settings.YawRate;
float pid_scale = (100.0f - scale * fabsf(raw_input[i])) / 100.0f;
float dynP8 = pids[PID_GROUP_MWR + i].p * pid_scale;
float dynD8 = pids[PID_GROUP_MWR + i].d * pid_scale;
// these terms are used by the integral loop this proportional term is scaled by throttle (this is different than MW
// that does not apply scale
float cfgP8 = pids[PID_GROUP_MWR + i].p;
float cfgI8 = pids[PID_GROUP_MWR + i].i;
// Dynamically adjust PID settings
struct pid mw_pid;
mw_pid.p = 0; // use zero Kp here because of strange setpoint. applied later.
mw_pid.d = dynD8;
mw_pid.i = cfgI8;
mw_pid.iLim = pids[PID_GROUP_MWR + i].iLim;
mw_pid.iAccumulator = pids[PID_GROUP_MWR + i].iAccumulator;
mw_pid.lastErr = pids[PID_GROUP_MWR + i].lastErr;
mw_pid.lastDer = pids[PID_GROUP_MWR + i].lastDer;
// Zero integral for aggressive maneuvers
if ((i < 2 && fabsf(gyro_filtered[i]) > 150.0f) ||
(i == 0 && fabsf(raw_input[i]) > 0.2f)) {
mw_pid.iAccumulator = 0;
mw_pid.i = 0;
}
// Apply controller as if we want zero change, then add stick input afterwards
actuatorDesiredAxis[i] = pid_apply_setpoint(&mw_pid, raw_input[i] / cfgP8, gyro_filtered[i], dT);
actuatorDesiredAxis[i] += raw_input[i]; // apply input
actuatorDesiredAxis[i] -= dynP8 * gyro_filtered[i]; // apply Kp term
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
// Store PID accumulators for next cycle
pids[PID_GROUP_MWR + i].iAccumulator = mw_pid.iAccumulator;
pids[PID_GROUP_MWR + i].lastErr = mw_pid.lastErr;
pids[PID_GROUP_MWR + i].lastDer = mw_pid.lastDer;
}
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT:
if(reinit) {
pids[PID_GROUP_ATT + i].iAccumulator = 0;
pids[PID_GROUP_RATE + i].iAccumulator = 0;
}
static uint32_t ident_iteration = 0;
static float ident_offsets[3] = {0};
if (PIOS_DELAY_DiffuS(system_ident_timeval) / 1000.0f > SYSTEM_IDENT_PERIOD && SystemIdentHandle()) {
ident_iteration++;
system_ident_timeval = PIOS_DELAY_GetRaw();
SystemIdentData systemIdent;
SystemIdentGet(&systemIdent);
const float SCALE_BIAS = 7.1f;
float roll_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_ROLL]);
float pitch_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_PITCH]);
float yaw_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_YAW]);
if (roll_scale > 0.25f)
roll_scale = 0.25f;
if (pitch_scale > 0.25f)
pitch_scale = 0.25f;
if (yaw_scale > 0.25f)
yaw_scale = 0.2f;
switch(ident_iteration & 0x07) {
case 0:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = yaw_scale;
break;
case 1:
ident_offsets[0] = roll_scale;
ident_offsets[1] = 0;
ident_offsets[2] = 0;
break;
case 2:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = -yaw_scale;
break;
case 3:
ident_offsets[0] = -roll_scale;
ident_offsets[1] = 0;
ident_offsets[2] = 0;
break;
case 4:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = yaw_scale;
break;
case 5:
ident_offsets[0] = 0;
ident_offsets[1] = pitch_scale;
ident_offsets[2] = 0;
break;
case 6:
ident_offsets[0] = 0;
ident_offsets[1] = 0;
ident_offsets[2] = -yaw_scale;
break;
case 7:
ident_offsets[0] = 0;
ident_offsets[1] = -pitch_scale;
ident_offsets[2] = 0;
break;
}
}
if (i == ROLL || i == PITCH) {
// Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT);
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] += ident_offsets[i];
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
} else {
// Get the desired rate. yaw is always in rate mode in system ident.
rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]);
// Compute the inner loop only for yaw
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] += ident_offsets[i];
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
}
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_COORDINATEDFLIGHT:
switch (i) {
case YAW:
if (reinit) {
pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0;
pids[PID_RATE_YAW].iAccumulator = 0;
axis_lock_accum[YAW] = 0;
}
//If we are not in roll attitude mode, trigger an error
if (stabDesired.StabilizationMode[ROLL] != STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE)
{
error = true;
break ;
}
if (fabsf(stabDesired.Yaw) < COORDINATED_FLIGHT_MAX_YAW_THRESHOLD) { //If yaw is within the deadband...
if (fabsf(stabDesired.Roll) > COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD) { // We're requesting more roll than the threshold
float accelsDataY;
AccelsyGet(&accelsDataY);
//Reset integral if we have changed roll to opposite direction from rudder. This implies that we have changed desired turning direction.
if ((stabDesired.Roll > 0 && actuatorDesiredAxis[YAW] < 0) ||
(stabDesired.Roll < 0 && actuatorDesiredAxis[YAW] > 0)){
pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0;
}
// Coordinate flight can simply be seen as ensuring that there is no lateral acceleration in the
// body frame. As such, we use the (noisy) accelerometer data as our measurement. Ideally, at
// some point in the future we will estimate acceleration and then we can use the estimated value
// instead of the measured value.
float errorSlip = -accelsDataY;
float command = pid_apply(&pids[PID_COORDINATED_FLIGHT_YAW], errorSlip, dT);
actuatorDesiredAxis[YAW] = bound_sym(command ,1.0);
// Reset axis-lock integrals
pids[PID_RATE_YAW].iAccumulator = 0;
axis_lock_accum[YAW] = 0;
} else if (fabsf(stabDesired.Roll) <= COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD) { // We're requesting less roll than the threshold
// Axis lock on no gyro change
axis_lock_accum[YAW] += (0 - gyro_filtered[YAW]) * dT;
rateDesiredAxis[YAW] = pid_apply(&pids[PID_ATT_YAW], axis_lock_accum[YAW], dT);
rateDesiredAxis[YAW] = bound_sym(rateDesiredAxis[YAW], settings.MaximumRate[YAW]);
actuatorDesiredAxis[YAW] = pid_apply_setpoint(&pids[PID_RATE_YAW], rateDesiredAxis[YAW], gyro_filtered[YAW], dT);
actuatorDesiredAxis[YAW] = bound_sym(actuatorDesiredAxis[YAW],1.0f);
// Reset coordinated-flight integral
pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0;
}
} else { //... yaw is outside the deadband. Pass the manual input directly to the actuator.
actuatorDesiredAxis[YAW] = bound_sym(stabDesiredAxis[YAW], 1.0);
// Reset all integrals
pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0;
pids[PID_RATE_YAW].iAccumulator = 0;
axis_lock_accum[YAW] = 0;
}
break;
case ROLL:
case PITCH:
default:
//Coordinated Flight has no effect in these modes. Trigger a configuration error.
error = true;
break;
}
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_POI:
// The sanity check enforces this is only selectable for Yaw
// for a gimbal you can select pitch too.
if(reinit) {
pids[PID_GROUP_ATT + i].iAccumulator = 0;
pids[PID_GROUP_RATE + i].iAccumulator = 0;
}
float error;
float angle;
if (CameraDesiredHandle()) {
switch(i) {
case PITCH:
CameraDesiredDeclinationGet(&angle);
error = circular_modulus_deg(angle - attitudeActual.Pitch);
break;
case ROLL:
{
uint8_t roll_fraction = 0;
#ifdef GIMBAL
if (BrushlessGimbalSettingsHandle()) {
BrushlessGimbalSettingsRollFractionGet(&roll_fraction);
}
#endif /* GIMBAL */
// For ROLL POI mode we track the FC roll angle (scaled) to
// allow keeping some motion
CameraDesiredRollGet(&angle);
angle *= roll_fraction / 100.0f;
error = circular_modulus_deg(angle - attitudeActual.Roll);
}
break;
case YAW:
CameraDesiredBearingGet(&angle);
error = circular_modulus_deg(angle - attitudeActual.Yaw);
break;
default:
error = true;
}
} else
error = true;
// Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], error, dT);
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.PoiMaximumRate[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
actuatorDesiredAxis[i] = bound_sym(stabDesiredAxis[i],1.0f);
break;
default:
error = true;
break;
}
}
if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE)
stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT);
#if defined(RATEDESIRED_DIAGNOSTICS)
RateDesiredSet(&rateDesired);
#endif
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
actuatorDesired.Throttle = stabDesired.Throttle;
if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_MANUAL) {
ActuatorDesiredSet(&actuatorDesired);
} else {
// Force all axes to reinitialize when engaged
for(uint8_t i=0; i< MAX_AXES; i++)
previous_mode[i] = 255;
}
if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED ||
(lowThrottleZeroIntegral && stabDesired.Throttle < 0))
{
// Force all axes to reinitialize when engaged
for(uint8_t i=0; i< MAX_AXES; i++)
previous_mode[i] = 255;
}
// Clear or set alarms. Done like this to prevent toggling each cycle
// and hammering system alarms
if (error)
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR);
else
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
