1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-04 12:24:11 +01:00
LibrePilot/flight/modules/Stabilization/altitudeloop.cpp

235 lines
8.0 KiB
C++
Raw Normal View History

2011-09-13 07:25:03 +02:00
/**
******************************************************************************
*
* @file altitudeloop.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
2011-09-13 07:25:03 +02:00
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
* of @ref ManualControlCommand is Auto.
*
* @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 <callbackinfo.h>
#include <pid.h>
#include <altitudeloop.h>
#include <CoordinateConversions.h>
#include <altitudeholdsettings.h>
#include <altitudeholdstatus.h>
#include <velocitystate.h>
#include <positionstate.h>
#include <vtolselftuningstats.h>
#include <stabilization.h>
}
#include <pidcontroldown.h>
2011-09-13 07:25:03 +02:00
// Private constants
#ifdef REVOLUTION
#define UPDATE_EXPECTED (1.0f / PIOS_SENSOR_RATE)
#define UPDATE_MIN 1.0e-6f
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
2011-09-13 07:25:03 +02:00
// Private types
// Private variables
static DelayedCallbackInfo *altitudeHoldCBInfo;
static PIDControlDown controlDown;
2012-02-08 16:42:10 +01:00
static AltitudeHoldSettingsData altitudeHoldSettings;
static ThrustModeType thrustMode;
static float thrustDemand = 0.0f;
2011-09-13 07:25:03 +02:00
// Private functions
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void altitudeHoldTask(void);
static void VelocityStateUpdatedCb(UAVObjEvent *ev);
2011-09-13 07:25:03 +02:00
/**
* Setup mode and setpoint
*
* reinit: true when althold/vario mode selected over a previous alternate thrust mode
2011-09-13 07:25:03 +02:00
*/
float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit)
2011-09-13 07:25:03 +02:00
{
static bool newaltitude = true;
2011-09-13 07:25:03 +02:00
if (reinit || !controlDown.IsActive()) {
controlDown.Activate();
newaltitude = true;
// calculate a thrustDemand on reinit only
thrustMode = mode;
altitudeHoldTask();
}
const float DEADBAND = 0.20f;
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
if (altitudeHoldSettings.CutThrustWhenZero && setpoint <= 0) {
// Cut thrust if desired
controlDown.UpdateVelocitySetpoint(0.0f);
thrustDemand = 0.0f;
thrustMode = DIRECT;
newaltitude = true;
return thrustDemand;
} 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
controlDown.UpdateVelocitySetpoint(-((altitudeHoldSettings.ThrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
thrustMode = ALTITUDEVARIO;
newaltitude = true;
} else if (mode == ALTITUDEVARIO && setpoint < DEADBAND_LOW) {
controlDown.UpdateVelocitySetpoint(-(-(altitudeHoldSettings.ThrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate));
thrustMode = ALTITUDEVARIO;
newaltitude = true;
} else if (newaltitude == true) {
controlDown.UpdateVelocitySetpoint(0.0f);
PositionStateData posState;
PositionStateGet(&posState);
controlDown.UpdatePositionSetpoint(posState.Down);
thrustMode = ALTITUDEHOLD;
newaltitude = false;
}
thrustDemand = boundf(thrustDemand, altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
return thrustDemand;
}
/**
* Disable the alt hold task loop velocity controller to save cpu cycles
*/
void stabilizationDisableAltitudeHold(void)
{
controlDown.Deactivate();
}
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void)
{
if (!controlDown.IsActive()) {
return;
}
AltitudeHoldStatusData altitudeHoldStatus;
AltitudeHoldStatusGet(&altitudeHoldStatus);
2013-12-07 23:31:26 +01:00
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
controlDown.UpdateVelocityState(velocityStateDown);
2013-12-07 23:31:26 +01:00
float local_thrustDemand = 0.0f;
switch (thrustMode) {
case ALTITUDEHOLD:
2014-09-19 01:29:04 +02:00
{
float positionStateDown;
PositionStateDownGet(&positionStateDown);
controlDown.UpdatePositionState(positionStateDown);
controlDown.ControlPosition();
altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEHOLD;
local_thrustDemand = controlDown.GetDownCommand();
2014-09-19 01:29:04 +02:00
}
break;
case ALTITUDEVARIO:
altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEVARIO;
local_thrustDemand = controlDown.GetDownCommand();
break;
case DIRECT:
altitudeHoldStatus.VelocityDesired = 0.0f;
altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_DIRECT;
break;
}
2013-12-07 23:31:26 +01:00
thrustDemand = local_thrustDemand;
AltitudeHoldStatusSet(&altitudeHoldStatus);
}
2013-12-29 19:09:54 +01:00
static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
}
/**
* Initialise the module, called on startup
*/
void stabilizationAltitudeloopInit()
{
AltitudeHoldSettingsInitialize();
AltitudeHoldStatusInitialize();
PositionStateInitialize();
VelocityStateInitialize();
VtolSelfTuningStatsInitialize();
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
SettingsUpdatedCb(NULL);
altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
VelocityStateConnectCallback(&VelocityStateUpdatedCb);
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
2012-01-03 00:19:14 +01:00
{
AltitudeHoldSettingsGet(&altitudeHoldSettings);
controlDown.UpdateParameters(altitudeHoldSettings.VerticalVelPID.Kp,
altitudeHoldSettings.VerticalVelPID.Ki,
altitudeHoldSettings.VerticalVelPID.Kd,
altitudeHoldSettings.VerticalVelPID.Beta,
(float)(UPDATE_EXPECTED),
altitudeHoldSettings.ThrustRate);
controlDown.UpdatePositionalParameters(altitudeHoldSettings.VerticalPosP);
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + altitudeHoldSettings.ThrustLimits.Neutral);
// initialise limits on thrust but note the FSM can override.
controlDown.SetThrustLimits(altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
// disable neutral thrust calcs which should only be done in a hold mode.
controlDown.DisableNeutralThrustCalc();
2012-01-03 00:19:14 +01:00
}
#endif /* ifdef REVOLUTION */