flight/modules/TxPID/txpid.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup TxPIDModule TxPID Module
   6  * @brief      Optional module to tune PID settings using R/C transmitter.
   7  * Updates PID settings in RAM in real-time using configured Accessory channels as controllers.
   8  * @{
   9  *
  10  * @file       txpid.c
  11  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
  12  * @brief      Optional module to tune PID settings using R/C transmitter.
  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 /**
  34  * Output object: StabilizationSettings
  35  *
  36  * This module will periodically update values of stabilization PID settings
  37  * depending on configured input control channels. New values of stabilization
  38  * settings are not saved to flash, but updated in RAM. It is expected that the
  39  * module will be enabled only for tuning. When desired values are found, they
  40  * can be read via GCS and saved permanently. Then this module should be
  41  * disabled again.
  42  *
  43  * UAVObjects are automatically generated by the UAVObjectGenerator from
  44  * the object definition XML file.
  45  *
  46  * Modules have no API, all communication to other modules is done through UAVObjects.
  47  * However modules may use the API exposed by shared libraries.
  48  * See the OpenPilot wiki for more details.
  49  * http://wiki.openpilot.org/display/Doc/OpenPilot+Architecture
  50  *
  51  */
  52
  53 #include "openpilot.h"
  54 #include <pios_struct_helper.h>
  55 #include "txpidsettings.h"
  56 #include "accessorydesired.h"
  57 #include "manualcontrolcommand.h"
  58 #include "stabilizationsettings.h"
  59 #include "stabilizationbank.h"
  60 #include "stabilizationsettingsbank1.h"
  61 #include "stabilizationsettingsbank2.h"
  62 #include "stabilizationsettingsbank3.h"
  63 #include "flightstatus.h"
  64 #include "hwsettings.h"
  65
  66 //
  67 // Configuration
  68 //
  69 #define SAMPLE_PERIOD_MS           200
  70 #define TELEMETRY_UPDATE_PERIOD_MS 0 // 0 = update on change (default)
  71
  72 // Sanity checks
  73 #if (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_INPUTS_NUMELEM) || \
  74     (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_MINPID_NUMELEM) || \
  75     (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_MAXPID_NUMELEM)
  76 #error Invalid TxPID UAVObject definition (inconsistent number of field elements)
  77 #endif
  78
  79 // Private types
  80
  81 // Private variables
  82
  83 // Private functions
  84 static void updatePIDs(UAVObjEvent *ev);
  85 static uint8_t update(float *var, float val);
  86 static float scale(float val, float inMin, float inMax, float outMin, float outMax);
  87
  88 /**
  89  * Initialise the module, called on startup
  90  * \returns 0 on success or -1 if initialisation failed
  91  */
  92 int32_t TxPIDInitialize(void)
  93 {
  94     bool txPIDEnabled;
  95     HwSettingsOptionalModulesData optionalModules;
  96
  97     HwSettingsInitialize();
  98     HwSettingsOptionalModulesGet(&optionalModules);
  99
 100     if (optionalModules.TxPID == HWSETTINGS_OPTIONALMODULES_ENABLED) {
 101         txPIDEnabled = true;
 102     } else {
 103         txPIDEnabled = false;
 104     }
 105
 106     if (txPIDEnabled) {
 107         TxPIDSettingsInitialize();
 108         AccessoryDesiredInitialize();
 109
 110         UAVObjEvent ev = {
 111             .obj    = AccessoryDesiredHandle(),
 112             .instId = 0,
 113             .event  = 0,
 114             .lowPriority = false,
 115         };
 116         EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
 117
 118 #if (TELEMETRY_UPDATE_PERIOD_MS != 0)
 119         // Change StabilizationSettings update rate from OnChange to periodic
 120         // to prevent telemetry link flooding with frequent updates in case of
 121         // control channel jitter.
 122         // Warning: saving to flash with this code active will change the
 123         // StabilizationSettings update rate permanently. Use Metadata via
 124         // browser to reset to defaults (telemetryAcked=true, OnChange).
 125         UAVObjMetadata metadata;
 126         StabilizationSettingsInitialize();
 127         StabilizationSettingsGetMetadata(&metadata);
 128         metadata.telemetryAcked = 0;
 129         metadata.telemetryUpdateMode   = UPDATEMODE_PERIODIC;
 130         metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
 131         StabilizationSettingsSetMetadata(&metadata);
 132 #endif
 133
 134         return 0;
 135     }
 136
 137     return -1;
 138 }
 139
 140 /* stub: module has no module thread */
 141 int32_t TxPIDStart(void)
 142 {
 143     return 0;
 144 }
 145
 146 MODULE_INITCALL(TxPIDInitialize, TxPIDStart);
 147
 148 /**
 149  * Update PIDs callback function
 150  */
 151 static void updatePIDs(UAVObjEvent *ev)
 152 {
 153     if (ev->obj != AccessoryDesiredHandle()) {
 154         return;
 155     }
 156
 157     TxPIDSettingsData inst;
 158     TxPIDSettingsGet(&inst);
 159
 160     if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) {
 161         return;
 162     }
 163
 164     uint8_t armed;
 165     FlightStatusArmedGet(&armed);
 166     if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
 167         (armed == FLIGHTSTATUS_ARMED_DISARMED)) {
 168         return;
 169     }
 170
 171     StabilizationBankData bank;
 172     switch (inst.BankNumber) {
 173     case 0:
 174         StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
 175         break;
 176
 177     case 1:
 178         StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
 179         break;
 180
 181     case 2:
 182         StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
 183         break;
 184
 185     default:
 186         return;
 187     }
 188     StabilizationSettingsData stab;
 189     StabilizationSettingsGet(&stab);
 190     AccessoryDesiredData accessory;
 191
 192     uint8_t needsUpdateBank = 0;
 193     uint8_t needsUpdateStab = 0;
 194
 195     // Loop through every enabled instance
 196     for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
 197         if (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
 198             float value;
 199             if (cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
 200                 ManualControlCommandThrottleGet(&value);
 201                 value = scale(value,
 202                               inst.ThrottleRange.Min,
 203                               inst.ThrottleRange.Max,
 204                               cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i],
 205                               cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]);
 206             } else if (AccessoryDesiredInstGet(
 207                            cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0,
 208                            &accessory) == 0) {
 209                 value = scale(accessory.AccessoryVal, -1.0f, 1.0f,
 210                               cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i],
 211                               cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]);
 212             } else {
 213                 continue;
 214             }
 215
 216             switch (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i]) {
 217             case TXPIDSETTINGS_PIDS_ROLLRATEKP:
 218                 needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
 219                 break;
 220             case TXPIDSETTINGS_PIDS_ROLLRATEKI:
 221                 needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
 222                 break;
 223             case TXPIDSETTINGS_PIDS_ROLLRATEKD:
 224                 needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
 225                 break;
 226             case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
 227                 needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
 228                 break;
 229             case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
 230                 needsUpdateBank |= update(&bank.RollPI.Kp, value);
 231                 break;
 232             case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
 233                 needsUpdateBank |= update(&bank.RollPI.Ki, value);
 234                 break;
 235             case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
 236                 needsUpdateBank |= update(&bank.RollPI.ILimit, value);
 237                 break;
 238             case TXPIDSETTINGS_PIDS_PITCHRATEKP:
 239                 needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
 240                 break;
 241             case TXPIDSETTINGS_PIDS_PITCHRATEKI:
 242                 needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
 243                 break;
 244             case TXPIDSETTINGS_PIDS_PITCHRATEKD:
 245                 needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
 246                 break;
 247             case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
 248                 needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
 249                 break;
 250             case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
 251                 needsUpdateBank |= update(&bank.PitchPI.Kp, value);
 252                 break;
 253             case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
 254                 needsUpdateBank |= update(&bank.PitchPI.Ki, value);
 255                 break;
 256             case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
 257                 needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
 258                 break;
 259             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
 260                 needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
 261                 needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
 262                 break;
 263             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
 264                 needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
 265                 needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
 266                 break;
 267             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
 268                 needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
 269                 needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
 270                 break;
 271             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
 272                 needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
 273                 needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
 274                 break;
 275             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
 276                 needsUpdateBank |= update(&bank.RollPI.Kp, value);
 277                 needsUpdateBank |= update(&bank.PitchPI.Kp, value);
 278                 break;
 279             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
 280                 needsUpdateBank |= update(&bank.RollPI.Ki, value);
 281                 needsUpdateBank |= update(&bank.PitchPI.Ki, value);
 282                 break;
 283             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
 284                 needsUpdateBank |= update(&bank.RollPI.ILimit, value);
 285                 needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
 286                 break;
 287             case TXPIDSETTINGS_PIDS_YAWRATEKP:
 288                 needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
 289                 break;
 290             case TXPIDSETTINGS_PIDS_YAWRATEKI:
 291                 needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
 292                 break;
 293             case TXPIDSETTINGS_PIDS_YAWRATEKD:
 294                 needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
 295                 break;
 296             case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
 297                 needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
 298                 break;
 299             case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
 300                 needsUpdateBank |= update(&bank.YawPI.Kp, value);
 301                 break;
 302             case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
 303                 needsUpdateBank |= update(&bank.YawPI.Ki, value);
 304                 break;
 305             case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
 306                 needsUpdateBank |= update(&bank.YawPI.ILimit, value);
 307                 break;
 308             case TXPIDSETTINGS_PIDS_GYROTAU:
 309                 needsUpdateStab |= update(&stab.GyroTau, value);
 310                 break;
 311             default:
 312                 PIOS_Assert(0);
 313             }
 314         }
 315     }
 316     if (needsUpdateStab) {
 317         StabilizationSettingsSet(&stab);
 318     }
 319     if (needsUpdateBank) {
 320         switch (inst.BankNumber) {
 321         case 0:
 322             StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *)&bank);
 323             break;
 324
 325         case 1:
 326             StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
 327             break;
 328
 329         case 2:
 330             StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *)&bank);
 331             break;
 332
 333         default:
 334             return;
 335         }
 336     }
 337 }
 338
 339 /**
 340  * Scales input val from [inMin..inMax] range to [outMin..outMax].
 341  * If val is out of input range (inMin <= inMax), it will be bound.
 342  * (outMin > outMax) is ok, in that case output will be decreasing.
 343  *
 344  * \returns scaled value
 345  */
 346 static float scale(float val, float inMin, float inMax, float outMin, float outMax)
 347 {
 348     // bound input value
 349     if (val > inMax) {
 350         val = inMax;
 351     }
 352     if (val < inMin) {
 353         val = inMin;
 354     }
 355
 356     // normalize input value to [0..1]
 357     if (inMax <= inMin) {
 358         val = 0.0f;
 359     } else {
 360         val = (val - inMin) / (inMax - inMin);
 361     }
 362
 363     // update output bounds
 364     if (outMin > outMax) {
 365         float t = outMin;
 366         outMin = outMax;
 367         outMax = t;
 368         val    = 1.0f - val;
 369     }
 370
 371     return (outMax - outMin) * val + outMin;
 372 }
 373
 374 /**
 375  * Updates var using val if needed.
 376  * \returns 1 if updated, 0 otherwise
 377  */
 378 static uint8_t update(float *var, float val)
 379 {
 380     /* FIXME: this is not an entirely correct way
 381      * to check if the two floating point
 382      * numbers are 'not equal'.
 383      * Epsilon of 1e-9 is probably okay for the range
 384      * of numbers we see here*/
 385     if (fabsf(*var - val) > 1e-9f) {
 386         *var = val;
 387         return 1;
 388     }
 389     return 0;
 390 }
 391
 392 /**
 393  * @}
 394  */
 395
 396 /**
 397  * @}
 398  */