void vtol_follower_control_settings_updated(UAVObjEvent * ev, void *ctx, void *obj, int len) { (void) ctx; (void) obj; (void) len; VtolPathFollowerSettingsGet(&guidanceSettings); // Configure the velocity control PID loops pid_configure(&vtol_pids[NORTH_VELOCITY], guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], // Kp guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Ki 0, // Kd guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); pid_configure(&vtol_pids[EAST_VELOCITY], guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], // Kp guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Ki 0, // Kd guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); // Configure the position control (velocity output) PID loops pid_configure(&vtol_pids[NORTH_POSITION], guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP], guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], 0, guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); pid_configure(&vtol_pids[EAST_POSITION], guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP], guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], 0, guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); // The parameters for vertical control are shared with Altitude Hold AltitudeHoldSettingsGet(&altitudeHoldSettings); pid_configure(&vtol_pids[DOWN_POSITION], altitudeHoldSettings.PositionKp, 0, 0, 0); pid_configure(&vtol_pids[DOWN_VELOCITY], altitudeHoldSettings.VelocityKp, altitudeHoldSettings.VelocityKi, 0, 1); // Note the ILimit here is 1 because we use this offset to set the throttle offset // Calculate the constants used in the deadband calculation cubic_deadband_setup(guidanceSettings.EndpointDeadbandWidth, guidanceSettings.EndpointDeadbandCenterGain, &vtol_end_m, &vtol_end_r); cubic_deadband_setup(guidanceSettings.PathDeadbandWidth, guidanceSettings.PathDeadbandCenterGain, &vtol_path_m, &vtol_path_r); // calculate the loiter time constants. loiter_brakealpha = expf(-(guidanceSettings.UpdatePeriod / 1000.0f) / guidanceSettings.LoiterBrakingTimeConstant); loiter_errordecayalpha = expf(-(guidanceSettings.UpdatePeriod / 1000.0f) / guidanceSettings.LoiterErrorDecayConstant); }
void vtol_follower_control_settings_updated(UAVObjEvent * ev) { VtolPathFollowerSettingsGet(&guidanceSettings); // Configure the velocity control PID loops pid_configure(&vtol_pids[NORTH_VELOCITY], guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], // Kp guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Ki 0, // Kd guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); pid_configure(&vtol_pids[EAST_VELOCITY], guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], // Kp guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Ki 0, // Kd guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); // Configure the position control (velocity output) PID loops pid_configure(&vtol_pids[NORTH_POSITION], guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP], guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], 0, guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); pid_configure(&vtol_pids[EAST_POSITION], guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP], guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], 0, guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); // The parameters for vertical control are shared with Altitude Hold AltitudeHoldSettingsGet(&altitudeHoldSettings); pid_configure(&vtol_pids[DOWN_POSITION], altitudeHoldSettings.PositionKp, 0, 0, 0); pid_configure(&vtol_pids[DOWN_VELOCITY], altitudeHoldSettings.VelocityKp, altitudeHoldSettings.VelocityKi, 0, 1); // Note the ILimit here is 1 because we use this offset to set the throttle offset // Calculate the constants used in the deadband calculation vtol_deadband_setup(guidanceSettings.EndpointDeadbandWidth, guidanceSettings.EndpointDeadbandCenterGain, &vtol_end_m, &vtol_end_r); vtol_deadband_setup(guidanceSettings.PathDeadbandWidth, guidanceSettings.PathDeadbandCenterGain, &vtol_path_m, &vtol_path_r); }
/** * Module thread, should not return. */ static void altitudeHoldTask(void *parameters) { bool engaged = false; AltitudeHoldDesiredData altitudeHoldDesired; StabilizationDesiredData stabilizationDesired; AltitudeHoldSettingsData altitudeHoldSettings; UAVObjEvent ev; struct pid velocity_pid; // Listen for object updates. AltitudeHoldDesiredConnectQueue(queue); AltitudeHoldSettingsConnectQueue(queue); FlightStatusConnectQueue(queue); AltitudeHoldSettingsGet(&altitudeHoldSettings); pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp, altitudeHoldSettings.VelocityKi, 0.0f, 1.0f); AlarmsSet(SYSTEMALARMS_ALARM_ALTITUDEHOLD, SYSTEMALARMS_ALARM_OK); // Main task loop const uint32_t dt_ms = 5; const float dt_s = dt_ms * 0.001f; uint32_t timeout = dt_ms; while (1) { if (PIOS_Queue_Receive(queue, &ev, timeout) != true) { } else if (ev.obj == FlightStatusHandle()) { uint8_t flight_mode; FlightStatusFlightModeGet(&flight_mode); if (flight_mode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !engaged) { // Copy the current throttle as a starting point for integral StabilizationDesiredThrottleGet(&velocity_pid.iAccumulator); velocity_pid.iAccumulator *= 1000.0f; // pid library scales up accumulator by 1000 engaged = true; } else if (flight_mode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) engaged = false; // Run loop at 20 Hz when engaged otherwise just slowly wait for it to be engaged timeout = engaged ? dt_ms : 100; } else if (ev.obj == AltitudeHoldDesiredHandle()) { AltitudeHoldDesiredGet(&altitudeHoldDesired); } else if (ev.obj == AltitudeHoldSettingsHandle()) { AltitudeHoldSettingsGet(&altitudeHoldSettings); pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp, altitudeHoldSettings.VelocityKi, 0.0f, 1.0f); } bool landing = altitudeHoldDesired.Land == ALTITUDEHOLDDESIRED_LAND_TRUE; // For landing mode allow throttle to go negative to allow the integrals // to stop winding up const float min_throttle = landing ? -0.1f : 0.0f; // When engaged compute altitude controller output if (engaged) { float position_z, velocity_z, altitude_error; PositionActualDownGet(&position_z); VelocityActualDownGet(&velocity_z); position_z = -position_z; // Use positive up convention velocity_z = -velocity_z; // Use positive up convention // Compute the altitude error altitude_error = altitudeHoldDesired.Altitude - position_z; // Velocity desired is from the outer controller plus the set point float velocity_desired = altitude_error * altitudeHoldSettings.PositionKp + altitudeHoldDesired.ClimbRate; float throttle_desired = pid_apply_antiwindup(&velocity_pid, velocity_desired - velocity_z, min_throttle, 1.0f, // positive limits since this is throttle dt_s); if (altitudeHoldSettings.AttitudeComp > 0) { // Throttle desired is at this point the mount desired in the up direction, we can // account for the attitude if desired AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); // Project a unit vector pointing up into the body frame and // get the z component float fraction = attitudeActual.q1 * attitudeActual.q1 - attitudeActual.q2 * attitudeActual.q2 - attitudeActual.q3 * attitudeActual.q3 + attitudeActual.q4 * attitudeActual.q4; // Add ability to scale up the amount of compensation to achieve // level forward flight fraction = powf(fraction, (float) altitudeHoldSettings.AttitudeComp / 100.0f); // Dividing by the fraction remaining in the vertical projection will // attempt to compensate for tilt. This acts like the thrust is linear // with the output which isn't really true. If the fraction is starting // to go negative we are inverted and should shut off throttle throttle_desired = (fraction > 0.1f) ? (throttle_desired / fraction) : 0.0f; } StabilizationDesiredGet(&stabilizationDesired); stabilizationDesired.Throttle = bound_min_max(throttle_desired, min_throttle, 1.0f); if (landing) { stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabilizationDesired.Roll = 0; stabilizationDesired.Pitch = 0; stabilizationDesired.Yaw = 0; } else { stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabilizationDesired.Roll = altitudeHoldDesired.Roll; stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; } StabilizationDesiredSet(&stabilizationDesired); } } }
/** * 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; }