diff --git a/flight/Libraries/math/pid.c b/flight/Libraries/math/pid.c index eaf4ab4b5f..e5869e49d2 100644 --- a/flight/Libraries/math/pid.c +++ b/flight/Libraries/math/pid.c @@ -43,11 +43,12 @@ static float deriv_gamma = 1.0; * Update the PID computation * @param[in] pid The PID struture which stores temporary information * @param[in] err The error term - * @param[in] dT The time step * @returns Output the computed controller value */ -float pid_apply(struct pid *pid, const float err, float dT) -{ +float pid_apply(struct pid *pid, const float err) +{ + float dT = pid->dT; + if (pid->i == 0) { // If Ki is zero, do not change the integrator. We do not reset to zero // because sometimes the accumulator term is set externally @@ -82,8 +83,10 @@ float pid_apply(struct pid *pid, const float err, float dT) * chapter. */ float pid_apply_antiwindup(struct pid *pid, const float err, - float min_bound, float max_bound, float dT) -{ + float min_bound, float max_bound) +{ + float dT = pid->dT; + if (pid->i == 0) { // If Ki is zero, do not change the integrator. We do not reset to zero // because sometimes the accumulator term is set externally @@ -123,15 +126,16 @@ float pid_apply_antiwindup(struct pid *pid, const float err, * @param[in] pid The PID struture which stores temporary information * @param[in] setpoint The setpoint to use * @param[in] measured The measured value of output - * @param[in] dT The time step * @returns Output the computed controller value * * This version of apply uses setpoint weighting for the derivative component so the gain * on the gyro derivative can be different than the gain on the setpoint derivative */ float pid_apply_setpoint(struct pid *pid, struct pid_deadband *deadband, const float setpoint, - const float measured, float dT) + const float measured) { + float dT = pid->dT; + float err = setpoint - measured; float err_d = (deriv_gamma * setpoint - measured); @@ -196,7 +200,7 @@ void pid_configure_derivative(float cutoff, float g) * @param[in] i The integral term * @param[in] d The derivative term */ -void pid_configure(struct pid *pid, float p, float i, float d, float iLim) +void pid_configure(struct pid *pid, float p, float i, float d, float iLim, float dT) { if (!pid) return; @@ -205,6 +209,7 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim) pid->i = i; pid->d = d; pid->iLim = iLim; + pid->dT = dT; } /** diff --git a/flight/Libraries/math/pid.h b/flight/Libraries/math/pid.h index 1b4efce9f1..05ecaf64d6 100644 --- a/flight/Libraries/math/pid.h +++ b/flight/Libraries/math/pid.h @@ -44,17 +44,20 @@ struct pid { float i; float d; float iLim; + + float dT; + float iAccumulator; float lastErr; float lastDer; }; //! Methods to use the pid structures -float pid_apply(struct pid *pid, const float err, float dT); -float pid_apply_antiwindup(struct pid *pid, const float err, float min_bound, float max_bound, float dT); -float pid_apply_setpoint(struct pid *pid, struct pid_deadband *deadband, const float setpoint, const float measured, float dT); +float pid_apply(struct pid *pid, const float err); +float pid_apply_antiwindup(struct pid *pid, const float err, float min_bound, float max_bound); +float pid_apply_setpoint(struct pid *pid, struct pid_deadband *deadband, const float setpoint, const float measured); void pid_zero(struct pid *pid); -void pid_configure(struct pid *pid, float p, float i, float d, float iLim); +void pid_configure(struct pid *pid, float p, float i, float d, float iLim, float dT); void pid_configure_derivative(float cutoff, float gamma); void pid_configure_deadband(struct pid_deadband *deadband, float width, float slope); diff --git a/flight/Modules/AltitudeHold/altitudehold.c b/flight/Modules/AltitudeHold/altitudehold.c index 0140901b0f..4bb7eab2d7 100644 --- a/flight/Modules/AltitudeHold/altitudehold.c +++ b/flight/Modules/AltitudeHold/altitudehold.c @@ -160,15 +160,16 @@ static void altitudeHoldTask(void *parameters) FlightStatusConnectQueue(queue); AltitudeHoldSettingsGet(&altitudeHoldSettings); + // Main task loop + const uint32_t dt_ms = 20; + pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp, - altitudeHoldSettings.VelocityKi, 0.0f, 1.0f); + altitudeHoldSettings.VelocityKi, 0.0f, 1.0f, + dt_ms / 1000.0f); AlarmsSet(SYSTEMALARMS_ALARM_ALTITUDEHOLD, SYSTEMALARMS_ALARM_OK); - // Main task loop - const uint32_t dt_ms = 20; uint32_t timeout = dt_ms; - uint32_t timeval = PIOS_DELAY_GetRaw(); while (1) { if (PIOS_Queue_Receive(queue, &ev, timeout) != true) { @@ -182,8 +183,6 @@ static void altitudeHoldTask(void *parameters) // Copy the current throttle as a starting point for integral StabilizationDesiredThrustGet(&velocity_pid.iAccumulator); engaged = true; - timeval = PIOS_DELAY_GetRaw(); - } else if (flight_mode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) engaged = false; @@ -195,8 +194,10 @@ static void altitudeHoldTask(void *parameters) } else if (ev.obj == AltitudeHoldSettingsHandle()) { AltitudeHoldSettingsGet(&altitudeHoldSettings); - pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp, - altitudeHoldSettings.VelocityKi, 0.0f, 1.0f); + pid_configure(&velocity_pid, + altitudeHoldSettings.VelocityKp, + altitudeHoldSettings.VelocityKi, + 0.0f, 1.0f, dt_ms / 1000.0f); continue; } @@ -219,14 +220,11 @@ static void altitudeHoldTask(void *parameters) // to stop winding up const float min_throttle = landing ? -0.1f : 0.0f; - // Velocity desired is from the outer controller plus the set point - float dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f; - timeval = PIOS_DELAY_GetRaw(); 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); + min_throttle, 1.0f // positive limits since this is throttle + ); AltitudeHoldStateData altitudeHoldState; altitudeHoldState.VelocityDesired = velocity_desired; diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index edf73de3f3..53e259f9ac 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -148,8 +148,8 @@ static volatile bool systemSettingsUpdated = true; // Private functions static void stabilizationTask(void* parameters); static void zero_pids(void); -static void calculate_pids(void); -static void update_settings(); +static void calculate_pids(float dT); +static void update_settings(float dT); static float get_throttle(StabilizationDesiredData *stabilization_desired, SystemSettingsAirframeTypeOptions *airframe_type); #ifndef NO_CONTROL_DEADBANDS @@ -354,7 +354,7 @@ static void stabilizationTask(void* parameters) PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); if (settings_flag) { - update_settings(); + update_settings(dT_expected); // Default 350ms. // 175ms to 39.3% of response @@ -580,7 +580,7 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = bound_sym(raw_input[i], settings.ManualRate[i]); // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], dT_expected); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; @@ -625,7 +625,7 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = bound_sym(curve_cmd * max_rate_filtered[i], max_rate_filtered[i]); // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], dT); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i], 1.0f); break; @@ -650,7 +650,7 @@ static void stabilizationTask(void* parameters) } // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], dT_expected); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]); actuatorDesiredAxis[i] = factor * raw_input[i] + (1.0f - factor) * actuatorDesiredAxis[i]; actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i], 1.0f); @@ -663,11 +663,11 @@ static void stabilizationTask(void* parameters) } // Compute the outer loop - rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT_expected); + rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i]); rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]); // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], dT_expected); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; @@ -692,7 +692,7 @@ static void stabilizationTask(void* parameters) // Compute desired rate as input biased towards leveling rateDesiredAxis[i] = raw_input[i] + weak_leveling; - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], dT_expected); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; @@ -714,11 +714,11 @@ static void stabilizationTask(void* parameters) 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_expected); + float tmpRateDesired = pid_apply(&pids[PID_GROUP_ATT + i], axis_lock_accum[i]); rateDesiredAxis[i] = bound_sym(tmpRateDesired, settings.MaximumRate[i]); } - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], dT_expected); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; @@ -733,7 +733,7 @@ static void stabilizationTask(void* parameters) 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_expected); + float rateDesiredAttitude = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i]); // Compute the desire rate for a rate control float rateDesiredRate = raw_input[i] * settings.ManualRate[i]; @@ -744,7 +744,7 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.ManualRate[i]); // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], dT_expected); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; @@ -790,17 +790,17 @@ static void stabilizationTask(void* parameters) if (axis_mode[i] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) { // Compute the outer loop - rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT_expected); + rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i]); rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]); // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], dT_expected); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]); } else { // Get the desired rate. yaw is always in rate mode in system ident. rateDesiredAxis[i] = bound_sym(raw_input[i], settings.ManualRate[i]); // Compute the inner loop only for yaw - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], dT_expected); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]); } const float scale = settings.AutotuneActuationEffort[i]; @@ -898,7 +898,7 @@ static void stabilizationTask(void* parameters) // instead of the measured value. float errorSlip = -accelsDataY; - float command = pid_apply(&pids[PID_COORDINATED_FLIGHT_YAW], errorSlip, dT_expected); + float command = pid_apply(&pids[PID_COORDINATED_FLIGHT_YAW], errorSlip); actuatorDesiredAxis[YAW] = bound_sym(command ,1.0); // Reset axis-lock integrals @@ -908,10 +908,10 @@ static void stabilizationTask(void* parameters) // Axis lock on no gyro change axis_lock_accum[YAW] += (0 - gyro_filtered[YAW]) * dT_expected; - rateDesiredAxis[YAW] = pid_apply(&pids[PID_ATT_YAW], axis_lock_accum[YAW], dT_expected); + rateDesiredAxis[YAW] = pid_apply(&pids[PID_ATT_YAW], axis_lock_accum[YAW]); rateDesiredAxis[YAW] = bound_sym(rateDesiredAxis[YAW], settings.MaximumRate[YAW]); - actuatorDesiredAxis[YAW] = pid_apply_setpoint(&pids[PID_RATE_YAW], NULL, rateDesiredAxis[YAW], gyro_filtered[YAW], dT_expected); + actuatorDesiredAxis[YAW] = pid_apply_setpoint(&pids[PID_RATE_YAW], NULL, rateDesiredAxis[YAW], gyro_filtered[YAW]); actuatorDesiredAxis[YAW] = bound_sym(actuatorDesiredAxis[YAW],1.0f); // Reset coordinated-flight integral @@ -972,11 +972,11 @@ static void stabilizationTask(void* parameters) error = true; // Compute the outer loop - rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], angle_error, dT_expected); + rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], angle_error); rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.PoiMaximumRate[i]); // Compute the inner loop - actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], dT_expected); + actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; @@ -1057,74 +1057,84 @@ static uint8_t remap_smoothing_mode(uint8_t m) } } -static void calculate_pids() +static void calculate_pids(float dT) { // Set the roll rate PID constants pid_configure(&pids[PID_RATE_ROLL], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], - settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]); + settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], + dT); // Set the pitch rate PID constants pid_configure(&pids[PID_RATE_PITCH], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], - settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]); + settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], + dT); // Set the yaw rate PID constants pid_configure(&pids[PID_RATE_YAW], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], - settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]); + settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], + dT); // Set the roll attitude PI constants pid_configure(&pids[PID_ATT_ROLL], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0, - settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]); + settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], + dT); // Set the pitch attitude PI constants pid_configure(&pids[PID_ATT_PITCH], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0, - settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]); + settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], + dT); // Set the yaw attitude PI constants pid_configure(&pids[PID_ATT_YAW], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0, - settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]); + settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], + dT); // Set the vbar roll settings pid_configure(&pids[PID_VBAR_ROLL], vbar_settings.VbarRollPID[VBARSETTINGS_VBARROLLPID_KP], vbar_settings.VbarRollPID[VBARSETTINGS_VBARROLLPID_KI], vbar_settings.VbarRollPID[VBARSETTINGS_VBARROLLPID_KD], - 0); + 0, + dT); // Set the vbar pitch settings pid_configure(&pids[PID_VBAR_PITCH], vbar_settings.VbarPitchPID[VBARSETTINGS_VBARPITCHPID_KP], vbar_settings.VbarPitchPID[VBARSETTINGS_VBARPITCHPID_KI], vbar_settings.VbarPitchPID[VBARSETTINGS_VBARPITCHPID_KD], - 0); + 0, + dT); // Set the vbar yaw settings pid_configure(&pids[PID_VBAR_YAW], vbar_settings.VbarYawPID[VBARSETTINGS_VBARYAWPID_KP], vbar_settings.VbarYawPID[VBARSETTINGS_VBARYAWPID_KI], vbar_settings.VbarYawPID[VBARSETTINGS_VBARYAWPID_KD], - 0); + 0, + dT); // Set the coordinated flight settings pid_configure(&pids[PID_COORDINATED_FLIGHT_YAW], settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_KP], settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_KI], 0, /* No derivative term */ - settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_ILIMIT]); + settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_ILIMIT], + dT); // Set up the derivative term pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma); @@ -1147,7 +1157,7 @@ static void calculate_pids() #endif } -static void update_settings() +static void update_settings(float dT) { SubTrimSettingsData subTrimSettings; @@ -1163,7 +1173,7 @@ static void update_settings() StabilizationSettingsGet(&settings); VbarSettingsGet(&vbar_settings); - calculate_pids(); + calculate_pids(dT); // Maximum deviation to accumulate for axis lock max_axis_lock = settings.MaxAxisLock; diff --git a/flight/Modules/Stabilization/virtualflybar.c b/flight/Modules/Stabilization/virtualflybar.c index 95f2b2b02d..be5c2240ce 100644 --- a/flight/Modules/Stabilization/virtualflybar.c +++ b/flight/Modules/Stabilization/virtualflybar.c @@ -53,7 +53,7 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle); // Compute the normal PID controller output - float pid_out = pid_apply_setpoint(pid, NULL, 0, gyro, dT); + float pid_out = pid_apply_setpoint(pid, NULL, 0, gyro); // Command signal can indicate how much to disregard the gyro feedback (fast flips) if (settings->VbarGyroSuppress > 0.0f) { diff --git a/flight/Modules/VtolPathFollower/vtol_follower_control.c b/flight/Modules/VtolPathFollower/vtol_follower_control.c index 5ea53f1e6e..350883c8ff 100644 --- a/flight/Modules/VtolPathFollower/vtol_follower_control.c +++ b/flight/Modules/VtolPathFollower/vtol_follower_control.c @@ -71,7 +71,7 @@ static float vtol_path_m=0, vtol_path_r=0, vtol_end_m=0, vtol_end_r=0; // Time constants converted to IIR parameter static float loiter_brakealpha=0.96f, loiter_errordecayalpha=0.88f; -static int32_t vtol_follower_control_impl(const float dT, +static int32_t vtol_follower_control_impl( const float *hold_pos_ned, float alt_rate, bool update_status); /** @@ -83,7 +83,7 @@ static int32_t vtol_follower_control_impl(const float dT, * * The calculated velocity to attempt is stored in @ref VelocityDesired */ -int32_t vtol_follower_control_path(const float dT, const PathDesiredData *pathDesired, +int32_t vtol_follower_control_path(const PathDesiredData *pathDesired, struct path_status *progress) { PositionActualData positionActual; @@ -140,8 +140,7 @@ int32_t vtol_follower_control_path(const float dT, const PathDesiredData *pathDe } // Wait here for new path segment - return vtol_follower_control_impl(dT, pathDesired->End, - 0, false); + return vtol_follower_control_impl(pathDesired->End, 0, false); } // Interpolate desired velocity and altitude along the path @@ -167,7 +166,7 @@ int32_t vtol_follower_control_path(const float dT, const PathDesiredData *pathDe commands_ned[2] = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], downError, -altitudeHoldSettings.MaxClimbRate * 0.1f, - altitudeHoldSettings.MaxDescentRate * 0.1f, dT); + altitudeHoldSettings.MaxDescentRate * 0.1f); VelocityDesiredData velocityDesired; VelocityDesiredGet(&velocityDesired); @@ -184,13 +183,12 @@ int32_t vtol_follower_control_path(const float dT, const PathDesiredData *pathDe /** * Controller to maintain/seek a position and optionally descend. - * @param[in] dT time since last eval * @param[in] hold_pos_ned a position to hold * @param[in] alt_rate if not 0, a requested descent/climb rate that overrides * hold_pos_ned * @param[in] update_status does this update path_status, or does somoene else? */ -static int32_t vtol_follower_control_impl(const float dT, +static int32_t vtol_follower_control_impl( const float *hold_pos_ned, float alt_rate, bool update_status) { PositionActualData positionActual; @@ -242,17 +240,17 @@ static int32_t vtol_follower_control_impl(const float dT, // Compute desired north command velocity from position error commands_ned[0] = pid_apply_antiwindup(&vtol_pids[NORTH_POSITION], damped_ne[0], - -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); + -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax); // Compute desired east command velocity from position error commands_ned[1] = pid_apply_antiwindup(&vtol_pids[EAST_POSITION], damped_ne[1], - -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); + -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax); if (fabsf(alt_rate) < 0.001f) { // Compute desired down comand velocity from the position difference commands_ned[2] = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], errors_ned[2], -altitudeHoldSettings.MaxClimbRate * 0.1f, - altitudeHoldSettings.MaxDescentRate * 0.1f, dT); + altitudeHoldSettings.MaxDescentRate * 0.1f); } else { // Just use the commanded rate commands_ned[2] = alt_rate; @@ -291,7 +289,6 @@ static int32_t vtol_follower_control_impl(const float dT, /** * Control algorithm to stay or approach at a fixed location. - * @param[in] dT the time since last evaluation * @param[in] ned The position to attempt to hold * This method does not attempt any particular path, simply a straight line * approach. The calculated velocity to attempt is stored in @ref @@ -300,38 +297,36 @@ static int32_t vtol_follower_control_impl(const float dT, * Takes in @ref PositionActual and compares it to @ref PositionDesired * and computes @ref VelocityDesired */ -int32_t vtol_follower_control_endpoint(const float dT, const float *hold_pos_ned) +int32_t vtol_follower_control_endpoint(const float *hold_pos_ned) { - return vtol_follower_control_impl(dT, hold_pos_ned, 0, true); + return vtol_follower_control_impl(hold_pos_ned, 0, true); } /** * Control algorithm to land at a fixed location - * @param[in] dT the time since last evaluation * @param[in] hold_pos_ned The position to attempt to land over (down ignored) * @param[out] landed True once throttle low and velocity at zero (UNIMPL) * * Takes in @ref PositionActual and compares it to the hold position * and computes @ref VelocityDesired */ -int32_t vtol_follower_control_land(const float dT, const float *hold_pos_ned, +int32_t vtol_follower_control_land(const float *hold_pos_ned, bool *landed) { - return vtol_follower_control_impl(dT, hold_pos_ned, + return vtol_follower_control_impl(hold_pos_ned, guidanceSettings.LandingRate, true); } /** * Control algorithm for loitering-- allow control of altitude rate. - * @param[in] dT time since last evaluation * @param[in] hold_pos_ned The position to control for. * @param[in] alt_adj If 0, holds at altitude in hold_pos_ned. Otherwise, * a rate in meters per second to descend. */ -int32_t vtol_follower_control_altrate(const float dT, - const float *hold_pos_ned, float alt_adj) +int32_t vtol_follower_control_altrate(const float *hold_pos_ned, + float alt_adj) { - return vtol_follower_control_impl(dT, hold_pos_ned, + return vtol_follower_control_impl(hold_pos_ned, alt_adj, true); } @@ -374,14 +369,14 @@ static int32_t vtol_follower_control_accel(float dT) // Compute desired north command from velocity error north_error = velocityDesired.North - velocityActual.North; north_acceleration += pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], north_error, - -MAX_ACCELERATION, MAX_ACCELERATION, dT) + + -MAX_ACCELERATION, MAX_ACCELERATION) + velocityDesired.North * guidanceSettings.VelocityFeedforward + -nedAccel.North * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]; // Compute desired east command from velocity error east_error = velocityDesired.East - velocityActual.East; east_acceleration += pid_apply_antiwindup(&vtol_pids[EAST_VELOCITY], east_error, - -MAX_ACCELERATION, MAX_ACCELERATION, dT) + + -MAX_ACCELERATION, MAX_ACCELERATION) + velocityDesired.East * guidanceSettings.VelocityFeedforward + -nedAccel.East * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]; @@ -394,7 +389,7 @@ static int32_t vtol_follower_control_accel(float dT) // Compute desired down command. Using NED accel as the damping term down_error = velocityDesired.Down - velocityActual.Down; // Negative is critical here since throttle is negative with down - accelDesired.Down = -pid_apply_antiwindup(&vtol_pids[DOWN_VELOCITY], down_error, -1, 0, dT); + accelDesired.Down = -pid_apply_antiwindup(&vtol_pids[DOWN_VELOCITY], down_error, -1, 0); // Store the desired acceleration AccelDesiredSet(&accelDesired); @@ -722,6 +717,7 @@ bool vtol_follower_control_loiter(float dT, float *hold_pos, float *att_adj, return true; } +// TODO: Follows the old (dangerous) model, doesn't properly infer dT void vtol_follower_control_settings_updated(UAVObjEvent * ev, void *ctx, void *obj, int len) { @@ -734,31 +730,35 @@ void vtol_follower_control_settings_updated(UAVObjEvent * ev, guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], // Kp guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], // Ki 0, // Kd - guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]); + guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], + DT); 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]); + guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT], + DT); // 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]); + guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], + DT); pid_configure(&vtol_pids[EAST_POSITION], guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP], guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], 0, - guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]); + guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], + DT); // 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_POSITION], altitudeHoldSettings.PositionKp, 0, 0, 0, DT); 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 + 0, 1, DT); // 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, diff --git a/flight/Modules/VtolPathFollower/vtol_follower_fsm.c b/flight/Modules/VtolPathFollower/vtol_follower_fsm.c index 32334dfcf6..b4e7062302 100644 --- a/flight/Modules/VtolPathFollower/vtol_follower_fsm.c +++ b/flight/Modules/VtolPathFollower/vtol_follower_fsm.c @@ -60,7 +60,6 @@ const static float RTH_MIN_ALTITUDE = 15.f; //!< Hover at least 15 m above home */ const static float RTH_ALT_ERROR = 0.8f; //!< The altitude to come within for RTH */ const static float RTH_MIN_RISE = 1.5f; //!< Always climb at least this much. -const static float DT = 0.05f; // TODO: make the self monitored const static float RTH_CLIMB_SPEED = 1.0f; //!< Climb at 1m/s //! Events that can be be injected into the FSM and trigger state changes @@ -404,7 +403,7 @@ static float vtol_hold_position_ned[3]; */ static int32_t do_hold() { - if (vtol_follower_control_endpoint(DT, vtol_hold_position_ned) == 0) { + if (vtol_follower_control_endpoint(vtol_hold_position_ned) == 0) { if (vtol_follower_control_attitude(DT, NULL) == 0) { return 0; } @@ -429,7 +428,7 @@ static PathDesiredData vtol_fsm_path_desired = { static int32_t do_path() { struct path_status progress; - if (vtol_follower_control_path(DT, &vtol_fsm_path_desired, &progress) == 0) { + if (vtol_follower_control_path(&vtol_fsm_path_desired, &progress) == 0) { if (vtol_follower_control_attitude(DT, NULL) == 0) { if (progress.fractional_progress >= 1.0f) { @@ -480,7 +479,7 @@ static int32_t do_requested_path() static int32_t do_land() { bool landed; - if (vtol_follower_control_land(DT, vtol_hold_position_ned, &landed) == 0) { + if (vtol_follower_control_land(vtol_hold_position_ned, &landed) == 0) { if (vtol_follower_control_attitude(DT, NULL) == 0) { return 0; } @@ -511,7 +510,7 @@ static int32_t do_loiter() hold_position(hold_pos[0], hold_pos[1], hold_pos[2]); } - if (vtol_follower_control_altrate(DT, vtol_hold_position_ned, + if (vtol_follower_control_altrate(vtol_hold_position_ned, alt_adj) == 0) { if (vtol_follower_control_attitude(DT, att_adj) == 0) { return 0; @@ -531,7 +530,7 @@ static int32_t do_loiter() */ static int32_t do_slow_altitude_change(float descent_rate) { - if (vtol_follower_control_altrate(DT, vtol_hold_position_ned, + if (vtol_follower_control_altrate(vtol_hold_position_ned, descent_rate) == 0) { if (vtol_follower_control_attitude(DT, NULL) == 0) { return 0; diff --git a/flight/Modules/VtolPathFollower/vtol_follower_priv.h b/flight/Modules/VtolPathFollower/vtol_follower_priv.h index b5908ea4e9..6bd4df53f7 100644 --- a/flight/Modules/VtolPathFollower/vtol_follower_priv.h +++ b/flight/Modules/VtolPathFollower/vtol_follower_priv.h @@ -32,6 +32,8 @@ #include "pathdesired.h" #include "paths.h" +const static float DT = 0.05f; // TODO: make the self monitored + /** * The set of goals the VTOL follower will attempt to achieve this selects * the particular FSM that is running. These goals are determined by the @@ -57,12 +59,12 @@ enum vtol_pid { }; // Control code public API methods -int32_t vtol_follower_control_path(const float dT, const PathDesiredData *pathDesired, struct path_status *progress); -int32_t vtol_follower_control_endpoint(const float dT, const float *hold_pos_ned); -int32_t vtol_follower_control_altrate(const float dT, const float *hold_pos_ned, +int32_t vtol_follower_control_path(const PathDesiredData *pathDesired, struct path_status *progress); +int32_t vtol_follower_control_endpoint(const float *hold_pos_ned); +int32_t vtol_follower_control_altrate(const float *hold_pos_ned, float alt_adj); int32_t vtol_follower_control_attitude(const float dT, const float *att_adj); -int32_t vtol_follower_control_land(const float dT, const float *hold_pos_ned, bool *landed); +int32_t vtol_follower_control_land(const float *hold_pos_ned, bool *landed); bool vtol_follower_control_loiter(float dT, float *hold_pos, float *att_adj, float *alt_adj); void vtol_follower_control_settings_updated(UAVObjEvent * ev, void *ctx,