src/main/programming/pid.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_PROGRAMMING_FRAMEWORK
  28
  29 #include "common/utils.h"
  30 #include "config/config_reset.h"
  31 #include "config/parameter_group.h"
  32 #include "config/parameter_group_ids.h"
  33 #include "navigation/navigation_private.h"
  34
  35 #include "programming/pid.h"
  36 #include "programming/logic_condition.h"
  37
  38 EXTENDED_FASTRAM programmingPidState_t programmingPidState[MAX_PROGRAMMING_PID_COUNT];
  39 static bool pidsInitiated = false;
  40
  41 PG_REGISTER_ARRAY_WITH_RESET_FN(programmingPid_t, MAX_PROGRAMMING_PID_COUNT, programmingPids, PG_PROGRAMMING_PID, 2);
  42
  43 void pgResetFn_programmingPids(programmingPid_t *instance)
  44 {
  45     for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
  46         RESET_CONFIG(programmingPid_t, &instance[i],
  47             .enabled = 0,
  48             .setpoint = {
  49                 .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
  50                 .value = 0
  51             },
  52             .measurement = {
  53                 .type = LOGIC_CONDITION_OPERAND_TYPE_VALUE,
  54                 .value = 0
  55             },
  56             .gains = {
  57                 .P = 0,
  58                 .I = 0,
  59                 .D = 0,
  60                 .FF = 0,
  61             }
  62         );
  63     }
  64 }
  65
  66 void programmingPidUpdateTask(timeUs_t currentTimeUs)
  67 {
  68     static timeUs_t previousUpdateTimeUs;
  69     const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
  70
  71     if (!pidsInitiated) {
  72         programmingPidInit();
  73         pidsInitiated = true;
  74     }
  75
  76     for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
  77         if (programmingPids(i)->enabled) {
  78             const int setpoint = logicConditionGetOperandValue(programmingPids(i)->setpoint.type, programmingPids(i)->setpoint.value);
  79             const int measurement = logicConditionGetOperandValue(programmingPids(i)->measurement.type, programmingPids(i)->measurement.value);
  80
  81             programmingPidState[i].output = navPidApply2(
  82                 &programmingPidState[i].controller,
  83                 setpoint,
  84                 measurement,
  85                 dT,
  86                 -1000,
  87                 1000,
  88                 PID_LIMIT_INTEGRATOR
  89             );
  90
  91         }
  92     }
  93
  94     previousUpdateTimeUs = currentTimeUs;
  95 }
  96
  97 void programmingPidInit(void)
  98 {
  99     for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
 100         navPidInit(
 101             &programmingPidState[i].controller,
 102             programmingPids(i)->gains.P / 1000.0f,
 103             programmingPids(i)->gains.I / 1000.0f,
 104             programmingPids(i)->gains.D / 1000.0f,
 105             programmingPids(i)->gains.FF / 1000.0f,
 106             5.0f,
 107             0.0f
 108         );
 109     }
 110 }
 111
 112 int32_t programmingPidGetOutput(uint8_t i) {
 113     return programmingPidState[constrain(i, 0, MAX_PROGRAMMING_PID_COUNT)].output;
 114 }
 115
 116 void programmingPidReset(void)
 117 {
 118     for (uint8_t i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
 119         navPidReset(&programmingPidState[i].controller);
 120     }
 121 }
 122
 123 #endif