flight/modules/Stabilization/virtualflybar.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup StabilizationModule Stabilization Module
   6  * @brief Virtual flybar mode
   7  * @note This file implements the logic for a virtual flybar
   8  * @{
   9  *
  10  * @file       virtualflybar.c
  11  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  12  * @brief      Attitude stabilization module.
  13  *
  14  * @see        The GNU Public License (GPL) Version 3
  15  *
  16  *****************************************************************************/
  17 /*
  18  * This program is free software; you can redistribute it and/or modify
  19  * it under the terms of the GNU General Public License as published by
  20  * the Free Software Foundation; either version 3 of the License, or
  21  * (at your option) any later version.
  22  *
  23  * This program is distributed in the hope that it will be useful, but
  24  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  25  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  26  * for more details.
  27  *
  28  * You should have received a copy of the GNU General Public License along
  29  * with this program; if not, write to the Free Software Foundation, Inc.,
  30  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  31  */
  32
  33 #include "openpilot.h"
  34 #include <pios_math.h>
  35 #include "stabilization.h"
  36 #include "stabilizationsettings.h"
  37
  38 // ! Private variables
  39 static float vbar_integral[3];
  40 static float vbar_decay = 0.991f;
  41
  42 int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings)
  43 {
  44     float gyro_gain = 1.0f;
  45     float kp = 0, ki = 0;
  46
  47     if (reinit) {
  48         vbar_integral[axis] = 0;
  49     }
  50
  51     // Track the angle of the virtual flybar which includes a slow decay
  52     vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT;
  53     vbar_integral[axis] = boundf(vbar_integral[axis], -settings->VbarMaxAngle, settings->VbarMaxAngle);
  54
  55     // Command signal can indicate how much to disregard the gyro feedback (fast flips)
  56     if (settings->VbarGyroSuppress > 0) {
  57         gyro_gain = (1.0f - fabsf(command) * settings->VbarGyroSuppress / 100.0f);
  58         gyro_gain = (gyro_gain < 0) ? 0 : gyro_gain;
  59     }
  60
  61     // Get the settings for the correct axis
  62     switch (axis) {
  63     case 0:
  64         kp = settings->VbarRollPI.Kp;
  65         ki = settings->VbarRollPI.Ki;
  66         break;
  67     case 1:
  68         kp = settings->VbarPitchPI.Kp;
  69         ki = settings->VbarPitchPI.Ki;;
  70         break;
  71     case 2:
  72         kp = settings->VbarYawPI.Kp;
  73         ki = settings->VbarYawPI.Ki;
  74         break;
  75     default:
  76         PIOS_DEBUG_Assert(0);
  77     }
  78
  79     // Command signal is composed of stick input added to the gyro and virtual flybar
  80     *output = command * StabilizationSettingsVbarSensitivityToArray(settings->VbarSensitivity)[axis] -
  81               gyro_gain * (vbar_integral[axis] * ki + gyro * kp);
  82
  83     return 0;
  84 }
  85
  86 /**
  87  * Want to keep the virtual flybar fixed in world coordinates as we pirouette
  88  * @param[in] z_gyro The deg/s of rotation along the z axis
  89  * @param[in] dT The time since last sample
  90  */
  91 int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT)
  92 {
  93     float cy = cosf(DEG2RAD(z_gyro) * dT);
  94     float sy = sinf(DEG2RAD(z_gyro) * dT);
  95
  96     float vbar_pitch = cy * vbar_integral[1] - sy * vbar_integral[0];
  97     float vbar_roll  = sy * vbar_integral[1] + cy * vbar_integral[0];
  98
  99     vbar_integral[1] = vbar_pitch;
 100     vbar_integral[0] = vbar_roll;
 101
 102     return 0;
 103 }