Skip to content

Commit c7fcec7

Browse files
epagrisepagris
authored
Merge: Smooth controller branch
Kalman-filter added - a debug servo has been also added - settings interface and timeutils have been extended - documentation updated - CLI cmd: addend conversion fixed
1 parent c140631 commit c7fcec7

16 files changed

Lines changed: 808 additions & 31 deletions

‎CMakeLists.txt‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -101,6 +101,14 @@ if (FLEXPTP_SERVO STREQUAL "PID")
101101
set(FLEXPTP_SERVO_SRC
102102
servo/pid_controller.c
103103
servo/pid_controller.h)
104+
elseif (FLEXPTP_SERVO STREQUAL "KALMAN")
105+
set(FLEXPTP_SERVO_SRC
106+
servo/kalman_filter.c
107+
servo/kalman_filter.h)
108+
elseif (FLEXPTP_SERVO STREQUAL "DEBUG")
109+
set(FLEXPTP_SERVO_SRC
110+
servo/debug_servo.c
111+
servo/debug_servo.h)
104112
else()
105113
set(FLEXPTP_SERVO_OK 0)
106114
endif()

‎README.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@ flexPTP offers the following capabilities:
1717
- supports **L2** and **L4** transport mechanisms
1818
- supports **one-step** and **two-step** signaling modes
1919
- can operate as a **IEEE 802.1AS** clock (slave or master)
20+
- offers **PID** and **Kalman-filter** servos
2021
- supports lwIP and EtherLib as underlying network stack
2122
- features a rich set of runtime-configurable options
2223
- features numerous logging features

‎manual/index.dox‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ flexPTP offers the following capabilities:
2323
- supports **L2** and **L4** transport mechanisms
2424
- supports **one-step** and **two-step** signaling modes
2525
- can operate as a **IEEE 802.1AS** clock (slave or master)
26+
- offers **PID** and **Kalman-filter** servos
2627
- supports lwIP and EtherLib as underlying network stack
2728
- features a rich set of runtime-configurable options
2829
- features numerous logging features

‎manual/servo.dox‎

Lines changed: 114 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,8 @@
22

33
# Clock servo
44

5+
## Interface
6+
57
The flexPTP requires a clock servo to calculate how to tune the clock in steady state. A proper servo must define the following four functions:
68

79
1. A function that initializes the servo. No parameters are passed. (refer to `PTP_SERVO_INIT()`)
@@ -13,8 +15,118 @@ Clock servo functions must be passed to the flexPTP core by filling the servo-re
1315

1416
Here we want to highlight that a servo init function is not constrained to only initialize the core of a controller. The developer is highly encouraged to include e.g. logging or debug functionality also in the controller.
1517

16-
## Examples
18+
## Bundled controllers
19+
20+
Currently, the library ships with two (plus one) different, predefined servos.
21+
22+
### PID-controller
23+
24+
_CMake: `set(FLEXPTP_SERVO "PID")`_
25+
26+
A simple PID-controller is implemented in the following sources: pid_controller.c, pid_controller.h
27+
28+
Parameters and default values (all parameters are unit-less):
29+
30+
| Name | Value |
31+
| ----- | ----- |
32+
| `K_P` | 0.238 |
33+
| `K_I` | 0 |
34+
| `K_D` | 3.0 |
35+
36+
Custom parameter values can be set in the `flexptp_options.h` by redefining each macro.
37+
38+
#### CLI commands
39+
40+
This servo defines the following CLI commands:
41+
42+
```
43+
ptp servo params [Kp Ki Kd] Set or query Kp, Ki, and Kd servo parameters
44+
ptp servo log internals {on|off} Enable or disable logging of servo internals
45+
```
46+
47+
#### Example usage definitions
48+
49+
To use this servo, set the servo definition macros the following way:
50+
51+
```
52+
#include <flexptp/servo/pid_controller.h>
53+
54+
#define PTP_SERVO_INIT() pid_ctrl_init()
55+
#define PTP_SERVO_DEINIT() pid_ctrl_deinit()
56+
#define PTP_SERVO_RESET() pid_ctrl_reset()
57+
#define PTP_SERVO_RUN(d, pscd) pid_ctrl_run(d, pscd)
58+
```
59+
60+
### Kalman-filter
61+
62+
_CMake: `set(FLEXPTP_SERVO "KALMAN")`_
63+
64+
The library offers a robust Kalman-filter-based servo as well defined in the following sources: kalman_filter.c, kalman_filter.h. This implementation is based on the paper [Performance Analysis of Kalman-Filter-Based Clock Synchronization in IEEE 1588 Networks](https://ieeexplore.ieee.org/document/5934411) by Giada Giorgi and Claudio Narduzzi.
65+
66+
The controller can be tuned along three parameters (\f$\sigma_{\theta}\f$, \f$\sigma_{\gamma}\f$, \f$\sigma_{C(t)}\f$) that are described in the paper.
67+
68+
Parameters and default values:
69+
70+
| Name | Value | Unit |
71+
| --------------------- | ----- | --------- |
72+
| `SIGMA_THETA_SQUARED` | 1E-16 | \f$s^2\f$ |
73+
| `SIGMA_GAMMA_SQUARED` | 1E-12 | \f$s^2\f$ |
74+
| `SIGMA_CT_SQUARED` | 1E-10 | \f$s^2\f$ |
75+
76+
Custom parameter values can be set in the `flexptp_options.h` by redefining each macro.
77+
78+
#### CLI commands
79+
80+
This servo defines the following CLI commands:
81+
82+
```
83+
ptp servo st [var|default] Set or get sigma_theta^2 (s^2)
84+
ptp servo sg [var|default] Set or get sigma_gamma^2 (s^2)
85+
ptp servo stm [var|default] Set or get sigma_theta_m^2 (s^2)
86+
```
87+
88+
#### Example usage definitions
89+
90+
To use this servo, set the servo definition macros the following way:
91+
92+
```
93+
#include <flexptp/servo/kalman_filter.h>
94+
95+
#define PTP_SERVO_INIT() kalman_filter_init()
96+
#define PTP_SERVO_DEINIT() kalman_filter_deinit()
97+
#define PTP_SERVO_RESET() kalman_filter_reset()
98+
#define PTP_SERVO_RUN(d, pscd) kalman_filter_run(d, pscd)
99+
```
100+
101+
### Debug servo
102+
103+
_CMake: `set(FLEXPTP_SERVO "DEBUG")`_
104+
105+
This servo can be used to monitor and manually tune the hardware clock. It produces a verbose, colorized logging of the clock state.
106+
107+
#### CLI commands
108+
109+
This servo defines the following CLI commands:
110+
111+
```
112+
ptp servo tune <tuning> Set relative tuning effective in next cycle
113+
ptp servo skew0 [skew|last] Set or get skew offset (ppb)
114+
ptp servo dt0 [dt|last] Set or get time offset (ns)
115+
```
116+
117+
#### Example usage definitions
118+
119+
To use this servo, set the servo definition macros the following way:
120+
121+
```
122+
#include <flexptp/servo/debug_servo.h>
123+
124+
#define PTP_SERVO_INIT() debug_servo_init()
125+
#define PTP_SERVO_DEINIT() debug_servo_deinit()
126+
#define PTP_SERVO_RESET() debug_servo_reset()
127+
#define PTP_SERVO_RUN(d, pscd) debug_servo_run(d, pscd)
128+
```
129+
17130

18-
A PID-controller based servo is bundled to the flexPTP package: pid_controller.c, pid_controller.h
19131

20132
*/

‎src/flexptp/cli_cmds.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -137,7 +137,7 @@ static CMD_FUNCTION(CB_ptpdomain) {
137137
#ifdef PTP_ADDEND_INTERFACE
138138
static CMD_FUNCTION(CB_addend) {
139139
if (argc > 0) {
140-
ptp_set_addend((uint32_t)atol(ppArgs[0]));
140+
ptp_set_addend((uint32_t)atoll(ppArgs[0]));
141141
}
142142
MSG("Addend: %u\n", ptp_get_addend());
143143
return 0;

‎src/flexptp/ptp_servo_types.h‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,10 @@
11
/**
2-
******************************************************************************
3-
* @file ptp_servo_types.h
4-
* @copyright András Wiesner, 2019-\showdate "%Y"
5-
* @brief This module defines the basic servo types.
6-
******************************************************************************
7-
*/
2+
******************************************************************************
3+
* @file ptp_servo_types.h
4+
* @copyright András Wiesner, 2019-\showdate "%Y"
5+
* @brief This module defines the basic servo types.
6+
******************************************************************************
7+
*/
88

99
#ifndef FLEXPTP_SIM_PTP_SERVO_TYPES_H_
1010
#define FLEXPTP_SIM_PTP_SERVO_TYPES_H_
@@ -13,13 +13,13 @@
1313

1414
/**
1515
* @brief Data to perform a full synchronization.
16-
*/
16+
*/
1717
typedef struct {
1818
PtpSyncCycleData scd; ///< Sync cycle data
1919

2020
// information about sync interval
21-
int8_t logMsgPeriod; ///< Logarithmic message period
22-
double msgPeriodMs; ///< Message period in ms
21+
int8_t logMsgPeriod; ///< Logarithmic message period
22+
double msgPeriodMs; ///< Message period in ms
2323
int64_t measSyncPeriodNs; ///< Measured synchronization period (t1->t1)
2424
} PtpServoAuxInput;
2525

‎src/flexptp/servo/debug_servo.c‎

Lines changed: 141 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,141 @@
1+
#include "debug_servo.h"
2+
#include "cliutils/cli.h"
3+
#include "flexptp/ptp_defs.h"
4+
#include "standard_output/standard_output.h"
5+
6+
#include <flexptp_options.h>
7+
#include <stdlib.h>
8+
#include <string.h>
9+
10+
// --------------
11+
12+
static double skew0; // clock skew offset (ppb)
13+
static double skew_prev; // clock skew in the previous cycle (ppb)
14+
static int32_t dt0; // time error offset (ns)
15+
static int32_t dt_prev; // time error in the previous cycle (ns)
16+
static int32_t offset_prev; // time error offset in the previous cycle (ns)
17+
18+
static uint64_t cycle; // cycle counter
19+
20+
static double tuning_next_ppb; // tuning effective in the next cycle (ppb)
21+
static bool tuning_next_cycle_valid; // indicates that the above tuning will be implemented
22+
23+
// ---------------
24+
25+
#ifdef CLI_REG_CMD
26+
27+
#ifndef CMD_FUNCTION
28+
#error "No CMD_FUNCTION macro has been defined, cannot register CLI functions!"
29+
#endif
30+
31+
typedef enum {
32+
DS_CMDH_TUNE,
33+
DS_CMDH_SET_SKEW0,
34+
DS_CMDH_SET_DT0,
35+
DS_CMDH_N
36+
} DebugServoCmdHandle;
37+
38+
static int cmd_handles[DS_CMDH_N];
39+
40+
static CMD_FUNCTION(tune) {
41+
tuning_next_ppb = atof(ppArgs[0]);
42+
tuning_next_cycle_valid = true;
43+
MSG("Tuning in next cycle: " PTP_COLOR_BGREEN "%.4f" PTP_COLOR_RESET " ppb\n", tuning_next_ppb);
44+
return 0;
45+
}
46+
47+
static CMD_FUNCTION(skew_offset) {
48+
if (argc > 0) {
49+
if (!strcmp("last", ppArgs[0])) {
50+
skew0 = skew_prev;
51+
} else {
52+
skew0 = atof(ppArgs[0]);
53+
}
54+
}
55+
MSG("Skew offset: " PTP_COLOR_BGREEN "%.4f" PTP_COLOR_RESET "ppb\n", skew0);
56+
return 0;
57+
}
58+
59+
static CMD_FUNCTION(time_offset) {
60+
if (argc > 0) {
61+
if (!strcmp("last", ppArgs[0])) {
62+
dt0 = dt_prev;
63+
} else {
64+
dt0 = atof(ppArgs[0]);
65+
}
66+
}
67+
MSG("Time offset: " PTP_COLOR_BYELLOW "%i" PTP_COLOR_RESET "ns", dt0);
68+
return 0;
69+
}
70+
71+
static void register_cli_cmds() {
72+
cmd_handles[DS_CMDH_TUNE] = CLI_REG_CMD("ptp servo tune <tuning>\t\t\tSet relative tuning effective in next cycle", 3, 1, tune);
73+
cmd_handles[DS_CMDH_SET_SKEW0] = CLI_REG_CMD("ptp servo skew0 [skew|last]\t\t\tSet or get skew offset (ppb)", 3, 0, skew_offset);
74+
cmd_handles[DS_CMDH_SET_DT0] = CLI_REG_CMD("ptp servo dt0 [dt|last]\t\t\tSet or get time offset (ns)", 3, 0, time_offset);
75+
}
76+
77+
#ifdef CLI_REMOVE_CMD
78+
static void remove_cli_cmds() {
79+
for (uint8_t i = 0; i < DS_CMDH_N; i++) {
80+
CLI_REMOVE_CMD(i);
81+
}
82+
}
83+
#endif
84+
85+
#endif
86+
87+
void debug_servo_init() {
88+
debug_servo_reset();
89+
90+
#ifdef CLI_REG_CMD
91+
register_cli_cmds();
92+
#endif
93+
}
94+
95+
void debug_servo_deinit() {
96+
#if defined(CLI_REG_CMD) && defined(CLI_REMOVE_CMD)
97+
remove_cli_cmds();
98+
#endif
99+
}
100+
101+
void debug_servo_reset() {
102+
skew0 = 0.0;
103+
dt0 = 0.0;
104+
dt_prev = 0.0;
105+
cycle = 0.0;
106+
tuning_next_ppb = 0.0;
107+
}
108+
109+
float debug_servo_run(int32_t dt, PtpServoAuxInput *pAux) {
110+
double tuning_ppb = 0.0;
111+
112+
// in the very first cycle skip running the filter
113+
if (cycle == 0) {
114+
goto retain_cycle_data;
115+
}
116+
117+
// calculate input data
118+
double skew = ((double)(dt - dt_prev)) / ((double)pAux->measSyncPeriodNs) * 1E+09; // skew
119+
double skew_rel = skew - skew0; // substract skew offset
120+
int32_t offset = dt - dt0 - skew_rel * pAux->measSyncPeriodNs * 1E-09; // time offset
121+
int32_t offset_delta = offset - offset_prev;
122+
123+
MSG(PTP_COLOR_BGREEN "% 8.4f" PTP_COLOR_BYELLOW " % 12i" PTP_COLOR_BRED " % 12i" PTP_COLOR_RESET "\n", skew_rel, offset, offset_delta);
124+
125+
offset_prev = offset;
126+
skew_prev = skew;
127+
128+
retain_cycle_data:
129+
dt_prev = dt;
130+
131+
cycle++;
132+
133+
if (tuning_next_cycle_valid) {
134+
tuning_ppb = tuning_next_ppb;
135+
tuning_next_cycle_valid = false;
136+
137+
MSG("Now tuning " PTP_COLOR_BGREEN "%.4f" PTP_COLOR_RESET "ppb!\n", tuning_ppb);
138+
}
139+
140+
return tuning_ppb;
141+
}

‎src/flexptp/servo/debug_servo.h‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
#ifndef SERVO_DEBUG_SERVO
2+
#define SERVO_DEBUG_SERVO
3+
4+
#include <stdint.h>
5+
6+
#include "../ptp_servo_types.h"
7+
8+
/**
9+
* Initialize the debug servo.
10+
*/
11+
void debug_servo_init();
12+
13+
/**
14+
* Deinitialize the debug servo.
15+
*/
16+
void debug_servo_deinit();
17+
18+
/**
19+
* Reset the debug servo.
20+
*/
21+
void debug_servo_reset();
22+
23+
/**
24+
* Run the debug servo.
25+
*
26+
* @param dt time error in nanoseconds
27+
* @param pAux auxiliary synchronization cycle context data
28+
*/
29+
float debug_servo_run(int32_t dt, PtpServoAuxInput * pAux);
30+
31+
32+
33+
#endif /* SERVO_DEBUG_SERVO */

0 commit comments

Comments
 (0)