Better and faster low level code.

[VigibotDev]

I use the RPLIDAR A2 on a PIC32 microcontroller, it's a web robot here : https://www.serveurperso.com/?page=robot

I find the original driver slow and hard to read. https://github.com/robopeak/rplidar_ros/blob/master/sdk/src/rplidar_driver.cpp

This is my legacy protocol (RPLIDAR A1) non blocking code :

void readLidar() {
 uint8_t current;
 static uint8_t n = 0;
 static uint8_t quality;
 static bool start;
 static uint16_t angleq6;
 static uint16_t distanceq2;
 static uint16_t i = 0;
 float angle;
 float distance;
 float lidarx;
 float lidary;

 while(RXLIDAR.available()) {
  current = RXLIDAR.read();

  switch(n) {

   case 0:
    if((current & 1) != (current >> 1 & 1)) {
     quality = current >> 2;
     start = current & 1;
     n = 1;
    }
    break;

   case 1:
    if(current & 1) {
     angleq6 = current >> 1;
     n = 2;
    } else
     n = 0;
    break;

   case 2:
    angleq6 |= current << 7;
    n = 3;
    break;

   case 3:
    distanceq2 = current;
    n = 4;
    break;

   case 4:
    distanceq2 |= current << 8;

    if(start) {
     if(i && i < NBMESURESMAX) {
      lidarReady = true;
      for(uint8_t j = i; j < NBMESURESMAX; j++) {
       lidarsx[j] = 0;
       lidarsy[j] = 0;
       lidarsCartex[j] = 0;
       lidarsCartey[j] = 0;
      }
     }
     i = 0;
    } else if(i == NBMESURESMAX) {
     lidarReady = false;
     n = 0;
     break;
    }

    if(distanceq2) {                                                  // Good sample
     angle = float(angleq6) / 64.0 * PI / 180.0;
     distance = float(distanceq2) / 4.0;
     lidarx = POSITIONLIDARX + distance * sin(angle);
     lidary = POSITIONLIDARY + distance * -cos(angle);

     if(lidarx > POSITIONSROBOTXMAX || lidarx < POSITIONSROBOTXMIN ||
        lidary > POSITIONSROBOTYMAX || lidary < POSITIONSROBOTYMIN) { // Inside the robot
      lidarsx[i] = int(lidarx);
      lidarsy[i] = int(lidary);
      i++;
     }
    }

    n = 0;
  }
 }
}

Now I must make the 4K (cabin) protocol decoder. This is my non terminated code :

#define RPLIDARRESPONSESIZE 84
#define RPLIDARNBSAMPLES 32

bool readLidar() {
 uint8_t current;
 static uint8_t n = 0;
 static uint8_t checksum;
 static uint8_t sum = 0;
 static uint16_t oldStartAngle;
 static uint16_t startAngle = 0;
 static bool start;
 static uint16_t diffAngle;
 static uint8_t m = 0;
 static uint8_t s = 0;
 static uint8_t angles[RPLIDARNBSAMPLES];
 static uint16_t distances[RPLIDARNBSAMPLES];

 while(RXLIDAR.available()) {
  current = RXLIDAR.read();

  switch(n) {

   case 0:
    if(current >> 4 == 0xA) {
     checksum = current & 0xF;
     n = 1;
    }
    break;

   case 1:
    if(current >> 4 == 0x5) {
     checksum |= current << 4;
     n = 2;
    } else
     n = 0;
    break;

   case 2:
    sum ^= current;
    oldStartAngle = startAngle;
    startAngle = current;
    n++;
    break;

   case 3:
    sum ^= current;
    startAngle |= (current & 0x7F) << 8;
    start = current >> 7;
    if(start)
     TXDEBUG.println('S');

    if(oldStartAngle > startAngle)
     diffAngle = (360 << 6) + startAngle - oldStartAngle;
    else
     diffAngle = startAngle - oldStartAngle;

    for(uint8_t i = 0; i < RPLIDARNBSAMPLES; i++) {
     //TXDEBUG.println(float(oldStartAngle * 32 + diffAngle * i - angles[i] * 64) / 1024.0);
    }
    n++;
    break;

   default:
    sum ^= current;
    switch(m) {
     case 0:
      angles[s] = (current & 3) << 4;
      distances[s] = current >> 2;
      m++;
      break;
     case 1:
      distances[s] |= current << 6;
      m++;
      break;
     case 2:
      angles[s + 1] = (current & 3) << 4;
      distances[s + 1] = current >> 2;
      m++;
      break;
     case 3:
      distances[s + 1] |= current << 6;
      m++;
      break;
     case 4:
      angles[s] |= current & 0xF;
      angles[s + 1] |= current >> 4;
      s += 2;
      m = 0;
      break;
    }
    n++;
    if(n == RPLIDARRESPONSESIZE + 1) {
     if(sum != checksum) {
      TXDEBUG.print(sum);
      TXDEBUG.print(" != ");
      TXDEBUG.println(checksum);
     }
     n = 0;
     sum = 0;
     m = 0;
     s = 0;
    }
    break;

  }
 }

}

[VigibotDev]

4K mode is hard to decode for realtime on a microcontroller. I need current and future (datasheet), I use past and current (bug)

#define RPLIDARNBSAMPLES 32

bool readLidar() {
 uint8_t current;
 static uint8_t n = 0;
 static uint8_t checksum;
 static uint8_t sum = 0;
 static uint16_t oldStartAngleq6;
 static uint16_t startAngleq6 = 0;
 static bool start;
 static uint16_t diffAngleq6;
 static uint8_t m = 0;
 static uint8_t s = 0;
 static int8_t derivAnglesq5[RPLIDARNBSAMPLES];
 static uint16_t distances[RPLIDARNBSAMPLES];
 float distance;
 float angle;
 static uint16_t j = 0;
 float lidarx;
 float lidary;

 while(RXLIDAR.available()) {
  current = RXLIDAR.read();

  switch(n) {

   case 0:
    if(current >> 4 == 0xA) {
     checksum = current & 0xF;
     n = 1;
    }
    break;

   case 1:
    if(current >> 4 == 0x5) {
     checksum |= current << 4;
     n = 2;
    } else
     n = 0;
    break;

   case 2:
    sum ^= current;
    oldStartAngleq6 = startAngleq6;
    startAngleq6 = current;
    n++;
    break;

   case 3:
    sum ^= current;
    startAngleq6 |= (current & 0x7F) << 8;
    start = current >> 7;
    if(start)
     TXDEBUG.println("start");

    if(oldStartAngleq6 > startAngleq6)
     diffAngleq6 = (360 << 6) + startAngleq6 - oldStartAngleq6;
    else
     diffAngleq6 = startAngleq6 - oldStartAngleq6;

    for(uint8_t i = 0; i < RPLIDARNBSAMPLES; i++) {
     distance = distances[i];
     if(distance) {                                                    // Si la lecture est valide

      angle = (float(oldStartAngleq6) + float(diffAngleq6) * float(i) / float(RPLIDARNBSAMPLES) - float(derivAnglesq5[i] << 1)) / 64.0 * PI / 180.0;;

      lidarx = POSITIONLIDARX + distance * sin(angle);
      lidary = POSITIONLIDARY + distance * -cos(angle);
      if(lidarx > POSITIONSROBOTXMAX || lidarx < POSITIONSROBOTXMIN ||
         lidary > POSITIONSROBOTYMAX || lidary < POSITIONSROBOTYMIN) { // Si la lecture est hors du robot
       lidarsx[j] = int(lidarx);
       lidarsy[j] = int(lidary);
       j++;
       if(j == NBMESURESMAX)
        j = 0;
      }
     }
    }

    n++;
    break;

   default: // Cabin
    sum ^= current;
    n++;
    switch(m) {
     case 0:
      derivAnglesq5[s] = (current & 3) << 6;
      distances[s] = current >> 2;
      m++;
      break;
     case 1:
      distances[s] |= current << 6;
      m++;
      break;
     case 2:
      derivAnglesq5[s + 1] = (current & 3) << 6;
      distances[s + 1] = current >> 2;
      m++;
      break;
     case 3:
      distances[s + 1] |= current << 6;
      m++;
      break;
     case 4:
      derivAnglesq5[s] |= (current & 0x0F) << 2;
      derivAnglesq5[s + 1] |= (current & 0xF0) >> 2;
      m = 0;
      s += 2;
      if(s == RPLIDARNBSAMPLES) {
       if(sum != checksum) {
        TXDEBUG.print(sum);
        TXDEBUG.print(" != ");
        TXDEBUG.println(checksum);
       }
       n = 0;
       s = 0;
       sum = 0;
      }
      break;
    }
    break;

  }
 }

}

[kintzhao]

can you give me a detail info about that 'I find the original driver slow and hard to read' ; more info can get form slamtec

[VigibotDev]

look at https://github.com/robopeak/rplidar_ros/blob/master/sdk/src/rplidar_driver.cpp

[kintzhao]

what is the frequent about your PIC32 microcontroller? I am sorry for CR your code. you can contact with support@slamtec.com, if you have any question in analysising rplidar Protocol.

[VigibotDev]

I run the first code I posted (2K legacy mode) on a 80MHz PIC32 + a lot of floating point and integer arithmetic, (a complete robot with SLAM-like) : https://www.serveurperso.com/?page=robot

Just want to upgrade to 4K but the protocol need to know point in the future to do math, it need a buffer, this kill my realtime optimization

I already contacted support but they can't hack the firmware to get 4K samples with a realtime protocol (like the 2K mode) @ faster baudrate than 115200

[VigibotDev]

This issue is just to share my optimized 2K driver, it can work on a 16MHz 8bits AVR Arduino !

[Titibo26]

This issue is just to share my optimized 2K driver, it can work on a 16MHz 8bits AVR Arduino !

As-tu finalement réussi à développer un driver 4K ? Merci 👍

[VigibotDev]

Oui, il supporte aussi le LD Lidar qui est meilleur en / performances / prix / taille / bruit / au soleil que le RP.

https://github.com/vigibot/vigiclient/blob/master/opencv/lidar/lidars.cpp https://github.com/vigibot/vigiclient/blob/master/opencv/lidar/lidars.hpp

C'est le driver "officiel" pour le SLAM open-source de l'asso Vigibot j'ai mis deux robots en ligne ici https://www.vigibot.com/ Le robot "Lidar" est équipé d'un RP Lidar avec le firmware 8K qui fonctionne en 4K Et le robot "Slam" est équipé d'un LD Lidar en 4.5K