mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-21 11:54:15 +01:00
OP-1309 integrated AltitudeHold into new Stabilization
This commit is contained in:
parent
66f2f8e840
commit
6993570cc9
@ -1,8 +1,8 @@
|
|||||||
/**
|
/**
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
*
|
*
|
||||||
* @file guidance.c
|
* @file altitudeloop.c
|
||||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||||
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
|
* @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint
|
||||||
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
|
* and sets @ref AttitudeDesired. It only does this when the FlightMode field
|
||||||
* of @ref ManualControlCommand is Auto.
|
* of @ref ManualControlCommand is Auto.
|
||||||
@ -26,89 +26,122 @@
|
|||||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
* 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 <openpilot.h>
|
||||||
|
|
||||||
#include <callbackinfo.h>
|
#include <callbackinfo.h>
|
||||||
|
|
||||||
#include <math.h>
|
|
||||||
#include <pid.h>
|
#include <pid.h>
|
||||||
|
#include <altitudeloop.h>
|
||||||
#include <CoordinateConversions.h>
|
#include <CoordinateConversions.h>
|
||||||
#include <altitudeholdsettings.h>
|
#include <altitudeholdsettings.h>
|
||||||
#include <altitudeholddesired.h> // object that will be updated by the module
|
|
||||||
#include <altitudeholdstatus.h>
|
#include <altitudeholdstatus.h>
|
||||||
#include <flightstatus.h>
|
|
||||||
#include <stabilizationdesired.h>
|
|
||||||
#include <accelstate.h>
|
|
||||||
#include <pios_constants.h>
|
|
||||||
#include <velocitystate.h>
|
#include <velocitystate.h>
|
||||||
#include <positionstate.h>
|
#include <positionstate.h>
|
||||||
// Private constants
|
// Private constants
|
||||||
|
|
||||||
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
|
|
||||||
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
|
|
||||||
|
|
||||||
#define STACK_SIZE_BYTES 1024
|
#ifdef REVOLUTION
|
||||||
#define DESIRED_UPDATE_RATE_MS 100 // milliseconds
|
|
||||||
|
#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 types
|
||||||
|
|
||||||
// Private variables
|
// Private variables
|
||||||
static DelayedCallbackInfo *altitudeHoldCBInfo;
|
static DelayedCallbackInfo *altitudeHoldCBInfo;
|
||||||
static AltitudeHoldSettingsData altitudeHoldSettings;
|
static AltitudeHoldSettingsData altitudeHoldSettings;
|
||||||
static struct pid pid0, pid1;
|
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
|
// Private functions
|
||||||
static void altitudeHoldTask(void);
|
static void altitudeHoldTask(void);
|
||||||
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
||||||
|
static void VelocityStateUpdatedCb(UAVObjEvent *ev);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Initialise the module, called on startup
|
* Setup mode and setpoint
|
||||||
* \returns 0 on success or -1 if initialisation failed
|
|
||||||
*/
|
*/
|
||||||
int32_t AltitudeHoldStart()
|
float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit)
|
||||||
{
|
{
|
||||||
// Start main task
|
static bool newaltitude = true;
|
||||||
SettingsUpdatedCb(NULL);
|
|
||||||
PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
|
|
||||||
|
|
||||||
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
|
* Initialise the module, called on startup
|
||||||
* \returns 0 on success or -1 if initialisation failed
|
|
||||||
*/
|
*/
|
||||||
int32_t AltitudeHoldInitialize()
|
void stabilizationAltitudeloopInit()
|
||||||
{
|
{
|
||||||
AltitudeHoldSettingsInitialize();
|
AltitudeHoldSettingsInitialize();
|
||||||
AltitudeHoldDesiredInitialize();
|
|
||||||
AltitudeHoldStatusInitialize();
|
AltitudeHoldStatusInitialize();
|
||||||
|
PositionStateInitialize();
|
||||||
|
VelocityStateInitialize();
|
||||||
|
|
||||||
|
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
|
||||||
// Create object queue
|
// Create object queue
|
||||||
|
|
||||||
altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
|
altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
|
||||||
AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
|
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 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;
|
AltitudeHoldStatusData altitudeHoldStatus;
|
||||||
|
|
||||||
AltitudeHoldStatusGet(&altitudeHoldStatus);
|
AltitudeHoldStatusGet(&altitudeHoldStatus);
|
||||||
|
|
||||||
// do the actual control loop(s)
|
// do the actual control loop(s)
|
||||||
AltitudeHoldDesiredData altitudeHoldDesired;
|
|
||||||
AltitudeHoldDesiredGet(&altitudeHoldDesired);
|
|
||||||
float positionStateDown;
|
float positionStateDown;
|
||||||
PositionStateDownGet(&positionStateDown);
|
PositionStateDownGet(&positionStateDown);
|
||||||
float velocityStateDown;
|
float velocityStateDown;
|
||||||
VelocityStateDownGet(&velocityStateDown);
|
VelocityStateDownGet(&velocityStateDown);
|
||||||
|
|
||||||
switch (altitudeHoldDesired.ControlMode) {
|
float dT;
|
||||||
case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE:
|
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
|
||||||
|
switch (thrustMode) {
|
||||||
|
case ALTITUDEHOLD:
|
||||||
// altitude control loop
|
// 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;
|
break;
|
||||||
case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY:
|
case ALTITUDEVARIO:
|
||||||
altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint;
|
altitudeHoldStatus.VelocityDesired = thrustSetpoint;
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
altitudeHoldStatus.VelocityDesired = 0;
|
altitudeHoldStatus.VelocityDesired = 0;
|
||||||
@ -162,37 +176,16 @@ static void altitudeHoldTask(void)
|
|||||||
|
|
||||||
AltitudeHoldStatusSet(&altitudeHoldStatus);
|
AltitudeHoldStatusSet(&altitudeHoldStatus);
|
||||||
|
|
||||||
float thrust;
|
switch (thrustMode) {
|
||||||
switch (altitudeHoldDesired.ControlMode) {
|
case DIRECT:
|
||||||
case ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST:
|
thrustDemand = thrustSetpoint;
|
||||||
thrust = altitudeHoldDesired.SetPoint;
|
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
// velocity control loop
|
// 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;
|
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)
|
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_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit);
|
||||||
pid_zero(&pid1);
|
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
|
* @file altitudeloop.h
|
||||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||||
* @brief Example module to be used as a template for actual modules.
|
* @brief Attitude stabilization module.
|
||||||
*
|
*
|
||||||
* @see The GNU Public License (GPL) Version 3
|
* @see The GNU Public License (GPL) Version 3
|
||||||
*
|
*
|
||||||
@ -23,9 +29,13 @@
|
|||||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||||
*/
|
*/
|
||||||
#ifndef EXAMPLEMODPERIODIC_H
|
|
||||||
#define EXAMPLEMODPERIODIC_H
|
|
||||||
|
|
||||||
int32_t ExampleModPeriodicInitialize();
|
#ifndef ALTITUDELOOP_H
|
||||||
int32_t GuidanceInitialize(void);
|
#define ALTITUDELOOP_H
|
||||||
#endif // EXAMPLEMODPERIODIC_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 <stabilization.h>
|
||||||
#include <cruisecontrol.h>
|
#include <cruisecontrol.h>
|
||||||
|
#include <altitudeloop.h>
|
||||||
#include <CoordinateConversions.h>
|
#include <CoordinateConversions.h>
|
||||||
|
|
||||||
// Private constants
|
// Private constants
|
||||||
@ -228,6 +229,13 @@ static void stabilizationOuterloopTask()
|
|||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
|
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:
|
default:
|
||||||
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
|
rateDesiredAxis[t] = stabilizationDesiredAxis[t];
|
||||||
break;
|
break;
|
||||||
|
@ -50,6 +50,7 @@
|
|||||||
#include <stabilization.h>
|
#include <stabilization.h>
|
||||||
#include <innerloop.h>
|
#include <innerloop.h>
|
||||||
#include <outerloop.h>
|
#include <outerloop.h>
|
||||||
|
#include <altitudeloop.h>
|
||||||
|
|
||||||
|
|
||||||
// Public variables
|
// Public variables
|
||||||
@ -109,6 +110,9 @@ int32_t StabilizationInitialize()
|
|||||||
|
|
||||||
stabilizationOuterloopInit();
|
stabilizationOuterloopInit();
|
||||||
stabilizationInnerloopInit();
|
stabilizationInnerloopInit();
|
||||||
|
#ifdef REVOLUTION
|
||||||
|
stabilizationAltitudeloopInit();
|
||||||
|
#endif
|
||||||
pid_zero(&stabSettings.outerPids[0]);
|
pid_zero(&stabSettings.outerPids[0]);
|
||||||
pid_zero(&stabSettings.outerPids[1]);
|
pid_zero(&stabSettings.outerPids[1]);
|
||||||
pid_zero(&stabSettings.outerPids[2]);
|
pid_zero(&stabSettings.outerPids[2]);
|
||||||
|
@ -106,7 +106,6 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
|
|||||||
UAVOBJSRCFILENAMES += oplinksettings
|
UAVOBJSRCFILENAMES += oplinksettings
|
||||||
UAVOBJSRCFILENAMES += oplinkstatus
|
UAVOBJSRCFILENAMES += oplinkstatus
|
||||||
UAVOBJSRCFILENAMES += altitudefiltersettings
|
UAVOBJSRCFILENAMES += altitudefiltersettings
|
||||||
UAVOBJSRCFILENAMES += altitudeholddesired
|
|
||||||
UAVOBJSRCFILENAMES += altitudeholdstatus
|
UAVOBJSRCFILENAMES += altitudeholdstatus
|
||||||
UAVOBJSRCFILENAMES += waypoint
|
UAVOBJSRCFILENAMES += waypoint
|
||||||
UAVOBJSRCFILENAMES += waypointactive
|
UAVOBJSRCFILENAMES += waypointactive
|
||||||
|
@ -106,7 +106,6 @@ UAVOBJSRCFILENAMES += altitudeholdsettings
|
|||||||
UAVOBJSRCFILENAMES += oplinksettings
|
UAVOBJSRCFILENAMES += oplinksettings
|
||||||
UAVOBJSRCFILENAMES += oplinkstatus
|
UAVOBJSRCFILENAMES += oplinkstatus
|
||||||
UAVOBJSRCFILENAMES += altitudefiltersettings
|
UAVOBJSRCFILENAMES += altitudefiltersettings
|
||||||
UAVOBJSRCFILENAMES += altitudeholddesired
|
|
||||||
UAVOBJSRCFILENAMES += altitudeholdstatus
|
UAVOBJSRCFILENAMES += altitudeholdstatus
|
||||||
UAVOBJSRCFILENAMES += waypoint
|
UAVOBJSRCFILENAMES += waypoint
|
||||||
UAVOBJSRCFILENAMES += waypointactive
|
UAVOBJSRCFILENAMES += waypointactive
|
||||||
|
@ -108,7 +108,6 @@ UAVOBJSRCFILENAMES += camerastabsettings
|
|||||||
UAVOBJSRCFILENAMES += altitudeholdsettings
|
UAVOBJSRCFILENAMES += altitudeholdsettings
|
||||||
UAVOBJSRCFILENAMES += altitudefiltersettings
|
UAVOBJSRCFILENAMES += altitudefiltersettings
|
||||||
UAVOBJSRCFILENAMES += revosettings
|
UAVOBJSRCFILENAMES += revosettings
|
||||||
UAVOBJSRCFILENAMES += altitudeholddesired
|
|
||||||
UAVOBJSRCFILENAMES += altitudeholdstatus
|
UAVOBJSRCFILENAMES += altitudeholdstatus
|
||||||
UAVOBJSRCFILENAMES += ekfconfiguration
|
UAVOBJSRCFILENAMES += ekfconfiguration
|
||||||
UAVOBJSRCFILENAMES += ekfstatevariance
|
UAVOBJSRCFILENAMES += ekfstatevariance
|
||||||
|
@ -35,7 +35,6 @@ HEADERS += \
|
|||||||
$$UAVOBJECT_SYNTHETICS/airspeedstate.h \
|
$$UAVOBJECT_SYNTHETICS/airspeedstate.h \
|
||||||
$$UAVOBJECT_SYNTHETICS/attitudestate.h \
|
$$UAVOBJECT_SYNTHETICS/attitudestate.h \
|
||||||
$$UAVOBJECT_SYNTHETICS/attitudesimulated.h \
|
$$UAVOBJECT_SYNTHETICS/attitudesimulated.h \
|
||||||
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
|
|
||||||
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
|
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
|
||||||
$$UAVOBJECT_SYNTHETICS/altitudeholdstatus.h \
|
$$UAVOBJECT_SYNTHETICS/altitudeholdstatus.h \
|
||||||
$$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \
|
$$UAVOBJECT_SYNTHETICS/altitudefiltersettings.h \
|
||||||
@ -137,7 +136,6 @@ SOURCES += \
|
|||||||
$$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \
|
$$UAVOBJECT_SYNTHETICS/airspeedstate.cpp \
|
||||||
$$UAVOBJECT_SYNTHETICS/attitudestate.cpp \
|
$$UAVOBJECT_SYNTHETICS/attitudestate.cpp \
|
||||||
$$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \
|
$$UAVOBJECT_SYNTHETICS/attitudesimulated.cpp \
|
||||||
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
|
|
||||||
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \
|
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \
|
||||||
$$UAVOBJECT_SYNTHETICS/altitudeholdstatus.cpp \
|
$$UAVOBJECT_SYNTHETICS/altitudeholdstatus.cpp \
|
||||||
$$UAVOBJECT_SYNTHETICS/debuglogsettings.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