limit table keys to i32; add table linear interpolation

7 files changed, 31 insertions, 202 deletions

diff --git a/fw/main.c b/fw/main.c
index 0fb50ac..d04b6d0 100644
--- a/fw/main.c
+++ b/fw/main.c
@@ -312,7 +312,7 @@ handleSigCtrlFrame(const CanFrame *frame) {
 // Generate the output signal being sent to one of the gauges.
 // Raw is the raw signal value extracted from a CAN frame.
 static Status
-driveGauge(Signal sig, Number raw) {
+driveGauge(Signal sig, I32 raw) {
 	Status status;
 	U16 val;
 
@@ -330,11 +330,11 @@ driveGauge(Signal sig, Number raw) {
 	CanFrame frame;
 	frame.id = (CanId){.isExt=false, .sid=0x123};
 	frame.rtr = false;
-	frame.dlc = 5u;
+	frame.dlc = 7u;
 	frame.data[0u] = sig & 0xFF; // signal
-	frame.data[1u] = raw.type & 0xFF; // key type
-	frame.data[2u] = raw.u8; // key (U8)
-	serU16Be(frame.data+3, val); // val
+	U32 uraw = *(U32 *)&raw;
+	serU32Be(frame.data+1, uraw); // key
+	serU16Be(frame.data+5, val); // val
 	canTx(&frame);
 
 	switch (sig) {
@@ -368,7 +368,7 @@ static Status
 handleSigFrame(const CanFrame *frame) {
 	Status status, result;
 	Signal sig;
-	Number raw;
+	I32 raw;
 
 	result = OK;
 
diff --git a/fw/signal.c b/fw/signal.c
index d724091..0edc66f 100644
--- a/fw/signal.c
+++ b/fw/signal.c
@@ -54,7 +54,9 @@ pluckBE(const SigFmt *sig, const CanFrame *frame, U32 *raw) {
 }
 
 Status
-sigPluck(const SigFmt *sig, const CanFrame *frame, Number *raw) {
+sigPluck(const SigFmt *sig, const CanFrame *frame, I32 *raw) {
+	U32 uraw;
+
 	// Preconditions
 	if (
 		(sig->start >= (8u * frame->dlc)) // signal starts outside DATA FIELD
@@ -64,45 +66,16 @@ sigPluck(const SigFmt *sig, const CanFrame *frame, Number *raw) {
 		return FAIL;
 	}
 
-	// Read signal into U32
 	switch (sig->order) {
 	case LITTLE_ENDIAN:
-		pluckLE(sig, frame, &raw->u32);
+		pluckLE(sig, frame, &uraw);
 		break;
 	case BIG_ENDIAN:
-		pluckBE(sig, frame, &raw->u32);
+		pluckBE(sig, frame, &uraw);
 		break;
 	default:
 		return FAIL; // invalid byte order
 	}
-
-	// Shrink signal to appropriate size and set +/- sign
-	if (sig->size <= 8u) {
-		raw->u8 = raw->u32 & 0xFF;
-		if (sig->isSigned) {
-			raw->type = NUM_I8;
-			raw->i8 = *(I8 *)&raw->u8;
-		} else {
-			raw->type = NUM_U8;
-		}
-	} else if (sig->size <= 16u) {
-		raw->u16 = raw->u32 & 0xFFFF;
-		if (sig->isSigned) {
-			raw->type = NUM_I16;
-			raw->i16 = *(I16 *)&raw->u16;
-		} else {
-			raw->type = NUM_U16;
-		}
-	} else if (sig->size <= 32u) {
-		if (sig->isSigned) {
-			raw->type = NUM_I32;
-			raw->i32 = *(I32 *)&raw->u32;
-		} else {
-			raw->type = NUM_U32;
-		}
-	} else {
-		return FAIL; // signal too big
-	}
-
+	*raw = *(I32 *)&uraw;
 	return OK;
 }
diff --git a/fw/signal.h b/fw/signal.h
index 6489c6f..ee06960 100644
--- a/fw/signal.h
+++ b/fw/signal.h
@@ -30,4 +30,4 @@ typedef struct {
 
 // Extract the raw signal value out of a CAN frame's DATA FIELD.
 // Assumes the frame's ID matches that of the signal.
-Status sigPluck(const SigFmt *sig, const CanFrame *frame, Number *raw);
+Status sigPluck(const SigFmt *sig, const CanFrame *frame, I32 *raw);
diff --git a/fw/table.c b/fw/table.c
index 0e65b83..1cbf199 100644
--- a/fw/table.c
+++ b/fw/table.c
@@ -41,45 +41,40 @@ tabRead(const Table *tab, U8 k, U32 *key, U16 *val) {
 	return status;
 }
 
+// Linear interpolation.
+static U16
+interp(I32 x, I32 x1, U16 y1, I32 x2, U16 y2) {
+	return (U16)(y1 + ((I32)y2-y1) * (x-x1) / (x2-x1));
+}
+
 Status
-tabLookup(const Table *tab, Number key, U16 *val) {
+tabLookup(const Table *tab, I32 key, U16 *val) {
 	U8 row;
-	Number tkey1, tkey2;
+	U32 utkey;
+	I32 tkey1, tkey2;
 	U16 tval1, tval2;
 	Status status;
-	int ord;
 
 	// Search for key
-	for (row = 0u; row < (TAB_ROWS-1u); row++) {
-		status = tabRead(tab, row, &tkey1.u32, &tval1);
+	for (row = 0u; row < TAB_ROWS; row++) {
+		status = tabRead(tab, row, &utkey, &tval1);
 		if (status != OK) {
 			return FAIL;
 		}
+		tkey1 = *(I32 *)&utkey;
 
-		status = u32ToNum(tkey1.u32, key.type, &tkey1);
-		if (status != OK) {
-			return FAIL;
-		}
-
-		status = cmpNum(key, tkey1, &ord);
-		if (status != OK) {
-			return FAIL;
-		}
-		if (ord == 0) { // found exact key
+		if (key == tkey1) { // found exact key
 			*val = tval1; 
 			return OK;
-		} else if (ord < 0) { // key < tkey
+		} else if (key < tkey1) {
 			if (row > 0u) { // not first row
 				// Interpolate with previous row
-				status = tabRead(tab, row-1u, &tkey2.u32, &tval2);
-				if (status != OK) {
-					return FAIL;
-				}
-				status = u32ToNum(tkey2.u32, key.type, &tkey2);
+				status = tabRead(tab, row-1u, &utkey, &tval2);
 				if (status != OK) {
 					return FAIL;
 				}
-				*val = (tval1 + tval2) / 2u; // TODO: linear interpolation
+				tkey2 = *(I32 *)&utkey;
+				*val = interp(key, tkey1, tval1, tkey2, tval2);
 			} else { // key < key of first row
 				*val = tval1; // use first row value
 			}
@@ -91,6 +86,5 @@ tabLookup(const Table *tab, Number key, U16 *val) {
 
 	// Reached last row
 	*val = tval1; // last value in table
-
 	return OK;
 }
diff --git a/fw/table.h b/fw/table.h
index c1cc03d..3851f9a 100644
--- a/fw/table.h
+++ b/fw/table.h
@@ -47,4 +47,4 @@ Status tabRead(const Table *tab, U8 k, U32 *key, U16 *val);
 // Lookup the value associated with given key.
 // If key falls between two rows, the value is interpolated
 // from the two adjacent.
-Status tabLookup(const Table *tab, Number key, U16 *val);
+Status tabLookup(const Table *tab, I32 key, U16 *val);
diff --git a/fw/types.c b/fw/types.c
deleted file mode 100644
index 24a7c3a..0000000
--- a/fw/types.c
+++ /dev/null
@@ -1,108 +0,0 @@
-#include <stdint.h>
-
-#include "types.h"
-
-#define cmp(a, b) (((a) > (b)) ? 1 : (((a) < (b)) ? -1 : 0))
-
-Status
-u32ToNum(U32 x, NumType t, Number *n) {
-	n->type = t;
-
-	switch (t) {
-	case NUM_U8:
-		n->u8 = x & 0xFF;
-		return OK;
-	case NUM_U16:
-		n->u16 = x & 0xFFFF;
-		return OK;
-	case NUM_U32:
-		n->u32 = x;
-		return OK;
-	case NUM_I8:
-		n->u8 = x & 0xFF;
-		n->i8 = *(I8 *)&n->u8;
-		return OK;
-	case NUM_I16:
-		n->u16 = x & 0xFFFF;
-		n->i16 = *(I16 *)&n->u16;
-		return OK;
-	case NUM_I32:
-		n->i32 = *(I32 *)&x;
-		return OK;
-	default:
-		return FAIL;
-	}
-}
-
-static Status
-cmpU32Num(U32 a, Number b, int *ord) {
-	switch (b.type) {
-	case NUM_U8:
-		*ord = cmp(a, b.u8);
-		return OK;
-	case NUM_U16:
-		*ord = cmp(a, b.u16);
-		return OK;
-	case NUM_U32:
-		*ord = cmp(a, b.u32);
-		return OK;
-	case NUM_I8:
-		*ord = cmp(a, b.i8);
-		return OK;
-	case NUM_I16:
-		*ord = cmp(a, b.i16);
-		return OK;
-	case NUM_I32:
-		*ord = cmp(a, b.i32);
-		return OK;
-	default:
-		return FAIL;
-	}
-}
-
-Status
-cmpNum(Number a, Number b, int *ord) {
-	switch (a.type) {
-	case NUM_U8:
-		a.i32 = a.u8;
-		break;
-	case NUM_U16:
-		a.i32 = a.u16;
-		break;
-	case NUM_U32:
-		return cmpU32Num(a.u32, b, ord);
-	case NUM_I8:
-		a.i32 = a.i8;
-		break;
-	case NUM_I16:
-		a.i32 = a.i16;
-		break;
-	case NUM_I32:
-		break; // do nothing
-	default:
-		return FAIL;
-	}
-
-	switch (b.type) {
-	case NUM_U8:
-		*ord = cmp(a.i32, b.u8);
-		return OK;
-	case NUM_U16:
-		*ord = cmp(a.i32, b.u16);
-		return OK;
-	case NUM_U32:
-		*ord = cmp(a.i32, b.u32);
-		return OK;
-	case NUM_I8:
-		*ord = cmp(a.i32, b.i8);
-		return OK;
-	case NUM_I16:
-		*ord = cmp(a.i32, b.i16);
-		return OK;
-	case NUM_I32:
-		*ord = cmp(a.i32, b.i32);
-		return OK;
-	default:
-		return FAIL;
-	}
-}
diff --git a/fw/types.h b/fw/types.h
index f772878..502adb2 100644
--- a/fw/types.h
+++ b/fw/types.h
@@ -18,33 +18,3 @@ typedef uint32_t U32;
 typedef int8_t I8;
 typedef int16_t I16;
 typedef int32_t I32;
-
-typedef enum {
-	NUM_U8,
-	NUM_U16,
-	NUM_U32,
-	NUM_I8,
-	NUM_I16,
-	NUM_I32,
-} NumType;
-
-// Number
-typedef struct {
-	NumType type;
-	union {
-		U8 u8;
-		U16 u16;
-		U32 u32;
-		I8 i8;
-		I16 i16;
-		I32 i32;
-	};
-} Number;
-
-Status u32ToNum(U32 x, NumType t, Number *n);
-
-// Compare two numbers.
-// -1 if a < b
-// 0 if a == b
-// 1 if a > b
-Status cmpNum(Number a, Number b, int *ord);