lookup output value in table

5 files changed, 209 insertions, 39 deletions

diff --git a/fw/main.c b/fw/main.c
index 5425321..06451ef 100644
--- a/fw/main.c
+++ b/fw/main.c
@@ -60,12 +60,14 @@ static const CanId rxb1Mask = {
 };
 
 // Calibration tables in EEPROM
-static const Table tachTbl = {0ul*TAB_SIZE}; // tachometer
-static const Table speedTbl = {1ul*TAB_SIZE}; // speedometer
-static const Table an1Tbl = {2ul*TAB_SIZE}; // analog channels...
-static const Table an2Tbl = {3ul*TAB_SIZE};
-static const Table an3Tbl = {4ul*TAB_SIZE};
-static const Table an4Tbl = {5ul*TAB_SIZE};
+static const Table tbls[NSIG] = {
+	[SIG_TACH] = {0ul*TAB_SIZE}, // tachometer
+	[SIG_SPEED] = {1ul*TAB_SIZE}, // speedometer
+	[SIG_AN1] = {2ul*TAB_SIZE}, // analog channels...
+	[SIG_AN2] = {3ul*TAB_SIZE},
+	[SIG_AN3] = {4ul*TAB_SIZE},
+	[SIG_AN4] = {5ul*TAB_SIZE},
+};
 
 // EEPROM address of encoding format structure for each signal.
 // Each of these addresses point to a SigFmt structure in the EEPROM.
@@ -199,8 +201,45 @@ handleIdCtrlFrame(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) {
+	Status status;
+	U16 val;
+
+	if (sig >= NSIG) {
+		return FAIL;
+	}
+
+	// Lookup gauge waveform value in EEPROM table
+	status = tabLookup(&tbls[sig], raw, &val);
+	if (status != OK) {
+		return FAIL;
+	}
+
+	switch (sig) {
+	case SIG_TACH:
+		// TODO
+		break;
+	case SIG_SPEED:
+		// TODO
+		break;
+	case SIG_AN1:
+		dacSet1a(val);
+		break;
+	case SIG_AN2:
+		dacSet1b(val);
+		break;
+	case SIG_AN3:
+		dacSet2a(val);
+		break;
+	case SIG_AN4:
+		dacSet2b(val);
+		break;
+	default:
+		return FAIL; // invalid signal
+	}
+
 	// TODO
 }
 
diff --git a/fw/table.c b/fw/table.c
index 41c1bc4..47a3ab9 100644
--- a/fw/table.c
+++ b/fw/table.c
@@ -8,22 +8,22 @@
 #include "table.h"
 
 Status
-tabWrite(const Table *tab, U8 k, U16 key, U16 val) {
+tabWrite(const Table *tab, U8 k, U32 key, U16 val) {
 	if (k >= TAB_ROWS) {
 		return FAIL;
 	}
 
 	U16 addr = tab->offset + k*TAB_ROW_SIZE;
-	U8 row[4u] = {
-		(key>>0u)&0xFF, (key>>8u)&0xFF,
-		(val>>0u)&0xFF, (val>>8u)&0xFF};
+	U8 row[sizeof(key) + sizeof(val)] = {
+		(key>>0u)&0xFF, (key>>8u)&0xFF, (key>>16u)&0xFF, (key>>24u)&0xFF, // LE
+		(val>>0u)&0xFF, (val>>8u)&0xFF}; // LE
 	return eepromWrite(addr, row, sizeof(row));
 }
 
 Status
-tabRead(const Table *tab, U8 k, U16 *key, U16 *val) {
+tabRead(const Table *tab, U8 k, U32 *key, U16 *val) {
 	U16 addr;
-	U8 row[4u];
+	U8 row[sizeof(*key) + sizeof(*val)];
 	Status status;
 
 	if (k >= TAB_ROWS) {
@@ -32,28 +32,40 @@ tabRead(const Table *tab, U8 k, U16 *key, U16 *val) {
 
 	addr = tab->offset + k*TAB_ROW_SIZE;
 	status = eepromRead(addr, row, sizeof(row));
-	*key = ((U16)row[0u]<<0u) | ((U16)row[1u]<<8u);
-	*val = ((U16)row[2u]<<0u) | ((U16)row[3u]<<8u);
+	*key = ((U32)row[0u]<<0u) | ((U32)row[1u]<<8u) | ((U32)row[2u]<<16u) | ((U32)row[3u]<<24u); // LE
+	*val = ((U16)row[4u]<<0u) | ((U16)row[5u]<<8u); // LE
 	return status;
 }
 
 Status
-tabLookup(const Table *tab, U16 key, U16 *val) {
+tabLookup(const Table *tab, Number key, U16 *val) {
 	U8 k;
-	U16 tkey, tval1, tval2;
+	Number tkey;
+	U16 tval1, tval2;
 	Status status;
+	int ord;
 
 	// Search for key
 	for (k = 0u; (k < TAB_ROWS-1u); k++) {
-		status = tabRead(tab, k, &tkey, &tval1);
+		status = tabRead(tab, k, &tkey.u32, &tval1);
 		if (status != OK) {
 			return FAIL;
 		}
-		if (key == tkey) { // found exact key
+
+		status = u32ToNum(tkey.u32, key.type, &tkey);
+		if (status != OK) {
+			return FAIL;
+		}
+
+		status = cmpNum(key, tkey, &ord);
+		if (status != OK) {
+			return FAIL;
+		}
+		if (ord == 0) { // found exact key
 			*val = tval1; 
 			return OK;
-		} else if (key > tkey) { // interpolate
-			status = tabRead(tab, k+1u, &tkey, &tval2);
+		} else if (ord > 0) { // interpolate
+			status = tabRead(tab, k+1u, &tkey.u32, &tval2);
 			if (status != OK) {
 				return FAIL;
 			}
diff --git a/fw/table.h b/fw/table.h
index cba263d..a5cfb7b 100644
--- a/fw/table.h
+++ b/fw/table.h
@@ -22,7 +22,7 @@
 
 enum {
 	TAB_KEY_SIZE = sizeof(U32),
-	TAB_VAL_SIZE = sizeof(U32),
+	TAB_VAL_SIZE = sizeof(U16),
 	TAB_ROWS = 32,
 	TAB_ROW_SIZE = TAB_KEY_SIZE + TAB_VAL_SIZE,
 	TAB_SIZE = TAB_ROWS * TAB_ROW_SIZE,
@@ -33,12 +33,12 @@ typedef struct {
 } Table;
 
 // Set the key and value of row k.
-Status tabWrite(const Table *tab, U8 k, U16 key, U16 val);
+Status tabWrite(const Table *tab, U8 k, U32 key, U16 val);
 
 // Read row k.
-Status tabRead(const Table *tab, U8 k, U16 *key, U16 *val);
+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, U16 key, U16 *val);
+Status tabLookup(const Table *tab, Number key, U16 *val);
diff --git a/fw/types.c b/fw/types.c
new file mode 100644
index 0000000..24a7c3a
--- /dev/null
+++ b/fw/types.c
@@ -0,0 +1,108 @@
+#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 55a17f6..f772878 100644
--- a/fw/types.h
+++ b/fw/types.h
@@ -15,25 +15,36 @@ typedef enum {
 typedef uint8_t U8;
 typedef uint16_t U16;
 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 {
-	enum {
-		NUM_U8,
-		NUM_U16,
-		NUM_U32,
-		NUM_I8,
-		NUM_I16,
-		NUM_I32,
-	} type;
-	U8 u8;
-	U16 u16;
-	U32 u32;
-	I8 i8;
-	I16 i16;
-	I32 i32;
+	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);