tempcomp: allow configuration with list of points

In addition to the quadratic function, allow configuration of the
compensation with a file containing list of (temperature, compensation)
points used for linear interpolation and extrapolation.

conf.c

@@ -185,7 +185,8 @@ static IPAddr bind_cmd_address4, bind_cmd_address6;
 static char *pidfile;
 
 /* Temperature sensor, update interval and compensation coefficients */
-static char *tempcomp_file = NULL;
+static char *tempcomp_sensor_file = NULL;
+static char *tempcomp_point_file = NULL;
 static double tempcomp_interval;
 static double tempcomp_T0, tempcomp_k0, tempcomp_k1, tempcomp_k2;
 
@@ -259,18 +260,31 @@ other_parse_error(const char *message)
 
 /* ================================================== */
 
-static void
+static int
-check_number_of_args(char *line, int num)
+get_number_of_args(char *line)
 {
+  int num = 0;
+
   /* The line is normalized, between arguments is just one space */
   if (*line == ' ')
     line++;
   if (*line)
-    num--;
+    num++;
   for (; *line; line++) {
     if (*line == ' ')
-      num--;
+      num++;
   }
+
+  return num;
+}
+
+/* ================================================== */
+
+static void
+check_number_of_args(char *line, int num)
+{
+  num -= get_number_of_args(line);
+
   if (num) {
     LOG_FATAL(LOGF_Configure, "%s arguments for %s directive at line %d%s%s",
         num > 0 ? "Missing" : "Too many",
@@ -330,7 +344,8 @@ CNF_Finalise(void)
   Free(rtc_file);
   Free(user);
   Free(mail_user_on_change);
-  Free(tempcomp_file);
+  Free(tempcomp_sensor_file);
+  Free(tempcomp_point_file);
 }
 
 /* ================================================== */
@@ -1156,8 +1171,13 @@ static void
 parse_tempcomp(char *line)
 {
   char *p;
+  int point_form;
 
+  point_form = get_number_of_args(line) == 3;
+
+  if (!point_form)
     check_number_of_args(line, 6);
+
   p = line;
   line = CPS_SplitWord(line);
 
@@ -1166,13 +1186,25 @@ parse_tempcomp(char *line)
     return;
   }
 
-  if (sscanf(line, "%lf %lf %lf %lf %lf", &tempcomp_interval, &tempcomp_T0, &tempcomp_k0, &tempcomp_k1, &tempcomp_k2) != 5) {
+  Free(tempcomp_point_file);
+
+  if (point_form) {
+    if (sscanf(line, "%lf", &tempcomp_interval) != 1) {
       command_parse_error();
       return;
     }
+    tempcomp_point_file = Strdup(CPS_SplitWord(line));
+  } else {
+    if (sscanf(line, "%lf %lf %lf %lf %lf", &tempcomp_interval,
+               &tempcomp_T0, &tempcomp_k0, &tempcomp_k1, &tempcomp_k2) != 5) {
+      command_parse_error();
+      return;
+    }
+    tempcomp_point_file = NULL;
+  }
 
-  Free(tempcomp_file);
+  Free(tempcomp_sensor_file);
-  tempcomp_file = Strdup(p);
+  tempcomp_sensor_file = Strdup(p);
 }
 
 /* ================================================== */
@@ -1683,9 +1715,10 @@ CNF_GetLockMemory(void)
 /* ================================================== */
 
 void
-CNF_GetTempComp(char **file, double *interval, double *T0, double *k0, double *k1, double *k2)
+CNF_GetTempComp(char **file, double *interval, char **point_file, double *T0, double *k0, double *k1, double *k2)
 {
-  *file = tempcomp_file;
+  *file = tempcomp_sensor_file;
+  *point_file = tempcomp_point_file;
   *interval = tempcomp_interval;
   *T0 = tempcomp_T0;
   *k0 = tempcomp_k0;

conf.h

@@ -94,7 +94,7 @@ extern void CNF_SetupAccessRestrictions(void);
 extern int CNF_GetSchedPriority(void);
 extern int CNF_GetLockMemory(void);
 
-extern void CNF_GetTempComp(char **file, double *interval, double *T0, double *k0, double *k1, double *k2);
+extern void CNF_GetTempComp(char **file, double *interval, char **point_file, double *T0, double *k0, double *k1, double *k2);
 
 extern char *CNF_GetUser(void);
 

tempcomp.c

@@ -27,6 +27,7 @@
 
 #include "config.h"
 
+#include "array.h"
 #include "conf.h"
 #include "local.h"
 #include "memory.h"
@@ -46,6 +47,37 @@ static char *filename;
 static double update_interval;
 static double T0, k0, k1, k2;
 
+struct Point {
+  double temp;
+  double comp;
+};
+
+static ARR_Instance points;
+
+static double
+get_tempcomp(double temp)
+{
+  unsigned int i;
+  struct Point *p1 = NULL, *p2 = NULL;
+
+  /* If not configured with points, calculate the compensation from the
+     specified quadratic function */
+  if (!points)
+    return k0 + (temp - T0) * k1 + (temp - T0) * (temp - T0) * k2;
+
+  /* Otherwise interpolate/extrapolate between two nearest points */
+
+  for (i = 1; i < ARR_GetSize(points); i++) {
+    p2 = (struct Point *)ARR_GetElement(points, i);
+    if (p2->temp >= temp)
+      break;
+  }
+  p1 = p2 - 1;
+
+  return (temp - p1->temp) / (p2->temp - p1->temp) *
+         (p2->comp - p1->comp) + p1->comp;
+}
+
 static void
 read_timeout(void *arg)
 {
@@ -55,11 +87,13 @@ read_timeout(void *arg)
   f = fopen(filename, "r");
 
   if (f && fscanf(f, "%lf", &temp) == 1) {
-    comp = k0 + (temp - T0) * k1 + (temp - T0) * (temp - T0) * k2;
+    comp = get_tempcomp(temp);
 
     if (fabs(comp) <= MAX_COMP) {
       comp = LCL_SetTempComp(comp);
 
+      DEBUG_LOG(LOGF_TempComp, "tempcomp updated to %f for %f", comp, temp);
+
       if (logfileid != -1) {
         struct timeval now;
 
@@ -83,10 +117,41 @@ read_timeout(void *arg)
   timeout_id = SCH_AddTimeoutByDelay(update_interval, read_timeout, NULL);
 }
 
+static void
+read_points(const char *filename)
+{
+  FILE *f;
+  char line[256];
+  struct Point *p;
+
+  f = fopen(filename, "r");
+  if (!f) {
+    LOG_FATAL(LOGF_TempComp, "Could not open tempcomp point file %s", filename);
+    return;
+  }
+
+  points = ARR_CreateInstance(sizeof (struct Point));
+
+  while (fgets(line, sizeof (line), f)) {
+    p = (struct Point *)ARR_GetNewElement(points);
+    if (sscanf(line, "%lf %lf", &p->temp, &p->comp) != 2) {
+      LOG_FATAL(LOGF_Configure, "Could not read tempcomp point from %s", filename);
+      break;
+    }
+  }
+
+  fclose(f);
+
+  if (ARR_GetSize(points) < 2)
+    LOG_FATAL(LOGF_Configure, "Not enough points in %s", filename);
+}
+
 void
 TMC_Initialise(void)
 {
-  CNF_GetTempComp(&filename, &update_interval, &T0, &k0, &k1, &k2);
+  char *point_file;
+
+  CNF_GetTempComp(&filename, &update_interval, &point_file, &T0, &k0, &k1, &k2);
 
   if (filename == NULL)
     return;
@@ -94,6 +159,9 @@ TMC_Initialise(void)
   if (update_interval <= 0.0)
     update_interval = 1.0;
 
+  if (point_file)
+    read_points(point_file);
+
   logfileid = CNF_GetLogTempComp() ? LOG_FileOpen("tempcomp",
       "   Date (UTC) Time        Temp.       Comp.")
     : -1;
@@ -107,5 +175,8 @@ TMC_Finalise(void)
   if (filename == NULL)
     return;
 
+  if (points)
+    ARR_DestroyInstance(points);
+
   SCH_RemoveTimeout(timeout_id);
 }