/**
* WARNING! This callback executes with critical flight control priority every
* time a gyroscope update happens do NOT put any time consuming calculations
* in this loop unless they really have to execute with every gyro update
*/
static void stabilizationInnerloopTask()
{
// watchdog and error handling
{
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
#endif
bool warn = false;
bool error = false;
bool crit = false;
// check if outer loop keeps executing
if (stabSettings.monitor.rateupdates > -64) {
stabSettings.monitor.rateupdates--;
}
if (stabSettings.monitor.rateupdates < -(2 * OUTERLOOP_SKIPCOUNT)) {
// warning if rate loop skipped more than 2 execution
warn = true;
}
if (stabSettings.monitor.rateupdates < -(4 * OUTERLOOP_SKIPCOUNT)) {
// critical if rate loop skipped more than 4 executions
crit = true;
}
// check if gyro keeps updating
if (stabSettings.monitor.gyroupdates < 1) {
// error if gyro didn't update at all!
error = true;
}
if (stabSettings.monitor.gyroupdates > 1) {
// warning if we missed a gyro update
warn = true;
}
if (stabSettings.monitor.gyroupdates > 3) {
// critical if we missed 3 gyro updates
crit = true;
}
stabSettings.monitor.gyroupdates = 0;
if (crit) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_CRITICAL);
} else if (error) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
} else if (warn) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
RateDesiredData rateDesired;
ActuatorDesiredData actuator;
StabilizationStatusInnerLoopData enabled;
FlightStatusControlChainData cchain;
RateDesiredGet(&rateDesired);
ActuatorDesiredGet(&actuator);
StabilizationStatusInnerLoopGet(&enabled);
FlightStatusControlChainGet(&cchain);
float *rate = &rateDesired.Roll;
float *actuatorDesiredAxis = &actuator.Roll;
int t;
float dT;
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) {
if (reinit) {
stabSettings.innerPids[t].iAccumulator = 0;
}
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings);
break;
case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING:
rate[t] = boundf(rate[t],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
);
stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit);
break;
case STABILIZATIONSTATUS_INNERLOOP_AXISLOCK:
if (fabsf(rate[t]) > stabSettings.settings.MaxAxisLockRate) {
// While getting strong commands act like rate mode
axis_lock_accum[t] = 0;
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[t] += (rate[t] - gyro_filtered[t]) * dT;
axis_lock_accum[t] = boundf(axis_lock_accum[t], -stabSettings.settings.MaxAxisLock, stabSettings.settings.MaxAxisLock);
rate[t] = axis_lock_accum[t] * stabSettings.settings.AxisLockKp;
}
// IMPORTANT: deliberately no "break;" here, execution continues with regular RATE control loop to avoid code duplication!
// keep order as it is, RATE must follow!
case STABILIZATIONSTATUS_INNERLOOP_RATE:
// limit rate to maximum configured limits (once here instead of 5 times in outer loop)
rate[t] = boundf(rate[t],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:
actuatorDesiredAxis[t] = rate[t];
break;
}
} else {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL:
actuatorDesiredAxis[t] = cruisecontrol_apply_factor(rate[t]);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:
actuatorDesiredAxis[t] = rate[t];
break;
}
}
actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
}
actuator.UpdateTime = dT * 1000;
if (cchain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
ActuatorDesiredSet(&actuator);
} else {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
}
}
{
uint8_t armed;
FlightStatusArmedGet(&armed);
float throttleDesired;
ManualControlCommandThrottleGet(&throttleDesired);
if (armed != FLIGHTSTATUS_ARMED_ARMED ||
((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
}
}
}
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
}
/**
* 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);
}
}