143 lines
3.3 KiB
C
143 lines
3.3 KiB
C
/* (C) András Wiesner, 2021 */
|
|
|
|
#include <flexptp/ptp_core.h>
|
|
#include <flexptp/servo/pd_controller.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
|
|
#include <flexptp_options.h>
|
|
|
|
// ----------------------------------
|
|
|
|
static struct {
|
|
bool internals;
|
|
} sLog;
|
|
|
|
static struct {
|
|
bool firstRun;
|
|
} sState;
|
|
|
|
// ----------------------------------
|
|
|
|
//static float P_FACTOR = 0.5 * 0.476;
|
|
//static float D_FACTOR = 2.0 * 0.476;
|
|
|
|
static float P_FACTOR = 0.5 * 0.476;
|
|
static float I_FACTOR = 0;
|
|
static float D_FACTOR = 3.0;
|
|
|
|
// ----------------------------------
|
|
|
|
static double rd_prev_ppb; // relative frequency error measured in previous iteration
|
|
static double integrator_value;
|
|
|
|
// ----------------------------------
|
|
|
|
#ifdef CLI_REG_CMD
|
|
|
|
static int CB_params(const CliToken_Type * ppArgs, uint8_t argc)
|
|
{
|
|
// set if parameters passed after command
|
|
if (argc >= 3) {
|
|
P_FACTOR = atof(ppArgs[0]);
|
|
I_FACTOR = atof(ppArgs[1]);
|
|
D_FACTOR = atof(ppArgs[2]);
|
|
}
|
|
|
|
MSG("> PTP params: K_p = %.3f, K_i = %.3f, K_d = %.3f\n", P_FACTOR, I_FACTOR, D_FACTOR);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int CB_logInternals(const CliToken_Type * ppArgs, uint8_t argc)
|
|
{
|
|
if (argc >= 1) {
|
|
int en = ONOFF(ppArgs[0]);
|
|
if (en >= 0) {
|
|
if (en && !sLog.internals) {
|
|
MSG("\nSyncIntv. [ns] | dt [ns] | gamma [ppb]\n\n");
|
|
}
|
|
sLog.internals = en;
|
|
return 0;
|
|
} else {
|
|
return -1;
|
|
}
|
|
} else {
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
static struct {
|
|
int params;
|
|
int internals;
|
|
} sCliCmdIdx = { 0 };
|
|
|
|
static void pd_ctrl_register_cli_commands()
|
|
{
|
|
sCliCmdIdx.params = cli_register_command("ptp servo params [Kp Kd] \t\t\tSet or query K_p and K_d servo parameters", 3, 0, CB_params);
|
|
sCliCmdIdx.internals = cli_register_command("ptp servo log internals {on|off} \t\t\tEnable or disable logging of servo internals", 4, 1, CB_logInternals);
|
|
}
|
|
|
|
static void pd_ctrl_remove_cli_commands()
|
|
{
|
|
cli_remove_command(sCliCmdIdx.params);
|
|
cli_remove_command(sCliCmdIdx.internals);
|
|
}
|
|
|
|
#endif // CLI_REG_CMD
|
|
|
|
void pd_ctrl_init()
|
|
{
|
|
pd_ctrl_reset();
|
|
|
|
#ifdef CLI_REG_CMD
|
|
pd_ctrl_register_cli_commands();
|
|
#endif // CLI_REG_CMD
|
|
}
|
|
|
|
void pd_ctrl_deinit()
|
|
{
|
|
#ifdef CLI_REG_CMD
|
|
pd_ctrl_remove_cli_commands();
|
|
#endif // CLI_REG_CMD
|
|
}
|
|
|
|
void pd_ctrl_reset()
|
|
{
|
|
sState.firstRun = true;
|
|
integrator_value = 0;
|
|
}
|
|
|
|
float pd_ctrl_run(int32_t dt, PtpServoAuxInput * pAux)
|
|
{
|
|
if (sState.firstRun) {
|
|
sState.firstRun = false;
|
|
rd_prev_ppb = dt;
|
|
integrator_value = 0;
|
|
return 0;
|
|
}
|
|
|
|
// calculate relative frequency error
|
|
//double rd_ppb = ((double) dt) / (pAux->msgPeriodMs * 1E+06 + dt) * 1E+09;
|
|
double rd_ppb = ((double)dt) / (pAux->measSyncPeriod + dt) * 1E+09;
|
|
|
|
// calculate difference
|
|
double rd_D_ppb = D_FACTOR * (rd_ppb - rd_prev_ppb);
|
|
|
|
// calculate output (run the PD controller)
|
|
double corr_ppb = -(P_FACTOR * (rd_ppb + rd_D_ppb) + integrator_value) * exp((pAux->measSyncPeriod * 1E-09) - 1.0);
|
|
|
|
// update integrator
|
|
integrator_value += I_FACTOR * rd_ppb;
|
|
|
|
// store error value (time difference) for use in next iteration
|
|
rd_prev_ppb = rd_ppb;
|
|
|
|
CLILOG(sLog.internals, "%d %f\n", dt, rd_ppb);
|
|
|
|
return corr_ppb;
|
|
}
|
|
|
|
// ----------------------------------
|