mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-11 19:24:10 +01:00
192 lines
6.6 KiB
C++
192 lines
6.6 KiB
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
|
|
* 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>
|
|
|
|
// 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
|
|
|
|
// Private types
|
|
|
|
// Private variables
|
|
static PIDControlDown controlDown;
|
|
static AltitudeHoldSettingsData altitudeHoldSettings;
|
|
static ThrustModeType thrustMode;
|
|
static float thrustDemand = 0.0f;
|
|
|
|
|
|
// Private functions
|
|
static void SettingsUpdatedCb(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) {
|
|
controlDown.Activate();
|
|
newaltitude = true;
|
|
}
|
|
|
|
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;
|
|
} 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;
|
|
}
|
|
|
|
AltitudeHoldStatusData altitudeHoldStatus;
|
|
AltitudeHoldStatusGet(&altitudeHoldStatus);
|
|
|
|
float velocityStateDown;
|
|
VelocityStateDownGet(&velocityStateDown);
|
|
controlDown.UpdateVelocityState(velocityStateDown);
|
|
|
|
switch (thrustMode) {
|
|
case ALTITUDEHOLD:
|
|
{
|
|
float positionStateDown;
|
|
PositionStateDownGet(&positionStateDown);
|
|
controlDown.UpdatePositionState(positionStateDown);
|
|
controlDown.ControlPosition();
|
|
altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
|
|
thrustDemand = controlDown.GetDownCommand();
|
|
|
|
// if thrust demand is high and we are below altitude by 2m, back off pitch
|
|
}
|
|
break;
|
|
|
|
case ALTITUDEVARIO:
|
|
altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
|
|
thrustDemand = controlDown.GetDownCommand();
|
|
break;
|
|
|
|
case DIRECT:
|
|
altitudeHoldStatus.VelocityDesired = 0.0f;
|
|
break;
|
|
}
|
|
|
|
AltitudeHoldStatusSet(&altitudeHoldStatus);
|
|
|
|
return thrustDemand;
|
|
}
|
|
|
|
/**
|
|
* Initialise the module, called on startup
|
|
*/
|
|
void stabilizationAltitudeloopInit()
|
|
{
|
|
AltitudeHoldSettingsInitialize();
|
|
AltitudeHoldStatusInitialize();
|
|
PositionStateInitialize();
|
|
VelocityStateInitialize();
|
|
VtolSelfTuningStatsInitialize();
|
|
|
|
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
|
|
VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
|
|
SettingsUpdatedCb(NULL);
|
|
}
|
|
|
|
|
|
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
|
{
|
|
AltitudeHoldSettingsGet(&altitudeHoldSettings);
|
|
|
|
controlDown.UpdateParameters(altitudeHoldSettings.VerticalVelPID.Kp,
|
|
altitudeHoldSettings.VerticalVelPID.Ki,
|
|
altitudeHoldSettings.VerticalVelPID.Kd,
|
|
altitudeHoldSettings.VerticalVelPID.Beta,
|
|
(float)(OUTERLOOP_SKIPCOUNT * 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);
|
|
}
|
|
|
|
|
|
#endif /* ifdef REVOLUTION */
|