mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-10 18:24:11 +01:00
270 lines
8.8 KiB
C++
270 lines
8.8 KiB
C++
/**
|
|
******************************************************************************
|
|
* @addtogroup OpenPilotModules OpenPilot Modules
|
|
* @{
|
|
* @addtogroup PathFollower CONTROL interface class
|
|
* @brief PID controller for NE
|
|
* @{
|
|
*
|
|
* @file PIDControlNE.h
|
|
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
|
|
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
|
* @brief Executes PID control loops for NE directions
|
|
*
|
|
* @see The GNU Public License (GPL) Version 3
|
|
*
|
|
*****************************************************************************/
|
|
/*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
extern "C" {
|
|
#include <openpilot.h>
|
|
|
|
#include <math.h>
|
|
#include <pid.h>
|
|
#include <CoordinateConversions.h>
|
|
#include <sin_lookup.h>
|
|
#include <pathdesired.h>
|
|
#include <paths.h>
|
|
#include "plans.h"
|
|
#include <pidstatus.h>
|
|
}
|
|
#include "pathfollowerfsm.h"
|
|
#include "pidcontrolne.h"
|
|
|
|
PIDControlNE::PIDControlNE()
|
|
: deltaTime(0), mNECommand(0), mNeutral(0), mVelocityMax(0), mMinCommand(0), mMaxCommand(0), mVelocityFeedforward(0), mActive(false)
|
|
{
|
|
memset((void *)PIDvel, 0, 2 * sizeof(pid2));
|
|
memset((void *)PIDposH, 0, 2 * sizeof(pid));
|
|
|
|
mVelocitySetpointTarget[0] = 0.0f;
|
|
mVelocitySetpointTarget[1] = 0.0f;
|
|
mVelocityState[0] = 0.0f;
|
|
mVelocityState[1] = 0.0f;
|
|
mPositionSetpointTarget[0] = 0.0f;
|
|
mPositionSetpointTarget[1] = 0.0f;
|
|
mPositionState[0] = 0.0f;
|
|
mPositionState[1] = 0.0f;
|
|
mVelocitySetpointCurrent[0] = 0.0f;
|
|
mVelocitySetpointCurrent[1] = 0.0f;
|
|
}
|
|
|
|
PIDControlNE::~PIDControlNE() {}
|
|
|
|
void PIDControlNE::Deactivate()
|
|
{
|
|
mActive = false;
|
|
}
|
|
|
|
void PIDControlNE::Activate()
|
|
{
|
|
mActive = true;
|
|
pid2_transfer(&PIDvel[0], 0.0f);
|
|
pid2_transfer(&PIDvel[1], 0.0f);
|
|
pid_zero(&PIDposH[0]);
|
|
pid_zero(&PIDposH[1]);
|
|
}
|
|
|
|
void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
|
|
{
|
|
float Td; // Derivative time constant
|
|
float Ti; // Integral time constant
|
|
float kt; // Feedback gain for integral windup avoidance
|
|
float N = 10.0f; // N is the factor used to determine the
|
|
// time constant for derivative filtering
|
|
// Why 10? Maybe should be configurable?
|
|
float Tf; // Low pass filter time constant for derivative filtering
|
|
|
|
float u0 = 0.0f;
|
|
|
|
// Define Td, handling zero kp term (for I or ID controller)
|
|
if (kp < 1e-6f) {
|
|
Td = 1e6f;
|
|
} else {
|
|
Td = kd / kp;
|
|
}
|
|
|
|
// Define Ti, Tt and kt, handling zero ki term (for P or PD controller)
|
|
if (ki < 1e-6f) { // Avoid Ti being infinite
|
|
kt = 0.0f;
|
|
} else {
|
|
Ti = kp / ki;
|
|
kt = 1.0f / Ti;
|
|
}
|
|
|
|
// Define Tf, according to controller type
|
|
if (kd < 1e-6f) {
|
|
// PI Controller or P Controller
|
|
Tf = 0;
|
|
} else {
|
|
Tf = Td / N;
|
|
}
|
|
|
|
if (beta > 1.0f) {
|
|
beta = 1.0f;
|
|
} else if (beta < 0.4f) {
|
|
beta = 0.4f;
|
|
}
|
|
|
|
pid2_configure(&PIDvel[0], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
|
|
pid2_configure(&PIDvel[1], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
|
|
deltaTime = dT;
|
|
mVelocityMax = velocityMax;
|
|
}
|
|
|
|
void PIDControlNE::UpdatePositionalParameters(float kp)
|
|
{
|
|
pid_configure(&PIDposH[0], kp, 0.0f, 0.0f, 0.0f);
|
|
pid_configure(&PIDposH[1], kp, 0.0f, 0.0f, 0.0f);
|
|
}
|
|
void PIDControlNE::UpdatePositionSetpoint(float setpointNorth, float setpointEast)
|
|
{
|
|
mPositionSetpointTarget[0] = setpointNorth;
|
|
mPositionSetpointTarget[1] = setpointEast;
|
|
}
|
|
void PIDControlNE::UpdatePositionState(float pvNorth, float pvEast)
|
|
{
|
|
mPositionState[0] = pvNorth;
|
|
mPositionState[1] = pvEast;
|
|
}
|
|
// This is a pure position hold position control
|
|
void PIDControlNE::ControlPosition()
|
|
{
|
|
// Current progress location relative to end
|
|
float velNorth = 0.0f;
|
|
float velEast = 0.0f;
|
|
|
|
velNorth = pid_apply(&PIDposH[0], mPositionSetpointTarget[0] - mPositionState[0], deltaTime);
|
|
velEast = pid_apply(&PIDposH[1], mPositionSetpointTarget[1] - mPositionState[1], deltaTime);
|
|
UpdateVelocitySetpoint(velNorth, velEast);
|
|
}
|
|
|
|
void PIDControlNE::ControlPositionWithPath(struct path_status *progress)
|
|
{
|
|
// Current progress location relative to end
|
|
float velNorth = progress->path_vector[0];
|
|
float velEast = progress->path_vector[1];
|
|
|
|
velNorth += pid_apply(&PIDposH[0], progress->correction_vector[0], deltaTime);
|
|
velEast += pid_apply(&PIDposH[1], progress->correction_vector[1], deltaTime);
|
|
UpdateVelocitySetpoint(velNorth, velEast);
|
|
}
|
|
|
|
void PIDControlNE::UpdateVelocitySetpoint(float setpointNorth, float setpointEast)
|
|
{
|
|
// scale velocity if it is above configured maximum
|
|
// for braking, we can not help it if initial velocity was greater
|
|
float velH = sqrtf(setpointNorth * setpointNorth + setpointEast * setpointEast);
|
|
|
|
if (velH > mVelocityMax) {
|
|
setpointNorth *= mVelocityMax / velH;
|
|
setpointEast *= mVelocityMax / velH;
|
|
}
|
|
|
|
mVelocitySetpointTarget[0] = setpointNorth;
|
|
mVelocitySetpointTarget[1] = setpointEast;
|
|
}
|
|
|
|
|
|
void PIDControlNE::UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
|
|
{
|
|
if (startingVelocity >= 0.0f) {
|
|
*updatedVelocity = startingVelocity - dT * brakeRate;
|
|
if (*updatedVelocity > currentVelocity) {
|
|
*updatedVelocity = currentVelocity;
|
|
}
|
|
if (*updatedVelocity < 0.0f) {
|
|
*updatedVelocity = 0.0f;
|
|
}
|
|
} else {
|
|
*updatedVelocity = startingVelocity + dT * brakeRate;
|
|
if (*updatedVelocity < currentVelocity) {
|
|
*updatedVelocity = currentVelocity;
|
|
}
|
|
if (*updatedVelocity > 0.0f) {
|
|
*updatedVelocity = 0.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
void PIDControlNE::UpdateVelocityStateWithBrake(float pvNorth, float pvEast, float path_time, float brakeRate)
|
|
{
|
|
mVelocityState[0] = pvNorth;
|
|
mVelocityState[1] = pvEast;
|
|
|
|
float velocitySetpointDesired[2];
|
|
|
|
UpdateBrakeVelocity(mVelocitySetpointTarget[0], path_time, brakeRate, pvNorth, &velocitySetpointDesired[0]);
|
|
UpdateBrakeVelocity(mVelocitySetpointTarget[1], path_time, brakeRate, pvEast, &velocitySetpointDesired[1]);
|
|
|
|
// If rate of change limits required, add here
|
|
for (int iaxis = 0; iaxis < 2; iaxis++) {
|
|
mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis];
|
|
}
|
|
}
|
|
|
|
void PIDControlNE::UpdateVelocityState(float pvNorth, float pvEast)
|
|
{
|
|
mVelocityState[0] = pvNorth;
|
|
mVelocityState[1] = pvEast;
|
|
|
|
// The FSM controls the actual descent velocity and introduces step changes as required
|
|
float velocitySetpointDesired[2];
|
|
velocitySetpointDesired[0] = mVelocitySetpointTarget[0];
|
|
velocitySetpointDesired[1] = mVelocitySetpointTarget[1];
|
|
|
|
// If rate of change limits required, add here
|
|
for (int iaxis = 0; iaxis < 2; iaxis++) {
|
|
mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis];
|
|
}
|
|
}
|
|
|
|
|
|
void PIDControlNE::UpdateCommandParameters(float minCommand, float maxCommand, float velocityFeedforward)
|
|
{
|
|
mMinCommand = minCommand;
|
|
mMaxCommand = maxCommand;
|
|
mVelocityFeedforward = velocityFeedforward;
|
|
}
|
|
|
|
|
|
void PIDControlNE::GetNECommand(float *northCommand, float *eastCommand)
|
|
{
|
|
PIDvel[0].va = mVelocitySetpointCurrent[0] * mVelocityFeedforward;
|
|
*northCommand = pid2_apply(&(PIDvel[0]), mVelocitySetpointCurrent[0], mVelocityState[0], mMinCommand, mMaxCommand);
|
|
PIDvel[1].va = mVelocitySetpointCurrent[1] * mVelocityFeedforward;
|
|
*eastCommand = pid2_apply(&(PIDvel[1]), mVelocitySetpointCurrent[1], mVelocityState[1], mMinCommand, mMaxCommand);
|
|
|
|
PIDStatusData pidStatus;
|
|
pidStatus.setpoint = mVelocitySetpointCurrent[0];
|
|
pidStatus.actual = mVelocityState[0];
|
|
pidStatus.error = mVelocitySetpointCurrent[0] - mVelocityState[0];
|
|
pidStatus.setpoint = mVelocitySetpointCurrent[0];
|
|
pidStatus.ulow = mMinCommand;
|
|
pidStatus.uhigh = mMaxCommand;
|
|
pidStatus.command = *northCommand;
|
|
pidStatus.P = PIDvel[0].P;
|
|
pidStatus.I = PIDvel[0].I;
|
|
pidStatus.D = PIDvel[0].D;
|
|
PIDStatusSet(&pidStatus);
|
|
}
|
|
|
|
void PIDControlNE::GetVelocityDesired(float *north, float *east)
|
|
{
|
|
*north = mVelocitySetpointCurrent[0];
|
|
*east = mVelocitySetpointCurrent[1];
|
|
}
|