dRonin  adbada4
dRonin firmware
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
txpid.c
Go to the documentation of this file.
1 
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, see <http://www.gnu.org/licenses/>
29  *
30  * Additional note on redistribution: The copyright and license notices above
31  * must be maintained in each individual source file that is a derivative work
32  * of this source file; otherwise redistribution is prohibited.
33  */
34 
49 #include "openpilot.h"
50 #include <eventdispatcher.h>
51 
52 #include "txpidsettings.h"
53 #include "manualcontrolcommand.h"
54 #include "stabilizationsettings.h"
55 #include "sensorsettings.h"
56 #include "vbarsettings.h"
57 #include "vtolpathfollowersettings.h"
58 
59 #include "flightstatus.h"
60 #include "modulesettings.h"
61 
62 //
63 // Configuration
64 //
65 #define SAMPLE_PERIOD_MS 200
66 #define TELEMETRY_UPDATE_PERIOD_MS 0 // 0 = update on change (default)
67 
68 // Sanity checks
69 #if (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_INPUTS_NUMELEM) || \
70  (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_MINPID_NUMELEM) || \
71  (TXPIDSETTINGS_PIDS_NUMELEM != TXPIDSETTINGS_MAXPID_NUMELEM)
72 #error Invalid TxPID UAVObject definition (inconsistent number of field elements)
73 #endif
74 
75 // Private types
76 struct txpid_struct {
77  TxPIDSettingsData inst;
78  StabilizationSettingsData stab;
79  SensorSettingsData sensor;
80  VbarSettingsData vbar;
81  VtolPathFollowerSettingsData vtolPathFollowerSettingsData;
82 #if (TELEMETRY_UPDATE_PERIOD_MS != 0)
83  UAVObjMetadata metadata;
84 #endif
85 };
86 
87 // Private variables
88 static struct txpid_struct *txpid_data;
89 
90 // Private functions
91 static void updatePIDs(const UAVObjEvent *ev,
92  void *ctx, void *obj, int len);
93 static bool update(float *var, float val);
94 static float scale(float val, float inMin, float inMax, float outMin, float outMax);
95 
100 int32_t TxPIDInitialize(void)
101 {
102  bool module_enabled = false;
103 
104 #ifdef MODULE_TxPID_BUILTIN
105  module_enabled = true;
106 #else
107  uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
108  ModuleSettingsAdminStateGet(module_state);
109  if (module_state[MODULESETTINGS_ADMINSTATE_TXPID] == MODULESETTINGS_ADMINSTATE_ENABLED) {
110  module_enabled = true;
111  }
112 #endif
113 
114  if (TxPIDSettingsInitialize() == -1) {
115  module_enabled = false;
116  return -1;
117  }
118 
119  if (module_enabled) {
120  txpid_data = PIOS_malloc(sizeof(*txpid_data));
121 
122  if (txpid_data != NULL) {
123  memset(txpid_data, 0x00, sizeof(*txpid_data));
124 
125  if (TxPIDSettingsInitialize() == -1) {
126  module_enabled = false;
127  return -1;
128  }
129 
130  return 0;
131  }
132  }
133 
134  return -1;
135 }
136 
141 static int32_t TxPIDStart(void)
142 {
143  if (txpid_data == NULL) {
144  return -1;
145  }
146 
147  UAVObjEvent ev = { 0 };
148  EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS);
149 
150 #if (TELEMETRY_UPDATE_PERIOD_MS != 0)
151  // Change StabilizationSettings update rate from OnChange to periodic
152  // to prevent telemetry link flooding with frequent updates in case of
153  // control channel jitter.
154  // Warning: saving to flash with this code active will change the
155  // StabilizationSettings update rate permanently. Use Metadata via
156  // browser to reset to defaults (telemetryAcked=true, OnChange).
157  if (StabilizationSettingsInitialize() == -1){
158  module_enabled = false;
159  return -1;
160  }
161  StabilizationSettingsGetMetadata(&txpid_data->metadata);
162  txpid_data->metadata.telemetryAcked = 0;
163  txpid_data->metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
164  txpid_data->metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
165  StabilizationSettingsSetMetadata(&txpid_data->metadata);
166 #endif
167 
168  return 0;
169 }
170 
171 MODULE_INITCALL(TxPIDInitialize, TxPIDStart);
172 
176 static void updatePIDs(const UAVObjEvent *ev,
177  void *ctx, void *obj, int len)
178 {
179  (void) ev; (void) ctx; (void) obj; (void) len;
180 
181  TxPIDSettingsGet(&txpid_data->inst);
182 
183  if (txpid_data->inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER)
184  return;
185 
186  uint8_t armed;
187  FlightStatusArmedGet(&armed);
188  if ((txpid_data->inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
189  (armed == FLIGHTSTATUS_ARMED_DISARMED))
190  return;
191 
192  StabilizationSettingsGet(&txpid_data->stab);
193  SensorSettingsGet(&txpid_data->sensor);
194  VbarSettingsGet(&txpid_data->vbar);
195 
196  // Check to make sure the settings UAVObject has been instantiated
197  if (VtolPathFollowerSettingsHandle()) {
198  VtolPathFollowerSettingsGet(&txpid_data->vtolPathFollowerSettingsData);
199  }
200 
201  bool vtolPathFollowerSettingsNeedsUpdate = false;
202 
203  bool stabilizationSettingsNeedsUpdate = false;
204  bool sensorSettingsNeedsUpdate = false;
205  bool vbarSettingsNeedsUpdate = false;
206 
207  // Loop through every enabled instance
208  for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
209  if (txpid_data->inst.PIDs[i] != TXPIDSETTINGS_PIDS_DISABLED) {
210 
211  float value;
212  if (txpid_data->inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
213  ManualControlCommandThrottleGet(&value);
214  value = scale(value,
215  txpid_data->inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN],
216  txpid_data->inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX],
217  txpid_data->inst.MinPID[i], txpid_data->inst.MaxPID[i]);
218  } else {
219  (void) 0;
220 
221  int idx = txpid_data->inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0;
222 
223  if (idx < 0) {
224  continue;
225  }
226 
227  if (idx >= MANUALCONTROLCOMMAND_ACCESSORY_NUMELEM) {
228  continue;
229  }
230 
231  float accessories[MANUALCONTROLCOMMAND_ACCESSORY_NUMELEM];
232 
233  ManualControlCommandAccessoryGet(accessories);
234 
235  value = scale(accessories[idx], -1.0, 1.0, txpid_data->inst.MinPID[i], txpid_data->inst.MaxPID[i]);
236  }
237 
238  switch (txpid_data->inst.PIDs[i]) {
239  case TXPIDSETTINGS_PIDS_ROLLRATEKP:
240  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
241  break;
242  case TXPIDSETTINGS_PIDS_ROLLRATEKI:
243  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
244  break;
245  case TXPIDSETTINGS_PIDS_ROLLRATEKD:
246  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
247  break;
248  case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
249  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
250  break;
251  case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
252  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
253  break;
254  case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
255  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
256  break;
257  case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
258  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
259  break;
260  case TXPIDSETTINGS_PIDS_PITCHRATEKP:
261  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
262  break;
263  case TXPIDSETTINGS_PIDS_PITCHRATEKI:
264  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
265  break;
266  case TXPIDSETTINGS_PIDS_PITCHRATEKD:
267  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
268  break;
269  case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
270  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
271  break;
272  case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
273  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
274  break;
275  case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
276  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
277  break;
278  case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
279  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
280  break;
281  case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
282  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
283  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
284  break;
285  case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
286  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
287  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
288  break;
289  case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
290  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
291  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
292  break;
293  case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
294  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
295  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
296  break;
297  case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
298  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
299  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
300  break;
301  case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
302  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
303  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
304  break;
305  case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
306  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
307  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
308  break;
309  case TXPIDSETTINGS_PIDS_YAWRATEKP:
310  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value);
311  break;
312  case TXPIDSETTINGS_PIDS_YAWRATEKI:
313  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value);
314  break;
315  case TXPIDSETTINGS_PIDS_YAWRATEKD:
316  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value);
317  break;
318  case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
319  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value);
320  break;
321  case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
322  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value);
323  break;
324  case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
325  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value);
326  break;
327  case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
328  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value);
329  break;
330  case TXPIDSETTINGS_PIDS_SENSORSCUTOFF:
331  sensorSettingsNeedsUpdate |= update(&txpid_data->sensor.LowpassCutoff, value);
332  break;
333  case TXPIDSETTINGS_PIDS_ROLLVBARSENSITIVITY:
334  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarSensitivity[VBARSETTINGS_VBARSENSITIVITY_ROLL], value);
335  break;
336  case TXPIDSETTINGS_PIDS_PITCHVBARSENSITIVITY:
337  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarSensitivity[VBARSETTINGS_VBARSENSITIVITY_PITCH], value);
338  break;
339  case TXPIDSETTINGS_PIDS_ROLLPITCHVBARSENSITIVITY:
340  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarSensitivity[VBARSETTINGS_VBARSENSITIVITY_ROLL], value);
341  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarSensitivity[VBARSETTINGS_VBARSENSITIVITY_PITCH], value);
342  break;
343 
344  case TXPIDSETTINGS_PIDS_YAWVBARSENSITIVITY:
345  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarSensitivity[VBARSETTINGS_VBARSENSITIVITY_YAW], value);
346  break;
347  case TXPIDSETTINGS_PIDS_ROLLVBARKP:
348  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarRollPID[VBARSETTINGS_VBARROLLPID_KP], value);
349  break;
350  case TXPIDSETTINGS_PIDS_ROLLVBARKI:
351  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarRollPID[VBARSETTINGS_VBARROLLPID_KI], value);
352  break;
353  case TXPIDSETTINGS_PIDS_ROLLVBARKD:
354  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarRollPID[VBARSETTINGS_VBARROLLPID_KD], value);
355  break;
356  case TXPIDSETTINGS_PIDS_PITCHVBARKP:
357  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarPitchPID[VBARSETTINGS_VBARPITCHPID_KP], value);
358  break;
359  case TXPIDSETTINGS_PIDS_PITCHVBARKI:
360  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarPitchPID[VBARSETTINGS_VBARPITCHPID_KI], value);
361  break;
362  case TXPIDSETTINGS_PIDS_PITCHVBARKD:
363  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarPitchPID[VBARSETTINGS_VBARPITCHPID_KD], value);
364  break;
365  case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKP:
366  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarRollPID[VBARSETTINGS_VBARROLLPID_KP], value);
367  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarPitchPID[VBARSETTINGS_VBARPITCHPID_KP], value);
368  break;
369  case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKI:
370  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarRollPID[VBARSETTINGS_VBARROLLPID_KI], value);
371  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarPitchPID[VBARSETTINGS_VBARPITCHPID_KI], value);
372  break;
373  case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKD:
374  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarRollPID[VBARSETTINGS_VBARROLLPID_KD], value);
375  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarPitchPID[VBARSETTINGS_VBARPITCHPID_KD], value);
376  break;
377  case TXPIDSETTINGS_PIDS_YAWVBARKP:
378  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarYawPID[VBARSETTINGS_VBARYAWPID_KP], value);
379  break;
380  case TXPIDSETTINGS_PIDS_YAWVBARKI:
381  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarYawPID[VBARSETTINGS_VBARYAWPID_KI], value);
382  break;
383  case TXPIDSETTINGS_PIDS_YAWVBARKD:
384  vbarSettingsNeedsUpdate |= update(&txpid_data->vbar.VbarYawPID[VBARSETTINGS_VBARYAWPID_KD], value);
385  break;
386  case TXPIDSETTINGS_PIDS_CAMERATILT:
387  stabilizationSettingsNeedsUpdate |= update(&txpid_data->stab.CameraTilt, value);
388  break;
389  case TXPIDSETTINGS_PIDS_HORIZONTALPOSKP:
390  vtolPathFollowerSettingsNeedsUpdate |= update(&txpid_data->vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP], value);
391  break;
392  case TXPIDSETTINGS_PIDS_HORIZONTALPOSKI:
393  vtolPathFollowerSettingsNeedsUpdate |= update(&txpid_data->vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], value);
394  break;
395  case TXPIDSETTINGS_PIDS_HORIZONTALPOSILIMIT:
396  vtolPathFollowerSettingsNeedsUpdate |= update(&txpid_data->vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], value);
397  break;
398  case TXPIDSETTINGS_PIDS_HORIZONTALVELKP:
399  vtolPathFollowerSettingsNeedsUpdate |= update(&txpid_data->vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], value);
400  break;
401  case TXPIDSETTINGS_PIDS_HORIZONTALVELKI:
402  vtolPathFollowerSettingsNeedsUpdate |= update(&txpid_data->vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], value);
403  break;
404  case TXPIDSETTINGS_PIDS_HORIZONTALVELKD:
405  vtolPathFollowerSettingsNeedsUpdate |= update(&txpid_data->vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD], value);
406  break;
407  default:
408  // Previously this would assert. But now the
409  // object may be missing and it's not worth a
410  // crash.
411  break;
412  }
413  }
414  }
415 
416  // Update UAVOs, if necessary
417  if (stabilizationSettingsNeedsUpdate) {
418  StabilizationSettingsSet(&txpid_data->stab);
419  }
420 
421  if (sensorSettingsNeedsUpdate) {
422  SensorSettingsSet(&txpid_data->sensor);
423  }
424 
425  if (vbarSettingsNeedsUpdate) {
426  VbarSettingsSet(&txpid_data->vbar);
427  }
428 
429  if (vtolPathFollowerSettingsNeedsUpdate) {
430  // Check to make sure the settings UAVObject has been instantiated
431  if (VtolPathFollowerSettingsHandle()) {
432  VtolPathFollowerSettingsSet(&txpid_data->vtolPathFollowerSettingsData);
433  }
434  }
435 }
436 
444 static float scale(float val, float inMin, float inMax, float outMin, float outMax)
445 {
446  // bound input value
447  if (val > inMax) val = inMax;
448  if (val < inMin) val = inMin;
449 
450  // normalize input value to [0..1]
451  if (inMax <= inMin)
452  val = 0.0;
453  else
454  val = (val - inMin) / (inMax - inMin);
455 
456  // update output bounds
457  if (outMin > outMax) {
458  float t = outMin;
459  outMin = outMax;
460  outMax = t;
461  val = 1.0f - val;
462  }
463 
464  return (outMax - outMin) * val + outMin;
465 }
466 
471 static bool update(float *var, float val)
472 {
473  if (*var != val) {
474  *var = val;
475  return true;
476  }
477  return false;
478 }
479 
MODULE_INITCALL
MODULE_INITCALL(TxPIDInitialize, TxPIDStart)
txpid_struct::vbar
VbarSettingsData vbar
Definition: txpid.c:80
updatePIDs
static void updatePIDs(const UAVObjEvent *ev, void *ctx, void *obj, int len)
Definition: txpid.c:176
txpid_struct
Definition: txpid.c:76
SAMPLE_PERIOD_MS
#define SAMPLE_PERIOD_MS
Definition: txpid.c:65
scale
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
Definition: txpid.c:444
txpid_struct::inst
TxPIDSettingsData inst
Definition: txpid.c:77
txpid_struct::vtolPathFollowerSettingsData
VtolPathFollowerSettingsData vtolPathFollowerSettingsData
Definition: txpid.c:81
TxPIDInitialize
int32_t TxPIDInitialize(void)
Definition: txpid.c:100
PIOS_malloc
void * PIOS_malloc(size_t size)
Definition: pios_heap.c:125
module_enabled
bool module_enabled
Definition: charonscreendisplay.c:79
txpid_struct::stab
StabilizationSettingsData stab
Definition: txpid.c:78
EventPeriodicCallbackCreate
int32_t EventPeriodicCallbackCreate(UAVObjEvent *ev, UAVObjEventCallback cb, uint16_t periodMs)
Definition: systemmod.c:884
TxPIDStart
static int32_t TxPIDStart(void)
Definition: txpid.c:141
eventdispatcher.h
Event dispatcher, distributes object events as callbacks. Alternative to using tasks and queues...
i
uint8_t i
Definition: msp_messages.h:97
txpid_struct::sensor
SensorSettingsData sensor
Definition: txpid.c:79
value
uint16_t value
Definition: storm32bgc.c:155
txpid_data
static struct txpid_struct * txpid_data
Definition: txpid.c:88
openpilot.h
Includes PiOS and core architecture components.
update
static bool update(float *var, float val)
Definition: txpid.c:471
TELEMETRY_UPDATE_PERIOD_MS
#define TELEMETRY_UPDATE_PERIOD_MS
Definition: txpid.c:66