Re-developing code

Note that the signed values might need to be changed! Currently handling
all numbers as unsigned and this might cause problems. Need to test with
ECU.