rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

OP-1848 altvario rewrite - initial checkin

Browse Source 
TODO: 1) update uavos 2) check dT
This commit is contained in:
abeck70 2015-04-21 23:29:08 +10:00
parent 21b6160397
commit 955f314541
6 changed files with 116 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
flight/modules/PathFollower/pidcontroldown.cpp → flight/libraries/pid/pidcontroldown.cpp
View File

@ -45,7 +45,6 @@ extern "C" {
#include <vtolselftuningstats.h>
}
#include "pathfollowerfsm.h"
#include "pidcontroldown.h"
#define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f
@ -59,7 +58,7 @@ extern "C" {
PIDControlDown::PIDControlDown()
: deltaTime(0.0f), mVelocitySetpointTarget(0.0f), mVelocitySetpointCurrent(0.0f), mVelocityState(0.0f), mDownCommand(0.0f),
mFSM(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f),
mCallback(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f),
mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false)
{
Deactivate();
@ -67,9 +66,9 @@ PIDControlDown::PIDControlDown()
PIDControlDown::~PIDControlDown() {}
void PIDControlDown::Initialize(PathFollowerFSM *fsm)
void PIDControlDown::Initialize(PIDControlDownCallback *callback)
{
mFSM = fsm;
mCallback = callback;
}
void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust)
@ -80,7 +79,6 @@ void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust)
void PIDControlDown::Deactivate()
{
// pid_zero(&PID);
mActive = false;
}
@ -333,9 +331,9 @@ void PIDControlDown::UpdateVelocityState(float pv)
{
mVelocityState = pv;
if (mFSM) {
if (mCallback) {
// The FSM controls the actual descent velocity and introduces step changes as required
float velocitySetpointDesired = mFSM->BoundVelocityDown(mVelocitySetpointTarget);
float velocitySetpointDesired = mCallback->BoundVelocityDown(mVelocitySetpointTarget);
// RateLimit(velocitySetpointDesired, mVelocitySetpointCurrent, 2.0f );
mVelocitySetpointCurrent = velocitySetpointDesired;
} else {
@ -354,8 +352,8 @@ float PIDControlDown::GetDownCommand(void)
float ulow = mMinThrust;
float uhigh = mMaxThrust;
if (mFSM) {
mFSM->BoundThrust(ulow, uhigh);
if (mCallback) {
mCallback->BoundThrust(ulow, uhigh);
}
float downCommand = pid2_apply(&PID, mVelocitySetpointCurrent, mVelocityState, ulow, uhigh);
pidStatus.setpoint = mVelocitySetpointCurrent;

6
flight/modules/PathFollower/inc/pidcontroldown.h → flight/libraries/pid/pidcontroldown.h
View File

@ -34,13 +34,13 @@ extern "C" {
#include <pid.h>
#include <stabilizationdesired.h>
}
#include "pathfollowerfsm.h"
#include "pidcontroldowncallback.h"
class PIDControlDown {
public:
PIDControlDown();
~PIDControlDown();
void Initialize(PathFollowerFSM *fsm);
void Initialize(PIDControlDownCallback *callback);
void SetThrustLimits(float min_thrust, float max_thrust);
void Deactivate();
void Activate();
@ -69,7 +69,7 @@ private:
float mVelocitySetpointCurrent;
float mVelocityState;
float mDownCommand;
PathFollowerFSM *mFSM;
PIDControlDownCallback *mCallback;
float mNeutral;
float mVelocityMax;
struct pid PIDpos;

42
flight/libraries/pid/pidcontroldowncallback.h Normal file
View File

@ -0,0 +1,42 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PID Library
* @brief Thrust control callback pure virtual
* @{
*
* @file pidcontroldowncallback.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Interface class for PathFollower FSMs
*
* @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
*/
#ifndef PIDCONTROLDOWNCALLBACK_H
#define PIDCONTROLDOWNCALLBACK_H
class PIDControlDownCallback {
public:
// PIDControlDownCalback() {};
virtual void BoundThrust(__attribute__((unused)) float &ulow, __attribute__((unused)) float &uhigh) = 0;
virtual float BoundVelocityDown(float velocity) = 0;
//virtual ~PIDControlDownCalback() = 0;
};
#endif // PIDCONTROLDOWNCALLBACK_H

3
flight/modules/PathFollower/inc/pathfollowerfsm.h
View File

@ -34,6 +34,7 @@
extern "C" {
#include <stabilizationdesired.h>
}
#include <pidcontroldowncallback.h>
typedef enum {
PFFSM_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup
@ -42,7 +43,7 @@ typedef enum {
PFFSM_STATE_ABORT // Abort on error
} PathFollowerFSMState_T;
class PathFollowerFSM {
class PathFollowerFSM: public PIDControlDownCallback {
public:
// PathFollowerFSM() {};
virtual void Inactive(void) {}

89
flight/modules/Stabilization/altitudeloop.c → flight/modules/Stabilization/altitudeloop.cpp
View File

@ -1,7 +1,7 @@
/**
******************************************************************************
*
* @file altitudeloop.c
* @file altitudeloop.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
@ -26,6 +26,7 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
@ -37,6 +38,11 @@
#include <altitudeholdstatus.h>
#include <velocitystate.h>
#include <positionstate.h>
#include <vtolselftuningstats.h>
}
#include <pidcontroldown.h>
// Private constants
@ -54,6 +60,7 @@
// Private types
// Private variables
static PIDControlDown controlDown;
static DelayedCallbackInfo *altitudeHoldCBInfo;
static AltitudeHoldSettingsData altitudeHoldSettings;
static struct pid pid0, pid1;
@ -61,7 +68,9 @@ static ThrustModeType thrustMode;
static PiOSDeltatimeConfig timeval;
static float thrustSetpoint = 0.0f;
static float thrustDemand = 0.0f;
static float startThrust = 0.5f;
// this is the max speed in m/s at the extents of thrust
static float thrustRate;
static uint8_t thrustExp;
// Private functions
@ -71,15 +80,15 @@ static void VelocityStateUpdatedCb(UAVObjEvent *ev);
/**
* Setup mode and setpoint
*
* reinit: true when althold/vario mode selected over a previous alternate thrust mode
*/
float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit)
{
static bool newaltitude = true;
if (reinit) {
startThrust = setpoint;
pid_zero(&pid0);
pid_zero(&pid1);
controlDown.Activate();
newaltitude = true;
}
@ -87,34 +96,28 @@ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
// this is the max speed in m/s at the extents of thrust
float thrustRate;
uint8_t thrustExp;
AltitudeHoldSettingsThrustExpGet(&thrustExp);
AltitudeHoldSettingsThrustRateGet(&thrustRate);
PositionStateData posState;
PositionStateGet(&posState);
if (altitudeHoldSettings.CutThrustWhenZero && setpoint <= 0) {
// Cut thrust if desired
thrustSetpoint = 0.0f;
controlDown.UpdateVelocitySetpoint(0.0f);
thrustDemand = 0.0f;
thrustMode = DIRECT;
newaltitude = true;
} else if (mode == ALTITUDEVARIO && setpoint > DEADBAND_HIGH) {
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
// then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255
thrustSetpoint = -((thrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
controlDown.UpdateVelocitySetpoint(-((thrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate));
thrustMode = ALTITUDEVARIO;
newaltitude = true;
} else if (mode == ALTITUDEVARIO && setpoint < DEADBAND_LOW) {
thrustSetpoint = -(-(thrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255 * thrustRate);
controlDown.UpdateVelocitySetpoint(-(-(thrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255 * thrustRate));
thrustMode = ALTITUDEVARIO;
newaltitude = true;
} else if (newaltitude == true) {
thrustSetpoint = posState.Down;
controlDown.UpdateVelocitySetpoint(0.0f);
PositionStateData posState;
PositionStateGet(&posState);
controlDown.UpdatePositionSetpoint(posState.Down);
thrustMode = ALTITUDEHOLD;
newaltitude = false;
}
@ -150,28 +153,27 @@ void stabilizationAltitudeloopInit()
static void altitudeHoldTask(void)
{
AltitudeHoldStatusData altitudeHoldStatus;
AltitudeHoldStatusGet(&altitudeHoldStatus);
// do the actual control loop(s)
float positionStateDown;
PositionStateDownGet(&positionStateDown);
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
controlDown.UpdateVelocityState(velocityStateDown);
float dT;
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
switch (thrustMode) {
case ALTITUDEHOLD:
{
// altitude control loop
// No scaling.
const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, &scaler, thrustSetpoint, positionStateDown, dT);
float positionStateDown;
PositionStateDownGet(&positionStateDown);
controlDown.UpdatePositionState(positionStateDown);
controlDown.ControlPosition();
altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
}
break;
case ALTITUDEVARIO:
altitudeHoldStatus.VelocityDesired = thrustSetpoint;
altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
break;
default:
altitudeHoldStatus.VelocityDesired = 0;
@ -186,10 +188,7 @@ static void altitudeHoldTask(void)
break;
default:
{
// velocity control loop
// No scaling.
const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f };
thrustDemand = startThrust - pid_apply_setpoint(&pid1, &scaler, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
thrustDemand = controlDown.GetDownCommand();
}
break;
}
@ -198,10 +197,30 @@ static void altitudeHoldTask(void)
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit);
pid_zero(&pid0);
pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
pid_zero(&pid1);
//pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit);
//pid_zero(&pid0);
//pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
//pid_zero(&pid1);
controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
vtolPathFollowerSettings->LandVerticalVelPID.Ki,
vtolPathFollowerSettings->LandVerticalVelPID.Kd,
vtolPathFollowerSettings->LandVerticalVelPID.Beta,
UPDATE_EXPECTED,
vtolPathFollowerSettings->VerticalVelMax);
// The following is not currently used in the landing control.
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
// initialise limits on thrust but note the FSM can override.
controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
AltitudeHoldSettingsThrustExpGet(&thrustExp);
AltitudeHoldSettingsThrustRateGet(&thrustRate);
}
static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)

8
make/apps-defs.mk
View File

@ -33,6 +33,10 @@ FLIGHTLIBINC = $(FLIGHTLIB)/inc
OPUAVOBJINC = $(OPUAVOBJ)/inc
OPUAVTALKINC = $(OPUAVTALK)/inc
## PID
PIDLIB =$(FLIGHTLIB)/pid
PIDLIBINC =$(FLIGHTLIB)/pid
## Math
MATHLIB = $(FLIGHTLIB)/math
MATHLIBINC = $(FLIGHTLIB)/math
@ -87,7 +91,10 @@ SRC += $(PIOSCOMMON)/pios_sensors.c
SRC += $(FLIGHTLIB)/sanitycheck.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
SRC += $(MATHLIB)/sin_lookup.c
## PID library functions
SRC += $(MATHLIB)/pid.c
CPPSRC += $(PIDLIB)/pidcontroldown.cpp
## PIOS Hardware (Common)
SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c
@ -166,6 +173,7 @@ EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(PIOSCOMMON)
EXTRAINCDIRS += $(OPSYSTEMINC)
EXTRAINCDIRS += $(MATHLIBINC)
EXTRAINCDIRS += $(PIDLIBINC)
EXTRAINCDIRS += $(OPUAVOBJINC)
EXTRAINCDIRS += $(OPUAVTALKINC)
EXTRAINCDIRS += $(OPUAVSYNTHDIR)