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 "txpidsettings.h"
  55 #include "accessorydesired.h"
  56 #include "manualcontrolcommand.h"
  57 #include "stabilizationsettings.h"
  58 #include "attitudesettings.h"
  59 #ifdef REVOLUTION
  60 #include "altitudeholdsettings.h"
  61 #endif
  62 #include "stabilizationbank.h"
  63 #include "stabilizationsettingsbank1.h"
  64 #include "stabilizationsettingsbank2.h"
  65 #include "stabilizationsettingsbank3.h"
  66 #include "flightstatus.h"
  67 #include "txpidstatus.h"
  68 #include "hwsettings.h"
  69
  70 //
  71 // Configuration
  72 //
  73 #define SAMPLE_PERIOD_MS           200
  74 #define TELEMETRY_UPDATE_PERIOD_MS 0 // 0 = update on change (default)
  75
  76 // Sanity checks
  77 #if (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_INPUTS_NUMELEM) || \
  78     (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_MINPID_NUMELEM) || \
  79     (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_MAXPID_NUMELEM)
  80 #error Invalid TxPID UAVObject definition (inconsistent number of field elements)
  81 #endif
  82
  83 // Private types
  84
  85 // Private variables
  86
  87 // Private functions
  88 static void updatePIDs(UAVObjEvent *ev);
  89 static uint8_t update(float *var, float val);
  90 static uint8_t updateUint8(uint8_t *var, float val);
  91 static uint8_t updateInt8(int8_t *var, float val);
  92 static float scale(float val, float inMin, float inMax, float outMin, float outMax);
  93
  94 /**
  95  * Initialise the module, called on startup
  96  * \returns 0 on success or -1 if initialisation failed
  97  */
  98 int32_t TxPIDInitialize(void)
  99 {
 100     bool txPIDEnabled;
 101     HwSettingsOptionalModulesData optionalModules;
 102
 103 #ifdef REVOLUTION
 104     AltitudeHoldSettingsInitialize();
 105 #endif
 106
 107     HwSettingsInitialize();
 108     HwSettingsOptionalModulesGet(&optionalModules);
 109
 110     if (optionalModules.TxPID == HWSETTINGS_OPTIONALMODULES_ENABLED) {
 111         txPIDEnabled = true;
 112     } else {
 113         txPIDEnabled = false;
 114     }
 115
 116     if (txPIDEnabled) {
 117         TxPIDSettingsInitialize();
 118         TxPIDStatusInitialize();
 119         AccessoryDesiredInitialize();
 120
 121         UAVObjEvent ev = {
 122             .obj    = AccessoryDesiredHandle(),
 123             .instId = 0,
 124             .event  = 0,
 125             .lowPriority = false,
 126         };
 127         EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
 128
 129 #if (TELEMETRY_UPDATE_PERIOD_MS != 0)
 130         // Change StabilizationSettings update rate from OnChange to periodic
 131         // to prevent telemetry link flooding with frequent updates in case of
 132         // control channel jitter.
 133         // Warning: saving to flash with this code active will change the
 134         // StabilizationSettings update rate permanently. Use Metadata via
 135         // browser to reset to defaults (telemetryAcked=true, OnChange).
 136         UAVObjMetadata metadata;
 137         StabilizationSettingsInitialize();
 138         StabilizationSettingsGetMetadata(&metadata);
 139         metadata.telemetryAcked = 0;
 140         metadata.telemetryUpdateMode   = UPDATEMODE_PERIODIC;
 141         metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
 142         StabilizationSettingsSetMetadata(&metadata);
 143
 144         AttitudeSettingsInitialize();
 145         AttitudeSettingsGetMetadata(&metadata);
 146         metadata.telemetryAcked = 0;
 147         metadata.telemetryUpdateMode   = UPDATEMODE_PERIODIC;
 148         metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
 149         AttitudeSettingsSetMetadata(&metadata);
 150 #endif /* if (TELEMETRY_UPDATE_PERIOD_MS != 0) */
 151
 152         return 0;
 153     }
 154
 155     return -1;
 156 }
 157
 158 /* stub: module has no module thread */
 159 int32_t TxPIDStart(void)
 160 {
 161     return 0;
 162 }
 163
 164 MODULE_INITCALL(TxPIDInitialize, TxPIDStart);
 165
 166 /**
 167  * Update PIDs callback function
 168  */
 169 static void updatePIDs(UAVObjEvent *ev)
 170 {
 171     if (ev->obj != AccessoryDesiredHandle()) {
 172         return;
 173     }
 174
 175     TxPIDSettingsData inst;
 176     TxPIDSettingsGet(&inst);
 177
 178     if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) {
 179         return;
 180     }
 181
 182     uint8_t armed;
 183     FlightStatusArmedGet(&armed);
 184     if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
 185         (armed == FLIGHTSTATUS_ARMED_DISARMED)) {
 186         return;
 187     }
 188
 189     StabilizationBankData bank;
 190     switch (inst.BankNumber) {
 191     case 0:
 192         StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
 193         break;
 194
 195     case 1:
 196         StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
 197         break;
 198
 199     case 2:
 200         StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank);
 201         break;
 202
 203     default:
 204         return;
 205     }
 206     StabilizationSettingsData stab;
 207     StabilizationSettingsGet(&stab);
 208
 209     AttitudeSettingsData att;
 210     AttitudeSettingsGet(&att);
 211
 212 #ifdef REVOLUTION
 213     AltitudeHoldSettingsData altitude;
 214     AltitudeHoldSettingsGet(&altitude);
 215 #endif
 216     AccessoryDesiredData accessory;
 217
 218     TxPIDStatusData txpid_status;
 219     TxPIDStatusGet(&txpid_status);
 220
 221     uint8_t needsUpdateBank     = 0;
 222     uint8_t needsUpdateStab     = 0;
 223     uint8_t needsUpdateAtt      = 0;
 224 #ifdef REVOLUTION
 225     uint8_t needsUpdateAltitude = 0;
 226 #endif
 227
 228     // Loop through every enabled instance
 229     for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
 230         if (TxPIDSettingsPIDsToArray(inst.PIDs)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
 231             float value;
 232             if (TxPIDSettingsInputsToArray(inst.Inputs)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
 233                 ManualControlCommandThrottleGet(&value);
 234                 value = scale(value,
 235                               inst.ThrottleRange.Min,
 236                               inst.ThrottleRange.Max,
 237                               TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
 238                               TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
 239             } else if (AccessoryDesiredInstGet(
 240                            TxPIDSettingsInputsToArray(inst.Inputs)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0,
 241                            &accessory) == 0) {
 242                 value = scale(accessory.AccessoryVal, -1.0f, 1.0f,
 243                               TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
 244                               TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
 245             } else {
 246                 continue;
 247             }
 248
 249             TxPIDStatusCurPIDToArray(txpid_status.CurPID)[i] = value;
 250
 251             switch (TxPIDSettingsPIDsToArray(inst.PIDs)[i]) {
 252             case TXPIDSETTINGS_PIDS_ROLLRATEKP:
 253                 needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
 254                 break;
 255             case TXPIDSETTINGS_PIDS_ROLLRATEKI:
 256                 needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
 257                 break;
 258             case TXPIDSETTINGS_PIDS_ROLLRATEKD:
 259                 needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
 260                 break;
 261             case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
 262                 needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
 263                 break;
 264             case TXPIDSETTINGS_PIDS_ROLLRATERESP:
 265                 needsUpdateBank |= update(&bank.ManualRate.Roll, value);
 266                 break;
 267             case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
 268                 needsUpdateBank |= update(&bank.RollPI.Kp, value);
 269                 break;
 270             case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
 271                 needsUpdateBank |= update(&bank.RollPI.Ki, value);
 272                 break;
 273             case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
 274                 needsUpdateBank |= update(&bank.RollPI.ILimit, value);
 275                 break;
 276             case TXPIDSETTINGS_PIDS_ROLLATTITUDERESP:
 277                 needsUpdateBank |= updateUint8(&bank.RollMax, value);
 278                 break;
 279             case TXPIDSETTINGS_PIDS_PITCHRATEKP:
 280                 needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
 281                 break;
 282             case TXPIDSETTINGS_PIDS_PITCHRATEKI:
 283                 needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
 284                 break;
 285             case TXPIDSETTINGS_PIDS_PITCHRATEKD:
 286                 needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
 287                 break;
 288             case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
 289                 needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
 290                 break;
 291             case TXPIDSETTINGS_PIDS_PITCHRATERESP:
 292                 needsUpdateBank |= update(&bank.ManualRate.Pitch, value);
 293                 break;
 294             case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
 295                 needsUpdateBank |= update(&bank.PitchPI.Kp, value);
 296                 break;
 297             case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
 298                 needsUpdateBank |= update(&bank.PitchPI.Ki, value);
 299                 break;
 300             case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
 301                 needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
 302                 break;
 303             case TXPIDSETTINGS_PIDS_PITCHATTITUDERESP:
 304                 needsUpdateBank |= updateUint8(&bank.PitchMax, value);
 305                 break;
 306             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
 307                 needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
 308                 needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
 309                 break;
 310             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
 311                 needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
 312                 needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
 313                 break;
 314             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
 315                 needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
 316                 needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
 317                 break;
 318             case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
 319                 needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
 320                 needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
 321                 break;
 322             case TXPIDSETTINGS_PIDS_ROLLPITCHRATERESP:
 323                 needsUpdateBank |= update(&bank.ManualRate.Roll, value);
 324                 needsUpdateBank |= update(&bank.ManualRate.Pitch, value);
 325                 break;
 326             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
 327                 needsUpdateBank |= update(&bank.RollPI.Kp, value);
 328                 needsUpdateBank |= update(&bank.PitchPI.Kp, value);
 329                 break;
 330             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
 331                 needsUpdateBank |= update(&bank.RollPI.Ki, value);
 332                 needsUpdateBank |= update(&bank.PitchPI.Ki, value);
 333                 break;
 334             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
 335                 needsUpdateBank |= update(&bank.RollPI.ILimit, value);
 336                 needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
 337                 break;
 338             case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDERESP:
 339                 needsUpdateBank |= updateUint8(&bank.RollMax, value);
 340                 needsUpdateBank |= updateUint8(&bank.PitchMax, value);
 341                 break;
 342             case TXPIDSETTINGS_PIDS_YAWRATEKP:
 343                 needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
 344                 break;
 345             case TXPIDSETTINGS_PIDS_YAWRATEKI:
 346                 needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
 347                 break;
 348             case TXPIDSETTINGS_PIDS_YAWRATEKD:
 349                 needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
 350                 break;
 351             case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
 352                 needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
 353                 break;
 354             case TXPIDSETTINGS_PIDS_YAWRATERESP:
 355                 needsUpdateBank |= update(&bank.ManualRate.Yaw, value);
 356                 break;
 357             case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
 358                 needsUpdateBank |= update(&bank.YawPI.Kp, value);
 359                 break;
 360             case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
 361                 needsUpdateBank |= update(&bank.YawPI.Ki, value);
 362                 break;
 363             case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
 364                 needsUpdateBank |= update(&bank.YawPI.ILimit, value);
 365                 break;
 366             case TXPIDSETTINGS_PIDS_YAWATTITUDERESP:
 367                 needsUpdateBank |= updateUint8(&bank.YawMax, value);
 368                 break;
 369             case TXPIDSETTINGS_PIDS_ROLLEXPO:
 370                 needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
 371                 break;
 372             case TXPIDSETTINGS_PIDS_PITCHEXPO:
 373                 needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
 374                 break;
 375             case TXPIDSETTINGS_PIDS_ROLLPITCHEXPO:
 376                 needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
 377                 needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
 378                 break;
 379             case TXPIDSETTINGS_PIDS_YAWEXPO:
 380                 needsUpdateBank |= updateInt8(&bank.StickExpo.Yaw, value);
 381                 break;
 382             case TXPIDSETTINGS_PIDS_GYROTAU:
 383                 needsUpdateStab |= update(&stab.GyroTau, value);
 384                 break;
 385             case TXPIDSETTINGS_PIDS_ACROPLUSFACTOR:
 386                 needsUpdateBank |= update(&bank.AcroInsanityFactor, value);
 387                 break;
 388             case TXPIDSETTINGS_PIDS_ACCELTAU:
 389                 needsUpdateAtt  |= update(&att.AccelTau, value);
 390                 break;
 391             case TXPIDSETTINGS_PIDS_ACCELKP:
 392                 needsUpdateAtt  |= update(&att.AccelKp, value);
 393                 break;
 394             case TXPIDSETTINGS_PIDS_ACCELKI:
 395                 needsUpdateAtt  |= update(&att.AccelKi, value);
 396                 break;
 397
 398 #ifdef REVOLUTION
 399             case TXPIDSETTINGS_PIDS_ALTITUDEPOSKP:
 400                 needsUpdateAltitude |= update(&altitude.VerticalPosP, value);
 401                 break;
 402             case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKP:
 403                 needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kp, value);
 404                 break;
 405             case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKI:
 406                 needsUpdateAltitude |= update(&altitude.VerticalVelPID.Ki, value);
 407                 break;
 408             case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKD:
 409                 needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kd, value);
 410                 break;
 411             case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYBETA:
 412                 needsUpdateAltitude |= update(&altitude.VerticalVelPID.Beta, value);
 413                 break;
 414 #endif
 415             default:
 416                 PIOS_Assert(0);
 417             }
 418         }
 419     }
 420     if (needsUpdateStab) {
 421         StabilizationSettingsSet(&stab);
 422     }
 423     if (needsUpdateAtt) {
 424         AttitudeSettingsSet(&att);
 425     }
 426 #ifdef REVOLUTION
 427     if (needsUpdateAltitude) {
 428         AltitudeHoldSettingsSet(&altitude);
 429     }
 430 #endif
 431     if (needsUpdateBank) {
 432         switch (inst.BankNumber) {
 433         case 0:
 434             StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *)&bank);
 435             break;
 436
 437         case 1:
 438             StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
 439             break;
 440
 441         case 2:
 442             StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *)&bank);
 443             break;
 444
 445         default:
 446             return;
 447         }
 448     }
 449
 450     if (needsUpdateStab ||
 451         needsUpdateAtt ||
 452 #ifdef REVOLUTION
 453         needsUpdateAltitude ||
 454 #endif /* REVOLUTION */
 455         needsUpdateBank) {
 456         TxPIDStatusSet(&txpid_status);;
 457     }
 458 }
 459
 460 /**
 461  * Scales input val from [inMin..inMax] range to [outMin..outMax].
 462  * If val is out of input range (inMin <= inMax), it will be bound.
 463  * (outMin > outMax) is ok, in that case output will be decreasing.
 464  *
 465  * \returns scaled value
 466  */
 467 static float scale(float val, float inMin, float inMax, float outMin, float outMax)
 468 {
 469     // bound input value
 470     if (val > inMax) {
 471         val = inMax;
 472     }
 473     if (val < inMin) {
 474         val = inMin;
 475     }
 476
 477     // normalize input value to [0..1]
 478     if (inMax <= inMin) {
 479         val = 0.0f;
 480     } else {
 481         val = (val - inMin) / (inMax - inMin);
 482     }
 483
 484     // update output bounds
 485     if (outMin > outMax) {
 486         float t = outMin;
 487         outMin = outMax;
 488         outMax = t;
 489         val    = 1.0f - val;
 490     }
 491
 492     return (outMax - outMin) * val + outMin;
 493 }
 494
 495 /**
 496  * Updates var using val if needed.
 497  * \returns 1 if updated, 0 otherwise
 498  */
 499 static uint8_t update(float *var, float val)
 500 {
 501     /* FIXME: this is not an entirely correct way
 502      * to check if the two floating point
 503      * numbers are 'not equal'.
 504      * Epsilon of 1e-9 is probably okay for the range
 505      * of numbers we see here*/
 506     if (fabsf(*var - val) > 1e-9f) {
 507         *var = val;
 508         return 1;
 509     }
 510     return 0;
 511 }
 512
 513 /**
 514  * Updates var using val if needed.
 515  * \returns 1 if updated, 0 otherwise
 516  */
 517 static uint8_t updateUint8(uint8_t *var, float val)
 518 {
 519     uint8_t roundedVal = (uint8_t)roundf(val);
 520
 521     if (*var != roundedVal) {
 522         *var = roundedVal;
 523         return 1;
 524     }
 525     return 0;
 526 }
 527
 528 /**
 529  * Updates var using val if needed.
 530  * \returns 1 if updated, 0 otherwise
 531  */
 532 static uint8_t updateInt8(int8_t *var, float val)
 533 {
 534     int8_t roundedVal = (int8_t)roundf(val);
 535
 536     if (*var != roundedVal) {
 537         *var = roundedVal;
 538         return 1;
 539     }
 540     return 0;
 541 }
 542
 543 /**
 544  * @}
 545  */
 546
 547 /**
 548  * @}
 549  */