Create2: Getting strange data reading sensor data

Question

I am new to Create2 and I hope someone can help me.
I have a problem with reading the sensor data or the data I get.
I have setup a nodeJS application running on a Raspberry PI 3B+. Sending data (e.g: drive commands) is no problem - the robot makes what I want.
At program start I send the optcodes 128 (start) and 132 (full mode). After that I send optcode 148 with the packageIds to start streaming. This works also very fine. I have defined the sensors I need in an array:

this.readCommands = [
        {
            packetId: 21, //Charging state
            len: 1,
            name: 'Charging state',
            signed: false,
            value: 0
        },
        {
            packetId: 25, //Battery charge
            len: 2,
            name: 'Battery charge',
            signed: false,
            value: 0
        },
        {
            packetId: 26, //Battery capacity
            len: 2,
            name: 'Battery capacity',
            signed: false,
            value: 0
        },        
        {
            packetId: 28, //Cliff left Signal
            len: 2,
            name: 'Cliff left Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 29, //Cliff front left Signal
            len: 2,
            name: 'Cliff font left Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 30, //Cliff front right Signal
            len: 2,
            name: 'Cliff front right Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 31, //Cliff right Signal
            len: 2,
            name: 'Cliff right Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 46, //Light Bump Left Signal
            len: 2,
            name: 'Light Bump Left Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 47, //Light Bump Front Left Signal
            len: 2,
            name: 'Light Bump Front Left Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 48, //Light Bump Center Left Signal
            len: 2,
            name: 'Light Bump Center Left Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 49, //Light Bump Center Right Signal
            len: 2,
            name: 'Light Bump Center Right Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 50, //Light Bump Front Right Signal
            len: 2,
            name: 'Light Bump Front Right Signal',
            signed: false,
            value: 0
        },
        {
            packetId: 51, //Light Bump Right Signal
            len: 2,
            name: 'Light Bump Right Signal',
            signed: false,
            value: 0
        }
];

To fill the 148-data:

const tempArr = [148, this.readCommands.length];
this.readCommands.forEach(c => tempArr.push(c.packetId));
this.writeToPort(Uint8Array.from(tempArr)).subscribe(() => {
});

When data arrives I collect the data and when I have a full package I try to interpret the data. The packets I get seem to be ok (also the checksum is fine)
Here one as UINT8s:

19, 38, 21, 0, 25, 9, 253, 26, 10, 137, 28, 10, 46, 29, 10, 77, 30, 10, 86, 31, 10, 83, 46, 0, 31, 47, 0, 65, 48, 0, 59, 49, 0, 22, 50, 0, 24, 51, 0, 6, 50

But when I take these values I have data, e.g. for cliff-signals, that are much more greater than 4095 as described in the OI spec.
Also I get a battery capacity of 35082 while battery charge is greater than 60000.
Here's my code for reading the packet:

if (!this._checkData(data))
            return;
let index = 2;
while (index < data.length-1) {
    const id = data.readUInt8(index);
    const cmd = this.readCommands.find(c => c.packetId === id);
    if (!cmd) {
       index++;
       continue;
    }
    switch (cmd.len) {
        case 1:
           cmd.value = cmd.signed? data.readInt8(index +1) : data.readUInt8(index+1);
           index+=2;
           break;
        case 2:
           cmd.value = cmd.signed ? data.readInt16LE(i+1) : data.readUInt16LE(i+1);
           index+=3;
           break;
        }
}

What am I doing wrong?
When I reset the robot I get the message:

Soft reset!
Roomba by iRobot!

stm32 2016-06-13-1127-L

battery-current-zero
 254

b
box vars restored!
languages: english (0)

Is the firmware too old? Can someone help me with that problem?

Daniel

Answer 1

I think you're swapping the bytes in your multi-byte messages. Considering just the battery charge and capacity, you said:

Here one as UINT8s:

19, 38, 21, 0, 25, 9, 253, 26, 10, 137, 28, 10, 46, 29, 10, 77, 30, 10, 86, 31, 10, 83, 46, 0, 31, 47, 0, 65, 48, 0, 59, 49, 0, 22, 50, 0, 24, 51, 0, 6, 50

I get a battery capacity of 35082 while battery charge is greater than 60000

So, looking at battery charge:

25, 9, 253

That's giving you packet ID 25, which is battery charge, and then two bytes, which in hex are:

9 = 0x09
253 = 0xFD

You need to merge the two bytes to get the full value; if you put them together as 0x09FD then you get 2,557. If you swap them, then you get 0xFD09, which is 64,777.

Similarly, with the battery capacity, you have the response

26, 10, 137

which would be the hex values 10 = 0x0A and 137 = 0x89. You can join them as 0x0A89 and get 2,697 or you can swap them and join as 0x890A and get 35,082.

A test like this, where you try interpreting the data both ways, can be an easy way to check if you're doing it correctly - with the bytes swapped, your charge is greater than your capacity, which doesn't make sense. When done correctly, though, it looks like the battery capacity is about 2.7 Ah and the current charge is about 2.6 Ah - looks good!

It took me a while to find which way it should be interpreted in the Open Interface specification (warning: direct download). I didn't see where it said anything about byte order until the "Quick Reference" section at the end of the spec, where it states

Some of the sensor data values are 16 bit values. These values are sent as two bytes, high byte first.

That's telling you that the first byte is the "most significant" byte and the second byte is the "least significant" byte, which is exactly how we're treating the data in the correct interpretation.

Hope this helps!