1 files changed, 19 insertions, 21 deletions

diff --git a/fw/fe310/eos/eve/eve_phy.c b/fw/fe310/eos/eve/eve_phy.c
index cfddc80..8f170ba 100644
--- a/fw/fe310/eos/eve/eve_phy.c
+++ b/fw/fe310/eos/eve/eve_phy.c
@@ -11,69 +11,67 @@ void eve_phy_acc_init(EVEPhyAcc *param, int a) {
 
 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)) {
+    if (param->k == 0) {
         return 0;
     }
     param->x0 = x0;
     param->y0 = y0;
+    param->t0 = t0;
     param->v0x = v0x;
     param->v0y = v0y;
 
     return 1;
 }
 
-int eve_phy_acc_tick(EVEPhyAcc *param, uint32_t dt, int *x, int *y) {
+int eve_phy_acc_tick(void *_param, int *x, int *y) {
+    EVEPhyAcc *param = (EVEPhyAcc *)_param;
     int k = param->k;
     int x0 = param->x0;
     int y0 = param->y0;
     int v0x = param->v0x;
     int v0y = param->v0y;
-    int _dt = dt;
+    uint32_t dt = eve_get_tick() - param->t0;
     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) / EVE_RTC_FREQ;
-    if (y) *y = y0 + (v0y * _dt + v0y * _dt / k * _dt) / EVE_RTC_FREQ;
+    if (x) *x = x0 + (v0x * (int)dt + v0x * (int)dt / k * (int)dt) / EVE_RTC_FREQ;
+    if (y) *y = y0 + (v0y * (int)dt + v0y * (int)dt / k * (int)dt) / EVE_RTC_FREQ;
 
     return more;
 }
 
 /* Linear harmonic oscillator */
-void eve_phy_lho_init(EVEPhyLHO *param, int x, int y, uint32_t T, double d, uint32_t t_max) {
+void eve_phy_lho_init(EVEPhyLHO *param, uint32_t T, double d, uint32_t t_max) {
     double f0 = 2 * M_PI / (T * EVE_RTC_FREQ / 1000);
 
     if (d < 0) d = 0;
     if (d > 1) d = 1;
-    param->x = x;
-    param->y = y;
     param->f = f0 * sqrt(1 - d * d);
     param->a = -d * f0;
     param->t_max = t_max * EVE_RTC_FREQ / 1000;
 }
 
-int eve_phy_lho_start(EVEPhyLHO *param, int x0, int y0) {
+int eve_phy_lho_start(EVEPhyLHO *param, int pivot_x, int pivot_y, int x0, int y0, uint32_t t0) {
+    param->pivot_x = pivot_x;
+    param->pivot_y = pivot_y;
     param->x0 = x0;
     param->y0 = y0;
+    param->t0 = t0;
 
     return 1;
 }
 
-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 eve_phy_lho_tick(void *_param, int *x, int *y) {
+    EVEPhyLHO *param = (EVEPhyLHO *)_param;
+    int ax = param->x0 - param->pivot_x;
+    int ay = param->y0 - param->pivot_y;
+    uint32_t dt = eve_get_tick() - param->t0;
     int more = 1;
 
     if (param->t_max && (dt >= param->t_max)) {
@@ -86,8 +84,8 @@ int eve_phy_lho_tick(EVEPhyLHO *param, uint32_t dt, int *x, int *y) {
         ay = ay * e;
         if ((ax == 0) && (ay == 0)) more = 0;
     }
-    if (x) *x = param->x + ax * cos(param->f * dt);
-    if (y) *y = param->y + ay * cos(param->f * dt);
+    if (x) *x = param->pivot_x + ax * cos(param->f * dt);
+    if (y) *y = param->pivot_y + ay * cos(param->f * dt);
 
     return more;
 }