FOC算法实现过程记录(v2):01.电压开环实现
小于 1 分钟
FOC算法实现过程记录(v2):01.电压开环实现
电压开环实现
motor.h#ifndef __Motor_H__
#define __Motor_H__
#include <Arduino.h>
#include <stdint.h>
#include "foc_math.h"
#include "pid_controller.h"
#include "pll.h"
#ifdef __cplusplus
extern "C"
{
#endif
// 状态
typedef enum
{
Align = 0,
Running = 1,
Stop = 2,
} State;
// 控制模式
typedef enum
{
OPEN_VOLTAGE_LOOP = 0,
CLOSE_CURRENT_LOOP = 1,
CLOSE_VELOCITY_LOOP = 2,
} ControlMode;
// 调制模式
typedef enum
{
SPWM = 0,
SVPWM = 1,
} Modulation;
typedef struct
{
// 极对数
uint8_t pole_pairs;
// ########################################
float power_supply;
// ########################################
float target; // 目标值
State state;
ControlMode controlMode; // 控制模式
Modulation modulation; // 调制模式
// ########################################
// clsoe_current_loop
PidController pid_id_controller;
PidController pid_iq_controller;
float Id_target, Iq_target;
float Id, Iq;
float Ia, Ib, Ic;
float Ialpha, Ibeta;
// open_volatage_loop
float Ud_target, Uq_target;
float Ud, Uq;
float Uahpha, Ubeta;
float Ua, Ub, Uc;
// ########################################
float theta_mes; // 测量角度 measurement
float theta_mes_offset; // 测量角度误差 measurement
PLL pll_theta; // 角度锁相环
LowpassFilter theta_filter; // 估计角度滤波器
float theta_est; // 估计角度 estimate
float theta_pdt; // 预测角度 prediect
float e_theta; // 电角度
float omega; // 角速度
float e_omega; // 电角速度
float sin_e_theta; // sin(e_theta)
float cos_e_theta; // cos(e_theta)
// ########################################
} Motor;
void motor_init(Motor *motor, uint8_t pole_pairs, float power_supply);
void motor_foc_loop(Motor *motor, float dt);
void motor_set_phrase_voltage(Motor *motor);
#ifdef __cplusplus
}
#endif
#endif
motor.c#include "motor.h"
#include "hardware.h"
void motor_init(Motor *motor, uint8_t pole_pairs, float power_supply)
{
// 初始化
motor->pole_pairs = pole_pairs;
motor->power_supply = power_supply;
// 初始化
motor->target = 5;
motor->controlMode = OPEN_VOLTAGE_LOOP;
motor->modulation = SPWM;
pid_controller_init(&motor->pid_id_controller, 30, 0, 0, 12, 0);
pid_controller_init(&motor->pid_iq_controller, 30, 0, 0, 12, 0);
// 初始化
motor->theta_mes = 0;
motor->theta_mes_offset = 0;
motor->theta_est = 0;
motor->theta_pdt = 0;
pll_init(&motor->pll_theta, 0.000050, 500, 0);
lowpass_filter_init(&motor->theta_filter, 0.000010);
motor->e_theta = 0;
motor->omega = 0.1f;
motor->e_omega = 0;
motor->sin_e_theta = 0;
motor->cos_e_theta = 0;
// 初始化硬件
// analogWriteResolution(12);
// analogReadResolution(14);
// pinMode(A5, INPUT); // sensor-analog
// pinMode(5, OUTPUT); // pwmA
// pinMode(9, OUTPUT); // pwmB
// pinMode(6, OUTPUT); // pwmC
pinMode(8, OUTPUT); // pwm_EN
digitalWrite(8, HIGH); // driver enable
sensor_align(motor);
}
void sensor_align(Motor *motor)
{
// 等效为在 dq轴 -90°方向 注入电压。
hardware_set_pwm_duty(
0, 0.5, // Ua = 0
1, 0.0, // Ub = 1
2, 0.0 // Uc = 1
);
delay(1000); // 1s
float x = 0;
for (uint8_t n = 1; n <= 100; n++)
{
float z = hardware_get_adc_value(ADC_CHANNEL_22) * M_TWOPI;
// 递归求均值
// X[n] = X[n-1] + 1/n * (Z[n] - X[n-1])
x = x + (z - x) / n;
delay(5);
}
motor->theta_mes_offset = -x;
hardware_set_pwm_duty(
0, 0.0, // Ua = 0
1, 0.0, // Ub = 0
2, 0.0 // Uc = 0
);
}
void motor_state_update(Motor *motor, float dt)
{
}
void motor_foc_loop(Motor *motor, float dt)
{
// ########################################################
// float angle = analogRead(A5) * M_TWOPI / 0x03fff; // [0,0x03fff] => [0,2*pi] 耗时50us
motor->theta_mes = _normalizeAngle(hardware_get_adc_value(ADC_CHANNEL_22) * M_TWOPI + motor->theta_mes_offset); // 测量角度
pll_update(&motor->pll_theta, motor->theta_mes, dt); // 锁相环更新
// motor->theta_est = lowpass_filter_update(&motor->theta_filter, motor->pll_theta.theta, dt); // 估计角度 滤波
// ########################################################
motor->theta_est = motor->theta_mes;
// motor->theta_est = motor->pll_theta.theta;
motor->omega = motor->pll_theta.omega;
// ########################################################
motor->e_theta = _normalizeAngle(motor->theta_est * motor->pole_pairs);
motor->e_omega = motor->omega * motor->pole_pairs;
motor->sin_e_theta = sinf(motor->e_theta); // Arduino UNO R4（带FPU） 最省时 sinf+cosf 耗时10us
motor->cos_e_theta = cosf(motor->e_theta);
// ########################################################
// motor->sin_e_theta = _sin(motor->e_theta); // 查表法 _sin+_cos 耗时46us
// motor->cos_e_theta = _cos(motor->e_theta);
// ########################################################
// motor->sin_e_theta = sin(motor->e_theta); // 双精度 耗时严重 sin+cos 100us~129us
// motor->cos_e_theta = cos(motor->e_theta);
// ########################################################
//
switch (motor->controlMode)
{
case CLOSE_VELOCITY_LOOP:
break;
case CLOSE_CURRENT_LOOP:
motor->Id_target = 0;
motor->Iq_target = motor->target;
motor_close_current_loop(motor, dt);
break;
case OPEN_VOLTAGE_LOOP:
motor->Ud_target = 0;
motor->Uq_target = motor->target;
motor_open_voltage_loop(motor, dt);
break;
default:
break;
}
//
}
void motor_open_voltage_loop(Motor *motor, float dt)
{
motor->Ud = motor->Ud_target;
motor->Uq = motor->Uq_target;
switch (motor->modulation)
{
case SPWM: // 帕克逆变换+克拉克逆变换 耗时13us
{
#define M_SQRT3_2 0.866025403785 // sqrt(3)/2
// 帕克逆变换
motor->Uahpha = motor->cos_e_theta * motor->Ud - motor->sin_e_theta * motor->Uq;
motor->Ubeta = motor->sin_e_theta * motor->Ud + motor->cos_e_theta * motor->Uq;
// 克拉克逆变换
motor->Ua = motor->Uahpha; // 等赋值形式
motor->Ub = -0.5f * motor->Uahpha + M_SQRT3_2 * motor->Ubeta;
motor->Uc = -(motor->Ua + motor->Ub); // 基尔霍夫电压电流定律
}
break;
case SVPWM:
{
#define M_PI_3 1.0471975512 // pi/3
#define M_SQRT3 1.73205080757 // sqrt(3)
int sector = floor(motor->e_theta / M_PI_3) + 1;
float T1 = M_SQRT3 * sinf(sector * M_PI_3 - motor->e_theta) * motor->Uq / motor->power_supply;
float T2 = M_SQRT3 * sinf(motor->e_theta - (sector - 1.0f) * M_PI_3) * motor->Uq / motor->power_supply;
float T0 = 1 - T1 - T2;
float Ta, Tb, Tc;
switch (sector)
{
case 1:
Ta = T1 + T2 + T0 / 2;
Tb = T2 + T0 / 2;
Tc = T0 / 2;
break;
case 2:
Ta = T1 + T0 / 2;
Tb = T1 + T2 + T0 / 2;
Tc = T0 / 2;
break;
case 3:
Ta = T0 / 2;
Tb = T1 + T2 + T0 / 2;
Tc = T2 + T0 / 2;
break;
case 4:
Ta = T0 / 2;
Tb = T1 + T0 / 2;
Tc = T1 + T2 + T0 / 2;
break;
case 5:
Ta = T2 + T0 / 2;
Tb = T0 / 2;
Tc = T1 + T2 + T0 / 2;
break;
case 6:
Ta = T1 + T2 + T0 / 2;
Tb = T0 / 2;
Tc = T1 + T0 / 2;
break;
default:
Ta = 0;
Tb = 0;
Tc = 0;
motor->Ua = Ta * motor->power_supply;
motor->Ub = Tb * motor->power_supply;
motor->Uc = Tc * motor->power_supply;
}
}
break;
default:
break;
}
// 耗时38~46us:
// [-1,+1] -> [-0.5,+0.5] -> [0,1] -> [0,0x0fff]
// analogWrite(5, (motor->Ua / motor->power_supply * 0.5 + 0.5f) * 0x0fff);
// analogWrite(9, (motor->Ub / motor->power_supply * 0.5 + 0.5f) * 0x0fff);
// analogWrite(6, (motor->Uc / motor->power_supply * 0.5 + 0.5f) * 0x0fff);
// 耗时：
hardware_set_pwm_duty(
0, (motor->Ua / motor->power_supply * 0.5 + 0.5f),
1, (motor->Ub / motor->power_supply * 0.5 + 0.5f),
2, (motor->Uc / motor->power_supply * 0.5 + 0.5f));
}
void motor_close_current_loop(Motor *motor, float dt)
{
// 电流采样
// close_current_loop
// motor->Ia = (analogRead(A0) * 5.0f / 0x03fff) - 2.5f; // [0,0x03fff] => [-2.5,+2.5] 耗时50us
// motor->Ib = (analogRead(A2) * 5.0f / 0x03fff) - 2.5f; // [0,0x03fff] => [-2.5,+2.5] 耗时50us
// motor->Ic = -(motor->Ia + motor->Ib);
// 电流采样
motor->Ia = (hardware_get_adc_value(CHANNEL_9) - 0.5) / 0.01 * 50; // [0,1] -> [-0.5,0.5] -> 0.01Ω 50倍 i=u/r
motor->Ib = -(hardware_get_adc_value(CHANNEL_1) - 0.5) / 0.01 * 50; // [0,1] -> [-0.5,0.5] -> 0.01Ω 50倍 i=u/r
motor->Ic = -(motor->Ia + motor->Ib);
// 克拉克变换
motor->Ialpha = motor->Ia; // 等幅值形式
motor->Ibeta = sqrtf(3) / 3 * motor->Ia + 2 * sqrtf(3) / 3 * motor->Ib;
// 帕克变换
motor->Id = motor->cos_e_theta * motor->Ialpha + motor->sin_e_theta * motor->Ibeta;
motor->Ia = -motor->sin_e_theta * motor->Ialpha + motor->cos_e_theta * motor->Ibeta;
// 电流环pid
motor->Ud_target = pid_controller_update(&motor->pid_id_controller, motor->Id - motor->Id_target, dt);
motor->Uq_target = pid_controller_update(&motor->pid_iq_controller, motor->Iq - motor->Iq_target, dt);
// 电压环
motor_open_voltage_loop(motor, dt);
}
hardware.h#ifndef __HARDWARE_H__
#define __HARDWARE_H__
#include <common_data.h>
#include <r_cgc.h>
#include <r_adc.h>
#include <r_gpt.h>
#include <r_ioport.h>
#ifdef __cplusplus
extern "C"
{
#endif
void hardware_init();
float hardware_get_adc_value(adc_channel_t channel);
void hardware_set_pwm_duty(uint8_t chA, float dutyA, uint8_t chB, float dutyB, uint8_t chC, float dutyC);
#ifdef __cplusplus
}
#endif
#endif
hardware.c#include "hardware.h"
/**
* Current Sensor
* i_a A0 P014 AN09
* i_b A2 P001 AN01
*
* Position Sensor
* theta A5 P100 AN022
*
* PWM
* A D5 P103 GPT2_A
* B D9 P304 GPT7_A
* C D6 P102 GPT2_B
*
* A D5-GPT2_A or D3-GPT1_B
* B D9-GPT7_A or D10-GPT3_B
* C D6-GPT2_B or D11-NULL
* EN 8
*/
//
//
//
//
// 产品页：https://store.arduino.cc/products/uno-r4-minima
// 电路图：https://docs.arduino.cc/resources/schematics/ABX00080-schematics.pdf
// 芯片手册：https://www.renesas.com/en/document/dst/ra4m1-group-datasheet
// Renesas Flexible Software Package (FSP)库函数手册: https://renesas.github.io/fsp/
adc_instance_ctrl_t adc_ctrl;
adc_cfg_t adc_cfg;
adc_extended_cfg_t adc_cfg_extend;
adc_channel_cfg_t adc_channel_cfg;
uint16_t *adc_channel = (uint16_t *)(R_ADC0->ADDR);
void adc_callback(adc_callback_args_t *p_args)
{
}
void adc_init()
{
adc_cfg.unit = 0;
adc_cfg.mode = ADC_MODE_CONTINUOUS_SCAN; // 连续采样
adc_cfg.resolution = ADC_RESOLUTION_14_BIT; // 14bit
adc_cfg.alignment = ADC_ALIGNMENT_RIGHT; // 右对齐
adc_cfg.trigger = ADC_TRIGGER_SOFTWARE; // 软件触发
adc_cfg.p_callback = adc_callback;
adc_cfg.p_context = NULL;
adc_cfg.scan_end_irq = FSP_INVALID_VECTOR; // 未定义的中断向量
adc_cfg.scan_end_ipl = (12); // 中断优先级
adc_cfg.scan_end_b_irq = FSP_INVALID_VECTOR;
adc_cfg.scan_end_b_ipl = (12);
adc_cfg.p_extend = &adc_cfg_extend;
adc_cfg_extend.add_average_count = ADC_ADD_OFF; // 不使用均值采样
adc_cfg_extend.clearing = ADC_CLEAR_AFTER_READ_OFF; // 读取后不清零
adc_cfg_extend.trigger_group_b = ADC_TRIGGER_SOFTWARE;
adc_cfg_extend.double_trigger_mode = ADC_DOUBLE_TRIGGER_DISABLED; //
adc_cfg_extend.adc_vref_control = ADC_VREF_CONTROL_AVCC0_AVSS0; // 参考电压为AVCC AVSS
adc_cfg_extend.enable_adbuf = 0;
adc_cfg_extend.window_a_irq = FSP_INVALID_VECTOR; // 未定义的中断向量
adc_cfg_extend.window_a_ipl = (12); // 中断优先级
adc_cfg_extend.window_b_irq = FSP_INVALID_VECTOR;
adc_cfg_extend.window_b_ipl = (12);
adc_channel_cfg.sample_hold_states = 24;
adc_channel_cfg.scan_mask = ADC_MASK_CHANNEL_9 | ADC_MASK_CHANNEL_1 | ADC_MASK_CHANNEL_22;
adc_channel_cfg.scan_mask_group_b = 0;
adc_channel_cfg.add_mask = 0;
adc_channel_cfg.p_window_cfg = NULL;
adc_channel_cfg.priority_group_a = ADC_GROUP_A_PRIORITY_OFF;
adc_channel_cfg.sample_hold_mask = 0;
R_ADC_Open(&adc_ctrl, &adc_cfg);
R_ADC_ScanCfg(&adc_ctrl, &adc_channel_cfg);
R_ADC_ScanStart(&adc_ctrl);
}
float hardware_get_adc_value(adc_channel_t channel)
{
// return R_ADC0->ADDR[channel];
uint16_t result;
R_ADC_Read(&adc_ctrl, channel, &result);
return result / 16383.0f;
}
// pwmA D5 P107_GPT0_A
// pwmB D9 P303_GPT7_B
// pwmC D6 P111_GPT3_A
uint8_t timer_ch[3] = {0, 7, 3};
gpt_io_pin_t pwm_ch[3] = {GPT_IO_PIN_GTIOCA, GPT_IO_PIN_GTIOCB, GPT_IO_PIN_GTIOCA};
bsp_io_port_pin_t io_port[3] = {BSP_IO_PORT_01_PIN_07, BSP_IO_PORT_03_PIN_03, BSP_IO_PORT_01_PIN_11};
gpt_instance_ctrl_t gpt_ctrls[3];
timer_cfg_t gpt_timer_cfgs[3];
gpt_extended_cfg_t gpt_ext_cfgs[3];
gpt_extended_pwm_cfg_t gpt_pwm_cfgs[3];
void gpt_init()
{
uint32_t start_mask = 0;
for (int8_t idx = 0; idx <= 2; idx++)
{
R_IOPORT_PinCfg(&g_ioport_ctrl, io_port[idx], (uint32_t)(IOPORT_CFG_PORT_DIRECTION_OUTPUT | IOPORT_CFG_PERIPHERAL_PIN | IOPORT_PERIPHERAL_GPT1));
// port_config
// timer_config
gpt_timer_cfgs[idx].channel = timer_ch[idx];
gpt_timer_cfgs[idx].mode = TIMER_MODE_PWM;
// 48Mhz dev1 cnt=1000 => 48khz
// 48Mhz dev2 cnt=1000 => 24khz
// 48Mhz dev4 cnt=1000 => 12khz
gpt_timer_cfgs[idx].source_div = TIMER_SOURCE_DIV_2; // // uint32_t pclkd_freq_hz = R_FSP_SystemClockHzGet(FSP_PRIV_CLOCK_PCLKD)
gpt_timer_cfgs[idx].period_counts = 1000; // pwm分辨率
gpt_timer_cfgs[idx].duty_cycle_counts = 500;
gpt_timer_cfgs[idx].p_callback = NULL;
gpt_timer_cfgs[idx].p_context = NULL;
gpt_timer_cfgs[idx].p_extend = &gpt_ext_cfgs[idx];
gpt_timer_cfgs[idx].cycle_end_ipl = BSP_IRQ_DISABLED;
gpt_timer_cfgs[idx].cycle_end_irq = FSP_INVALID_VECTOR;
// // gpt_cfg
gpt_ext_cfgs[idx].gtioca.output_enabled = pwm_ch[idx] == GPT_IO_PIN_GTIOCA;
gpt_ext_cfgs[idx].gtioca.stop_level = GPT_PIN_LEVEL_LOW;
gpt_ext_cfgs[idx].gtiocb.output_enabled = pwm_ch[idx] == GPT_IO_PIN_GTIOCB;
gpt_ext_cfgs[idx].gtiocb.stop_level = GPT_PIN_LEVEL_LOW;
gpt_ext_cfgs[idx].start_source = (gpt_source_t)(GPT_SOURCE_NONE);
gpt_ext_cfgs[idx].stop_source = (gpt_source_t)(GPT_SOURCE_NONE);
gpt_ext_cfgs[idx].clear_source = (gpt_source_t)(GPT_SOURCE_NONE);
gpt_ext_cfgs[idx].count_up_source = (gpt_source_t)(GPT_SOURCE_NONE);
gpt_ext_cfgs[idx].count_down_source = (gpt_source_t)(GPT_SOURCE_NONE);
gpt_ext_cfgs[idx].capture_a_source = (gpt_source_t)(GPT_SOURCE_NONE);
gpt_ext_cfgs[idx].capture_b_source = (gpt_source_t)(GPT_SOURCE_NONE);
gpt_ext_cfgs[idx].capture_a_ipl = BSP_IRQ_DISABLED;
gpt_ext_cfgs[idx].capture_b_ipl = BSP_IRQ_DISABLED;
gpt_ext_cfgs[idx].capture_a_irq = FSP_INVALID_VECTOR;
gpt_ext_cfgs[idx].capture_b_irq = FSP_INVALID_VECTOR;
gpt_ext_cfgs[idx].capture_filter_gtioca = GPT_CAPTURE_FILTER_NONE;
gpt_ext_cfgs[idx].capture_filter_gtiocb = GPT_CAPTURE_FILTER_NONE;
gpt_ext_cfgs[idx].p_pwm_cfg = &gpt_pwm_cfgs[idx];
gpt_ext_cfgs[idx].gtior_setting.gtior = 0U;
// // pwm_cfg
gpt_pwm_cfgs[idx].trough_ipl = BSP_IRQ_DISABLED;
gpt_pwm_cfgs[idx].trough_irq = FSP_INVALID_VECTOR;
gpt_pwm_cfgs[idx].poeg_link = GPT_POEG_LINK_POEG0;
gpt_pwm_cfgs[idx].output_disable = GPT_OUTPUT_DISABLE_NONE;
gpt_pwm_cfgs[idx].adc_trigger = GPT_ADC_TRIGGER_NONE;
gpt_pwm_cfgs[idx].dead_time_count_up = 0;
gpt_pwm_cfgs[idx].dead_time_count_down = 0;
gpt_pwm_cfgs[idx].adc_a_compare_match = 0;
gpt_pwm_cfgs[idx].adc_b_compare_match = 0;
gpt_pwm_cfgs[idx].interrupt_skip_source = GPT_INTERRUPT_SKIP_SOURCE_NONE;
gpt_pwm_cfgs[idx].interrupt_skip_count = GPT_INTERRUPT_SKIP_COUNT_0;
gpt_pwm_cfgs[idx].interrupt_skip_adc = GPT_INTERRUPT_SKIP_ADC_NONE;
gpt_pwm_cfgs[idx].gtioca_disable_setting = GPT_GTIOC_DISABLE_PROHIBITED;
gpt_pwm_cfgs[idx].gtiocb_disable_setting = GPT_GTIOC_DISABLE_PROHIBITED;
// memset(&gpt_ctrl[idx], 0, sizeof(gpt_instance_ctrl_t));
fsp_err_t err = FSP_SUCCESS;
// Initializes the timer module and applies configurations.
err = R_GPT_Open(&gpt_ctrls[idx], &gpt_timer_cfgs[idx]);
assert(FSP_SUCCESS == err);
// Enables external event triggers that start, stop, clear, or capture the counter.
err = R_GPT_Enable(&gpt_ctrls[idx]);
assert(FSP_SUCCESS == err);
// // Enable output for GTIOCA and/or GTIOCB
err = R_GPT_OutputEnable(&gpt_ctrls[idx], pwm_ch[idx]);
assert(FSP_SUCCESS == err);
// Starts timer.
// err = R_GPT_Start(&gpt_ctrls[idx]);
// assert(FSP_SUCCESS == err);
start_mask |= (1 << timer_ch[idx]);
}
// Starts timer at the same time
gpt_ctrls[0].p_reg->GTSTR |= start_mask;
}
/**
* dutyA [0.0,1.0]
*/
void hardware_set_pwm_duty(uint8_t chA, float dutyA, uint8_t chB, float dutyB, uint8_t chC, float dutyC)
{
R_GPT_DutyCycleSet(&gpt_ctrls[chA], dutyA * 1000, pwm_ch[chA]);
R_GPT_DutyCycleSet(&gpt_ctrls[chB], dutyB * 1000, pwm_ch[chB]);
R_GPT_DutyCycleSet(&gpt_ctrls[chC], dutyC * 1000, pwm_ch[chC]);
}
void hardware_init()
{
adc_init();
gpt_init();
}
pll.h#ifndef __PLL_H__
#define __PLL_H__
#include "foc_math.h"
#include "lowpass_filter.h"
#include "pid_controller.h"
#ifdef __cplusplus
extern "C"
{
#endif
typedef struct
{
float theta; // 角度
float omega; // 角速度
LowpassFilter filter;
PidController pid;
} PLL;
void pll_init(PLL *pll, float Ts, float Kp, float Ki);
float pll_update(PLL *pll, float theta, float dt);
#ifdef __cplusplus
}
#endif
#endif
pll.c#include "pll.h"
void pll_init(PLL *pll, float Ts, float Kp, float Ki)
{
pll->theta = 0;
pll->omega = 0;
lowpass_filter_init(&pll->filter, Ts);
pid_controller_init(&pll->pid, Kp, Ki, 0, 1e10f, 1e10f);
}
float pll_update(PLL *pll, float theta, float dt)
{
// 相位差 = sin(theta)*cos(target) - cos(theta)*sin(target) ≈ sin(theta - target) ≈ PI - normalizeAngle(theta - target)
// 角速度 = PI控制器(lpf低通滤波器(鉴相器))
// 角度 = 角速度 * dt
// float phase_diff = cosf(pll->theta) * sinf(theta) - sinf(pll->theta) * cosf(theta);
float phase_diff = sinf(_normalizeAngle(pll->theta - theta));
// float phase_diff = _normalizeAngle(pll->theta - theta) - M_PI;
pll->omega = pid_controller_update(&pll->pid, lowpass_filter_update(&pll->filter, phase_diff, dt), dt);
pll->theta += pll->omega * dt;
return pll->theta;
}