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