Dynamic frequency ajustment.

3 files changed, 42 insertions, 21 deletions

diff --git a/monitor/disp.c b/monitor/disp.c
index 96d994d..afefe41 100644
--- a/monitor/disp.c
+++ b/monitor/disp.c
@@ -74,11 +74,12 @@ void disp_display(t_mon *mon) {
 
     werase(wpstatus);
     
-    wprintw(wpstatus, "Step:\t\t%d\t%s",
-        mon->step,
-        (mon->status == MS_AUTO ? "A" : (mon->status == MS_RUN ? "M" : "")));
-    if (mon->status == MS_FREQ) wprintw(wpstatus, "%dHz", mon->freq);
-    if (mon->status == MS_AUTO) wprintw(wpstatus, " %dHz", mon->max_freq);
+    wprintw(wpstatus, "Step:\t\t%d\t", mon->step);
+    if (mon->status == MS_RUN) wprintw(wpstatus, "M");
+    if (mon->status == MS_FREQ)
+        wprintw(wpstatus, "%dHz\t%dHz", mon->actual_freq, mon->target_freq);
+    if (mon->status == MS_AUTO)
+        wprintw(wpstatus, "%dHz\t", mon->actual_freq);
     wprintw(wpstatus, "\t%s\n",
         (mon->ticker_mode == TM_SECOND ? "TS" : (mon->ticker_mode == TM_FAST ? "TF" : "TZ")));
     
diff --git a/monitor/mon.c b/monitor/mon.c
index 4a51f6e..22197e9 100644
--- a/monitor/mon.c
+++ b/monitor/mon.c
@@ -4,6 +4,8 @@
 
 #include "mon.h"
 
+#define ABS(x) ((x)>0?(x):-(x))
+
 /*
     Monitor commands :
     (empty cmd)         Send input & update output
@@ -51,10 +53,13 @@ int mon_read_prologue(t_mon *mon) {
     }
 
     mon->step = 0;
-    mon->freq = 1;
-    mon->max_freq = 1000;
     mon->status = MS_RUN;
 
+    mon->target_freq = 1;
+    mon->max_freq = 10;
+    mon->actual_freq = 0;
+    mon->calc_time_usec = 10;
+
     mon->clk = time(NULL);
     mon->ticker_input = -1;
     mon->ticker_mode = TM_SECOND;
@@ -81,23 +86,33 @@ void mon_loop(t_mon *mon) {
             mon_step(mon);
             steps++;
             if (time(NULL) != prev_time) {
-                mon->max_freq = steps;
+                mon->actual_freq = steps;
+                if (mon->actual_freq > mon->max_freq)
+                    mon->max_freq = mon->actual_freq;
                 steps = 0;
                 prev_time = time(NULL);
             }
         } else if (mon->status == MS_FREQ) {
-            if (steps < mon->freq) {
-                mon_step(mon);
-                steps++;
-                usleep(1000000 / mon->freq - 1000000 / mon->max_freq);
-            } else {
-                if (prev_time != time(NULL)) {
-                    steps = 0;
-                    prev_time = time(NULL);
-                } else {
-                    usleep(10000);
+            if (prev_time != time(NULL)) {
+                mon->actual_freq = steps;
+                if (mon->actual_freq > mon->max_freq)
+                    mon->max_freq = mon->actual_freq;
+                steps = 0;
+                prev_time = time(NULL);
+
+                if (mon->target_freq != mon->actual_freq && mon->actual_freq != 0) {
+                    if (mon->actual_freq * 100 > 130 * mon->target_freq)
+                        mon->calc_time_usec = 1000000 / mon->max_freq;
+                    if (mon->actual_freq * 100 > mon->target_freq * 105)
+                        mon->calc_time_usec -= 10;
+                    if (mon->actual_freq < mon->target_freq)
+                        mon->calc_time_usec += 10;
                 }
             }
+            mon_step(mon);
+            steps++;
+            int sleep = 1000000 / mon->target_freq - mon->calc_time_usec;
+            if (sleep > 0) usleep(sleep);
         } else {
             usleep(10000);
         }
@@ -115,9 +130,11 @@ void mon_handle_command(t_mon *mon, const char *c) {
         mon->status = MS_RUN;
     } else if (c[0] == 'f') {
         const char *p = c + 1;
-        mon->freq = 0;
+        mon->target_freq = 0;
         while (isspace(*p)) p++;
-        while (isdigit(*p)) mon->freq = 10 * mon->freq + (*(p++) - '0');
+        while (isdigit(*p)) mon->target_freq = 10 * mon->target_freq + (*(p++) - '0');
+        if (mon->target_freq == 0) mon->target_freq = 1000;
+        mon->calc_time_usec = 1000;
         mon->status = MS_FREQ;
     } else if (c[0] == 'i') {
         const char *p = c + 1;
diff --git a/monitor/mon.h b/monitor/mon.h
index 7abcf8c..0040eda 100644
--- a/monitor/mon.h
+++ b/monitor/mon.h
@@ -40,8 +40,11 @@ typedef struct {
     t_output *outputs;
 
     int step;
-    int freq;
+
     int max_freq;
+    int target_freq;
+    int actual_freq;
+    int calc_time_usec;
 
     t_status status;