flight/modules/ManualControl/stabilizedhandler.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup ManualControl
   6  * @brief Interpretes the control input in ManualControlCommand
   7  * @{
   8  *
   9  * @file       stabilizedhandler.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  11  *
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  ******************************************************************************/
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  */
  30
  31 #include "inc/manualcontrol.h"
  32 #include <mathmisc.h>
  33 #include <manualcontrolcommand.h>
  34 #include <stabilizationdesired.h>
  35 #include <flightmodesettings.h>
  36 #include <stabilizationbank.h>
  37
  38 // Private constants
  39
  40 // Private types
  41
  42 // Private functions
  43 static float applyExpo(float value, float expo);
  44
  45
  46 static float applyExpo(float value, float expo)
  47 {
  48     // note: fastPow makes a small error, therefore result needs to be bound
  49     float exp = boundf(fastPow(1.00695f, expo), 0.5f, 2.0f);
  50
  51     // magic number scales expo
  52     // so that
  53     // expo=100 yields value**10
  54     // expo=0 yields value**1
  55     // expo=-100 yields value**(1/10)
  56     // (pow(2.0,1/100)~=1.00695)
  57     if (value > 0.0f) {
  58         return boundf(fastPow(value, exp), 0.0f, 1.0f);
  59     } else if (value < -0.0f) {
  60         return boundf(-fastPow(-value, exp), -1.0f, 0.0f);
  61     } else {
  62         return 0.0f;
  63     }
  64 }
  65
  66
  67 /**
  68  * @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization"
  69  * @input: ManualControlCommand
  70  * @output: StabilizationDesired
  71  */
  72 void stabilizedHandler(bool newinit)
  73 {
  74     if (newinit) {
  75         StabilizationDesiredInitialize();
  76         StabilizationBankInitialize();
  77     }
  78     ManualControlCommandData cmd;
  79     ManualControlCommandGet(&cmd);
  80
  81     FlightModeSettingsData settings;
  82     FlightModeSettingsGet(&settings);
  83
  84     StabilizationDesiredData stabilization;
  85     StabilizationDesiredGet(&stabilization);
  86
  87     StabilizationBankData stabSettings;
  88     StabilizationBankGet(&stabSettings);
  89
  90     cmd.Roll  = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
  91     cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
  92     cmd.Yaw   = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
  93     uint8_t *stab_settings;
  94     FlightStatusData flightStatus;
  95     FlightStatusGet(&flightStatus);
  96
  97     switch (flightStatus.FlightMode) {
  98     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
  99         stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
 100         break;
 101     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
 102         stab_settings = (uint8_t *)FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
 103         break;
 104     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
 105         stab_settings = (uint8_t *)FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
 106         break;
 107     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
 108         stab_settings = (uint8_t *)FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
 109         break;
 110     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
 111         stab_settings = (uint8_t *)FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
 112         break;
 113     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
 114         stab_settings = (uint8_t *)FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
 115         break;
 116     default:
 117         // Major error, this should not occur because only enter this block when one of these is true
 118         AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
 119         stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
 120         return;
 121     }
 122
 123     stabilization.Roll =
 124         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll :
 125         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
 126         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Roll * stabSettings.ManualRate.Roll :
 127         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax :
 128         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
 129         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
 130         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll :
 131         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll :
 132         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax :
 133         0; // this is an invalid mode
 134
 135     stabilization.Pitch =
 136         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch :
 137         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
 138         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
 139         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax :
 140         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
 141         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
 142         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
 143         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
 144         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
 145         0; // this is an invalid mode
 146
 147     // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
 148     stabilization.StabilizationMode.Roll  = stab_settings[0];
 149     stabilization.StabilizationMode.Pitch = stab_settings[1];
 150     // Other axes (yaw) cannot be Rattitude, so use Rate
 151     // Should really do this for Attitude mode as well?
 152     if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
 153         stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
 154         stabilization.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw;
 155     } else {
 156         stabilization.StabilizationMode.Yaw = stab_settings[2];
 157         stabilization.Yaw =
 158             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw :
 159             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
 160             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
 161             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax :
 162             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
 163             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
 164             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
 165             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
 166             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
 167             0; // this is an invalid mode
 168     }
 169
 170     stabilization.StabilizationMode.Thrust = stab_settings[3];
 171     stabilization.Thrust = cmd.Thrust;
 172     StabilizationDesiredSet(&stabilization);
 173 }
 174
 175
 176 /**
 177  * @}
 178  * @}
 179  */