rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

OP-1309 inner stabilization loop in new incarnation - is not used yet until outer loop rewritten as well

Browse Source
This commit is contained in:
Corvus Corax 2014-04-26 22:37:49 +02:00
parent 33acbc4209
commit 3f7c9e3679
8 changed files with 330 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
flight/modules/Stabilization/inc/cruisecontrol.h Normal file
View File

@ -0,0 +1,38 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief cruisecontrol mode
* @note This file implements the logic for a cruisecontrol
* @{
*
* @file cruisecontrol.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization module.
*
* @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 CRUISECONTROL_H
#define CRUISECONTROL_H
float cruisecontrol_apply_factor(float raw);
#endif /* CRUISECONTROL_H */

38
flight/modules/Stabilization/inc/innerloop.h Normal file
View File

@ -0,0 +1,38 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief innerloop mode
* @note This file implements the logic for a innerloop
* @{
*
* @file innerloop.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization module.
*
* @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 INNERLOOP_H
#define INNERLOOP_H
void stabilizationInnerloopInit();
#endif /* INNERLOOP_H */

11
flight/modules/Stabilization/inc/stabilization.h
View File

@ -33,10 +33,19 @@
#ifndef STABILIZATION_H
#define STABILIZATION_H
enum { ROLL, PITCH, YAW, MAX_AXES };
#include <stabilizationsettings.h>
int32_t StabilizationInitialize();
typedef struct {
StabilizationSettingsData settings;
float gyro_alpha;
} StabilizationData;
extern StabilizationData stabSettings;
#endif // STABILIZATION_H
/**

231
flight/modules/Stabilization/innerloop.c Normal file
View File

@ -0,0 +1,231 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup StabilizationModule Stabilization Module
* @brief Stabilization PID loops in an airframe type independent manner
* @note This object updates the @ref ActuatorDesired "Actuator Desired" based on the
* PID loops on the @ref AttitudeDesired "Attitude Desired" and @ref AttitudeState "Attitude State"
* @{
*
* @file rateloop.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief Attitude stabilization module.
*
* @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 <openpilot.h>
#include <pios_struct_helper.h>
#include <pid.h>
#include <callbackinfo.h>
#include <ratedesired.h>
#include <actuatordesired.h>
#include <gyrostate.h>
#include <airspeedstate.h>
#include <stabilizationstatus.h>
#include <flightstatus.h>
#include <stabilizationbank.h>
#include <stabilization.h>
#include <relay_tuning.h>
#include <virtualflybar.h>
#include <cruisecontrol.h>
// Private constants
#define STACK_SIZE_BYTES 256
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL
#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 AXES 4
// Private variables
static DelayedCallbackInfo *callbackHandle;
static struct pid pids[3];
static float gyro_filtered[3] = { 0, 0, 0 };
static uint8_t previous_mode[AXES] = { 255, 255, 255, 255 };
static PiOSDeltatimeConfig timeval;
static float speedScaleFactor = 1.0f;
// Private functions
static void stabilizationInnerloopTask();
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
#ifdef REVOLUTION
static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
#endif
void stabilizationInnerloopInit()
{
RateDesiredInitialize();
ActuatorDesiredInitialize();
GyroStateInitialize();
StabilizationStatusInitialize();
FlightStatusInitialize();
StabilizationBankInitialize();
#ifdef REVOLUTION
AirspeedStateInitialize();
AirspeedStateConnectCallback(AirSpeedUpdatedCb);
#endif
PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&stabilizationInnerloopTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_STABILIZATION1, STACK_SIZE_BYTES);
GyroStateConnectCallback(GyroStateUpdatedCb);
StabilizationBankConnectCallback(BankUpdatedCb);
BankUpdatedCb(NULL);
}
/**
* WARNING! This callback executes with critical flight control priority every
* time a gyroscope update happens do NOT put any time consuming calculations
* in this loop unless they really have to execute with every gyro update
*/
static void stabilizationInnerloopTask()
{
RateDesiredData rateDesired;
ActuatorDesiredData actuator;
StabilizationStatusInnerLoopData enabled;
FlightStatusControlChainData cchain;
RateDesiredGet(&rateDesired);
ActuatorDesiredGet(&actuator);
StabilizationStatusInnerLoopGet(&enabled);
FlightStatusControlChainGet(&cchain);
float *rate = &rateDesired.Roll;
float *actuatorDesiredAxis = &actuator.Roll;
int t;
float dT;
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) {
if (reinit) {
pids[t].iAccumulator = 0;
}
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings);
break;
case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING:
stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit);
break;
case STABILIZATIONSTATUS_INNERLOOP_RATE:
actuatorDesiredAxis[t] = pid_apply_setpoint(&pids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:
actuatorDesiredAxis[t] = rate[t];
break;
}
} else {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL:
actuatorDesiredAxis[t] = cruisecontrol_apply_factor(rate[t]);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:
actuatorDesiredAxis[t] = rate[t];
break;
}
}
actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
}
if (cchain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
ActuatorDesiredSet(&actuator);
} else {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
}
}
}
static void GyroStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
GyroStateData gyroState;
GyroStateGet(&gyroState);
gyro_filtered[0] = gyro_filtered[0] * stabSettings.gyro_alpha + gyroState.x * (1 - stabSettings.gyro_alpha);
gyro_filtered[1] = gyro_filtered[1] * stabSettings.gyro_alpha + gyroState.y * (1 - stabSettings.gyro_alpha);
gyro_filtered[2] = gyro_filtered[2] * stabSettings.gyro_alpha + gyroState.z * (1 - stabSettings.gyro_alpha);
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
}
static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
StabilizationBankData bank;
StabilizationBankGet(&bank);
// Set the roll rate PID constants
pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_ROLL], bank.RollRatePID.Kp,
bank.RollRatePID.Ki,
bank.RollRatePID.Kd,
bank.RollRatePID.ILimit);
// Set the pitch rate PID constants
pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_PITCH], bank.PitchRatePID.Kp,
bank.PitchRatePID.Ki,
bank.PitchRatePID.Kd,
bank.PitchRatePID.ILimit);
// Set the yaw rate PID constants
pid_configure(&pids[STABILIZATIONSTATUS_INNERLOOP_YAW], bank.YawRatePID.Kp,
bank.YawRatePID.Ki,
bank.YawRatePID.Kd,
bank.YawRatePID.ILimit);
}
#ifdef REVOLUTION
static void AirSpeedUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
// Scale PID coefficients based on current airspeed estimation - needed for fixed wing planes
AirspeedStateData airspeedState;
AirspeedStateGet(&airspeedState);
if (stabSettings.settings.ScaleToAirspeed < 0.1f || airspeedState.CalibratedAirspeed < 0.1f) {
// feature has been turned off
speedScaleFactor = 1.0f;
} else {
// scale the factor to be 1.0 at the specified airspeed (for example 10m/s) but scaled by 1/speed^2
speedScaleFactor = boundf((stabSettings.settings.ScaleToAirspeed * stabSettings.settings.ScaleToAirspeed) / (airspeedState.CalibratedAirspeed * airspeedState.CalibratedAirspeed),
stabSettings.settings.ScaleToAirspeedLimits.Min,
stabSettings.settings.ScaleToAirspeedLimits.Max);
}
}
#endif
/**
* @}
* @}
*/

2
flight/modules/Stabilization/relay_tuning.c
View File

@ -63,7 +63,7 @@ int stabilization_relay_rate(float error, float *output, int axis, bool reinit)
portTickType thisTime = xTaskGetTickCount();
static bool rateRelayRunning[MAX_AXES];
static bool rateRelayRunning[3];
// This indicates the current estimate of the smoothed error. So when it is high
// we are waiting for it to go low.

2
flight/modules/Stabilization/stabilization.c
View File

@ -71,6 +71,8 @@
#define UPDATE_MAX 1.0f
#define UPDATE_ALPHA 1.0e-2f
typedef enum { ROLL = 0, PITCH = 1, YAW = 2, MAX_AXES = 3 } blaenum;
#define MAX_QUEUE_SIZE 1
#if defined(PIOS_STABILIZATION_STACK_SIZE)

8
flight/modules/Stabilization/virtualflybar.c
View File

@ -37,7 +37,7 @@
#include "stabilizationsettings.h"
// ! Private variables
static float vbar_integral[MAX_AXES];
static float vbar_integral[3];
static float vbar_decay = 0.991f;
int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings)
@ -61,15 +61,15 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
// Get the settings for the correct axis
switch (axis) {
case ROLL:
case 0:
kp = settings->VbarRollPI.Kp;
ki = settings->VbarRollPI.Ki;
break;
case PITCH:
case 1:
kp = settings->VbarPitchPI.Kp;
ki = settings->VbarPitchPI.Ki;;
break;
case YAW:
case 2:
kp = settings->VbarYawPI.Kp;
ki = settings->VbarYawPI.Ki;
break;

6
shared/uavobjectdefinition/callbackinfo.xml
View File

@ -6,6 +6,8 @@
<elementname>EventDispatcher</elementname>
<elementname>StateEstimation</elementname>
<elementname>AltitudeHold</elementname>
<elementname>Stabilization0</elementname>
<elementname>Stabilization1</elementname>
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>
@ -16,6 +18,8 @@
<elementname>EventDispatcher</elementname>
<elementname>StateEstimation</elementname>
<elementname>AltitudeHold</elementname>
<elementname>Stabilization0</elementname>
<elementname>Stabilization1</elementname>
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>
@ -30,6 +34,8 @@
<elementname>EventDispatcher</elementname>
<elementname>StateEstimation</elementname>
<elementname>AltitudeHold</elementname>
<elementname>Stabilization0</elementname>
<elementname>Stabilization1</elementname>
<elementname>PathPlanner0</elementname>
<elementname>PathPlanner1</elementname>
<elementname>ManualControl</elementname>