extract little-endian signals from frames

1 files changed, 18 insertions, 3 deletions

diff --git a/fw/signal.c b/fw/signal.c
index 30916e0..2bbb716 100644
--- a/fw/signal.c
+++ b/fw/signal.c
@@ -11,7 +11,22 @@
 // Assumes signal is within the frame's DATA FIELD.
 Status
 pluckLE(const SigFmt *sig, const CanFrame *frame, U32 *raw) {
-	// TODO
+	U8 i, end, mask, byte;
+
+	*raw = 0ul;
+	end = sig->start + sig->size;
+
+	// First iteration starts at arbitrary bit: namely the (i%8)'th bit.
+	// Subsequent iterations start at bit 0 of each byte.
+
+	for (i = sig->start; i < end; i += 8u-(i%8u)) {
+		mask = 0xFF << (i%8u);
+		if (i/8u == end/8u) { // if end in this byte
+			mask &= 0xFF >> (8u - (end%8u)); // ignore top bits
+		}
+		byte = (frame->data[i/8u] & mask) >> (i%8u);
+		*raw |= (U32)byte << (i - sig->start);
+	}
 }
 
 // Extract a big-endian value from a frame.
@@ -21,7 +36,7 @@ pluckBE(const SigFmt *sig, const CanFrame *frame, U32 *raw) {
 	U8 i, end, mask;
 
 	*raw = 0ul;
-	end = sig->start+sig->size;
+	end = sig->start + sig->size;
 
 	// First iteration starts at arbitrary bit: namely the (i%8)'th bit.
 	// Subsequent iterations start at bit 0 of each byte.
@@ -29,7 +44,7 @@ pluckBE(const SigFmt *sig, const CanFrame *frame, U32 *raw) {
 	for (i = sig->start; i < end; i += 8u-(i%8u)) {
 		mask = 0xFF << (i%8u);
 		if (i/8u == end/8u) { // if end in this byte
-			mask >>= (8u - (end%8u)); // ignore top bits
+			mask &= 0xFF >> (8u - (end%8u)); // ignore top bits
 			*raw <<= (end%8u) - (i%8u); // include bits between i and end
 		} else {
 			*raw <<= 8u - (i%8u); // include bits between i and end of byte