diff options
Diffstat (limited to 'fw/fe310/eos/eve/eve_phy.c')
-rw-r--r-- | fw/fe310/eos/eve/eve_phy.c | 30 |
1 files changed, 23 insertions, 7 deletions
diff --git a/fw/fe310/eos/eve/eve_phy.c b/fw/fe310/eos/eve/eve_phy.c index 1e255fe..cfddc80 100644 --- a/fw/fe310/eos/eve/eve_phy.c +++ b/fw/fe310/eos/eve/eve_phy.c @@ -9,30 +9,44 @@ void eve_phy_acc_init(EVEPhyAcc *param, int a) { param->a = a; } -void eve_phy_acc_start(EVEPhyAcc *param, int x0, int y0, int v0x, int v0y) { - double v0 = sqrt(v0x * v0x + v0y * v0y); +int eve_phy_acc_start(EVEPhyAcc *param, int x0, int y0, uint32_t t0, int v0x, int v0y) { + double v0; + uint32_t dt = eve_get_tick() - t0; + v0 = sqrt(v0x * v0x + v0y * v0y); + param->k = 2 * v0 / param->a * EVE_RTC_FREQ; + if ((param->k < 0) && (dt >= -param->k / 2)) { + return 0; + } param->x0 = x0; param->y0 = y0; param->v0x = v0x; param->v0y = v0y; - param->k = 2 * v0 / param->a * EVE_RTC_FREQ; + + return 1; } -int eve_phy_acc_tick(EVEPhyAcc *param, int dt, int *x, int *y) { +int eve_phy_acc_tick(EVEPhyAcc *param, uint32_t dt, int *x, int *y) { int k = param->k; int x0 = param->x0; int y0 = param->y0; int v0x = param->v0x; int v0y = param->v0y; + int _dt = dt; int more = 1; + if (k == 0) { + if (x) *x = x0; + if (y) *y = y0; + return 0; + } + if ((k < 0) && (dt >= -k / 2)) { dt = -k / 2; more = 0; } - if (x) *x = x0 + (v0x * dt + v0x * dt / k * dt) / (int)(EVE_RTC_FREQ); - if (y) *y = y0 + (v0y * dt + v0y * dt / k * dt) / (int)(EVE_RTC_FREQ); + if (x) *x = x0 + (v0x * _dt + v0x * _dt / k * _dt) / EVE_RTC_FREQ; + if (y) *y = y0 + (v0y * _dt + v0y * _dt / k * _dt) / EVE_RTC_FREQ; return more; } @@ -53,9 +67,11 @@ void eve_phy_lho_init(EVEPhyLHO *param, int x, int y, uint32_t T, double d, uint int eve_phy_lho_start(EVEPhyLHO *param, int x0, int y0) { param->x0 = x0; param->y0 = y0; + + return 1; } -int eve_phy_lho_tick(EVEPhyLHO *param, int dt, int *x, int *y) { +int eve_phy_lho_tick(EVEPhyLHO *param, uint32_t dt, int *x, int *y) { int ax = param->x0 - param->x; int ay = param->y0 - param->y; int more = 1; |