fix table lookup

2 files changed, 33 insertions, 23 deletions

diff --git a/fw/main.c b/fw/main.c
index b4fba7d..0fb50ac 100644
--- a/fw/main.c
+++ b/fw/main.c
@@ -316,16 +316,6 @@ driveGauge(Signal sig, Number raw) {
 	Status status;
 	U16 val;
 
-	// TODO: remove
-	CanFrame frame;
-	frame.id = (CanId){.isExt=false, .sid=0x123};
-	frame.rtr = false;
-	frame.dlc = 3u;
-	frame.data[0u] = sig & 0xFF;
-	frame.data[1u] = raw.type & 0xFF;
-	frame.data[2u] = raw.u8;
-	canTx(&frame);
-
 	if (sig >= NSIG) {
 		return ERR;
 	}
@@ -336,6 +326,17 @@ driveGauge(Signal sig, Number raw) {
 		return ERR;
 	}
 
+	// TODO: remove
+	CanFrame frame;
+	frame.id = (CanId){.isExt=false, .sid=0x123};
+	frame.rtr = false;
+	frame.dlc = 5u;
+	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
+	canTx(&frame);
+
 	switch (sig) {
 	case SIG_TACH:
 		// TODO
diff --git a/fw/table.c b/fw/table.c
index c78d5e8..0e65b83 100644
--- a/fw/table.c
+++ b/fw/table.c
@@ -43,40 +43,49 @@ tabRead(const Table *tab, U8 k, U32 *key, U16 *val) {
 
 Status
 tabLookup(const Table *tab, Number key, U16 *val) {
-	U8 k;
-	Number tkey;
+	U8 row;
+	Number tkey1, tkey2;
 	U16 tval1, tval2;
 	Status status;
 	int ord;
 
 	// Search for key
-	for (k = 0u; (k < TAB_ROWS-1u); k++) {
-		status = tabRead(tab, k, &tkey.u32, &tval1);
+	for (row = 0u; row < (TAB_ROWS-1u); row++) {
+		status = tabRead(tab, row, &tkey1.u32, &tval1);
 		if (status != OK) {
 			return FAIL;
 		}
 
-		status = u32ToNum(tkey.u32, key.type, &tkey);
+		status = u32ToNum(tkey1.u32, key.type, &tkey1);
 		if (status != OK) {
 			return FAIL;
 		}
 
-		status = cmpNum(key, tkey, &ord);
+		status = cmpNum(key, tkey1, &ord);
 		if (status != OK) {
 			return FAIL;
 		}
 		if (ord == 0) { // found exact key
 			*val = tval1; 
 			return OK;
-		} else if (ord > 0) { // interpolate
-			status = tabRead(tab, k+1u, &tkey.u32, &tval2);
-			if (status != OK) {
-				return FAIL;
+		} else if (ord < 0) { // key < tkey
+			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);
+				if (status != OK) {
+					return FAIL;
+				}
+				*val = (tval1 + tval2) / 2u; // TODO: linear interpolation
+			} else { // key < key of first row
+				*val = tval1; // use first row value
 			}
-			*val = (tval1 + tval2) / 2u; 
 			return OK;
-		} else { // less
-			// continue
+		} else { // key > tkey
+			// continue until key <= tkey
 		}
 	}