mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
OP-1309 integrated AltitudeHold into new Stabilization
This commit is contained in:
Corvus Corax
parent
66f2f8e840
commit
6993570cc9
@ -1,8 +1,8 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file guidance.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @file altitudeloop.c
|
||||
* @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.
|
||||
@ -26,89 +26,122 @@
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
/**
|
||||
* Input object: ActiveWaypoint
|
||||
* Input object: PositionState
|
||||
* Input object: ManualControlCommand
|
||||
* Output object: AttitudeDesired
|
||||
*
|
||||
* This module will periodically update the value of the AttitudeDesired object.
|
||||
*
|
||||
* The module executes in its own thread in this example.
|
||||
*
|
||||
* Modules have no API, all communication to other modules is done through UAVObjects.
|
||||
* However modules may use the API exposed by shared libraries.
|
||||
* See the OpenPilot wiki for more details.
|
||||
* http://www.openpilot.org/OpenPilot_Application_Architecture
|
||||
*
|
||||
*/
|
||||
|
||||
#include <openpilot.h>
|
||||
|
||||
#include <callbackinfo.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <pid.h>
|
||||
#include <altitudeloop.h>
|
||||
#include <CoordinateConversions.h>
|
||||
#include <altitudeholdsettings.h>
|
||||
#include <altitudeholddesired.h> // object that will be updated by the module
|
||||
#include <altitudeholdstatus.h>
|
||||
#include <flightstatus.h>
|
||||
#include <stabilizationdesired.h>
|
||||
#include <accelstate.h>
|
||||
#include <pios_constants.h>
|
||||
#include <velocitystate.h>
|
||||
#include <positionstate.h>
|
||||
// Private constants
|
||||
|
||||
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
|
||||
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
|
||||
|
||||
#define STACK_SIZE_BYTES 1024
|
||||
#define DESIRED_UPDATE_RATE_MS 100 // milliseconds
|
||||
#ifdef REVOLUTION
|
||||
|
||||
#define UPDATE_EXPECTED (1.0f / 666.0f)
|
||||
#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
|
||||
|
||||
#define STACK_SIZE_BYTES 512
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static DelayedCallbackInfo *altitudeHoldCBInfo;
|
||||
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;
|
||||
|
||||
|
||||
// Private functions
|
||||
static void altitudeHoldTask(void);
|
||||
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
||||
static void VelocityStateUpdatedCb(UAVObjEvent *ev);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
* Setup mode and setpoint
|
||||
*/
|
||||
int32_t AltitudeHoldStart()
|
||||
float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit)
|
||||
{
|
||||
// Start main task
|
||||
SettingsUpdatedCb(NULL);
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
|
||||
static bool newaltitude = true;
|
||||
|
||||
return 0;
|
||||
if (reinit) {
|
||||
startThrust = setpoint;
|
||||
pid_zero(&pid0);
|
||||
pid_zero(&pid1);
|
||||
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;
|
||||
|
||||
// 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;
|
||||
} 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;
|
||||
} 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;
|
||||
} else if (newaltitude == true) {
|
||||
thrustSetpoint = posState.Down;
|
||||
thrustMode = ALTITUDEHOLD;
|
||||
newaltitude = false;
|
||||
}
|
||||
|
||||
return thrustDemand;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
int32_t AltitudeHoldInitialize()
|
||||
void stabilizationAltitudeloopInit()
|
||||
{
|
||||
AltitudeHoldSettingsInitialize();
|
||||
AltitudeHoldDesiredInitialize();
|
||||
AltitudeHoldStatusInitialize();
|
||||
PositionStateInitialize();
|
||||
VelocityStateInitialize();
|
||||
|
||||
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
|
||||
// Create object queue
|
||||
|
||||
altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
|
||||
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
|
||||
VelocityStateConnectCallback(&VelocityStateUpdatedCb);
|
||||
|
||||
return 0;
|
||||
// Start main task
|
||||
SettingsUpdatedCb(NULL);
|
||||
}
|
||||
MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
|
||||
|
||||
|
||||
/**
|
||||
@ -116,44 +149,25 @@ MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart);
|
||||
*/
|
||||
static void altitudeHoldTask(void)
|
||||
{
|
||||
static float startThrust = 0.5f;
|
||||
|
||||
// make sure we run only when we are supposed to run
|
||||
FlightStatusData flightStatus;
|
||||
|
||||
FlightStatusGet(&flightStatus);
|
||||
switch (flightStatus.FlightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO:
|
||||
break;
|
||||
default:
|
||||
pid_zero(&pid0);
|
||||
pid_zero(&pid1);
|
||||
StabilizationDesiredThrustGet(&startThrust);
|
||||
PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
|
||||
return;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
AltitudeHoldStatusData altitudeHoldStatus;
|
||||
|
||||
AltitudeHoldStatusGet(&altitudeHoldStatus);
|
||||
|
||||
// do the actual control loop(s)
|
||||
AltitudeHoldDesiredData altitudeHoldDesired;
|
||||
AltitudeHoldDesiredGet(&altitudeHoldDesired);
|
||||
float positionStateDown;
|
||||
PositionStateDownGet(&positionStateDown);
|
||||
float velocityStateDown;
|
||||
VelocityStateDownGet(&velocityStateDown);
|
||||
|
||||
switch (altitudeHoldDesired.ControlMode) {
|
||||
case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE:
|
||||
float dT;
|
||||
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
|
||||
switch (thrustMode) {
|
||||
case ALTITUDEHOLD:
|
||||
// altitude control loop
|
||||
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.SetPoint, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
|
||||
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, thrustSetpoint, positionStateDown, dT);
|
||||
break;
|
||||
case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY:
|
||||
altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint;
|
||||
case ALTITUDEVARIO:
|
||||
altitudeHoldStatus.VelocityDesired = thrustSetpoint;
|
||||
break;
|
||||
default:
|
||||
altitudeHoldStatus.VelocityDesired = 0;
|
||||
@ -162,37 +176,16 @@ static void altitudeHoldTask(void)
|
||||
|
||||
AltitudeHoldStatusSet(&altitudeHoldStatus);
|
||||
|
||||
float thrust;
|
||||
switch (altitudeHoldDesired.ControlMode) {
|
||||
case ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST:
|
||||
thrust = altitudeHoldDesired.SetPoint;
|
||||
switch (thrustMode) {
|
||||
case DIRECT:
|
||||
thrustDemand = thrustSetpoint;
|
||||
break;
|
||||
default:
|
||||
// velocity control loop
|
||||
thrust = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
|
||||
thrustDemand = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT);
|
||||
|
||||
if (thrust >= 1.0f) {
|
||||
thrust = 1.0f;
|
||||
}
|
||||
if (thrust <= 0.0f) {
|
||||
thrust = 0.0f;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
StabilizationDesiredData stab;
|
||||
StabilizationDesiredGet(&stab);
|
||||
stab.Roll = altitudeHoldDesired.Roll;
|
||||
stab.Pitch = altitudeHoldDesired.Pitch;
|
||||
stab.Yaw = altitudeHoldDesired.Yaw;
|
||||
stab.Thrust = thrust;
|
||||
stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
|
||||
|
||||
StabilizationDesiredSet(&stab);
|
||||
|
||||
PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
|
||||
}
|
||||
|
||||
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
@ -203,3 +196,11 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
#endif /* ifdef REVOLUTION */
|
||||
@ -1,133 +0,0 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup ManualControl
|
||||
* @brief Interpretes the control input in ManualControlCommand
|
||||
* @{
|
||||
*
|
||||
* @file altitudehandler.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
*
|
||||
* @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
|
||||
*/
|
||||
|
||||
#include "inc/manualcontrol.h"
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <stabilizationbank.h>
|
||||
#include <altitudeholddesired.h>
|
||||
#include <altitudeholdsettings.h>
|
||||
#include <positionstate.h>
|
||||
|
||||
#if defined(REVOLUTION)
|
||||
// Private constants
|
||||
|
||||
// Private types
|
||||
|
||||
// Private functions
|
||||
|
||||
/**
|
||||
* @brief Handler to control deprecated flight modes controlled by AltitudeHold module
|
||||
* @input: ManualControlCommand
|
||||
* @output: AltitudeHoldDesired
|
||||
*/
|
||||
void altitudeHandler(bool newinit)
|
||||
{
|
||||
const float DEADBAND = 0.20f;
|
||||
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
|
||||
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
|
||||
|
||||
if (newinit) {
|
||||
StabilizationBankInitialize();
|
||||
AltitudeHoldDesiredInitialize();
|
||||
AltitudeHoldSettingsInitialize();
|
||||
PositionStateInitialize();
|
||||
}
|
||||
|
||||
|
||||
// this is the max speed in m/s at the extents of thrust
|
||||
float thrustRate;
|
||||
uint8_t thrustExp;
|
||||
|
||||
static uint8_t flightMode;
|
||||
static bool newaltitude = true;
|
||||
|
||||
ManualControlCommandData cmd;
|
||||
ManualControlCommandGet(&cmd);
|
||||
|
||||
FlightStatusFlightModeGet(&flightMode);
|
||||
|
||||
AltitudeHoldDesiredData altitudeHoldDesiredData;
|
||||
AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
|
||||
|
||||
AltitudeHoldSettingsThrustExpGet(&thrustExp);
|
||||
AltitudeHoldSettingsThrustRateGet(&thrustRate);
|
||||
|
||||
StabilizationBankData stabSettings;
|
||||
StabilizationBankGet(&stabSettings);
|
||||
|
||||
PositionStateData posState;
|
||||
PositionStateGet(&posState);
|
||||
|
||||
altitudeHoldDesiredData.Roll = cmd.Roll * stabSettings.RollMax;
|
||||
altitudeHoldDesiredData.Pitch = cmd.Pitch * stabSettings.PitchMax;
|
||||
altitudeHoldDesiredData.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw;
|
||||
|
||||
if (newinit) {
|
||||
newaltitude = true;
|
||||
}
|
||||
|
||||
uint8_t cutOff;
|
||||
AltitudeHoldSettingsCutThrustWhenZeroGet(&cutOff);
|
||||
if (cutOff && cmd.Thrust < 0) {
|
||||
// Cut thrust if desired
|
||||
altitudeHoldDesiredData.SetPoint = cmd.Thrust;
|
||||
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST;
|
||||
newaltitude = true;
|
||||
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust > 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
|
||||
altitudeHoldDesiredData.SetPoint = -((thrustExp * powf((cmd.Thrust - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (cmd.Thrust - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate);
|
||||
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
|
||||
newaltitude = true;
|
||||
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust < DEADBAND_LOW) {
|
||||
altitudeHoldDesiredData.SetPoint = -(-(thrustExp * powf((DEADBAND_LOW - (cmd.Thrust < 0 ? 0 : cmd.Thrust)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - cmd.Thrust) / DEADBAND_LOW) / 255 * thrustRate);
|
||||
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
|
||||
newaltitude = true;
|
||||
} else if (newaltitude == true) {
|
||||
altitudeHoldDesiredData.SetPoint = posState.Down;
|
||||
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE;
|
||||
newaltitude = false;
|
||||
}
|
||||
|
||||
AltitudeHoldDesiredSet(&altitudeHoldDesiredData);
|
||||
}
|
||||
|
||||
#else /* if defined(REVOLUTION) */
|
||||
void altitudeHandler(__attribute__((unused)) bool newinit)
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // should not be called
|
||||
}
|
||||
#endif // REVOLUTION
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -1,9 +1,15 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup StabilizationModule Stabilization Module
|
||||
* @brief altitudeloop mode
|
||||
* @note This file implements the logic for a altitudeloop
|
||||
* @{
|
||||
*
|
||||
* @file examplemodperiodic.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Example module to be used as a template for actual modules.
|
||||
* @file altitudeloop.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @brief Attitude stabilization module.
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -23,9 +29,13 @@
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
#ifndef EXAMPLEMODPERIODIC_H
|
||||
#define EXAMPLEMODPERIODIC_H
|
||||
|
||||
int32_t ExampleModPeriodicInitialize();
|
||||
int32_t GuidanceInitialize(void);
|
||||
#endif // EXAMPLEMODPERIODIC_H
|
||||
#ifndef ALTITUDELOOP_H
|
||||
#define ALTITUDELOOP_H
|
||||
|
||||
typedef enum { ALTITUDEHOLD = 0, ALTITUDEVARIO = 1, DIRECT = 2 } ThrustModeType;
|
||||
|
||||
void stabilizationAltitudeloopInit();
|
||||
float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit);
|
||||
|
||||
#endif /* ALTITUDELOOP_H */
|
||||
@ -46,6 +46,7 @@
|
||||
|
||||
#include <stabilization.h>
|
||||
#include <cruisecontrol.h>
|
||||
#include <altitudeloop.h>
|
||||
#include <CoordinateConversions.h>
|
||||
|
||||
// Private constants
|
||||
@ -228,6 +229,13 @@ static void stabilizationOuterloopTask()
|
||||
}
|
||||
} else {
|
||||
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
|
||||
case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE:
|
||||
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit);
|
||||
break;
|
||||
case STABILIZATIONSTATUS_OUTERLOOP_VERTICALVELOCITY:
|
||||
rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit);
|
||||
break;
|
||||
case STABILIZATIONSTATUS_OUTERLOOP_DIRECT:
|
||||
default:
|
||||
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
|
||||
break;
|
||||
|
||||
@ -50,6 +50,7 @@
|
||||
#include <stabilization.h>
|
||||
#include <innerloop.h>
|
||||
#include <outerloop.h>
|
||||
#include <altitudeloop.h>
|
||||
|
||||
|
||||
// Public variables
|
||||
@ -109,6 +110,9 @@ int32_t StabilizationInitialize()
|
||||
|
||||
stabilizationOuterloopInit();
|
||||
stabilizationInnerloopInit();
|
||||
#ifdef REVOLUTION
|
||||
stabilizationAltitudeloopInit();
|
||||
#endif
|
||||
pid_zero(&stabSettings.outerPids[0]);
|
||||
pid_zero(&stabSettings.outerPids[1]);
|
||||
pid_zero(&stabSettings.outerPids[2]);
|
||||
|
||||
@ -106,7 +106,6 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
|
||||
UAVOBJSRCFILENAMES += oplinksettings
|
||||
UAVOBJSRCFILENAMES += oplinkstatus
|
||||
UAVOBJSRCFILENAMES += altitudefiltersettings
|
||||
UAVOBJSRCFILENAMES += altitudeholddesired
|
||||
UAVOBJSRCFILENAMES += altitudeholdstatus
|
||||
UAVOBJSRCFILENAMES += waypoint
|
||||
UAVOBJSRCFILENAMES += waypointactive
|
||||
|
||||
@ -106,7 +106,6 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
|
||||
UAVOBJSRCFILENAMES += oplinksettings
|
||||
UAVOBJSRCFILENAMES += oplinkstatus
|
||||
UAVOBJSRCFILENAMES += altitudefiltersettings
|
||||
UAVOBJSRCFILENAMES += altitudeholddesired
|
||||
UAVOBJSRCFILENAMES += altitudeholdstatus
|
||||
UAVOBJSRCFILENAMES += waypoint
|
||||
UAVOBJSRCFILENAMES += waypointactive
|
||||
|
||||
@ -108,7 +108,6 @@ UAVOBJSRCFILENAMES += camerastabsettings
|
||||
UAVOBJSRCFILENAMES += altitudeholdsettings
|
||||
UAVOBJSRCFILENAMES += altitudefiltersettings
|
||||
UAVOBJSRCFILENAMES += revosettings
|
||||
UAVOBJSRCFILENAMES += altitudeholddesired
|
||||
UAVOBJSRCFILENAMES += altitudeholdstatus
|
||||
UAVOBJSRCFILENAMES += ekfconfiguration
|
||||
UAVOBJSRCFILENAMES += ekfstatevariance
|
||||
|
||||
@ -35,7 +35,6 @@ HEADERS += \
|
||||
$$UAVOBJECT_SYNTHETICS/airspeedstate.h \
|
||||
$$UAVOBJECT_SYNTHETICS/attitudestate.h \
|
||||
$$UAVOBJECT_SYNTHETICS/attitudesimulated.h \
|
||||
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
|
||||
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
|
||||
$$UAVOBJECT_SYNTHETICS/altitudeholdstatus.h \
|
||||
$$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \
|
||||
@ -137,7 +136,6 @@ SOURCES += \
|
||||
$$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/attitudestate.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/altitudeholdstatus.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/debuglogsettings.cpp \
|
||||
|
||||
@ -1,14 +0,0 @@
|
||||
<xml>
|
||||
<object name="AltitudeHoldDesired" singleinstance="true" settings="false" category="Control">
|
||||
<description>Holds the desired altitude (from manual control) as well as the desired attitude to pass through</description>
|
||||
<field name="SetPoint" units="" type="float" elements="1"/>
|
||||
<field name="ControlMode" units="" type="enum" elements="1" options="Altitude,Velocity,Thrust" />
|
||||
<field name="Roll" units="deg" type="float" elements="1"/>
|
||||
<field name="Pitch" units="deg" type="float" elements="1"/>
|
||||
<field name="Yaw" units="deg/s" type="float" elements="1"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
Loading…
x
Reference in New Issue
Block a user