Commit f82c33d7 authored by Tomasz Wlostowski's avatar Tomasz Wlostowski

software: C library/test program for V3

parent ef09fc06
CFLAGS = -I../include -g -Imini_bone
CFLAGS = -I../include -g -Imini_bone -Ispll
OBJS_LIB = fdelay_lib.o rr_io.o i2c_master.o mini_bone/minibone_lib.o mini_bone/ptpd_netif.o fdelay_bus.o
OBJS_LIB = fdelay_lib.o rr_io.o i2c_master.o onewire.o mini_bone/minibone_lib.o mini_bone/ptpd_netif.o fdelay_bus.o
all: $(OBJS_LIB)
all: testprog lib testprog2
lib: $(OBJS_LIB)
gcc -shared -o libfinedelay.so $(OBJS_LIB)
testprog: lib fdelay_test.o
gcc -o fdelay_test $(OBJS_LIB) fdelay_test.o -lm
testprog2: lib fdelay_cal.o
gcc -o fdelay_cal $(OBJS_LIB) fdelay_cal.o -lm
clean:
rm -f libfinedelay.so $(OBJS_LIB)
\ No newline at end of file
#include <stdio.h>
#include <stdint.h>
#include "fdelay_lib.h"
#include "fdelay_private.h"
#include "fd_main_regs.h"
#include "onewire.h"
#include "rr_io.h"
typedef struct {
float kp, ki, err, pwm, setpoint, i, bias;
} pi_t;
pi_t pi_state = {15.0, 5.0, 0, 0, 20, 0, 2048};
void pi_update(fdelay_device_t *dev, float temp)
{
fd_decl_private(dev);
pi_state.err = temp - pi_state.setpoint;
pi_state.i += pi_state.err;
pi_state.pwm = pi_state.bias + pi_state.kp * pi_state.err + pi_state.ki * pi_state.i;
dbg("t %.1f err:%.1f DRIVE: %d\n", temp, pi_state.err, (int)pi_state.pwm);
fd_writel(FD_I2CR_DBGOUT_W((int)pi_state.pwm), FD_REG_I2CR);
}
extern int64_t get_tics();
static int64_t last_tics = 0;
#define TEMP_REG_PERIOD 1000000LL
int pi_set_temp(fdelay_device_t *dev, float new_temp)
{
int temp;
float temp_f;
if(get_tics() - last_tics < TEMP_REG_PERIOD)
return 0;
last_tics = get_tics();
if(ds18x_read_temp(dev, &temp) < 0)
return 0;
temp_f = (float)temp / 16.0;
pi_state.setpoint = new_temp;
pi_update(dev, temp_f);
dbg("Temperature: %.1f degC err %.1f\n", temp_f, pi_state.err);
return fabs(pi_state.err) < 0.1 ? 1: 0;
}
void my_writel(void *priv, uint32_t data, uint32_t addr)
{
rr_writel(data, addr);
}
uint32_t my_readl(void *priv, uint32_t addr)
{
uint32_t d = rr_readl(addr);
return d;
}
main()
{
fdelay_device_t *dev = malloc(sizeof(fdelay_device_t));
rr_init();
dev->writel = my_writel;
dev->readl = my_readl;
dev->base_addr = 0x80000;
if(fdelay_init(dev) < 0)
return -1;
float t_min = 40.0, t_max = 80.0, t_cur;
t_cur = t_min;
for(;;)
{
if(pi_set_temp(dev, t_cur))
{
fd_decl_private(dev);
calibrate_outputs(dev);
fprintf(stderr, "> %.1f %d %d %d %d\n", t_cur, hw->frr_cur[0],
hw->frr_cur[1], hw->frr_cur[2], hw->frr_cur[3]);
t_cur += 1.0;
if(t_cur > t_max)
break;
}
usleep(10000);
}
}
This diff is collapsed.
......@@ -22,16 +22,44 @@ main()
dev.writel = my_writel;
dev.readl = my_readl;
dev.base_addr = 0x80400;
dev.base_addr = 0x84000;
if(fdelay_init(&dev) < 0)
return -1;
fdelay_configure_trigger(&dev, 1,1);
fdelay_configure_output(&dev,1,1,500000, 200000);
fdelay_configure_output(&dev,2,1,504000, 200000);
fdelay_configure_output(&dev,3,1,500000, 200000);
fdelay_configure_output(&dev,4,1,500000, 200000);
fdelay_configure_output(&dev,1,1,500000, 100000, 100000, 0);
fdelay_configure_output(&dev,2,1,500000, 100000, 100000, 0);
fdelay_configure_output(&dev,3,1,500000, 100000, 100000, 0);
fdelay_configure_output(&dev,4,1,500000, 100000, 100000, 0);
fdelay_configure_readout(&dev, 1);
// fd_update_spll(&dev);
int64_t prev = 0, dp, pmin=10000000000LL,pmax=0;
#if 0
for(;;)
{
fdelay_time_t ts;
if(fdelay_read(&dev, &ts, 1) == 1)
{
int64_t ts_p = fdelay_to_picos(ts), d;
d=ts_p - prev;
if(prev > 0)
{
if(d<pmin) pmin=d;
if(d>pmax) pmax=d;
fprintf(stderr,"Got it %lld:%d:%d delta %lld span %lld\n", ts.utc, ts.coarse, ts.frac, d, pmax-pmin);
}
prev = ts_p;
}
}
#endif
for(;;)
{
fdelay_update_calibration(&dev);
sleep(1);
}
}
......@@ -2,7 +2,7 @@
#include "fdelay_lib.h"
#include "fdelay_private.h"
#include "fdelay_regs.h"
#include "fd_main_regs.h"
#define M_SDA_OUT(x) { \
......@@ -73,6 +73,7 @@ static void mi2c_stop(fdelay_device_t *dev)
void mi2c_get_byte(fdelay_device_t *dev, unsigned char *data, int ack)
{
fd_decl_private(dev)
int i;
unsigned char indata = 0;
......@@ -137,7 +138,7 @@ int eeprom_read(fdelay_device_t *dev, uint8_t i2c_addr, uint32_t offset, uint8_t
mi2c_start(dev);
mi2c_put_byte(dev, (i2c_addr << 1) | 1);
mi2c_get_byte(dev, &c, 0);
printf("readback: %x\n", c);
// printf("readback: %x\n", c);
*buf++ = c;
mi2c_stop(dev);
}
......
#include <stdio.h>
#include "board.h"
#include "hw/softpll_regs.h"
#include "irq.h"
static volatile struct SPLL_WB *SPLL = (volatile struct SPLL_WB *) BASE_SOFTPLL;
/* The includes below contain code (not only declarations) to enable the compiler
to inline functions where necessary and save some CPU cycles */
#include "spll_defs.h"
#include "spll_common.h"
#include "spll_helper.h"
volatile int irq_count = 0,eee,yyy;
struct spll_helper_state helper;
void _irq_entry()
{
volatile uint32_t trr;
int src = -1, tag;
if(! (SPLL->CSR & SPLL_TRR_CSR_EMPTY))
{
trr = SPLL->TRR_R0;
src = SPLL_TRR_R0_CHAN_ID_R(trr);
tag = SPLL_TRR_R0_VALUE_R(trr);
eee = tag;
}
helper_update(&helper, tag, src);
yyy=helper.phase.pi.y;
irq_count++;
clear_irq();
}
void spll_init()
{
volatile int dummy;
disable_irq();
SPLL->CSR= 0 ;
SPLL->OCER = 0;
SPLL->RCER = 0;
SPLL->DEGLITCH_THR = 2000;
while(! (SPLL->TRR_CSR & SPLL_TRR_CSR_EMPTY)) dummy = SPLL->TRR_R0;
dummy = SPLL->PER_HPLL;
SPLL->EIC_IER = 1;
}
void spll_test()
{
int i = 0;
volatile int dummy;
spll_init();
helper_start(&helper, 6);
enable_irq();
for(;;)
{
mprintf("cnt %d serr %d src %d y %d d %d\n", irq_count, eee, serr, yyy, delta);
}
}
\ No newline at end of file
/*
White Rabbit Softcore PLL (SoftPLL) - common definitions
*/
/* PI regulator state */
typedef struct {
int ki, kp; /* integral and proportional gains (1<<PI_FRACBITS == 1.0f) */
int integrator; /* current integrator value */
int bias; /* DC offset always added to the output */
int anti_windup; /* when non-zero, anti-windup is enabled */
int y_min; /* min/max output range, used by claming and antiwindup algorithms */
int y_max;
int x,y; /* Current input and output value */
} spll_pi_t;
/* Processes a single sample (x) using PI controller (pi). Returns the value (y) which should
be used to drive the actuator. */
static inline int pi_update(spll_pi_t *pi, int x)
{
int i_new, y;
pi->x = x;
i_new = pi->integrator + x;
y = ((i_new * pi->ki + x * pi->kp) >> PI_FRACBITS) + pi->bias;
/* clamping (output has to be in <y_min, y_max>) and anti-windup:
stop the integretor if the output is already out of range and the output
is going further away from y_min/y_max. */
if(y < pi->y_min)
{
y = pi->y_min;
if((pi->anti_windup && (i_new > pi->integrator)) || !pi->anti_windup)
pi->integrator = i_new;
} else if (y > pi->y_max) {
y = pi->y_max;
if((pi->anti_windup && (i_new < pi->integrator)) || !pi->anti_windup)
pi->integrator = i_new;
} else
pi->integrator = i_new;
pi->y = y;
return y;
}
/* initializes the PI controller state. Currently almost a stub. */
static inline void pi_init(spll_pi_t *pi)
{
pi->integrator = 0;
}
/* lock detector state */
typedef struct {
int lock_cnt;
int lock_samples;
int delock_samples;
int threshold;
int locked;
} spll_lock_det_t;
/* Lock detector state machine. Takes an error sample (y) and checks if it's withing an acceptable range
(i.e. <-ld.threshold, ld.threshold>. If it has been inside the range for (ld.lock_samples) cyckes, the
FSM assumes the PLL is locked. */
static inline int ld_update(spll_lock_det_t *ld, int y)
{
if (abs(y) <= ld->threshold)
{
if(ld->lock_cnt < ld->lock_samples)
ld->lock_cnt++;
if(ld->lock_cnt == ld->lock_samples)
ld->locked = 1;
} else {
if(ld->lock_cnt > ld->delock_samples)
ld->lock_cnt--;
if(ld->lock_cnt == ld->delock_samples)
{
ld->lock_cnt= 0;
ld->locked = 0;
}
}
return ld->locked;
}
static void ld_init(spll_lock_det_t *ld)
{
ld->locked = 0;
ld->lock_cnt = 0;
}
/*
White Rabbit Softcore PLL (SoftPLL) - common definitions
*/
#include <stdio.h>
/* Reference clock frequency */
#define CLOCK_FREQ 125000000
/* Bit size of phase tags generated by the DMTDs. Used to sign-extend the tags. */
#define TAG_BITS 20
/* Helper PLL N divider (1/2**N is the frequency offset) */
#define HPLL_N 14
/* Fractional bits in PI controller coefficients */
#define PI_FRACBITS 12
/* State of the Helper PLL producing a clock (clk_dmtd_i) which is
slightly offset in frequency from the recovered/reference clock (clk_rx_i or clk_ref_i), so the
Main PLL can use it to perform linear phase measurements. This structure keeps the state of the pre-locking
stage */
struct spll_helper_prelock_state {
spll_pi_t pi;
spll_lock_det_t ld;
int f_setpoint;
int ref_select;
fdelay_device_t *dev;
};
volatile int serr;
void helper_prelock_init(struct spll_helper_prelock_state *s)
{
/* Frequency branch PI controller */
s->pi.y_min = 5;
s->pi.y_max = 65530;
s->pi.anti_windup = 0;
s->pi.kp = 28*32*16;
s->pi.ki = 50*32*16;
s->pi.bias = 32000;
/* Freqency branch lock detection */
s->ld.threshold = 2;
s->ld.lock_samples = 1000;
s->ld.delock_samples = 990;
s->f_setpoint = 131072 / (1<<HPLL_N);
pi_init(&s->pi);
ld_init(&s->ld);
}
void helper_prelock_enable(struct spll_helper_prelock_state *state, int ref_channel, int enable)
{
fdelay_device_t *dev = state->dev;
fd_decl_private(dev);
fd_writel(0, FD_REG_SPLLR);
}
#define SPLL_LOCKED 1
#define SPLL_LOCKING 0
int helper_prelock_update(struct spll_helper_prelock_state *s, int tag)
{
fdelay_device_t *dev = s->dev;
fd_decl_private(dev);
int y;
volatile uint32_t per = fd_readl(FD_REG_SPLLR);
short err = (short) (tag & 0xffff);
serr = (int)err;
err -= s->f_setpoint;
y = pi_update(&s->pi, err);
fd_writel(y, FD_REG_SDACR);
if(ld_update(&s->ld, err))
return SPLL_LOCKED;
return SPLL_LOCKING;
}
struct spll_helper_phase_state {
spll_pi_t pi;
spll_lock_det_t ld;
int p_setpoint, tag_d0;
int ref_src;
fdelay_device_t *dev;
};
void helper_phase_init(struct spll_helper_phase_state *s)
{
/* Phase branch PI controller */
s->pi.y_min = 5;
s->pi.y_max = 65530;
s->pi.kp = (int)(2.0 * 32.0 * 16.0);
s->pi.ki = (int)(0.05 * 32.0 * 3.0);
s->pi.anti_windup = 0;
s->pi.bias = 32000;
/* Phase branch lock detection */
s->ld.threshold = 500;
s->ld.lock_samples = 10000;
s->ld.delock_samples = 9900;
s->ref_src = 6;
s->p_setpoint = -1;
pi_init(&s->pi);
ld_init(&s->ld);
}
void helper_phase_enable(struct spll_helper_phase_state *state, int ref_channel, int enable)
{
fdelay_device_t *dev = state->dev;
fd_decl_private(dev);
fd_writel(FD_SPLLR_MODE, FD_REG_SPLLR);
}
volatile int delta;
int helper_phase_update(struct spll_helper_phase_state *s, int tag, int source)
{
fdelay_device_t *dev = s->dev;
fd_decl_private(dev);
int err, y;
serr = source;
// if(source == s->ref_src)
{
if(s->p_setpoint < 0)
{
s->p_setpoint = tag;
return;
}
err = tag - s->p_setpoint;
delta = tag - s->tag_d0;
s->tag_d0 = tag;
s->p_setpoint += (1<<HPLL_N);
if(s->p_setpoint > (1<<TAG_BITS))
s->p_setpoint -= (1<<TAG_BITS);
y = pi_update(&s->pi, err);
//printf("t %d sp %d\n", tag, s->p_setpoint);
fd_writel(y, FD_REG_SDACR);
if(ld_update(&s->ld, err))
{
return SPLL_LOCKED;
};
}
return SPLL_LOCKING;
}
#define HELPER_PRELOCKING 1
#define HELPER_PHASE 2
#define HELPER_LOCKED 3
struct spll_helper_state {
struct spll_helper_prelock_state prelock;
struct spll_helper_phase_state phase;
int state;
int ref_channel;
};
void helper_start(fdelay_device_t *dev, struct spll_helper_state *s)
{
s->state = HELPER_PRELOCKING;
s->ref_channel = 0;
s->prelock.dev = dev;
s->phase.dev = dev;
helper_prelock_init(&s->prelock);
helper_phase_init(&s->phase);
helper_prelock_enable(&s->prelock, 0, 1);
}
void helper_update(struct spll_helper_state *s)
{
fdelay_device_t *dev = s->prelock.dev;
fd_decl_private(dev);
uint32_t spllr = fd_readl(FD_REG_SPLLR);
if(! (spllr & FD_SPLLR_TAG_RDY))
return;
int tag = FD_SPLLR_TAG_R(spllr);
switch(s->state)
{
case HELPER_PRELOCKING:
if(helper_prelock_update(&s->prelock, tag) == SPLL_LOCKED)
{
s->state = HELPER_PHASE;
helper_prelock_enable(&s->prelock, 0, 0);
s->phase.pi.bias = s->prelock.pi.y;
helper_phase_enable(&s->phase, 0, 1);
}
break;
case HELPER_PHASE:
helper_phase_update(&s->phase, tag, 0);
break;
}
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment