/**
* Module starting
*/
int32_t ManualControlStart()
{
// Run this initially to make sure the configuration is checked
configuration_check();
// Whenever the configuration changes, make sure it is safe to fly
SystemSettingsConnectCallback(configurationUpdatedCb);
ManualControlSettingsConnectCallback(configurationUpdatedCb);
ManualControlCommandConnectCallback(commandUpdatedCb);
// clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
SettingsUpdatedCb(NULL);
// Make sure unarmed on power up
armHandler(true, frameType);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
takeOffLocationHandlerInit();
#endif
// Start main task
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AltitudeInitialize()
{
BaroSensorInitialize();
RevoSettingsInitialize();
RevoSettingsConnectCallback(&SettingsUpdatedCb);
SettingsUpdatedCb(NULL);
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialize();
#endif
return 0;
}
/**
* Module initialization
*/
int32_t StabilizationInitialize()
{
// Initialize variables
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// Listen for updates.
// AttitudeActualConnectQueue(queue);
AttitudeRawConnectQueue(queue);
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
// Start main task
return 0;
}
/**
* Module initialization
*/
int32_t StabilizationStart()
{
StabilizationSettingsConnectCallback(SettingsUpdatedCb);
ManualControlCommandConnectCallback(FlightModeSwitchUpdatedCb);
StabilizationBankConnectCallback(BankUpdatedCb);
StabilizationSettingsBank1ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank2ConnectCallback(SettingsBankUpdatedCb);
StabilizationSettingsBank3ConnectCallback(SettingsBankUpdatedCb);
StabilizationDesiredConnectCallback(StabilizationDesiredUpdatedCb);
SettingsUpdatedCb(StabilizationSettingsHandle());
StabilizationDesiredUpdatedCb(StabilizationDesiredHandle());
FlightModeSwitchUpdatedCb(ManualControlCommandHandle());
BankUpdatedCb(StabilizationBankHandle());
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_STABILIZATION);
#endif
return 0;
}
/**
* Module thread, should not return.
*/
static void pathfollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
uint32_t lastUpdateTime;
AirspeedActualConnectCallback(airspeedActualUpdatedCb);
FixedWingPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
FixedWingAirspeedsConnectCallback(SettingsUpdatedCb);
PathDesiredConnectCallback(SettingsUpdatedCb);
// Force update of all the settings
SettingsUpdatedCb(NULL);
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);
path_desired_updated = false;
PathDesiredGet(&pathDesired);
PathDesiredConnectCallback(pathDesiredUpdated);
// Main task loop
lastUpdateTime = PIOS_Thread_Systime();
while (1) {
// Conditions when this runs:
// 1. Must have FixedWing type airframe
// 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
SystemSettingsGet(&systemSettings);
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON) &&
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL) )
{
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL);
PIOS_Thread_Sleep(1000);
continue;
}
// Continue collecting data if not enough time
PIOS_Thread_Sleep_Until(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod);
static uint8_t last_flight_mode;
FlightStatusGet(&flightStatus);
PathStatusGet(&pathStatus);
PositionActualData positionActual;
static enum {FW_FOLLOWER_IDLE, FW_FOLLOWER_RUNNING, FW_FOLLOWER_ERR} state = FW_FOLLOWER_IDLE;
// Check whether an update to the path desired occured and we should
// process it. This makes sure that the follower alarm state is
// updated.
bool process_path_desired_update =
(last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ||
last_flight_mode == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) &&
path_desired_updated;
path_desired_updated = false;
// Process most of these when the flight mode changes
// except when in path following mode in which case
// each iteration must make sure this has the latest
// PathDesired
if (flightStatus.FlightMode != last_flight_mode ||
process_path_desired_update) {
last_flight_mode = flightStatus.FlightMode;
switch(flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
state = FW_FOLLOWER_RUNNING;
PositionActualGet(&positionActual);
pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT;
pathDesired.Start[0] = positionActual.North;
pathDesired.Start[1] = positionActual.East;
pathDesired.Start[2] = positionActual.Down;
pathDesired.End[0] = 0;
pathDesired.End[1] = 0;
pathDesired.End[2] = -30.0f;
pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius;
pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed;
pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed;
PathDesiredSet(&pathDesired);
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
state = FW_FOLLOWER_RUNNING;
PositionActualGet(&positionActual);
pathDesired.Mode = PATHDESIRED_MODE_CIRCLEPOSITIONRIGHT;
pathDesired.Start[0] = positionActual.North;
pathDesired.Start[1] = positionActual.East;
pathDesired.Start[2] = positionActual.Down;
pathDesired.End[0] = positionActual.North;
pathDesired.End[1] = positionActual.East;
pathDesired.End[2] = positionActual.Down;
pathDesired.ModeParameters = fixedwingpathfollowerSettings.OrbitRadius;
pathDesired.StartingVelocity = fixedWingAirspeeds.CruiseSpeed;
pathDesired.EndingVelocity = fixedWingAirspeeds.CruiseSpeed;
PathDesiredSet(&pathDesired);
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
case FLIGHTSTATUS_FLIGHTMODE_TABLETCONTROL:
state = FW_FOLLOWER_RUNNING;
PathDesiredGet(&pathDesired);
switch(pathDesired.Mode) {
case PATHDESIRED_MODE_ENDPOINT:
case PATHDESIRED_MODE_VECTOR:
case PATHDESIRED_MODE_CIRCLERIGHT:
case PATHDESIRED_MODE_CIRCLELEFT:
break;
default:
state = FW_FOLLOWER_ERR;
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
PathStatusSet(&pathStatus);
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_CRITICAL);
break;
}
break;
default:
state = FW_FOLLOWER_IDLE;
break;
}
}
switch(state) {
case FW_FOLLOWER_RUNNING:
{
updatePathVelocity();
uint8_t result = updateFixedDesiredAttitude();
if (result) {
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
} else {
AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING);
}
PathStatusSet(&pathStatus);
break;
}
case FW_FOLLOWER_IDLE:
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
downVelIntegral = 0;
bearingIntegral = 0;
speedIntegral = 0;
accelIntegral = 0;
powerIntegral = 0;
airspeedErrorInt = 0;
AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER);
break;
case FW_FOLLOWER_ERR:
default:
// Leave alarms set above
break;
}
}
}
int32_t LoggingStart(void)
{
DebugLogSettingsConnectCallback(SettingsUpdatedCb);
DebugLogControlConnectCallback(ControlUpdatedCb);
FlightStatusConnectCallback(FlightStatusUpdatedCb);
SettingsUpdatedCb(DebugLogSettingsHandle());
UAVObjEvent ev = {
.obj = DebugLogSettingsHandle(),
.instId = 0,
.event = EV_UPDATED_PERIODIC,
.lowPriority = true,
};
EventPeriodicCallbackCreate(&ev, StatusUpdatedCb, 1000);
// invoke a periodic dispatcher callback - the event struct is a dummy, it could be filled with anything!
StatusUpdatedCb(&ev);
return 0;
}
MODULE_INITCALL(LoggingInitialize, LoggingStart);
static void StatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_DEBUGLOG_Info(&status.Flight, &status.Entry, &status.FreeSlots, &status.UsedSlots);
DebugLogStatusSet(&status);
}
static void FlightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FlightStatusGet(&flightstatus);
if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_ONLYWHENARMED) {
if (flightstatus.Armed != FLIGHTSTATUS_ARMED_ARMED) {
PIOS_DEBUGLOG_Printf("FlightStatus Disarmed: On board Logging disabled.");
PIOS_DEBUGLOG_Enable(0);
} else {
PIOS_DEBUGLOG_Enable(1);
PIOS_DEBUGLOG_Printf("FlightStatus Armed: On board logging enabled.");
}
}
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
DebugLogSettingsGet(&settings);
if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_ALWAYS) {
PIOS_DEBUGLOG_Enable(1);
PIOS_DEBUGLOG_Printf("On board logging enabled.");
} else if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_DISABLED) {
PIOS_DEBUGLOG_Printf("On board logging disabled.");
PIOS_DEBUGLOG_Enable(0);
} else {
FlightStatusUpdatedCb(NULL);
}
}
static void ControlUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
DebugLogControlGet(&control);
if (control.Operation == DEBUGLOGCONTROL_OPERATION_RETRIEVE) {
memset(entry, 0, sizeof(DebugLogEntryData));
if (PIOS_DEBUGLOG_Read(entry, control.Flight, control.Entry) != 0) {
// reading from log failed, mark as non existent in output
entry->Flight = control.Flight;
entry->Entry = control.Entry;
entry->Type = DEBUGLOGENTRY_TYPE_EMPTY;
}
DebugLogEntrySet(entry);
} else if (control.Operation == DEBUGLOGCONTROL_OPERATION_FORMATFLASH) {
uint8_t armed;
FlightStatusArmedGet(&armed);
if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
PIOS_DEBUGLOG_Format();
}
}
StatusUpdatedCb(ev);
}
/**
* 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)
{
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);
}
}
