flexPTP 1.0
An IEEE 1588 PTP implementation designed for microcontrollers
Loading...
Searching...
No Matches
pid_controller.c
Go to the documentation of this file.
1/* (C) AndrĂ¡s Wiesner, 2021 */
2
3#include "pid_controller.h"
4
5#include <math.h>
6#include <stdlib.h>
7#include <string.h>
8
9#include <flexptp_options.h>
10
11// ----------------------------------
12
13static bool logInternals = false;
14static bool firstRun = true;
15
16// ----------------------------------
17
18// static float P_FACTOR = 0.5 * 0.476;
19// static float D_FACTOR = 2.0 * 0.476;
20
21// PID servo coefficients
22static float P_FACTOR = 0.5 * 0.476;
23static float I_FACTOR = 0;
24static float D_FACTOR = 3.0;
25
26// ----------------------------------
27
28static double rd_prev_ppb;
29static double integrator_value;
30
31// ----------------------------------
32
33#ifdef CLI_REG_CMD
34
35static int CB_params(const CliToken_Type *ppArgs, uint8_t argc) {
36 // set if parameters passed after command
37 if (argc >= 3) {
38 P_FACTOR = atof(ppArgs[0]);
39 I_FACTOR = atof(ppArgs[1]);
40 D_FACTOR = atof(ppArgs[2]);
41 }
42
43 MSG("> PTP params: K_p = %.3f, K_i = %.3f, K_d = %.3f\n", P_FACTOR, I_FACTOR, D_FACTOR);
44
45 return 0;
46}
47
48static int CB_logInternals(const CliToken_Type *ppArgs, uint8_t argc) {
49 if (argc >= 1) {
50 int en = ONOFF(ppArgs[0]);
51 if (en >= 0) {
52 if (en && !logInternals) {
53 MSG("\nSyncIntv. [ns] | dt [ns] | gamma [ppb]\n\n");
54 }
55 logInternals = en;
56 return 0;
57 } else {
58 return -1;
59 }
60 } else {
61 return -1;
62 }
63}
64
65static struct {
66 int params;
67 int internals;
68} sCliCmdIdx = {0};
69
70static void pid_ctrl_register_cli_commands() {
71 sCliCmdIdx.params = CLI_REG_CMD("ptp servo params [Kp Kd] \t\t\tSet or query K_p and K_d servo parameters", 3, 0, CB_params);
72 sCliCmdIdx.internals = CLI_REG_CMD("ptp servo log internals {on|off} \t\t\tEnable or disable logging of servo internals", 4, 1, CB_logInternals);
73}
74
75static void pid_ctrl_remove_cli_commands() {
76 cli_remove_command(sCliCmdIdx.params);
77 cli_remove_command(sCliCmdIdx.internals);
78}
79
80#endif // CLI_REG_CMD
81
84
85#ifdef CLI_REG_CMD
86 pid_ctrl_register_cli_commands();
87#endif // CLI_REG_CMD
88}
89
91#ifdef CLI_REG_CMD
92 pid_ctrl_remove_cli_commands();
93#endif // CLI_REG_CMD
94}
95
97 firstRun = true;
99}
100
101float pid_ctrl_run(int32_t dt, PtpServoAuxInput *pAux) {
102 if (firstRun) {
103 firstRun = false;
104 rd_prev_ppb = dt;
106 return 0;
107 }
108
109 // calculate relative frequency error
110 // double rd_ppb = ((double) dt) / (pAux->msgPeriodMs * 1E+06 + dt) * 1E+09;
111 double rd_ppb = ((double)dt) / (pAux->measSyncPeriodNs + dt) * 1E+09;
112
113 // calculate difference
114 double rd_D_ppb = D_FACTOR * (rd_ppb - rd_prev_ppb);
115
116 // calculate output (run the PD controller)
117 double corr_ppb = -(P_FACTOR * (rd_ppb + rd_D_ppb) + integrator_value) * exp((pAux->measSyncPeriodNs * 1E-09) - 1.0);
118
119 // update integrator
120 integrator_value += I_FACTOR * rd_ppb;
121
122 // store error value (time difference) for use in next iteration
123 rd_prev_ppb = rd_ppb;
124
125 CLILOG(logInternals, "%d %f\n", dt, rd_ppb);
126
127 return corr_ppb;
128}
129
130// ----------------------------------
#define CLI_REG_CMD(cmd_hintline, n_cmd, n_min_arg, cb)
#define CLILOG(en,...)
#define ONOFF(str)
static double rd_prev_ppb
relative frequency error measured in previous iteration
static float P_FACTOR
Proportional factor.
static bool logInternals
Decides if servo's internal operation shoud be reported or not.
void pid_ctrl_deinit()
void pid_ctrl_init()
static float D_FACTOR
Differentiating factor.
static bool firstRun
Indicates if first run did not occur yet.
static double integrator_value
value stored in the integrator
void pid_ctrl_reset()
float pid_ctrl_run(int32_t dt, PtpServoAuxInput *pAux)
static float I_FACTOR
Integrating factor.
Data to perform a full synchronization.
double measSyncPeriodNs
Measured synchronization period (t1->t1)