Problem in converting the MSPv2 to get flow X, flow Y, and distance from module "optical flow lidar sensor"

[vjkin]

Hi, I currently using the module "optical flow lidar sensor" of MATEK SYS to read the flow X, flow Y and distance from optical flow PMW3901 and VL53L0x.
I used uart to take the information from this module, the data which send to my arduino UNO R3 is in MSPv2.
I stuck in converting the data from those hex code. I have known that the data in byte 10 and 11 are the information of distance by reversing the two byte 10 and 11 (the data of distance is in 14 byte lines). I also shift the byte 10 and 11 which is in 18 byte lines to get flow X.
The problem is that I guess the byte 14 and byte 15 in 18 byte lines are the information and I try converting the these byte by reversing, shift left, shift right, etc, but I can't get the right information about the flow Y. It doesn't true with my measurement (I converted flow X and flow Y into displacement to measure. The unit of displacement is milimeters).
This is my module and my code:

[OptiLida.ino|attachment](upload://qbIasURLPhSnKVtiz69vyntw8Iv.ino) (2.4 KB)

#include <SoftwareSerial.h>

SoftwareSerial mspSerial(5, 6); // RX TX

int16_t dx ;
int16_t dy;
uint16_t distance;
int16_t d;
#define fov_degrees  42 // FOV of the sensor in degrees
#define image_width_pixels  30 // Image width in pixels
float dx_meters, dy_meters;

void setup() {
    mspSerial.begin(115200);
    Serial.begin(115200);
}

void loop() 
{
    readData();
    Serial.println("X: " + String(dx_meters));
    Serial.println(" Y: " + String(dy_meters));
}

void readData() 
{
delay(10);
byte count = 0;
int16_t flowX ;
int16_t flowY ;
while (mspSerial.available()) 
{
  count += 1;
  byte c = mspSerial.read();
  switch (count) {
    case 10:
        distance = c;
        flowX = c;
        break;
    case 11:
        distance <<= 8;
        distance += c;
        distance = (distance & 0xFF00) >> 8 | (distance & 0x00FF) << 8; // Reverse the order of bytes
        flowX <<= 8;
        flowX += c;
        flowX |= (flowX & 0xFF00) >>8; 
        break;
    case 14:
        flowY = c;
        break;                
    case 15:
        flowY <<= 8;
        flowY += c;
        flowY |= (flowY & 0xFF00) >> 8;
        break;
  }
}
if(count==14) d=distance;
if(count==18) {dx=flowX/100; dy=flowY/100;};
if (d > 2000) d = 2000;
pixelToMeter(dx,  dy,  d) ;
}

void pixelToMeter(float dx, float dy, float d) 
{ 
float fov_radians = (fov_degrees) * PI / 180; // Convert FOV to radians
float fov_width_meters = 2 * d*0.01 * tan(fov_radians / 2); // Calculate FOV width in meters
float scale_factor = fov_width_meters / image_width_pixels; // Calculate scale factor

// Convert pixel displacement to meters
dx_meters += dx * scale_factor;
dy_meters += dy * scale_factor;
// Serial.println(dy);
// Serial.println(dx);
}

This is my result:

As you see, the X is right when I move it 8cm. But the Y is not like that when I move 8cm.

This is hex code:

I think there is someting I don't understand clearly about this hex code (or this module). I really need help.
Any help is appreciated! Thanks a lot.

[alto777]

@vjkin

Thanks a lot.

I am not in the lab, but what a cool part. I recognized MATEK SYS and indeed this is meant for quadcopters. I may have to get a copy to play with.

It would be vastly preferred to gather an entire packet without prejudice into a character array of sufficient length.

If you are getting anything plausible, rework your while loop to stash the characters and print the entire packet.

Then it can be processed to derive the data.

But first, the packet. It is not clear how you are determining the beginning and ending, does the MATEK module spit out an entire packet with more longer time between such bursts?

a7

[jremington]

@vjkin

The problem is that I guess the byte 14 and byte 15 in 18 byte lines are the information

You should not be guessing. The correct choice should be explained in the data sheet. Please post a link to the product page or user manual/data sheet for the sensor.

[alto777]

@jremington

should be explained in the data sheet

You'd think.

I found an explicit statement that there is no data sheet as such (!), which further suggested reading the library code for using the device in the context of the MSP protocol.

I gave up at that point. Too much like work.

a7

[jremington]

I see, an OEM part, then.

This new optical tracking sensor from Sparkfun looks quite interesting for ground vehicles, and it does have a data sheet.

[blh64]

The problem is that your X and Y flows are 32 bit numbers, not 16 bit

Digging a little further than @alto777 , it looks like the protocol is like this:

Byte
'$' == Start of Header
'X' == Header X
'<' == Native MSP2 protocol
The next 5 bytes are the header, followed by the data, followed by a checksum

The cmd in the header is 0x1F02 = MSP2_SENSOR_OPTIC_FLOW

and the packet for that command is
8 bit quantity (?)
32 bit X
32 bit Y

You can dig all this out of the link from their website: ardupilot/libraries/AP_MSP/msp.cpp at master · ArduPilot/ardupilot · GitHub

[alto777]

OK, nice. Now we cooking.

Read characters unti the '$', then read and stash them for the packet length.

Check or don't the checksum. Depends on how bad a bad packet would be.

Then you can pluck the values you need out of the character array so built.

Rinse and repeat.

This turns up with nothing more than "robin2 serial", and is worth the time you give it:

Serial Input Basics - updated

Updated Version Please note that this is a revised version of the advice in this earlier Thread which has become very long. As far as possible I have kept the code examples identical or simplifed them slightly. It should not be necessary to refer to the older Thread, but feel free to do so. Contents The following sections are in this Tutorial Introduction Serial data is slow by Arduino standards Example 1 - Receiving single characters Why code is organized into functions Exampl…

HTH

a7

[vjkin]

@alto777

But first, the packet. It is not clear how you are determining the beginning and ending, does the MATEK module spit out an entire packet with more longer time between such bursts?

Thank you for your reply. I used this code to check the data send through Uart of the module:

void inloop()
{
  if (mspSerial.available()) {
  while (mspSerial.available()) {
    uint8_t byteReceived = mspSerial.read();
    inhex(byteReceived);
  }
  Serial.println();
  delay(10);
}
}

void inhex(uint8_t byte)
{
  Serial.print(byte, HEX);
  Serial.print(" ");
}

I set the delay 10ms because I see that it give me the data start at 24 58 3C ...
It is the same as the structure of the data in this link: MSP V2 · iNavFlight/inav Wiki · GitHub

This is the example of the data which is shown in the link:

I see that the data is like the one in the image of hex code which I shown in post #1.
I also see that this module is used for Ardupilot and Inav, so I thought that the hex code auto configured on this module. Therefore, I just read it with delay 10ms and process it to get the distance, flow X, and flow Y by using the code in post #1.

[vjkin]

@vjkin

I also see that this module is used for Ardupilot and Inav,

I think like this because I don't see any datasheet explain the hex code in this link: https://www.mateksys.com/?portfolio=3901-l0x#tab-id-4

[vjkin]

Thank you for your reply. I will test it again with your opinion and study the link you gave here.

[jremington]

If you post some more examples of the message hex dump (preferably only the lines containing the "3c 0 2 1F 9" byte signature, annotated according to how the sensor is being moved), it should be easy to confirm the data structure.

Please do not post pictures of code or text. Use copy/paste with code tags instead.

[aimatt]

I'm also working on this from a Python perspective. I'm close.

                ('fov_degrees', 42.0), # Field of view in degrees
                ('image_width_pixels', 35), # Width of image in pixels

import serial
import time
import numpy as np

class MatekOpticalFlowSensor:
    FUNC_LIDAR = 7937
    FUNC_FLOW = 7938
    HEADER_SIZE = 8

    def __init__(self, port, baudrate=115200, timeout=1):
        self.serial_port = serial.Serial(port, baudrate, timeout=timeout)
        self.data = {
            'height': [],
            'xm': [],
            'ym': []
        }

    def crc8_dvb_s2(self, crc, a):
        crc ^= a
        for _ in range(8):
            if crc & 0x80:
                crc = (crc << 1) ^ 0xD5
            else:
                crc = crc << 1
            crc &= 0xFF  # Ensure crc is always 8 bits
        return crc

    def calculate_checksum(self, data):
        crc = 0
        for byte in data:
            crc = self.crc8_dvb_s2(crc, byte)
        return crc

    def parse_lidar_payload(self, payload):
        quality = payload[0]
        range_data = int.from_bytes(payload[1:5], byteorder='little')
        return quality, range_data

    def parse_flow_payload(self, payload):
        quality = payload[0]
        x_velocity = int.from_bytes(payload[1:5], byteorder='little', signed=True)
        y_velocity = int.from_bytes(payload[5:9], byteorder='little', signed=True)

        print(f"parse_flow_payload payload: {[hex(x) for x in payload]} size: {len(payload)}, payload[1:5]: {payload[1:5]}, payload[5:9]: {payload[5:9]}")
        print(f"parse_flow_payload x_velocity: {x_velocity}, y_velocity: {y_velocity}")

        return quality, x_velocity, y_velocity

    def poll_flow(self, num_samples=1):
        self.data = {
            'height': [],
            'xm': [],
            'ym': []
        }

        lidar_count = 0
        flow_count = 0
        start_time = time.time()

        while len(self.data['height']) < num_samples or len(self.data['xm']) < num_samples or len(self.data['ym']) < num_samples:
            while True:
                if self.serial_port.read(1) == b'$':
                    break

            while self.serial_port.in_waiting < self.HEADER_SIZE - 1:
                time.sleep(0.01)

            header = b'$' + self.serial_port.read(self.HEADER_SIZE - 1)
            if header[1:2] != b'X' or header[2:3] not in b'<>!':
                print(f"Invalid header: {header}")
                continue

            flag = header[3]
            func = int.from_bytes(header[4:6], byteorder='little')
            size = int.from_bytes(header[6:8], byteorder='little')

            while self.serial_port.in_waiting < size + 1:
                time.sleep(0.01)

            payload = self.serial_port.read(size)
            msg_crc = self.serial_port.read(1)[0]

            checksum_data = header[3:] + payload
            calculated_checksum = self.calculate_checksum(checksum_data)

            if calculated_checksum != msg_crc:
                print(f"Checksum mismatch: expected {msg_crc}, calculated {calculated_checksum}")
                continue

            if func == self.FUNC_LIDAR and size == 5:
                lidar_count += 1
                quality, range_data = self.parse_lidar_payload(payload)
                if len(self.data['height']) < num_samples:
                    # millimeters
                    self.data['height'].append(range_data / 1000.)
            elif func == self.FUNC_FLOW and size == 9:
                flow_count += 1
                quality, x_velocity, y_velocity = self.parse_flow_payload(payload)
                if quality < 255:
                    if len(self.data['xm']) < num_samples:
                        self.data['xm'].append(x_velocity)
                    if len(self.data['ym']) < num_samples:
                        self.data['ym'].append(y_velocity)
            else:
                print(f"Unknown packet: func={func}, size={size}, payload={payload}")

        end_time = time.time()
        elapsed_time = end_time - start_time

        lidar_frequency = lidar_count / elapsed_time
        flow_frequency = flow_count / elapsed_time

        # print(f"Lidar message frequency: {lidar_frequency} Hz")
        # print(f"Optical flow message frequency: {flow_frequency} Hz")

        return self.data

    def poll_sensor(self):
        current_time = self.get_clock().now().to_msg().sec
        try:
            self._sensor.poll_flow()
            delta_time = current_time - self.last_time if self.last_time is not None else self._dt

            # Get raw sensor value (average of data for more stable reading)
            # 13.000000
            # x_dps: -0.016250

            xv = np.average(self._sensor.data['xm']) # x-axis velocity
            yv = np.average(self._sensor.data['ym']) # y-axis velocity
            self._pos_z = np.average(self._sensor.data['height']) # Meters off the ground

            self.get_logger().info(f"xv: {xv:.6f}, yv: {yv:.6f}, _pos_z: {self._pos_z:.6f}")

            fov_degrees = self.get_parameter('fov_degrees').value
            fov_radians = np.radians(fov_degrees)
            fov_width_meters = 2.0 * self._pos_z * np.tan(fov_radians / 2.0)
            self.get_logger().info(f"fov_degrees: {fov_degrees},  fov_radians: {fov_radians}. fov_width_meters: {fov_width_meters:.6f}, self._pos_z: {self._pos_z}")
            scale_factor = fov_width_meters / self.get_parameter('image_width_pixels').value

            # Calculate the distance traveled using self._pos_z in meters over time
            dist_x = xv * scale_factor
            dist_y = yv * scale_factor
            
            # Calculate velocities in meters per second
            x_vel = dist_x / delta_time
            y_vel = dist_y / delta_time
            self.linear_velocity.x = 0.0
            self.linear_velocity.y = 0.0

            if not np.isnan(x_vel) and not np.isinf(x_vel):
                self.linear_velocity.x = x_vel

            if not np.isnan(y_vel) and not np.isinf(y_vel):
                self.linear_velocity.y = y_vel
            
            self.get_logger().info(f"linear_velocity: {self.linear_velocity}")

            # Update positions
            self._pos_x += dist_x
            self._pos_y += dist_y

            # self.get_logger().info(f"_pos_x: {self._pos_x:.6f}, _pos_y: {self._pos_y:.6f}")
        except (RuntimeError, AttributeError) as e:
            self.get_logger().error('Exception occurred during poll_sensor: ' + str(e))

        self.last_time = current_time

Here is some payload during movement in the x axis:

fov_degrees: 42.0,  fov_radians: 0.7330382858376184. fov_width_meters: 0.066792, self._pos_z: 0.087
linear_velocity: geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)

payload: ['0x68', '0x2', '0x0', '0x0', '0x0', '0x0', '0x0', '0x0', '0x0'] size: 9, payload[1:5]: b'\x02\x00\x00\x00', payload[5:9]: b'\x00\x00\x00\x00'
x_velocity: 2, y_velocity: 0
payload: ['0x67', '0x1', '0x0', '0x0', '0x0', '0x0', '0x0', '0x0', '0x0'] size: 9, payload[1:5]: b'\x01\x00\x00\x00', payload[5:9]: b'\x00\x00\x00\x00'
x_velocity: 1, y_velocity: 0
payload: ['0x69', '0xff', '0xff', '0xff', '0xff', '0x1', '0x0', '0x0', '0x0'] size: 9, payload[1:5]: b'\xff\xff\xff\xff', payload[5:9]: b'\x01\x00\x00\x00'
x_velocity: -1, y_velocity: 1
payload: ['0x65', '0xa', '0x0', '0x0', '0x0', '0xff', '0xff', '0xff', '0xff'] size: 9, payload[1:5]: b'\n\x00\x00\x00', payload[5:9]: b'\xff\xff\xff\xff'
x_velocity: 10, y_velocity: -1
payload: ['0x65', '0x0', '0x0', '0x0', '0x0', '0x0', '0x0', '0x0', '0x0'] size: 9, payload[1:5]: b'\x00\x00\x00\x00', payload[5:9]: b'\x00\x00\x00\x00'
x_velocity: 0, y_velocity: 0
payload: ['0x6a', '0xff', '0xff', '0xff', '0xff', '0x1', '0x0', '0x0', '0x0'] size: 9, payload[1:5]: b'\xff\xff\xff\xff', payload[5:9]: b'\x01\x00\x00\x00'
x_velocity: -1, y_velocity: 1
payload: ['0x66', '0xfe', '0xff', '0xff', '0xff', '0xfe', '0xff', '0xff', '0xff'] size: 9, payload[1:5]: b'\xfe\xff\xff\xff', payload[5:9]: b'\xfe\xff\xff\xff'
x_velocity: -2, y_velocity: -2
payload: ['0x65', '0xf7', '0xff', '0xff', '0xff', '0xfe', '0xff', '0xff', '0xff'] size: 9, payload[1:5]: b'\xf7\xff\xff\xff', payload[5:9]: b'\xfe\xff\xff\xff'
x_velocity: -9, y_velocity: -2
payload: ['0x67', '0x1', '0x0', '0x0', '0x0', '0x0', '0x0', '0x0', '0x0'] size: 9, payload[1:5]: b'\x01\x00\x00\x00', payload[5:9]: b'\x00\x00\x00\x00'
x_velocity: 1, y_velocity: 0
payload: ['0x6a', '0xff', '0xff', '0xff', '0xff', '0x0', '0x0', '0x0', '0x0'] size: 9, payload[1:5]: b'\xff\xff\xff\xff', payload[5:9]: b'\x00\x00\x00\x00'
x_velocity: -1, y_velocity: 0
payload: ['0x68', '0x1', '0x0', '0x0', '0x0', '0x0', '0x0', '0x0', '0x0'] size: 9, payload[1:5]: b'\x01\x00\x00\x00', payload[5:9]: b'\x00\x00\x00\x00'
x_velocity: 1, y_velocity: 0
payload: ['0x65', '0xff', '0xff', '0xff', '0xff', '0x0', '0x0', '0x0', '0x0'] size: 9, payload[1:5]: b'\xff\xff\xff\xff', payload[5:9]: b'\x00\x00\x00\x00'

[drgames580501]

I managed to get it to work. A necessary condition is a varied surface on which you move the module, for example a carpet with patterns (for some reason only the Y axis is sensitive to this, since the X axis correctly detects movement even on a single-color surface without patterns)

(I use a translator, since you most likely do not understand russian)