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 LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  11  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  12  *
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  ******************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 #include "inc/manualcontrol.h"
  33 #include <mathmisc.h>
  34 #include <sin_lookup.h>
  35 #include <manualcontrolcommand.h>
  36 #include <stabilizationdesired.h>
  37 #include <flightmodesettings.h>
  38 #include <stabilizationbank.h>
  39 #include <flightstatus.h>
  40
  41 // Private constants
  42
  43 // Private types
  44
  45 // Private functions
  46 static float applyExpo(float value, float expo);
  47
  48 // Private variables
  49 static uint8_t currentFpvTiltAngle = 0;
  50 static float cosAngle = 0.0f;
  51 static float sinAngle = 0.0f;
  52
  53
  54 static float applyExpo(float value, float expo)
  55 {
  56     // note: fastPow makes a small error, therefore result needs to be bound
  57     float exp = boundf(fastPow(1.01395948f, expo), 0.25f, 4.0f);
  58
  59     // magic number scales expo
  60     // so that
  61     // expo=100 yields value**10
  62     // expo=0 yields value**1
  63     // expo=-100 yields value**(1/10)
  64     // (pow(4.0,1/100)~=1.01395948)
  65     if (value > 0.0f) {
  66         return boundf(fastPow(value, exp), 0.0f, 1.0f);
  67     } else if (value < -0.0f) {
  68         return boundf(-fastPow(-value, exp), -1.0f, 0.0f);
  69     } else {
  70         return 0.0f;
  71     }
  72 }
  73
  74
  75 /**
  76  * @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization"
  77  * @input: ManualControlCommand
  78  * @output: StabilizationDesired
  79  */
  80 void stabilizedHandler(__attribute__((unused)) bool newinit)
  81 {
  82     static bool inited = false;
  83
  84     if (!inited) {
  85         inited = true;
  86         StabilizationDesiredInitialize();
  87         StabilizationBankInitialize();
  88     }
  89
  90     ManualControlCommandData cmd;
  91     ManualControlCommandGet(&cmd);
  92
  93     FlightModeSettingsData settings;
  94     FlightModeSettingsGet(&settings);
  95
  96     StabilizationDesiredData stabilization;
  97     StabilizationDesiredGet(&stabilization);
  98
  99     StabilizationBankData stabSettings;
 100     StabilizationBankGet(&stabSettings);
 101
 102     cmd.Roll  = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
 103     cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
 104     cmd.Yaw   = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
 105
 106     if (stabSettings.FpvCamTiltCompensation > 0) {
 107         // Reduce Cpu load
 108         if (currentFpvTiltAngle != stabSettings.FpvCamTiltCompensation) {
 109             cosAngle = cos_lookup_deg((float)stabSettings.FpvCamTiltCompensation);
 110             sinAngle = sin_lookup_deg((float)stabSettings.FpvCamTiltCompensation);
 111             currentFpvTiltAngle = stabSettings.FpvCamTiltCompensation;
 112         }
 113         float rollCommand = cmd.Roll;
 114         float yawCommand  = cmd.Yaw;
 115
 116         // http://shrediquette.blogspot.de/2016/01/some-thoughts-on-camera-tilt.html
 117         // When Roll right, add negative Yaw.
 118         // When Yaw left, add negative Roll.
 119         cmd.Roll = boundf((cosAngle * rollCommand) + (sinAngle * yawCommand), -1.0f, 1.0f);
 120         cmd.Yaw  = boundf((cosAngle * yawCommand) - (sinAngle * rollCommand), -1.0f, 1.0f);
 121     }
 122
 123     uint8_t *stab_settings;
 124     FlightStatusData flightStatus;
 125     FlightStatusGet(&flightStatus);
 126     switch (flightStatus.FlightMode) {
 127     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
 128         stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
 129         break;
 130     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
 131         stab_settings = (uint8_t *)FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
 132         break;
 133     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
 134         stab_settings = (uint8_t *)FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
 135         break;
 136     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
 137         stab_settings = (uint8_t *)FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
 138         break;
 139     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
 140         stab_settings = (uint8_t *)FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
 141         break;
 142     case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
 143         stab_settings = (uint8_t *)FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
 144         break;
 145 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 146     case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
 147         // let autotune.c handle it
 148         // because it must switch to Attitude after <user configurable> seconds
 149         return;
 150
 151 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 152     default:
 153         // Major error, this should not occur because only enter this block when one of these is true
 154         AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
 155         stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
 156         return;
 157     }
 158
 159     stabilization.Roll =
 160         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Roll :
 161         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll :
 162         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Roll * stabSettings.ManualRate.Roll :
 163         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.RollMax :
 164         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax :
 165         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll :
 166         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll :
 167         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Roll * stabSettings.ManualRate.Roll :
 168         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll * stabSettings.RollMax :
 169 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 170         (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Roll * stabSettings.RollMax :
 171 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 172         0; // this is an invalid mode
 173
 174     stabilization.Pitch =
 175         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Pitch :
 176         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
 177         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
 178         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.PitchMax :
 179         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
 180         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
 181         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch :
 182         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Pitch * stabSettings.ManualRate.Pitch :
 183         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch * stabSettings.PitchMax :
 184 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 185         (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Pitch * stabSettings.PitchMax :
 186 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 187         0; // this is an invalid mode
 188
 189     // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
 190     stabilization.StabilizationMode.Roll  = stab_settings[0];
 191     stabilization.StabilizationMode.Pitch = stab_settings[1];
 192     // Other axes (yaw) cannot be Rattitude, so use Rate
 193     // Should really do this for Attitude mode as well?
 194     if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
 195         stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
 196         stabilization.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw;
 197     } else {
 198         stabilization.StabilizationMode.Yaw = stab_settings[2];
 199         stabilization.Yaw =
 200             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL) ? cmd.Yaw :
 201             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
 202             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATETRAINER) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
 203             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.YawMax :
 204             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
 205             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
 206             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw :
 207             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ACRO) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
 208             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw * stabSettings.YawMax :
 209 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 210             (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT) ? cmd.Yaw * stabSettings.ManualRate.Yaw :
 211 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 212             0; // this is an invalid mode
 213     }
 214
 215     stabilization.StabilizationMode.Thrust = stab_settings[3];
 216     stabilization.Thrust = cmd.Thrust;
 217     StabilizationDesiredSet(&stabilization);
 218 }
 219
 220
 221 /**
 222  * @}
 223  * @}
 224  */