1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
|
#include <stdbool.h>
#include <stdint.h>
#include "types.h"
#include "can.h"
#include "eeprom.h"
#include "signal.h"
#include "serial.h"
#include "table.h"
Status
tabWrite(const Table *tab, U8 k, U32 key, U16 val) {
U16 addr;
U8 row[sizeof(key) + sizeof(val)];
if (k >= TAB_ROWS) {
return FAIL;
}
addr = tab->offset + k*TAB_ROW_SIZE;
serU32Be(row, key);
serU16Be(row+sizeof(key), val);
return eepromWrite(addr, row, sizeof(row));
}
Status
tabRead(const Table *tab, U8 k, U32 *key, U16 *val) {
U16 addr;
U8 row[sizeof(*key) + sizeof(*val)];
Status status;
if (k >= TAB_ROWS) {
return FAIL;
}
addr = tab->offset + k*TAB_ROW_SIZE;
status = eepromRead(addr, row, sizeof(row));
*key = deserU32Be(row);
*val = deserU16Be(row+sizeof(U32));
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, I32 key, U16 *val) {
U8 row;
U32 utkey;
I32 tkey1, tkey2;
U16 tval1, tval2;
Status status;
// Search for key
for (row = 0u; row < TAB_ROWS; row++) {
status = tabRead(tab, row, &utkey, &tval1);
if (status != OK) {
return FAIL;
}
tkey1 = *(I32 *)&utkey;
if (key == tkey1) { // found exact key
*val = tval1;
return OK;
} else if (key < tkey1) {
if (row > 0u) { // not first row
// Interpolate with previous row
status = tabRead(tab, row-1u, &utkey, &tval2);
if (status != OK) {
return FAIL;
}
tkey2 = *(I32 *)&utkey;
*val = interp(key, tkey1, tval1, tkey2, tval2);
} else { // key < key of first row
*val = tval1; // use first row value
}
return OK;
} else { // key > tkey
// continue until key <= tkey
}
}
// Reached last row
*val = tval1; // last value in table
return OK;
}
|