static void SettingsUpdatedCb(UAVObjEvent * ev)
{
memset(pids,0,sizeof (pid_type) * PID_MAX);
StabilizationSettingsGet(&settings);
// Set the roll rate PID constants
pids[0].p = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP];
pids[0].i = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI];
pids[0].d = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD];
pids[0].iLim = settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT];
// Set the pitch rate PID constants
pids[1].p = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP];
pids[1].i = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI];
pids[1].d = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD];
pids[1].iLim = settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT];
// Set the yaw rate PID constants
pids[2].p = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP];
pids[2].i = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI];
pids[2].d = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD];
pids[2].iLim = settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT];
// Set the roll attitude PI constants
pids[3].p = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP];
pids[3].i = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI];
pids[3].iLim = settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT];
// Set the pitch attitude PI constants
pids[4].p = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP];
pids[4].i = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI];
pids[4].iLim = settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT];
// Set the yaw attitude PI constants
pids[5].p = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP];
pids[5].i = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI];
pids[5].iLim = settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT];
// Maximum deviation to accumulate for axis lock
max_axis_lock = settings.MaxAxisLock;
max_axislock_rate = settings.MaxAxisLockRate;
// Settings for weak leveling
weak_leveling_kp = settings.WeakLevelingKp;
weak_leveling_max = settings.MaxWeakLevelingRate;
// Whether to zero the PID integrals while throttle is low
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// The dT has some jitter iteration to iteration that we don't want to
// make thie result unpredictable. Still, it's nicer to specify the constant
// based on a time (in ms) rather than a fixed multiplier. The error between
// update rates on OP (~300 Hz) and CC (~475 Hz) is negligible for this
// calculation
const float fakeDt = 0.0025;
if(settings.GyroTau < 0.0001)
gyro_alpha = 0; // not trusting this to resolve to 0
else
gyro_alpha = exp(-fakeDt / settings.GyroTau);
}
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
{
StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
uint8_t * stab_settings;
switch(cmd->FlightMode) {
case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings;
break;
case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings;
break;
case MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings;
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
break;
}
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
0; // this is an invalid mode
;
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
0; // this is an invalid mode
stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.MaximumRate[STABILIZATIONSETTINGS_MAXIMUMRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) :
0; // this is an invalid mode
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization);
}
static void SettingsUpdatedCb(UAVObjEvent * ev)
{
if (ev == NULL || ev->obj == TrimAnglesSettingsHandle())
{
TrimAnglesSettingsData trimAnglesSettings;
TrimAnglesGet(&trimAngles);
TrimAnglesSettingsGet(&trimAnglesSettings);
// Set the trim angles
trimAngles.Roll = trimAnglesSettings.Roll;
trimAngles.Pitch = trimAnglesSettings.Pitch;
TrimAnglesSet(&trimAngles);
}
if (ev == NULL || ev->obj == StabilizationSettingsHandle())
{
StabilizationSettingsGet(&settings);
// Update the PID settings
calculate_pids();
// Maximum deviation to accumulate for axis lock
max_axis_lock = settings.MaxAxisLock;
max_axislock_rate = settings.MaxAxisLockRate;
// Settings for weak leveling
weak_leveling_kp = settings.WeakLevelingKp;
weak_leveling_max = settings.MaxWeakLevelingRate;
// Whether to zero the PID integrals while throttle is low
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
gyro_filter_updated = true;
}
if (ev == NULL || ev->obj == MWRateSettingsHandle()) {
MWRateSettingsGet(&mwrate_settings);
}
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedActual which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
static void updateVtolDesiredAttitude()
{
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
StabilizationSettingsData stabSettings;
float northError;
float northCommand;
float eastError;
float eastCommand;
float downError;
float downCommand;
VtolPathFollowerSettingsGet(&guidanceSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = velocityActual.North;
float eastVel = velocityActual.East;
float downVel = velocityActual.Down;
// Compute desired north command from velocity error
northError = velocityDesired.North - northVel;
northCommand = pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], northError,
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch, dT) + velocityDesired.North * guidanceSettings.VelocityFeedforward;
// Compute desired east command from velocity error
eastError = velocityDesired.East - eastVel;
eastCommand = pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], eastError,
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch, dT) + velocityDesired.East * guidanceSettings.VelocityFeedforward;
// Compute desired down command. Using NED accel as the damping term
downError = velocityDesired.Down - downVel;
// Negative is critical here since throttle is negative with down
downCommand = -pid_apply_antiwindup(&vtol_pids[DOWN_VELOCITY], downError, -1, 1, dT) +
nedAccel.Down * guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD];
// If this setting is zero then the throttle level available when enabled is used for hover:wq
float used_throttle_offset = (guidanceSettings.HoverThrottle == 0) ? throttleOffset : guidanceSettings.HoverThrottle;
stabDesired.Throttle = bound_min_max(downCommand + used_throttle_offset, 0, 1);
// Project the north and east command signals into the pitch and roll based on yaw.
// For this to behave well the craft should move similarly for 5 deg roll versus 5 deg pitch.
// Notice the inputs are crudely bounded by the anti-winded but if both N and E were
// saturated and the craft were at 45 degrees that would result in a value greater than
// the limit, so apply limit again here.
stabDesired.Pitch = bound_min_max(-northCommand * cosf(attitudeActual.Yaw * DEG2RAD) +
-eastCommand * sinf(attitudeActual.Yaw * DEG2RAD),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
stabDesired.Roll = bound_min_max(-northCommand * sinf(attitudeActual.Yaw * DEG2RAD) +
eastCommand * cosf(attitudeActual.Yaw * DEG2RAD),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
if(guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Throttle = manualControl.Throttle;
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDEPLUS;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDEPLUS;
float yaw;
switch(guidanceSettings.YawMode) {
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_RATE:
/* This is awkward. This allows the transmitter to control the yaw while flying navigation */
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK:
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE:
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.YawMax * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI:
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
break;
}
StabilizationDesiredSet(&stabDesired);
}
/**
* 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);
}
}
}
/**
* Called after measuring roll and pitch to update the
* stabilization settings
*
* takes in @ref RelayTuning and outputs @ref StabilizationSettings
*/
static void update_stabilization_settings()
{
RelayTuningData relayTuning;
RelayTuningGet(&relayTuning);
RelayTuningSettingsData relaySettings;
RelayTuningSettingsGet(&relaySettings);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
// Eventually get these settings from RelayTuningSettings
const float gain_ratio_r = 1.0f / 3.0f;
const float zero_ratio_r = 1.0f / 3.0f;
const float gain_ratio_p = 1.0f / 5.0f;
const float zero_ratio_p = 1.0f / 5.0f;
// For now just run over roll and pitch
for (uint32_t i = 0; i < 2; i++) {
float wu = 1000.0f * 2 * PI / relayTuning.Period[i]; // ultimate freq = output osc freq (rad/s)
float wc = wu * gain_ratio_r; // target openloop crossover frequency (rad/s)
float zc = wc * zero_ratio_r; // controller zero location (rad/s)
float kpu = 4.0f / PI / relayTuning.Gain[i]; // ultimate gain, i.e. the proportional gain for instablity
float kp = kpu * gain_ratio_r; // proportional gain
float ki = zc * kp; // integral gain
// Now calculate gains for the next loop out knowing it is the integral of
// the inner loop -- the plant is position/velocity = scale*1/s
float wc2 = wc * gain_ratio_p; // crossover of the attitude loop
float kp2 = wc2; // kp of attitude
float ki2 = wc2 * zero_ratio_p * kp2; // ki of attitude
switch(i) {
case 0: // roll
stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp;
stabSettings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki;
stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2;
stabSettings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2;
break;
case 1: // Pitch
stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] = kp;
stabSettings.PitchRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI] = ki;
stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KP] = kp2;
stabSettings.PitchPI[STABILIZATIONSETTINGS_ROLLPI_KI] = ki2;
break;
default:
// Oh shit oh shit oh shit
break;
}
}
switch(relaySettings.Behavior) {
case RELAYTUNINGSETTINGS_BEHAVIOR_MEASURE:
// Just measure, don't update the stab settings
break;
case RELAYTUNINGSETTINGS_BEHAVIOR_COMPUTE:
StabilizationSettingsSet(&stabSettings);
break;
case RELAYTUNINGSETTINGS_BEHAVIOR_SAVE:
StabilizationSettingsSet(&stabSettings);
UAVObjSave(StabilizationSettingsHandle(), 0);
break;
}
}
/**
* Module thread, should not return.
*/
static void AutotuneTask(void *parameters)
{
//AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
enum AUTOTUNE_STATE state = AT_INIT;
portTickType lastUpdateTime = xTaskGetTickCount();
while(1) {
PIOS_WDG_UpdateFlag(PIOS_WDG_AUTOTUNE);
// TODO:
// 1. get from queue
// 2. based on whether it is flightstatus or manualcontrol
portTickType diffTime;
const uint32_t PREPARE_TIME = 2000;
const uint32_t MEAURE_TIME = 30000;
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
// Only allow this module to run when autotuning
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
state = AT_INIT;
vTaskDelay(50);
continue;
}
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings);
ManualControlSettingsData manualSettings;
ManualControlSettingsGet(&manualSettings);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
RelayTuningSettingsData relaySettings;
RelayTuningSettingsGet(&relaySettings);
bool rate = relaySettings.Mode == RELAYTUNINGSETTINGS_MODE_RATE;
if (rate) { // rate mode
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Roll = manualControl.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL];
stabDesired.Pitch = manualControl.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH];
} else {
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = manualControl.Roll * stabSettings.RollMax;
stabDesired.Pitch = manualControl.Pitch * stabSettings.PitchMax;
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
stabDesired.Throttle = manualControl.Throttle;
switch(state) {
case AT_INIT:
lastUpdateTime = xTaskGetTickCount();
// Only start when armed and flying
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0)
state = AT_START;
break;
case AT_START:
diffTime = xTaskGetTickCount() - lastUpdateTime;
// Spend the first block of time in normal rate mode to get airborne
if (diffTime > PREPARE_TIME) {
state = AT_ROLL;
lastUpdateTime = xTaskGetTickCount();
}
break;
case AT_ROLL:
diffTime = xTaskGetTickCount() - lastUpdateTime;
// Run relay mode on the roll axis for the measurement time
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE :
STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE;
if (diffTime > MEAURE_TIME) { // Move on to next state
state = AT_PITCH;
lastUpdateTime = xTaskGetTickCount();
}
break;
case AT_PITCH:
diffTime = xTaskGetTickCount() - lastUpdateTime;
// Run relay mode on the pitch axis for the measurement time
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = rate ? STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE :
STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE;
if (diffTime > MEAURE_TIME) { // Move on to next state
state = AT_FINISHED;
lastUpdateTime = xTaskGetTickCount();
}
break;
case AT_FINISHED:
// Wait until disarmed and landed before updating the settings
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0)
state = AT_SET;
break;
case AT_SET:
update_stabilization_settings();
state = AT_INIT;
break;
default:
// Set an alarm or some shit like that
break;
}
StabilizationDesiredSet(&stabDesired);
vTaskDelay(10);
}
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedActual which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
static void updateGroundDesiredAttitude()
{
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
GroundPathFollowerSettingsData guidanceSettings;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
SystemSettingsGet(&systemSettings);
GroundPathFollowerSettingsGet(&guidanceSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = velocityActual.North;
float eastVel = velocityActual.East;
// Calculate direction from velocityDesired and set stabDesired.Yaw
stabDesired.Yaw = atan2f( velocityDesired.East, velocityDesired.North ) * RAD2DEG;
// Calculate throttle and set stabDesired.Throttle
float velDesired = sqrtf(powf(velocityDesired.East,2) + powf(velocityDesired.North,2));
float velActual = sqrtf(powf(eastVel,2) + powf(northVel,2));
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
switch (guidanceSettings.ThrottleControl) {
case GROUNDPATHFOLLOWERSETTINGS_THROTTLECONTROL_MANUAL:
{
stabDesired.Throttle = manualControlData.Throttle;
break;
}
case GROUNDPATHFOLLOWERSETTINGS_THROTTLECONTROL_PROPORTIONAL:
{
float velRatio = velDesired / guidanceSettings.HorizontalVelMax;
stabDesired.Throttle = guidanceSettings.MaxThrottle * velRatio;
if (guidanceSettings.ManualOverride == GROUNDPATHFOLLOWERSETTINGS_MANUALOVERRIDE_TRUE) {
stabDesired.Throttle = stabDesired.Throttle * manualControlData.Throttle;
}
break;
}
case GROUNDPATHFOLLOWERSETTINGS_THROTTLECONTROL_AUTO:
{
float velError = velDesired - velActual;
stabDesired.Throttle = pid_apply(&ground_pids[VELOCITY], velError, dT) + velDesired * guidanceSettings.VelocityFeedforward;
if (guidanceSettings.ManualOverride == GROUNDPATHFOLLOWERSETTINGS_MANUALOVERRIDE_TRUE) {
stabDesired.Throttle = stabDesired.Throttle * manualControlData.Throttle;
}
break;
}
default:
{
PIOS_Assert(0);
break;
}
}
// Limit throttle as per settings
stabDesired.Throttle = bound_min_max(stabDesired.Throttle, 0, guidanceSettings.MaxThrottle);
// Set StabilizationDesired object
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
StabilizationDesiredSet(&stabDesired);
}
/**
* Compute desired attitude from the desired velocity
*
* Takes in @ref NedActual which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
static void updateVtolDesiredAttitude()
{
static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();;
float dT;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
AttitudeDesiredData attitudeDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
GuidanceSettingsData guidanceSettings;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
float northError;
float northCommand;
float eastError;
float eastCommand;
float downError;
float downCommand;
// Check how long since last update
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
SystemSettingsGet(&systemSettings);
GuidanceSettingsGet(&guidanceSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
AttitudeDesiredGet(&attitudeDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
attitudeDesired.Yaw = 0; // try and face north
// Compute desired north command
northError = velocityDesired.North - velocityActual.North;
northIntegral = bound(northIntegral + northError * dT,
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
northCommand = (northError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
northIntegral * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI] -
nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
// Compute desired east command
eastError = velocityDesired.East - velocityActual.East;
eastIntegral = bound(eastIntegral + eastError * dT,
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
eastCommand = (eastError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
eastIntegral * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI] -
nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
// Compute desired down command
downError = velocityDesired.Down - velocityActual.Down;
downIntegral = bound(downIntegral + downError * dT,
-guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT],
guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT]);
downCommand = (downError * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KP] +
downIntegral * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI] -
nedAccel.Down * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]);
attitudeDesired.Throttle = bound(downCommand, 0, 1);
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch
attitudeDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) +
-eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
attitudeDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) +
eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
if(guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
attitudeDesired.Throttle = manualControl.Throttle;
}
AttitudeDesiredSet(&attitudeDesired);
}
/**
* Module task
*/
static void manualControlTask(void *parameters)
{
ManualControlSettingsData settings;
StabilizationSettingsData stabSettings;
ManualControlCommandData cmd;
ActuatorDesiredData actuator;
AttitudeDesiredData attitude;
portTickType lastSysTime;
portTickType armedDisarmStart = 0;
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);
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1) {
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
// Read settings
ManualControlSettingsGet(&settings);
StabilizationSettingsGet(&stabSettings);
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
}
// Calculate roll command in range +1 to -1
cmd.Roll = scaleChannel(cmd.Channel[settings.Roll], settings.ChannelMax[settings.Roll],
settings.ChannelMin[settings.Roll], settings.ChannelNeutral[settings.Roll]);
// Calculate pitch command in range +1 to -1
cmd.Pitch = scaleChannel(cmd.Channel[settings.Pitch], settings.ChannelMax[settings.Pitch],
settings.ChannelMin[settings.Pitch], settings.ChannelNeutral[settings.Pitch]);
// Calculate yaw command in range +1 to -1
cmd.Yaw = scaleChannel(cmd.Channel[settings.Yaw], settings.ChannelMax[settings.Yaw],
settings.ChannelMin[settings.Yaw], settings.ChannelNeutral[settings.Yaw]);
// Calculate throttle command in range +1 to -1
cmd.Throttle = scaleChannel(cmd.Channel[settings.Throttle], settings.ChannelMax[settings.Throttle],
settings.ChannelMin[settings.Throttle], settings.ChannelNeutral[settings.Throttle]);
if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE)
cmd.Accessory1 = scaleChannel(cmd.Channel[settings.Accessory1], settings.ChannelMax[settings.Accessory1],
settings.ChannelMin[settings.Accessory1], settings.ChannelNeutral[settings.Accessory1]);
else
cmd.Accessory1 = 0;
if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE)
cmd.Accessory2 = scaleChannel(cmd.Channel[settings.Accessory2], settings.ChannelMax[settings.Accessory2],
settings.ChannelMin[settings.Accessory2], settings.ChannelNeutral[settings.Accessory2]);
else
cmd.Accessory2 = 0;
if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE)
cmd.Accessory3 = scaleChannel(cmd.Channel[settings.Accessory3], settings.ChannelMax[settings.Accessory3],
settings.ChannelMin[settings.Accessory3], settings.ChannelNeutral[settings.Accessory3]);
else
cmd.Accessory3 = 0;
// Update flight mode
flightMode = scaleChannel(cmd.Channel[settings.FlightMode], settings.ChannelMax[settings.FlightMode],
settings.ChannelMin[settings.FlightMode], settings.ChannelNeutral[settings.FlightMode]);
if (flightMode < -FLIGHT_MODE_LIMIT) { // Position 1
for(int i = 0; i < 3; i++) {
if(settings.Pos1StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE;
else if(settings.Pos1StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE;
else if(settings.Pos1StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE;
}
if(settings.Pos1FlightMode == MANUALCONTROLSETTINGS_POS1FLIGHTMODE_MANUAL)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL;
else if(settings.Pos1FlightMode == MANUALCONTROLSETTINGS_POS1FLIGHTMODE_STABILIZED)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED;
else if(settings.Pos1FlightMode == MANUALCONTROLSETTINGS_POS1FLIGHTMODE_AUTO)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
} else if (flightMode > FLIGHT_MODE_LIMIT) { // Position 3
for(int i = 0; i < 3; i++) {
if(settings.Pos3StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_NONE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE;
else if(settings.Pos3StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_RATE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE;
else if(settings.Pos3StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_ATTITUDE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE;
}
if(settings.Pos3FlightMode == MANUALCONTROLSETTINGS_POS3FLIGHTMODE_MANUAL)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL;
else if(settings.Pos3FlightMode == MANUALCONTROLSETTINGS_POS3FLIGHTMODE_STABILIZED)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED;
else if(settings.Pos3FlightMode == MANUALCONTROLSETTINGS_POS3FLIGHTMODE_AUTO)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
} else { // Position 2
for(int i = 0; i < 3; i++) {
if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_NONE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE;
else if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_RATE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE;
else if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_ATTITUDE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE;
}
if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_MANUAL)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL;
else if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_STABILIZED)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED;
else if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_AUTO)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
}
// 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);
}
/* Look for arm or disarm signal */
if ((cmd.Throttle <= 0.05) && (cmd.Roll <= -0.95)) {
if ((armedDisarmStart == 0) || (lastSysTime < armedDisarmStart)) // store when started, deal with rollover
armedDisarmStart = lastSysTime;
else if ((lastSysTime - armedDisarmStart) > (1000 * portTICK_RATE_MS))
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
} else if ((cmd.Throttle <= 0.05) && (cmd.Roll >= 0.95)) {
if ((armedDisarmStart == 0) || (lastSysTime < armedDisarmStart)) // store when started, deal with rollover
armedDisarmStart = lastSysTime;
else if ((lastSysTime - armedDisarmStart) > (1000 * portTICK_RATE_MS))
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
} else {
armedDisarmStart = 0;
}
// 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;
AttitudeDesiredSet(&attitude);
}
if (cmd.Accessory3 < -.5) { //TODO: Make what happens here depend on GCS
AHRSSettingsData attitudeSettings;
AHRSSettingsGet(&attitudeSettings);
// Hard coding a maximum bias of 15 for now... maybe mistake
attitudeSettings.PitchBias = cmd.Accessory1 * 15;
attitudeSettings.RollBias = cmd.Accessory2 * 15;
AHRSSettingsSet(&attitudeSettings);
}
}
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t TxPIDInitialize(void)
{
bool txPIDEnabled;
HwSettingsOptionalModulesData optionalModules;
#ifdef REVOLUTION
AltitudeHoldSettingsInitialize();
#endif
HwSettingsInitialize();
HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules.TxPID == HWSETTINGS_OPTIONALMODULES_ENABLED) {
txPIDEnabled = true;
} else {
txPIDEnabled = false;
}
if (txPIDEnabled) {
TxPIDSettingsInitialize();
TxPIDStatusInitialize();
AccessoryDesiredInitialize();
UAVObjEvent ev = {
.obj = AccessoryDesiredHandle(),
.instId = 0,
.event = 0,
.lowPriority = false,
};
EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
#if (TELEMETRY_UPDATE_PERIOD_MS != 0)
// Change StabilizationSettings update rate from OnChange to periodic
// to prevent telemetry link flooding with frequent updates in case of
// control channel jitter.
// Warning: saving to flash with this code active will change the
// StabilizationSettings update rate permanently. Use Metadata via
// browser to reset to defaults (telemetryAcked=true, OnChange).
UAVObjMetadata metadata;
StabilizationSettingsInitialize();
StabilizationSettingsGetMetadata(&metadata);
metadata.telemetryAcked = 0;
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
StabilizationSettingsSetMetadata(&metadata);
AttitudeSettingsInitialize();
AttitudeSettingsGetMetadata(&metadata);
metadata.telemetryAcked = 0;
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
AttitudeSettingsSetMetadata(&metadata);
#endif /* if (TELEMETRY_UPDATE_PERIOD_MS != 0) */
return 0;
}
return -1;
}
/* stub: module has no module thread */
int32_t TxPIDStart(void)
{
return 0;
}
MODULE_INITCALL(TxPIDInitialize, TxPIDStart);
/**
* Update PIDs callback function
*/
static void updatePIDs(UAVObjEvent *ev)
{
if (ev->obj != AccessoryDesiredHandle()) {
return;
}
TxPIDSettingsData inst;
TxPIDSettingsGet(&inst);
if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) {
return;
}
uint8_t armed;
FlightStatusArmedGet(&armed);
if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
(armed == FLIGHTSTATUS_ARMED_DISARMED)) {
return;
}
StabilizationBankData bank;
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank);
break;
default:
return;
}
StabilizationSettingsData stab;
StabilizationSettingsGet(&stab);
AttitudeSettingsData att;
AttitudeSettingsGet(&att);
#ifdef REVOLUTION
AltitudeHoldSettingsData altitude;
AltitudeHoldSettingsGet(&altitude);
#endif
AccessoryDesiredData accessory;
TxPIDStatusData txpid_status;
TxPIDStatusGet(&txpid_status);
bool easyTuneEnabled = false;
uint8_t needsUpdateBank = 0;
uint8_t needsUpdateStab = 0;
uint8_t needsUpdateAtt = 0;
#ifdef REVOLUTION
uint8_t needsUpdateAltitude = 0;
#endif
// Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
if (TxPIDSettingsPIDsToArray(inst.PIDs)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
float value;
if (TxPIDSettingsInputsToArray(inst.Inputs)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
ManualControlCommandThrottleGet(&value);
value = scale(value,
inst.ThrottleRange.Min,
inst.ThrottleRange.Max,
TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
} else if (AccessoryDesiredInstGet(
TxPIDSettingsInputsToArray(inst.Inputs)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0,
&accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0f, 1.0f,
TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
} else {
continue;
}
TxPIDStatusCurPIDToArray(txpid_status.CurPID)[i] = value;
switch (TxPIDSettingsPIDsToArray(inst.PIDs)[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_EASYTUNERATEROLL:
easyTuneEnabled = true;
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.RollRatePID.Ki, value * inst.EasyTunePitchRollRateFactors.I);
needsUpdateBank |= update(&bank.RollRatePID.Kd, value * inst.EasyTunePitchRollRateFactors.D);
break;
case TXPIDSETTINGS_PIDS_EASYTUNERATEPITCH:
easyTuneEnabled = true;
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value * inst.EasyTunePitchRollRateFactors.I);
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value * inst.EasyTunePitchRollRateFactors.D);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI:
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD:
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATERESP:
needsUpdateBank |= updateUint16(&bank.ManualRate.Roll, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
needsUpdateBank |= update(&bank.RollPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.RollMax, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI:
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD:
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATERESP:
needsUpdateBank |= updateUint16(&bank.ManualRate.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.PitchMax, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATERESP:
needsUpdateBank |= updateUint16(&bank.ManualRate.Roll, value);
needsUpdateBank |= updateUint16(&bank.ManualRate.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
needsUpdateBank |= update(&bank.RollPI.Ki, value);
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.RollMax, value);
needsUpdateBank |= updateUint8(&bank.PitchMax, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKI:
needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKD:
needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATERESP:
needsUpdateBank |= updateUint16(&bank.ManualRate.Yaw, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdateBank |= update(&bank.YawPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
needsUpdateBank |= update(&bank.YawPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdateBank |= update(&bank.YawPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.YawMax, value);
break;
case TXPIDSETTINGS_PIDS_ROLLEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
break;
case TXPIDSETTINGS_PIDS_PITCHEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_YAWEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Yaw, value);
break;
case TXPIDSETTINGS_PIDS_GYROTAU:
needsUpdateStab |= update(&stab.GyroTau, value);
break;
case TXPIDSETTINGS_PIDS_ACROROLLFACTOR:
needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Roll, value);
break;
case TXPIDSETTINGS_PIDS_ACROPITCHFACTOR:
needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ACROROLLPITCHFACTOR:
needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Roll, value);
needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ACCELTAU:
needsUpdateAtt |= update(&att.AccelTau, value);
break;
case TXPIDSETTINGS_PIDS_ACCELKP:
needsUpdateAtt |= update(&att.AccelKp, value);
break;
case TXPIDSETTINGS_PIDS_ACCELKI:
needsUpdateAtt |= update(&att.AccelKi, value);
break;
#ifdef REVOLUTION
case TXPIDSETTINGS_PIDS_ALTITUDEPOSKP:
needsUpdateAltitude |= update(&altitude.VerticalPosP, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKP:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKI:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKD:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYBETA:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Beta, value);
break;
#endif
default:
PIOS_Assert(0);
}
}
}
if (needsUpdateStab) {
StabilizationSettingsSet(&stab);
}
if (needsUpdateAtt) {
AttitudeSettingsSet(&att);
}
#ifdef REVOLUTION
if (needsUpdateAltitude) {
AltitudeHoldSettingsSet(&altitude);
}
#endif
if (easyTuneEnabled && (inst.EasyTuneRatePIDRecalculateYaw != TXPIDSETTINGS_EASYTUNERATEPIDRECALCULATEYAW_FALSE)) {
float newKp = (bank.RollRatePID.Kp + bank.PitchRatePID.Kp) * .5f * inst.EasyTuneYawRateFactors.P;
needsUpdateBank |= update(&bank.YawRatePID.Kp, newKp);
needsUpdateBank |= update(&bank.YawRatePID.Ki, newKp * inst.EasyTuneYawRateFactors.I);
needsUpdateBank |= update(&bank.YawRatePID.Kd, newKp * inst.EasyTuneYawRateFactors.D);
}
if (needsUpdateBank) {
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *)&bank);
break;
default:
return;
}
}
if (needsUpdateStab ||
needsUpdateAtt ||
#ifdef REVOLUTION
needsUpdateAltitude ||
#endif /* REVOLUTION */
needsUpdateBank) {
TxPIDStatusSet(&txpid_status);;
}
}
/**
* Scales input val from [inMin..inMax] range to [outMin..outMax].
* If val is out of input range (inMin <= inMax), it will be bound.
* (outMin > outMax) is ok, in that case output will be decreasing.
*
* \returns scaled value
*/
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
{
// bound input value
if (val > inMax) {
val = inMax;
}
if (val < inMin) {
val = inMin;
}
// normalize input value to [0..1]
if (inMax <= inMin) {
val = 0.0f;
} else {
val = (val - inMin) / (inMax - inMin);
}
// update output bounds
if (outMin > outMax) {
float t = outMin;
outMin = outMax;
outMax = t;
val = 1.0f - val;
}
return (outMax - outMin) * val + outMin;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static uint8_t update(float *var, float val)
{
/* FIXME: this is not an entirely correct way
* to check if the two floating point
* numbers are 'not equal'.
* Epsilon of 1e-9 is probably okay for the range
* of numbers we see here*/
if (fabsf(*var - val) > 1e-9f) {
*var = val;
return 1;
}
return 0;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static uint8_t updateUint16(uint16_t *var, float val)
{
uint16_t roundedVal = (uint16_t)roundf(val);
if (*var != roundedVal) {
*var = roundedVal;
return 1;
}
return 0;
}
/**
* Module task
*/
static void stabilizationTask(void* parameters)
{
StabilizationSettingsData stabSettings;
ActuatorDesiredData actuatorDesired;
AttitudeDesiredData attitudeDesired;
AttitudeActualData attitudeActual;
ManualControlCommandData manualControl;
SystemSettingsData systemSettings;
portTickType lastSysTime;
float pitchErrorGlobal, pitchErrorLastGlobal;
float yawErrorGlobal, yawErrorLastGlobal;
float pitchError, pitchErrorLast;
float yawError, yawErrorLast;
float rollError, rollErrorLast;
float pitchDerivative;
float yawDerivative;
float rollDerivative;
float pitchIntegral, pitchIntegralLimit;
float yawIntegral, yawIntegralLimit;
float rollIntegral, rollIntegralLimit;
// Initialize
pitchIntegral = 0.0;
yawIntegral = 0.0;
rollIntegral = 0.0;
pitchErrorLastGlobal = 0.0;
yawErrorLastGlobal = 0.0;
rollErrorLast = 0.0;
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1)
{
// Read settings and other objects
StabilizationSettingsGet(&stabSettings);
SystemSettingsGet(&systemSettings);
ManualControlCommandGet(&manualControl);
AttitudeDesiredGet(&attitudeDesired);
AttitudeActualGet(&attitudeActual);
// For all three axis, calculate Error and ErrorLast - translating from global to local reference frame.
// global pitch error
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
{
pitchErrorGlobal = angleDifference(
bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch
);
}
else
{
// in AUTO mode, Stabilization is used to steer the plane freely,
// while Navigation dictates the flight path, including maneuvers outside stable limits.
pitchErrorGlobal = angleDifference(attitudeDesired.Pitch - attitudeActual.Pitch);
}
// global yaw error
if ( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL || manualControl.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
{
// VTOLS consider yaw. AUTO mode considers YAW, too.
yawErrorGlobal = angleDifference(attitudeDesired.Yaw - attitudeActual.Yaw);
} else {
// FIXED WING STABILIZATION however does not.
yawErrorGlobal = 0;
}
// local pitch error
pitchError = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal;
// local roll error (no translation needed - always local)
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
{
rollError = angleDifference(
bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll
);
}
else
{
// in AUTO mode, Stabilization is used to steer the plane freely,
// while Navigation dictates the flight path, including maneuvers outside stable limits.
rollError = angleDifference(attitudeDesired.Roll - attitudeActual.Roll);
}
// local yaw error
yawError = cos(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal;
// for the derivative, the local last errors are needed. Therefore global lasts are translated into local lasts
// pitch last
pitchErrorLast = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal;
// yaw last
yawErrorLast = cos(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal;
// global last variables are no longer needed
pitchErrorLastGlobal = pitchErrorGlobal;
yawErrorLastGlobal = yawErrorGlobal;
// local Pitch stabilization control loop
pitchDerivative = pitchError - pitchErrorLast;
pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi;
pitchIntegral = bound(pitchIntegral+pitchError*stabSettings.UpdatePeriod, -pitchIntegralLimit, pitchIntegralLimit);
actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);
// local Roll stabilization control loop
rollDerivative = rollError - rollErrorLast;
rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi;
rollIntegral = bound(rollIntegral+rollError*stabSettings.UpdatePeriod, -rollIntegralLimit, rollIntegralLimit);
actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
rollErrorLast = rollError;
sdf;
// local Yaw stabilization control loop (only enabled on VTOL airframes)
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
{
yawDerivative = yawError - yawErrorLast;
yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
yawIntegral = bound(yawIntegral+yawError*stabSettings.UpdatePeriod, -yawIntegralLimit, yawIntegralLimit);
actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
}
else
{
actuatorDesired.Yaw = 0.0;
}
// Setup throttle
actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax);
// Write actuator desired (if not in manual mode)
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )
{
ActuatorDesiredSet(&actuatorDesired);
}
else
{
pitchIntegral = 0.0;
yawIntegral = 0.0;
rollIntegral = 0.0;
pitchErrorLastGlobal = 0.0;
yawErrorLastGlobal = 0.0;
rollErrorLast = 0.0;
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
// Wait until next update
vTaskDelayUntil(&lastSysTime, stabSettings.UpdatePeriod / portTICK_RATE_MS );
}
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t TxPIDInitialize(void)
{
bool module_enabled;
#ifdef MODULE_TxPID_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_TXPID] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
if (module_enabled) {
TxPIDSettingsInitialize();
AccessoryDesiredInitialize();
UAVObjEvent ev = {
.obj = AccessoryDesiredHandle(),
.instId = 0,
.event = 0,
};
EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS);
#if (TELEMETRY_UPDATE_PERIOD_MS != 0)
// Change StabilizationSettings update rate from OnChange to periodic
// to prevent telemetry link flooding with frequent updates in case of
// control channel jitter.
// Warning: saving to flash with this code active will change the
// StabilizationSettings update rate permanently. Use Metadata via
// browser to reset to defaults (telemetryAcked=true, OnChange).
UAVObjMetadata metadata;
StabilizationSettingsInitialize();
StabilizationSettingsGetMetadata(&metadata);
metadata.telemetryAcked = 0;
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
StabilizationSettingsSetMetadata(&metadata);
#endif
return 0;
}
return -1;
}
MODULE_INITCALL(TxPIDInitialize, NULL);
/**
* Update PIDs callback function
*/
static void updatePIDs(UAVObjEvent* ev)
{
if (ev->obj != AccessoryDesiredHandle())
return;
TxPIDSettingsData inst;
TxPIDSettingsGet(&inst);
if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER)
return;
uint8_t armed;
FlightStatusArmedGet(&armed);
if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
(armed == FLIGHTSTATUS_ARMED_DISARMED))
return;
StabilizationSettingsData stab;
StabilizationSettingsGet(&stab);
#ifdef UAVOBJ_INIT_vtolpathfollowersettings
VtolPathFollowerSettingsData vtolPathFollowerSettingsData;
// Check to make sure the settings UAVObject has been instantiated
if (VtolPathFollowerSettingsHandle()) {
VtolPathFollowerSettingsGet(&vtolPathFollowerSettingsData);
}
bool vtolPathFollowerSettingsNeedsUpdate = false;
#endif
AccessoryDesiredData accessory;
bool stabilizationSettingsNeedsUpdate = false;
// Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
if (inst.PIDs[i] != TXPIDSETTINGS_PIDS_DISABLED) {
float value;
if (inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
ManualControlCommandThrottleGet(&value);
value = scale(value,
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN],
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX],
inst.MinPID[i], inst.MaxPID[i]);
} else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0, 1.0, inst.MinPID[i], inst.MaxPID[i]);
} else {
continue;
}
switch (inst.PIDs[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKD:
stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_GYROCUTOFF:
stabilizationSettingsNeedsUpdate |= update(&stab.GyroCutoff, value);
break;
case TXPIDSETTINGS_PIDS_ROLLVBARSENSITIVITY:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_ROLL], value);
break;
case TXPIDSETTINGS_PIDS_PITCHVBARSENSITIVITY:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_PITCH], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHVBARSENSITIVITY:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_ROLL], value);
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_PITCH], value);
break;
case TXPIDSETTINGS_PIDS_YAWVBARSENSITIVITY:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_YAW], value);
break;
case TXPIDSETTINGS_PIDS_ROLLVBARKP:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLVBARKI:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLVBARKD:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_PITCHVBARKP:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_PITCHVBARKI:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_PITCHVBARKD:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKP:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KP], value);
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKI:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KI], value);
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKD:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KD], value);
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_YAWVBARKP:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_YAWVBARKI:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_YAWVBARKD:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KD], value);
break;
#ifdef UAVOBJ_INIT_vtolpathfollowersettings
case TXPIDSETTINGS_PIDS_HORIZONTALPOSKP:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALPOSKI:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALPOSILIMIT:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALVELKP:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALVELKI:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALVELKD:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD], value);
break;
#endif /* UAVOBJ_INIT_vtolpathfollowersettings */
default:
// Previously this would assert. But now the
// object may be missing and it's not worth a
// crash.
break;
}
}
}
// Update UAVOs, if necessary
if (stabilizationSettingsNeedsUpdate) {
StabilizationSettingsSet(&stab);
}
#ifdef UAVOBJ_INIT_vtolpathfollowersettings
if (vtolPathFollowerSettingsNeedsUpdate) {
// Check to make sure the settings UAVObject has been instantiated
if (VtolPathFollowerSettingsHandle()) {
VtolPathFollowerSettingsSet(&vtolPathFollowerSettingsData);
}
}
#endif
}
/**
* Scales input val from [inMin..inMax] range to [outMin..outMax].
* If val is out of input range (inMin <= inMax), it will be bound.
* (outMin > outMax) is ok, in that case output will be decreasing.
*
* \returns scaled value
*/
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
{
// bound input value
if (val > inMax) val = inMax;
if (val < inMin) val = inMin;
// normalize input value to [0..1]
if (inMax <= inMin)
val = 0.0;
else
val = (val - inMin) / (inMax - inMin);
// update output bounds
if (outMin > outMax) {
float t = outMin;
outMin = outMax;
outMax = t;
val = 1.0f - val;
}
return (outMax - outMin) * val + outMin;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static bool update(float *var, float val)
{
if (*var != val) {
*var = val;
return true;
}
return false;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t TxPIDInitialize(void)
{
bool txPIDEnabled;
HwSettingsOptionalModulesData optionalModules;
HwSettingsInitialize();
HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules.TxPID == HWSETTINGS_OPTIONALMODULES_ENABLED) {
txPIDEnabled = true;
} else {
txPIDEnabled = false;
}
if (txPIDEnabled) {
TxPIDSettingsInitialize();
AccessoryDesiredInitialize();
UAVObjEvent ev = {
.obj = AccessoryDesiredHandle(),
.instId = 0,
.event = 0,
.lowPriority = false,
};
EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
#if (TELEMETRY_UPDATE_PERIOD_MS != 0)
// Change StabilizationSettings update rate from OnChange to periodic
// to prevent telemetry link flooding with frequent updates in case of
// control channel jitter.
// Warning: saving to flash with this code active will change the
// StabilizationSettings update rate permanently. Use Metadata via
// browser to reset to defaults (telemetryAcked=true, OnChange).
UAVObjMetadata metadata;
StabilizationSettingsInitialize();
StabilizationSettingsGetMetadata(&metadata);
metadata.telemetryAcked = 0;
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
StabilizationSettingsSetMetadata(&metadata);
#endif
return 0;
}
return -1;
}
/* stub: module has no module thread */
int32_t TxPIDStart(void)
{
return 0;
}
MODULE_INITCALL(TxPIDInitialize, TxPIDStart);
/**
* Update PIDs callback function
*/
static void updatePIDs(UAVObjEvent *ev)
{
if (ev->obj != AccessoryDesiredHandle()) {
return;
}
TxPIDSettingsData inst;
TxPIDSettingsGet(&inst);
if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) {
return;
}
uint8_t armed;
FlightStatusArmedGet(&armed);
if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
(armed == FLIGHTSTATUS_ARMED_DISARMED)) {
return;
}
StabilizationBankData bank;
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
break;
default:
return;
}
StabilizationSettingsData stab;
StabilizationSettingsGet(&stab);
AccessoryDesiredData accessory;
uint8_t needsUpdateBank = 0;
uint8_t needsUpdateStab = 0;
// Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
if (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
float value;
if (cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
ManualControlCommandThrottleGet(&value);
value = scale(value,
inst.ThrottleRange.Min,
inst.ThrottleRange.Max,
cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i],
cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]);
} else if (AccessoryDesiredInstGet(
cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0,
&accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0f, 1.0f,
cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i],
cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]);
} else {
continue;
}
switch (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI:
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD:
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
needsUpdateBank |= update(&bank.RollPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI:
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD:
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
needsUpdateBank |= update(&bank.RollPI.Ki, value);
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKI:
needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKD:
needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdateBank |= update(&bank.YawPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
needsUpdateBank |= update(&bank.YawPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdateBank |= update(&bank.YawPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_GYROTAU:
needsUpdateStab |= update(&stab.GyroTau, value);
break;
default:
PIOS_Assert(0);
}
}
}
if (needsUpdateStab) {
StabilizationSettingsSet(&stab);
}
if (needsUpdateBank) {
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *)&bank);
break;
default:
return;
}
}
}
/**
* Scales input val from [inMin..inMax] range to [outMin..outMax].
* If val is out of input range (inMin <= inMax), it will be bound.
* (outMin > outMax) is ok, in that case output will be decreasing.
*
* \returns scaled value
*/
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
{
// bound input value
if (val > inMax) {
val = inMax;
}
if (val < inMin) {
val = inMin;
}
// normalize input value to [0..1]
if (inMax <= inMin) {
val = 0.0f;
} else {
val = (val - inMin) / (inMax - inMin);
}
// update output bounds
if (outMin > outMax) {
float t = outMin;
outMin = outMax;
outMax = t;
val = 1.0f - val;
}
return (outMax - outMin) * val + outMin;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static uint8_t update(float *var, float val)
{
/* FIXME: this is not an entirely correct way
* to check if the two floating point
* numbers are 'not equal'.
* Epsilon of 1e-9 is probably okay for the range
* of numbers we see here*/
if (fabsf(*var - val) > 1e-9f) {
*var = val;
return 1;
}
return 0;
}
/**
* Compute desired attitude from the desired velocity
* @param[in] dT the time since last evaluation
* @param[in] att_adj an adjustment to the attitude for loiter mode
*
* Takes in @ref NedActual which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
int32_t vtol_follower_control_attitude(float dT, const float *att_adj)
{
vtol_follower_control_accel(dT);
float default_adj[2] = {0,0};
if (!att_adj) {
att_adj = default_adj;
}
AccelDesiredData accelDesired;
AccelDesiredGet(&accelDesired);
StabilizationSettingsData stabSet;
StabilizationSettingsGet(&stabSet);
float northCommand = accelDesired.North;
float eastCommand = accelDesired.East;
// Project the north and east acceleration signals into body frame
float yaw;
AttitudeActualYawGet(&yaw);
float forward_accel_desired = -northCommand * cosf(yaw * DEG2RAD) + -eastCommand * sinf(yaw * DEG2RAD);
float right_accel_desired = -northCommand * sinf(yaw * DEG2RAD) + eastCommand * cosf(yaw * DEG2RAD);
StabilizationDesiredData stabDesired;
// Set the angle that would achieve the desired acceleration given the thrust is enough for a hover
stabDesired.Pitch = bound_sym(RAD2DEG * atanf(forward_accel_desired / GRAVITY), guidanceSettings.MaxRollPitch) + att_adj[1];
stabDesired.Roll = bound_sym(RAD2DEG * atanf(right_accel_desired / GRAVITY), guidanceSettings.MaxRollPitch) + att_adj[0];
// Re-bound based on maximum attitude settings
stabDesired.Pitch = bound_sym(stabDesired.Pitch, stabSet.PitchMax);
stabDesired.Roll = bound_sym(stabDesired.Roll, stabSet.RollMax);
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
// Calculate the throttle setting or use pass through from transmitter
if (guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
ManualControlCommandThrottleGet(&stabDesired.Throttle);
} else {
float downCommand = vtol_follower_control_altitude(accelDesired.Down);
stabDesired.Throttle = bound_min_max(downCommand, 0, 1);
}
// Various ways to control the yaw that are essentially manual passthrough. However, because we do not have a fine
// grained mechanism of manual setting the yaw as it normally would we need to duplicate that code here
switch(guidanceSettings.YawMode) {
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_RATE:
/* This is awkward. This allows the transmitter to control the yaw while flying navigation */
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSet.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK:
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSet.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE:
{
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSet.YawMax * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
}
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_PATH:
{
// Face forward on the path
VelocityDesiredData velocityDesired;
VelocityDesiredGet(&velocityDesired);
float total_vel2 = velocityDesired.East*velocityDesired.East + velocityDesired.North*velocityDesired.North;
float path_direction = atan2f(velocityDesired.East, velocityDesired.North) * RAD2DEG;
if (total_vel2 > 1) {
stabDesired.Yaw = path_direction;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
} else {
stabDesired.Yaw = 0;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
}
}
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI:
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
break;
}
StabilizationDesiredSet(&stabDesired);
return 0;
}
/**
* Module task
*/
static void stabilizationTask(void* parameters)
{
UAVObjEvent ev;
StabilizationSettingsData stabSettings;
ActuatorDesiredData actuatorDesired;
AttitudeDesiredData attitudeDesired;
AttitudeActualData attitudeActual;
ManualControlCommandData manualControl;
SystemSettingsData systemSettings;
portTickType lastSysTime;
portTickType thisSysTime;
float pitchErrorGlobal, pitchErrorLastGlobal;
float yawErrorGlobal, yawErrorLastGlobal;
float pitchError, pitchErrorLast;
float yawError, yawErrorLast;
float rollError, rollErrorLast;
float pitchDerivative;
float yawDerivative;
float rollDerivative;
float pitchIntegral, pitchIntegralLimit;
float yawIntegral, yawIntegralLimit;
float rollIntegral, rollIntegralLimit;
float yawPrevious;
float yawChange;
// Initialize
pitchIntegral = 0.0;
yawIntegral = 0.0;
rollIntegral = 0.0;
pitchErrorLastGlobal = 0.0;
yawErrorLastGlobal = 0.0;
rollErrorLast = 0.0;
yawPrevious = 0.0;
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1)
{
// Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
if ( xQueueReceive(queue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE )
{
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING);
}
// Check how long since last update
thisSysTime = xTaskGetTickCount();
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
// Read settings and other objects
StabilizationSettingsGet(&stabSettings);
SystemSettingsGet(&systemSettings);
ManualControlCommandGet(&manualControl);
AttitudeDesiredGet(&attitudeDesired);
AttitudeActualGet(&attitudeActual);
// For all three axis, calculate Error and ErrorLast - translating from global to local reference frame.
// global pitch error
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
{
pitchErrorGlobal = angleDifference(
bound(attitudeDesired.Pitch, -stabSettings.PitchMax, stabSettings.PitchMax) - attitudeActual.Pitch
);
}
else
{
// in AUTO mode, Stabilization is used to steer the plane freely,
// while Navigation dictates the flight path, including maneuvers outside stable limits.
pitchErrorGlobal = angleDifference(attitudeDesired.Pitch - attitudeActual.Pitch);
}
// global yaw error
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
{
// VTOLS consider yaw. AUTO mode considers YAW, too.
if(stabSettings.YawMode == STABILIZATIONSETTINGS_YAWMODE_RATE) { // rate stabilization on yaw
yawChange = (attitudeActual.Yaw - yawPrevious) / dT;
yawPrevious = attitudeActual.Yaw;
yawErrorGlobal = angleDifference(bound(attitudeDesired.Yaw, -stabSettings.YawMax, stabSettings.YawMax) - yawChange);
} else { // heading stabilization
yawError = angleDifference(attitudeDesired.Yaw - attitudeActual.Yaw);
}
} else {
// FIXED WING STABILIZATION however does not.
yawErrorGlobal = 0;
}
// local pitch error
pitchError = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal;
// local roll error (no translation needed - always local)
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO )
{
rollError = angleDifference(
bound(attitudeDesired.Roll, -stabSettings.RollMax, stabSettings.RollMax) - attitudeActual.Roll
);
}
else
{
// in AUTO mode, Stabilization is used to steer the plane freely,
// while Navigation dictates the flight path, including maneuvers outside stable limits.
rollError = angleDifference(attitudeDesired.Roll - attitudeActual.Roll);
}
// local yaw error
yawError = cos(DEG2RAD * attitudeActual.Roll) * yawErrorGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorGlobal;
// for the derivative, the local last errors are needed. Therefore global lasts are translated into local lasts
// pitch last
pitchErrorLast = cos(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal;
// yaw last
yawErrorLast = cos(DEG2RAD * attitudeActual.Roll) * yawErrorLastGlobal + sin(DEG2RAD * attitudeActual.Roll) * pitchErrorLastGlobal;
// global last variables are no longer needed
pitchErrorLastGlobal = pitchErrorGlobal;
yawErrorLastGlobal = yawErrorGlobal;
// local Pitch stabilization control loop
pitchDerivative = pitchError - pitchErrorLast;
pitchIntegralLimit = PITCH_INTEGRAL_LIMIT / stabSettings.PitchKi;
pitchIntegral = bound(pitchIntegral+pitchError*dT, -pitchIntegralLimit, pitchIntegralLimit);
actuatorDesired.Pitch = stabSettings.PitchKp*pitchError + stabSettings.PitchKi*pitchIntegral + stabSettings.PitchKd*pitchDerivative;
actuatorDesired.Pitch = bound(actuatorDesired.Pitch, -1.0, 1.0);
// local Roll stabilization control loop
rollDerivative = rollError - rollErrorLast;
rollIntegralLimit = ROLL_INTEGRAL_LIMIT / stabSettings.RollKi;
rollIntegral = bound(rollIntegral+rollError*dT, -rollIntegralLimit, rollIntegralLimit);
actuatorDesired.Roll = stabSettings.RollKp*rollError + stabSettings.RollKi*rollIntegral + stabSettings.RollKd*rollDerivative;
actuatorDesired.Roll = bound(actuatorDesired.Roll, -1.0, 1.0);
rollErrorLast = rollError;
// local Yaw stabilization control loop (only enabled on VTOL airframes)
if (( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_VTOL )||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) ||
( systemSettings.AirframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP))
{
yawDerivative = yawError - yawErrorLast;
yawIntegralLimit = YAW_INTEGRAL_LIMIT / stabSettings.YawKi;
yawIntegral = bound(yawIntegral+yawError*dT, -yawIntegralLimit, yawIntegralLimit);
actuatorDesired.Yaw = stabSettings.YawKp*yawError + stabSettings.YawKi*yawIntegral + stabSettings.YawKd*yawDerivative;;
actuatorDesired.Yaw = bound(actuatorDesired.Yaw, -1.0, 1.0);
}
else
{
actuatorDesired.Yaw = 0.0;
}
// Setup throttle
actuatorDesired.Throttle = bound(attitudeDesired.Throttle, 0.0, stabSettings.ThrottleMax);
// Save dT
actuatorDesired.UpdateTime = dT * 1000;
// Write actuator desired (if not in manual mode)
if ( manualControl.FlightMode != MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL )
{
ActuatorDesiredSet(&actuatorDesired);
}
else
{
pitchIntegral = 0.0;
yawIntegral = 0.0;
rollIntegral = 0.0;
pitchErrorLastGlobal = 0.0;
yawErrorLastGlobal = 0.0;
rollErrorLast = 0.0;
}
// Clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
