Negative TachoMotor position sensor bugfix

[tuhe]

Hi!

I think this is a super cool project and I am really happy to see you are working on the new Control Plus hub :-). I noticed a small bug in the sensor readings. If I attach a CPlusMotor using

@attach(CPlusLargeMotor, name='stearing', port=2, capabilities=['sense_pos'])

and then steer it towards negative positions, sense_pos will return very large numbers. It seems negative numbers are encoded in the format max_value-abs(value), and the following (really dumb) fix to peripheral.py seems to solve the issue:

   async def update_value(self, msg_bytes):
        msg = bytearray(msg_bytes)
        if len(self.capabilities)==0:
            self.value = msg
        if len(self.capabilities)==1:
            capability = self.capabilities[0]
            datasets, bytes_per_dataset = self.datasets[capability][0:2]
            for i in range(datasets):
                msg_ptr = i*bytes_per_dataset
                val = self._convert_bytes(msg[msg_ptr: msg_ptr+bytes_per_dataset], bytes_per_dataset)

                # my fix
                if msg_bytes[-1] == 255:
                    b2 = [255 - b for b in msg_bytes]
                    msg2 = bytearray(b2)
                    val2 = self._convert_bytes(msg2[msg_ptr: msg_ptr + bytes_per_dataset], bytes_per_dataset)
                    val = -val2

                if datasets==1:
                    self.value[capability] = val
                else:
                    self.value[capability][i] = val

                if val > 10000:
                    raise Exception("bad position")

        if len(self.capabilities) > 1:
            await self._parse_combined_sensor_values(msg)

although the proper way is obviously to change _convert_bytes.

On a related note, I have model 42099 (which I guess from the examples you also got?) and I wonder if you have some input on the following: I have written a small control program for the 42099 model which takes keyboard inputs and try to accelerate and turn the front wheels using .set_pos(...). I change the position in small increments (+/- 15 degrees) based on keyboard inputs. Sometimes, it seems the motor will not fully perform the command, and instead freeze and make a high-pitched whining sound for about 0.5-1 seconds (and in this mode subsequent commands are paused). I have tried to change the max_power to 100 but this does not seem to fully solve the issue. I have also tried mucking around with the other input parameters to set_pos, including the speed, but to no avail (I confess I do not know what all of them really do).

This behavior is not one I have observed with the lego smartphone app, which seems to respond much more "crisply" to inputs. So my question is fairly high-level: If I wanted to write a control app to control the wheels, and avoid the above behavior (or more specifically, imitate the lego smartphone app), do you know if I should still use .set_pos and is there something obvious I am missing? I am attaching the code just in case it has any interest to anyone.

lego_device.txt

[positron96]

Hi, I've noticed that same bug as well, my fix is to do a two-s-complement conversion from 32bit unsigned to 32bit signed int with something like:

if v & (1<<31) != 0:
            v = v - (1<<32)

I've put it into _change handler, but it should better be put in decoding code.

[dlech]

Why not just use the struct module for unpacking binary data?

import struct

struct.unpack_from('<i', buffer, offset)  # little-endian signed 32-bit integer
struct.unpack_from('<I', buffer, offset)  # little-endian unsigned 32-bit integer

[janvrany]

I've got this problem also with motor speed reading when reversing.

Indeed we need to know whether the data is signed on unsigned int and interpret it accordingly. When in update_value, how we can tell the whether it is signed or unsigned is not (yet) clear to me, though.

[dlech]

As far as I know, sensor values should always be treated as signed.

https://lego.github.io/lego-ble-wireless-protocol-docs/index.html#value-format

This doesn't say it explicitly, but I know from extensive experience with the EV3 source code (EV3 uses the same UART protocol for I/O devices) that these are all little-endian signed values.

[janvrany]

This does the trick for me:

diff --git a/bricknil/sensor/peripheral.py b/bricknil/sensor/peripheral.py
index bf9f258..d3dfdf1 100644
--- a/bricknil/sensor/peripheral.py
+++ b/bricknil/sensor/peripheral.py
@@ -130,12 +130,12 @@ class Peripheral(Process):
                 If multiple values, then a list of those values
                 Value can be either uint8, uint16, or uint32 depending on value of `byte_count`
         """
-        if byte_count == 1:   # just a uint8
-            val = msg_bytes[0]
-        elif byte_count == 2: # uint16 little-endian
-            val = struct.unpack('<H', msg_bytes)[0]
-        elif byte_count == 4: # uint32 little-endian
-            val = struct.unpack('<I', msg_bytes)[0]
+        if byte_count == 1:   # just an int8
+            val = struct.unpack('<b', msg_bytes)[0]
+        elif byte_count == 2: # int16 little-endian
+            val = struct.unpack('<h', msg_bytes)[0]
+        elif byte_count == 4: # int32 little-endian
+            val = struct.unpack('<i', msg_bytes)[0]
         else:
             self.message_error(f'Cannot convert array of {msg_bytes} length {len(msg_bytes)} to python datatype')
             val = None

[dlech]

Cool. You should send a pull request. There should also be a data type received for each mode that could/should be used instead of looking at the byte count. In practice, I don't think any sensors use floating point data values, but in theory, 4 bytes could mean 32-bit integer or 32-bit floating point.

[janvrany]

See PR #10 with the above fix. I did not include support for floating point values - to me it looks the information about the type is nowhere to access when updating the value. Perhaps we need to extend datasets slots to contain 3-element tuple instead of just 2 and type information there...