1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00

Merge branch 'abeck/OP-1848r-altvario' into next

This commit is contained in:
abeck70 2015-05-13 17:34:53 +10:00
commit c6024573e3
24 changed files with 690 additions and 318 deletions

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,17 +58,17 @@ 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),
mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false)
mCallback(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f),
mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false), mAllowNeutralThrustCalc(true)
{
Deactivate();
}
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;

View File

@ -34,16 +34,20 @@ 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();
bool IsActive()
{
return mActive;
}
void UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax);
void UpdateNeutralThrust(float neutral);
void UpdateVelocitySetpoint(float setpoint);
@ -58,6 +62,14 @@ public:
void ControlPositionWithPath(struct path_status *progress);
void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
void UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate);
void DisableNeutralThrustCalc()
{
mAllowNeutralThrustCalc = false;
}
void EnableNeutralThrustCalc()
{
mAllowNeutralThrustCalc = true;
}
private:
void setup_neutralThrustCalc();
@ -69,7 +81,7 @@ private:
float mVelocitySetpointCurrent;
float mVelocityState;
float mDownCommand;
PathFollowerFSM *mFSM;
PIDControlDownCallback *mCallback;
float mNeutral;
float mVelocityMax;
struct pid PIDpos;
@ -77,7 +89,6 @@ private:
float mPositionState;
float mMinThrust;
float mMaxThrust;
uint8_t mActive;
struct NeutralThrustEstimation {
uint32_t count;
@ -90,6 +101,8 @@ private:
bool have_correction;
};
struct NeutralThrustEstimation neutralThrustEst;
bool mActive;
bool mAllowNeutralThrustCalc;
};
#endif // PIDCONTROLDOWN_H

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

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) {}

View File

@ -126,7 +126,7 @@ void VtolAutoTakeoffController::SettingsUpdated(void)
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.ILimit,
vtolPathFollowerSettings->HorizontalVelPID.Beta,
dT,
vtolPathFollowerSettings->HorizontalVelMax);

View File

@ -132,7 +132,7 @@ void VtolBrakeController::SettingsUpdated(void)
controlNE.UpdateParameters(vtolPathFollowerSettings->BrakeHorizontalVelPID.Kp,
vtolPathFollowerSettings->BrakeHorizontalVelPID.Ki,
vtolPathFollowerSettings->BrakeHorizontalVelPID.Kd,
vtolPathFollowerSettings->BrakeHorizontalVelPID.ILimit,
vtolPathFollowerSettings->BrakeHorizontalVelPID.Beta,
dT,
10.0f * vtolPathFollowerSettings->HorizontalVelMax); // avoid constraining initial fast entry into brake
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);

View File

@ -127,7 +127,7 @@ void VtolFlyController::SettingsUpdated(void)
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.ILimit,
vtolPathFollowerSettings->HorizontalVelPID.Beta,
dT,
vtolPathFollowerSettings->HorizontalVelMax);
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
@ -136,7 +136,7 @@ void VtolFlyController::SettingsUpdated(void)
controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp,
vtolPathFollowerSettings->VerticalVelPID.Ki,
vtolPathFollowerSettings->VerticalVelPID.Kd,
vtolPathFollowerSettings->VerticalVelPID.ILimit, // TODO Change to BETA
vtolPathFollowerSettings->VerticalVelPID.Beta,
dT,
vtolPathFollowerSettings->VerticalVelMax);
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
@ -145,6 +145,9 @@ void VtolFlyController::SettingsUpdated(void)
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
// disable neutral thrust calcs which should only be done in a hold mode.
controlDown.DisableNeutralThrustCalc();
}
/**

View File

@ -120,7 +120,7 @@ void VtolLandController::SettingsUpdated(void)
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.ILimit,
vtolPathFollowerSettings->HorizontalVelPID.Beta,
dT,
vtolPathFollowerSettings->HorizontalVelMax);

View File

@ -107,7 +107,7 @@ void VtolVelocityController::SettingsUpdated(void)
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.ILimit,
vtolPathFollowerSettings->HorizontalVelPID.Beta,
dT,
vtolPathFollowerSettings->HorizontalVelMax);
@ -181,7 +181,7 @@ int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused)
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
// default thrust mode to altvario

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,12 @@
#include <altitudeholdstatus.h>
#include <velocitystate.h>
#include <positionstate.h>
#include <vtolselftuningstats.h>
#include <stabilization.h>
}
#include <pidcontroldown.h>
// Private constants
@ -50,78 +57,134 @@
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define STACK_SIZE_BYTES 512
// Private types
// Private variables
static DelayedCallbackInfo *altitudeHoldCBInfo;
static PIDControlDown controlDown;
static AltitudeHoldSettingsData altitudeHoldSettings;
static struct pid pid0, pid1;
static ThrustModeType thrustMode;
static PiOSDeltatimeConfig timeval;
static float thrustSetpoint = 0.0f;
static float thrustDemand = 0.0f;
static float startThrust = 0.5f;
static float thrustDemand = 0.0f;
// Private functions
static void altitudeHoldTask(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void altitudeHoldTask(void);
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);
if (reinit || !controlDown.IsActive()) {
controlDown.Activate();
newaltitude = true;
// calculate a thrustDemand on reinit only
altitudeHoldTask();
}
const float DEADBAND = 0.20f;
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;
thrustDemand = 0.0f;
thrustMode = DIRECT;
newaltitude = true;
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
thrustSetpoint = -((thrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
thrustMode = ALTITUDEVARIO;
newaltitude = true;
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) {
thrustSetpoint = -(-(thrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255 * thrustRate);
thrustMode = ALTITUDEVARIO;
newaltitude = true;
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) {
thrustSetpoint = posState.Down;
thrustMode = ALTITUDEHOLD;
newaltitude = false;
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);
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
controlDown.UpdateVelocityState(velocityStateDown);
float local_thrustDemand = 0.0f;
switch (thrustMode) {
case ALTITUDEHOLD:
{
float positionStateDown;
PositionStateDownGet(&positionStateDown);
controlDown.UpdatePositionState(positionStateDown);
controlDown.ControlPosition();
altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired();
altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEHOLD;
local_thrustDemand = controlDown.GetDownCommand();
}
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;
}
AltitudeHoldStatusSet(&altitudeHoldStatus);
thrustDemand = local_thrustDemand;
}
static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
}
/**
* Initialise the module, called on startup
*/
@ -131,82 +194,39 @@ void stabilizationAltitudeloopInit()
AltitudeHoldStatusInitialize();
PositionStateInitialize();
VelocityStateInitialize();
VtolSelfTuningStatsInitialize();
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
// Create object queue
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
SettingsUpdatedCb(NULL);
altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
VelocityStateConnectCallback(&VelocityStateUpdatedCb);
// Start main task
SettingsUpdatedCb(NULL);
}
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void)
{
AltitudeHoldStatusData altitudeHoldStatus;
AltitudeHoldStatusGet(&altitudeHoldStatus);
// do the actual control loop(s)
float positionStateDown;
PositionStateDownGet(&positionStateDown);
float velocityStateDown;
VelocityStateDownGet(&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);
}
break;
case ALTITUDEVARIO:
altitudeHoldStatus.VelocityDesired = thrustSetpoint;
break;
default:
altitudeHoldStatus.VelocityDesired = 0;
break;
}
AltitudeHoldStatusSet(&altitudeHoldStatus);
switch (thrustMode) {
case DIRECT:
thrustDemand = thrustSetpoint;
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);
}
break;
}
}
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);
}
static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
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();
}

View File

@ -37,5 +37,6 @@ typedef enum { ALTITUDEHOLD = 0, ALTITUDEVARIO = 1, DIRECT = 2 } ThrustModeType;
void stabilizationAltitudeloopInit();
float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit);
void stabilizationDisableAltitudeHold(void);
#endif /* ALTITUDELOOP_H */

View File

@ -103,7 +103,33 @@ static void stabilizationOuterloopTask()
float *stabilizationDesiredAxis = &stabilizationDesired.Roll;
float *rateDesiredAxis = &rateDesired.Roll;
int t;
float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST] != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]);
previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
#ifdef REVOLUTION
if (reinit && (previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE ||
previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO)) {
// disable the altvario velocity control loop
stabilizationDisableAltitudeHold();
}
#endif /* REVOLUTION */
switch (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]) {
#ifdef REVOLUTION
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEHOLD, reinit);
break;
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEVARIO, reinit);
break;
#endif /* REVOLUTION */
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
default:
rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST];
break;
}
float local_error[3];
{
@ -151,151 +177,134 @@ static void stabilizationOuterloopTask()
}
for (t = 0; t < AXES; t++) {
bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) {
reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]);
previous_mode[t] = StabilizationStatusOuterLoopToArray(enabled)[t];
if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) {
if (reinit) {
stabSettings.outerPids[t].iAccumulator = 0;
}
switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
break;
case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
{
float stickinput[3];
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
// limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
-StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t],
StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]
);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
// - this makes the stick region into a square;
// - this is a feature!
// - hold a roll angle and add just pitch without the stick sensitivity changing
float magnitude = fabsf(stickinput[t]);
if (t < 2) {
magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
}
// modify magnitude to move the Att to Rate transition to the place
// specified by the user
// we are looking for where the stick angle == transition angle
// and the Att rate equals the Rate rate
// that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
// == Rate x StickAngle [Rate pulling up according to stick angle]
// * StickAngle [X Ratt proportion]
// so 1-x == x*x or x*x+x-1=0 where xE(0,1)
// (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
// and quadratic formula says that is 0.618033989f
// I tested 14.01 and came up with .61 without even remembering this number
// I thought that moving the P,I, and maxangle terms around would change this value
// and that I would have to take these into account, but varying
// all P's and I's by factors of 1/2 to 2 didn't change it noticeably
// and varying maxangle from 4 to 120 didn't either.
// so for now I'm not taking these into account
// while working with this, it occurred to me that Attitude mode,
// set up with maxangle=190 would be similar to Ratt, and it is.
#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
// the following assumes the transition would otherwise be at 0.618033989f
// and THAT assumes that Att ramps up to max roll rate
// when a small number of degrees off of where it should be
// if below the transition angle (still in attitude mode)
// '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) {
magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position;
} else {
magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position)
* (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
/ (1.0f - stabSettings.rattitude_mode_transition_stick_position)
+ STICK_VALUE_AT_MODE_TRANSITION;
}
rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate;
}
if (reinit) {
stabSettings.outerPids[t].iAccumulator = 0;
}
switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE:
rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT);
break;
case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
// FIXME: local_error[] is rate - attitude for Weak Leveling
// The only ramifications are:
// Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
// Changing Rate mode max rate currently requires a change to Kp
// That would be changed to Attitude mode max angle affecting Kp
// Also does not take dT into account
{
float stickinput[3];
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rate_input = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
// Compute desired rate as input biased towards leveling
rateDesiredAxis[t] = rate_input + weak_leveling;
case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE:
{
float stickinput[3];
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
// limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together
rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT),
-StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t],
StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]
);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
// - this makes the stick region into a square;
// - this is a feature!
// - hold a roll angle and add just pitch without the stick sensitivity changing
float magnitude = fabsf(stickinput[t]);
if (t < 2) {
magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1]));
}
break;
case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes
// now test limits for pitch and/or roll
if (t == 1) { // pitch
if (attitudeState.Pitch < -stabSettings.stabBank.PitchMax) {
// attitude exceeds pitch max.
// zero rate desired if also -ve
if (rateDesiredAxis[t] < 0.0f) {
rateDesiredAxis[t] = 0.0f;
}
} else if (attitudeState.Pitch > stabSettings.stabBank.PitchMax) {
// attitude exceeds pitch max
// zero rate desired if also +ve
if (rateDesiredAxis[t] > 0.0f) {
rateDesiredAxis[t] = 0.0f;
}
// modify magnitude to move the Att to Rate transition to the place
// specified by the user
// we are looking for where the stick angle == transition angle
// and the Att rate equals the Rate rate
// that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion]
// == Rate x StickAngle [Rate pulling up according to stick angle]
// * StickAngle [X Ratt proportion]
// so 1-x == x*x or x*x+x-1=0 where xE(0,1)
// (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2
// and quadratic formula says that is 0.618033989f
// I tested 14.01 and came up with .61 without even remembering this number
// I thought that moving the P,I, and maxangle terms around would change this value
// and that I would have to take these into account, but varying
// all P's and I's by factors of 1/2 to 2 didn't change it noticeably
// and varying maxangle from 4 to 120 didn't either.
// so for now I'm not taking these into account
// while working with this, it occurred to me that Attitude mode,
// set up with maxangle=190 would be similar to Ratt, and it is.
#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
// the following assumes the transition would otherwise be at 0.618033989f
// and THAT assumes that Att ramps up to max roll rate
// when a small number of degrees off of where it should be
// if below the transition angle (still in attitude mode)
// '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ
if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) {
magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position;
} else {
magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position)
* (1.0f - STICK_VALUE_AT_MODE_TRANSITION)
/ (1.0f - stabSettings.rattitude_mode_transition_stick_position)
+ STICK_VALUE_AT_MODE_TRANSITION;
}
rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate;
}
break;
case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING:
// FIXME: local_error[] is rate - attitude for Weak Leveling
// The only ramifications are:
// Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS
// Changing Rate mode max rate currently requires a change to Kp
// That would be changed to Attitude mode max angle affecting Kp
// Also does not take dT into account
{
float stickinput[3];
stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f);
stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f);
stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f);
float rate_input = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t];
float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp;
weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate);
// Compute desired rate as input biased towards leveling
rateDesiredAxis[t] = rate_input + weak_leveling;
}
break;
case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS:
rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes
// now test limits for pitch and/or roll
if (t == 1) { // pitch
if (attitudeState.Pitch < -stabSettings.stabBank.PitchMax) {
// attitude exceeds pitch max.
// zero rate desired if also -ve
if (rateDesiredAxis[t] < 0.0f) {
rateDesiredAxis[t] = 0.0f;
}
} else if (t == 0) { // roll
if (attitudeState.Roll < -stabSettings.stabBank.RollMax) {
// attitude exceeds roll max.
// zero rate desired if also -ve
if (rateDesiredAxis[t] < 0.0f) {
rateDesiredAxis[t] = 0.0f;
}
} else if (attitudeState.Roll > stabSettings.stabBank.RollMax) {
// attitude exceeds roll max
// zero rate desired if also +ve
if (rateDesiredAxis[t] > 0.0f) {
rateDesiredAxis[t] = 0.0f;
}
} else if (attitudeState.Pitch > stabSettings.stabBank.PitchMax) {
// attitude exceeds pitch max
// zero rate desired if also +ve
if (rateDesiredAxis[t] > 0.0f) {
rateDesiredAxis[t] = 0.0f;
}
}
break;
} else if (t == 0) { // roll
if (attitudeState.Roll < -stabSettings.stabBank.RollMax) {
// attitude exceeds roll max.
// zero rate desired if also -ve
if (rateDesiredAxis[t] < 0.0f) {
rateDesiredAxis[t] = 0.0f;
}
} else if (attitudeState.Roll > stabSettings.stabBank.RollMax) {
// attitude exceeds roll max
// zero rate desired if also +ve
if (rateDesiredAxis[t] > 0.0f) {
rateDesiredAxis[t] = 0.0f;
}
}
}
break;
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
break;
}
} else {
switch (StabilizationStatusOuterLoopToArray(enabled)[t]) {
#ifdef REVOLUTION
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
break;
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO:
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit);
break;
#endif /* REVOLUTION */
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
break;
}
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
default:
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
break;
}
}
@ -314,7 +323,7 @@ static void stabilizationOuterloopTask()
}
}
// update cruisecontrol based on attitude
// update cruisecontrol based on attitude
cruisecontrol_compute_factor(&attitudeState, rateDesired.Thrust);
stabSettings.monitor.rateupdates = 0;
}

View File

@ -55,6 +55,9 @@
#include "accessorydesired.h"
#include "manualcontrolcommand.h"
#include "stabilizationsettings.h"
#ifdef REVOLUTION
#include "altitudeholdsettings.h"
#endif
#include "stabilizationbank.h"
#include "stabilizationsettingsbank1.h"
#include "stabilizationsettingsbank2.h"
@ -95,6 +98,10 @@ int32_t TxPIDInitialize(void)
bool txPIDEnabled;
HwSettingsOptionalModulesData optionalModules;
#ifdef REVOLUTION
AltitudeHoldSettingsInitialize();
#endif
HwSettingsInitialize();
HwSettingsOptionalModulesGet(&optionalModules);
@ -188,10 +195,17 @@ static void updatePIDs(UAVObjEvent *ev)
}
StabilizationSettingsData stab;
StabilizationSettingsGet(&stab);
#ifdef REVOLUTION
AltitudeHoldSettingsData altitude;
AltitudeHoldSettingsGet(&altitude);
#endif
AccessoryDesiredData accessory;
uint8_t needsUpdateBank = 0;
uint8_t needsUpdateStab = 0;
uint8_t needsUpdateBank = 0;
uint8_t needsUpdateStab = 0;
#ifdef REVOLUTION
uint8_t needsUpdateAltitude = 0;
#endif
// Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
@ -351,6 +365,23 @@ static void updatePIDs(UAVObjEvent *ev)
case TXPIDSETTINGS_PIDS_ACROPLUSFACTOR:
needsUpdateBank |= update(&bank.AcroInsanityFactor, value);
break;
#ifdef REVOLUTION
case TXPIDSETTINGS_PIDS_ALTITUDEPOSKP:
needsUpdateAltitude |= update(&altitude.VerticalPosP, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKP:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKI:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKD:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYBETA:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Beta, value);
break;
#endif
default:
PIOS_Assert(0);
}
@ -359,6 +390,11 @@ static void updatePIDs(UAVObjEvent *ev)
if (needsUpdateStab) {
StabilizationSettingsSet(&stab);
}
#ifdef REVOLUTION
if (needsUpdateAltitude) {
AltitudeHoldSettingsSet(&altitude);
}
#endif
if (needsUpdateBank) {
switch (inst.BankNumber) {
case 0:

View File

@ -24,6 +24,9 @@ endif
include ../board-info.mk
include $(ROOT_DIR)/make/firmware-defs.mk
# REVO C++ support
USE_CXX = YES
# ARM DSP library
USE_DSP_LIB ?= NO
@ -70,7 +73,7 @@ ifndef TESTAPP
## Application Core
SRC += ../pios_usb_board_data.c
SRC += $(OPMODULEDIR)/System/systemmod.c
SRC += $(OPSYSTEM)/discoveryf4bare.c
CPPSRC += $(OPSYSTEM)/discoveryf4bare.cpp
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(FLIGHTLIB)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
@ -91,6 +94,7 @@ ifndef TESTAPP
SRC += $(FLIGHTLIB)/insgps13state.c
SRC += $(FLIGHTLIB)/auxmagsupport.c
SRC += $(FLIGHTLIB)/lednotification.c
CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp
## UAVObjects
include ./UAVObjects.inc

View File

@ -19,6 +19,8 @@
# These are the UAVObjects supposed to be build as part of the OpenPilot target
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusgrounddrive
UAVOBJSRCFILENAMES += pidstatus
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += statusvtolautotakeoff
UAVOBJSRCFILENAMES += vtolselftuningstats

View File

@ -31,7 +31,7 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include "inc/openpilot.h"
#include <uavobjectsinit.h>
@ -74,6 +74,7 @@ static void initTask(void *parameters);
/* Prototype of generated InitModules() function */
extern void InitModules(void);
}
/**
* OpenPilot Main function:

View File

@ -24,6 +24,9 @@ endif
include ../board-info.mk
include $(ROOT_DIR)/make/firmware-defs.mk
# REVO C++ support
USE_CXX = YES
# ARM DSP library
USE_DSP_LIB ?= NO
@ -65,7 +68,7 @@ ifndef TESTAPP
## Application Core
SRC += ../pios_usb_board_data.c
SRC += $(OPMODULEDIR)/System/systemmod.c
SRC += $(OPSYSTEM)/revolution.c
CPPSRC += $(OPSYSTEM)/revolution.cpp
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(FLIGHTLIB)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
@ -84,6 +87,8 @@ ifndef TESTAPP
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/insgps13state.c
SRC += $(FLIGHTLIB)/auxmagsupport.c
CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp
## UAVObjects
include ./UAVObjects.inc

View File

@ -31,6 +31,7 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include "inc/openpilot.h"
#include <uavobjectsinit.h>
@ -73,6 +74,7 @@ static void initTask(void *parameters);
/* Prototype of generated InitModules() function */
extern void InitModules(void);
}
/**
* OpenPilot Main function:

View File

@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>974</width>
<height>857</height>
<height>755</height>
</rect>
</property>
<property name="sizePolicy">
@ -136,8 +136,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>939</width>
<height>776</height>
<width>950</width>
<height>775</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_3">
@ -8244,8 +8244,8 @@ border-radius: 5;</string>
<rect>
<x>0</x>
<y>0</y>
<width>952</width>
<height>763</height>
<width>950</width>
<height>736</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_29">
@ -18236,8 +18236,8 @@ border-radius: 5;</string>
<rect>
<x>0</x>
<y>0</y>
<width>952</width>
<height>763</height>
<width>950</width>
<height>671</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_8" stretch="0,0,0,0,0,0">
@ -24082,8 +24082,8 @@ font:bold;</string>
<rect>
<x>0</x>
<y>0</y>
<width>952</width>
<height>763</height>
<width>950</width>
<height>671</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_18">
@ -25209,8 +25209,7 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:AltitudePI</string>
<string>element:Kp</string>
<string>fieldname:VerticalPosP</string>
<string>scale:0.01</string>
<string>haslimits:yes</string>
<string>buttongroup:98,10</string>
@ -25294,15 +25293,18 @@ border-radius: 5;</string>
</widget>
</item>
<item row="3" column="1">
<widget class="QSlider" name="AltKdSlider">
<widget class="QSlider" name="VelKiSlider">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;How fast the vehicle should adjust its neutral throttle estimation. Altitude assumes that when engaged the throttle is in the range required to hover. If the throttle is a lot higher or lower, it needs to adjust this &amp;quot;throttle trim&amp;quot; Higher values make it do this adjustment faster, but this could lead to ugly oscillations. Leave at default unless you know what you are doing.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;How fast the vehicle should attain its target velocity. For neutral throttle estimation, the altitude module assumes that when engaged the throttle thrust limit neutral setting is in the range required to hover. If the throttle required is a lot higher or lower, it needs to adjust this &amp;quot;throttle trim&amp;quot;. Higher values make it do this adjustment faster, but this could lead to ugly oscillations. Leave at default unless you know what you are doing.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="maximum">
<number>1000</number>
<number>100</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>50</number>
<number>25</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
@ -25313,17 +25315,17 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:VelocityPI</string>
<string>fieldname:VerticalVelPID</string>
<string>element:Ki</string>
<string>scale:0.00001</string>
<string>scale:0.01</string>
<string>haslimits:yes</string>
<string>buttongroup:98</string>
</stringlist>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QSpinBox" name="AltKi">
<item row="3" column="2">
<widget class="QSpinBox" name="VelKi">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
@ -25355,13 +25357,13 @@ border-radius: 5;</string>
<number>100</number>
</property>
<property name="value">
<number>51</number>
<number>25</number>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:VelocityPI</string>
<string>element:Kp</string>
<string>fieldname:VerticalVelPID</string>
<string>element:Ki</string>
<string>scale:0.01</string>
<string>haslimits:yes</string>
<string>buttongroup:98,10</string>
@ -25369,8 +25371,68 @@ border-radius: 5;</string>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QSpinBox" name="AltKd">
<item row="4" column="0">
<widget class="QLabel" name="label_25_kd">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>58</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
</size>
</property>
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>Velocity Derivative</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QSlider" name="VelKdSlider">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Small abouts of Kd can reduce oscillations in the velocity controller.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="maximum">
<number>300</number>
</property>
<property name="value">
<number>50</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksBelow</enum>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:VerticalVelPID</string>
<string>element:Kd</string>
<string>scale:0.0001</string>
<string>haslimits:yes</string>
<string>buttongroup:98</string>
</stringlist>
</property>
</widget>
</item>
<item row="4" column="2">
<widget class="QSpinBox" name="VelKd">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
@ -25399,7 +25461,7 @@ border-radius: 5;</string>
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="maximum">
<number>1000</number>
<number>300</number>
</property>
<property name="value">
<number>51</number>
@ -25407,9 +25469,172 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:VelocityPI</string>
<string>element:Ki</string>
<string>scale:0.00001</string>
<string>fieldname:VerticalVelPID</string>
<string>element:Kd</string>
<string>scale:0.0001</string>
<string>haslimits:yes</string>
<string>buttongroup:98,10</string>
</stringlist>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_25_beta">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>58</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
</size>
</property>
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>Velocity Beta</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QSlider" name="VelBetaSlider">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;The beta value applies a setpoint weighting that reduces the sensitivity to quick changes in the desired velocity. Transitions from altitude hold to descent/climb can be made smooth applying a Beta value of around 80 to 90%.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="minimum">
<number>70</number>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>90</number>
</property>
<property name="sliderPosition">
<number>90</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksBelow</enum>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:VerticalVelPID</string>
<string>element:Beta</string>
<string>scale:0.01</string>
<string>haslimits:yes</string>
<string>buttongroup:98</string>
</stringlist>
</property>
</widget>
</item>
<item row="5" column="2">
<widget class="QSpinBox" name="VelBeta">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>50</width>
<height>22</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>50</width>
<height>22</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="minimum">
<number>70</number>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>90</number>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:VerticalVelPID</string>
<string>element:Beta</string>
<string>scale:0.01</string>
<string>haslimits:yes</string>
<string>buttongroup:98,10</string>
</stringlist>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QSpinBox" name="VelKp">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>50</width>
<height>22</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>50</width>
<height>22</height>
</size>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
<property name="maximum">
<number>100</number>
</property>
<property name="value">
<number>50</number>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:VerticalVelPID</string>
<string>element:Kp</string>
<string>scale:0.01</string>
<string>haslimits:yes</string>
<string>buttongroup:98,10</string>
</stringlist>
@ -25419,7 +25644,7 @@ border-radius: 5;</string>
<item row="1" column="1">
<widget class="QSlider" name="AltKpSlider">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;How fast the vehicle should climb or descent to compensate a certain altitude difference. Higher values could result in more accurate altitude hold but also more violent control actions, lower values are safer and ensure smoother flight. The default value should be fine for the majority of crafts.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;How fast the vehicle should climb or descent to compensate a certain altitude difference. Higher values could result in more accurate altitude hold but also more violent control actions, lower values are safer and ensure smoother flight. The default value should be fine for the majority of crafts.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="maximum">
<number>100</number>
@ -25434,10 +25659,9 @@ border-radius: 5;</string>
<enum>QSlider::TicksBelow</enum>
</property>
<property name="objrelation" stdset="0">
<stringlist>
<stringlist notr="true">
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:AltitudePI</string>
<string>element:Kp</string>
<string>fieldname:VerticalPosP</string>
<string>scale:0.01</string>
<string>haslimits:yes</string>
<string>buttongroup:98</string>
@ -25446,7 +25670,7 @@ border-radius: 5;</string>
</widget>
</item>
<item row="2" column="1">
<widget class="QSlider" name="AltKiSlider">
<widget class="QSlider" name="VelKpSlider">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;How much the vehicle should throttle up or down to compensate or achieve a certain vertical speed. Higher values lead to more aggressive throttle changes and could lead to oscillations. This is the most likely candidate to change depending on the crafts engine thrust. Heavy craft with weak engines might require higher values.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
@ -25465,7 +25689,7 @@ border-radius: 5;</string>
<property name="objrelation" stdset="0">
<stringlist>
<string>objname:AltitudeHoldSettings</string>
<string>fieldname:VelocityPI</string>
<string>fieldname:VerticalVelPID</string>
<string>element:Kp</string>
<string>scale:0.01</string>
<string>haslimits:yes</string>
@ -27173,10 +27397,10 @@ Useful if you have accidentally changed some settings.</string>
<tabstop>pushButton_7</tabstop>
<tabstop>AltKpSlider</tabstop>
<tabstop>AltKp</tabstop>
<tabstop>AltKiSlider</tabstop>
<tabstop>AltKi</tabstop>
<tabstop>AltKdSlider</tabstop>
<tabstop>AltKd</tabstop>
<tabstop>VelKpSlider</tabstop>
<tabstop>VelKp</tabstop>
<tabstop>VelKiSlider</tabstop>
<tabstop>VelKi</tabstop>
<tabstop>pushButton_8</tabstop>
<tabstop>AltThrExpSlider_2</tabstop>
<tabstop>AltThrExp_2</tabstop>

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
@ -88,7 +92,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
@ -167,6 +174,7 @@ EXTRAINCDIRS += $(FLIGHTLIBINC)
EXTRAINCDIRS += $(PIOSCOMMON)
EXTRAINCDIRS += $(OPSYSTEMINC)
EXTRAINCDIRS += $(MATHLIBINC)
EXTRAINCDIRS += $(PIDLIBINC)
EXTRAINCDIRS += $(OPUAVOBJINC)
EXTRAINCDIRS += $(OPUAVTALKINC)
EXTRAINCDIRS += $(OPUAVSYNTHDIR)

View File

@ -1,14 +1,15 @@
<xml>
<object name="AltitudeHoldSettings" singleinstance="true" settings="true" category="Control">
<description>Settings for the @ref AltitudeHold module</description>
<field name="AltitudePI" units="(m/s)/m" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.8,0,0" />
<field name="VelocityPI" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.3,0.3,2.0" />
<description>Settings for the @ref AltitudeHold module</description>
<field name="CutThrustWhenZero" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True" />
<field name="ThrustExp" units="" type="uint8" elements="1" defaultvalue="128" />
<field name="ThrustRate" units="m/s" type="float" elements="1" defaultvalue="5" />
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
<field name="ThrustExp" units="" type="uint8" elements="1" defaultvalue="128" />
<field name="ThrustRate" units="m/s" type="float" elements="1" defaultvalue="5" />
<field name="ThrustLimits" units="" type="float" elementnames="Min,Neutral,Max" defaultvalue="0.2, 0.5, 0.9"/>
<field name="VerticalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.4"/>
<field name="VerticalVelPID" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.5, 0.45, 0.001, 0.95"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>

View File

@ -2,6 +2,8 @@
<object name="AltitudeHoldStatus" singleinstance="true" settings="false" category="Control">
<description>Status Data from Altitude Hold Control Loops</description>
<field name="VelocityDesired" units="m/s" type="float" elements="1"/>
<field name="State" units="" type="enum" elements="1" options="Direct,AltitudeVario,AltitudeHold" defaultvalue="Direct"/>
<field name="ThrustDemand" units="" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>

View File

@ -20,7 +20,7 @@
Roll+Pitch Attitude.Kp, Roll+Pitch Attitude.Ki, Roll+Pitch Attitude.ILimit, Roll+Pitch Attitude.Resp,
Yaw Attitude.Kp, Yaw Attitude.Ki, Yaw Attitude.ILimit, Yaw Attitude.Resp,
Roll.Expo, Pitch.Expo, Roll+Pitch.Expo, Yaw.Expo,
GyroTau,AcroPlusFactor"
GyroTau,AcroPlusFactor,Altitude Pos.Kp,Altitude Velocity.Kp,Altitude Velocity.Ki,Altitude Velocity.Kd,Altitude Velocity.Beta"
defaultvalue="Disabled"/>
<field name="MinPID" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/>
<field name="MaxPID" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/>

View File

@ -7,8 +7,8 @@
<field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="1.0"/>
<field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.25"/>
<field name="VerticalPosP" units="" type="float" elements="1" defaultvalue="0.4"/>
<field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="8.0, 0.5, 0.0, 15"/>
<field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.3, 0.3, 0.0, 1.0"/>
<field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="8.0, 0.5, 0.0, 0.95"/>
<field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.3, 0.3, 0.0, 0.95"/>
<field name="ThrustLimits" units="" type="float" elementnames="Min,Neutral,Max" defaultvalue="0.2, 0.5, 0.9"/>
<field name="VelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="2"/>
<field name="ThrustControl" units="" type="enum" elements="1" options="manual,auto" defaultvalue="manual"/>
@ -21,10 +21,10 @@
<field name="UpdatePeriod" units="ms" type="uint16" elements="1" defaultvalue="50"/>
<field name="BrakeRate" units="m/s2" type="float" elements="1" defaultvalue="2.5"/>
<field name="BrakeMaxPitch" units="deg" type="float" elements="1" defaultvalue="25"/>
<field name="BrakeHorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="12.0, 0.0, 0.03, 15"/>
<field name="BrakeHorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="12.0, 0.0, 0.03, 0.95"/>
<field name="BrakeVelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="0"/>
<field name="LandVerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.35, 3.0, 0.05, 0.9"/>
<field name="AutoTakeoffVerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.35, 3.0, 0.05, 0.9"/>
<field name="LandVerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.42, 3.0, 0.02, 0.95"/>
<field name="AutoTakeoffVerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.42, 3.0, 0.02, 0.95"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>