SLAMTEC RPLIDAR A2: développement en C d'un driver 4K ultra rapide

Je confirme que c'est pas uniquement le signe ces 2 petits bits. Le petit bit qui reste est le bit de poids le plus fort. Cette info est en fait codée sur 5 bit + le signe.

Petit test en considérant qu'il n'y a qu'un seul bite (signe) pour les offsets zéro et deux d'une cabine (voir page 19)

deltaAnglesQ5 est maintenant deltaAnglesQ6 (j'ai pas renommé)

   default:
    sum ^= current;
    switch(m) {
     case 0:
      deltaAnglesQ5[s] = (current & 0b1) << 7;
      distances[s] = current >> 1;
      m++;
      break;
     case 1:
      distances[s] |= current << 7;
      m++;
      break;
     case 2:
      deltaAnglesQ5[s + 1] = (current & 0b1) << 7;
      distances[s + 1] = current >> 1;
      m++;
      break;
     case 3:
      distances[s + 1] |= current << 7;
      m++;
      break;
     case 4:
      deltaAnglesQ5[s] |= (current & 0b1111) << 3;
      deltaAnglesQ5[s + 1] |= (current & 0b11110000) >> 1;

 1bit.png   44,98 Ko   0 téléchargement(s)

Bon du coup je me replonge dans la doc technique : ( Les doc techniques ?? ) 
Ancienne version :   rpk-02-communication-protocol.pdf   1,49 Mo   28 téléchargement(s)
Nouvelles versions :   LR001_SLAMTEC_rplidar_protocol_v0.2_en.pdf   1,92 Mo   123 téléchargement(s)

EDIT : La version la plus récente :   LR001_SLAMTEC_rplidar_protocol_v1.1_en.pdf   1,93 Mo   26 téléchargement(s)

Cette nouvelle version du document corrige bien l'erreur qui me perturbait ... Et elle spécifie désormais clairement que la chose : 

 cabin.JPG   48,17 Ko   0 téléchargement(s)

Donc plus de doutes à avoir de ce côté là. 

J'ai donc creusé ailleurs. 

Vu que c'est visiblement un problème de calcul d'angle je suis retourné sur le calcul de l'angle : 

      // Calcul de l'angle en float
      angle = (
                float(oldStartAngleQ6) +                                   // Je prend l'ancienne valeur de l'angle absolu initial car on bosse avec une nouvelle entête.
                float(diffAngleQ6) * float(i) / float(RPLIDARNBSAMPLES) -  // Calcul de l'angle théorique par division bêtes...
                (float(deltaAnglesQ5[i]) * 2)                              // Petite correction angulaire "pour prendre en compte les variations de vitesse du moteur" le "* 2" c'est juste pour passer de Q5 en Q6
               ) / 64.0 * PI / 180.0;

Vu que path et pascal avait pas l'air super d'accord, sur la façon de faire qui dans les deux cas me paraît équivalente j'ai décidé par acquis de conscience de faire une 3ème méthode qui me paraît aussi équivalent ... 

j'ai modifié le deltaAngleQ5 de pascal en deltaAngleQ3 comme indiqué sur la doc et j'ai sortis le bit de signe : 

j'ai essayer sans inverser les bits quand c'est négatif

    switch(m) {
     case 0:                                         // Figure 4-11 offset +0 d'une cabine
      deltaAnglesQ3[s] = (current & 0b1) << 4;
      signDeltaAngleQ3[s] = (current & 0b10) >> 1;
      distances[s] = current >> 2;
      m++;
      break;
     case 1:                                         // Figure 4-11 offset +1 d'une cabine
      distances[s] |= current << 6;
      m++;
      break;
     case 2:                                         // Figure 4-11 offset +2 d'une cabine
      deltaAnglesQ3[s + 1] = (current & 0b1) << 4;
      signDeltaAngleQ3[s + 1] = (current & 0b10) >> 1;
      distances[s + 1] = current >> 2;
      m++;
      break;
     case 3:                                         // Figure 4-11 offset +3 d'une cabine
      distances[s + 1] |= current << 6;
      m++;
      break;
     case 4:                                         // Figure 4-11 offset +4 d'une cabine
      deltaAnglesQ3[s] |= (current & 0b1111);
      deltaAnglesQ3[s + 1] |= (current & 0b11110000) >> 4;
      m = 0;
      s += 2;

et j'ai aussi essayé en inversant les bits quand c'est négatif...

    switch(m) {
     case 0:                                         // Figure 4-11 offset +0 d'une cabine
      deltaAnglesQ3[s] = (current & 0b1) << 4;
      signDeltaAngleQ3[s] = (current & 0b10) >> 1;
      distances[s] = current >> 2;
      m++;
      break;
     case 1:                                         // Figure 4-11 offset +1 d'une cabine
      distances[s] |= current << 6;
      m++;
      break;
     case 2:                                         // Figure 4-11 offset +2 d'une cabine
      deltaAnglesQ3[s + 1] = (current & 0b1) << 4;
      signDeltaAngleQ3[s + 1] = (current & 0b10) >> 1;
      distances[s + 1] = current >> 2;
      m++;
      break;
     case 3:                                         // Figure 4-11 offset +3 d'une cabine
      distances[s + 1] |= current << 6;
      m++;
      break;
     case 4:                                         // Figure 4-11 offset +4 d'une cabine
      deltaAnglesQ3[s] |= (current & 0b1111);
      if (signDeltaAngleQ3[s]) deltaAnglesQ3[s] = ~deltaAnglesQ3[s];
      deltaAnglesQ3[s + 1] |= (current & 0b11110000) >> 4;
      if (signDeltaAngleQ3[s+1]) deltaAnglesQ3[s+1] = ~deltaAnglesQ3[s+1];
      m = 0;
      s += 2;

et quelque soit la partie ci dessus ça donne ce calcul d'angle

      angle = (
                float(oldStartAngleQ6)/64.0 +                                   // Je prend l'ancienne valeur de l'angle absolu initial car on bosse avec une nouvelle entête.
                float(diffAngleQ6) * float(i) / (64.0 * float(RPLIDARNBSAMPLES)) +  // Calcul de l'angle théorique par division bêtes...
                (2.0 * float(signDeltaAngleQ3[i]) - 1.0)*(float(deltaAnglesQ3[i]) /8 )                              // Petite correction angulaire "pour prendre en compte les variations de vitesse du moteur" le "* 2" c'est juste pour passer de Q5 en Q6
               ) * PI / 180.0;

On se retrouve dans les deux cas avec le même type de résultat , un problème d'angle... ( Sauf que moi j'ai plutôt un décalage de 3° ... )

Du coup l'erreur ne semble pas être là non plus... 


Puis j'ai tout simplement commenté la correction angulaire ...  Et là miracle le résultat à l'air nickel ! 

Hum hum ... Donc même si la façon d'utiliser la valeur de correction semble ne pas être le problème, la correction est quand même problématique !!! 
Je me suis donc intéressé de plus près à cette correction. 

D'après la doc, c'est censé être une correction en degré signée sur 6bits
 typé Q3 sur 5bits => deltaAnglesQ3[i]  + 1 bits de signe ( le bit de poids fort ) =>  signDeltaAngleQ3[i]

En français  ça veut dire que :

la valeur deltaAnglesQ3 varie entre 0 et 31   ( 0 et 0b11111 )  car sur 5 bits,  
en degré cela équivaut à une variation entre 0 et 31/(2^3)  => entre 0 et  3,875     ( avec une division par  2^3 car on est en Q3 d'après mes discussion avec pascal) 

donc en prenant le bit de signe on est censé corriger entre -3,875 et 3,875 ° 

Ce qui semblerait expliquer mon décalage ...

Et si le deltaTheta fournit par le RPLidar état faux / que sa valeur varie en fonction de la distance?? ça expliquerait pourquoi en le commentant le résultat serait nickel et pourquoi en le gardant on aurait ce résultat... 

Du coup j'ai fais un petit test afin de regarder les valeurs de  : 

      angle = (
                float(oldStartAngleQ6)/64.0 +                                   // Je prend l'ancienne valeur de l'angle absolu initial car on bosse avec une nouvelle $
                float(diffAngleQ6) * float(i) / (64.0 * float(RPLIDARNBSAMPLES))   // Calcul de l'angle théorique par division bêtes...
                //+ (2.0 * float(signDeltaAngleQ3[i]) - 1.0)*(float(deltaAnglesQ3[i]) / 8.0 )                              // Petite correction angulaire "pour prendre e$
               ) * PI / 180.0;
      

      distance = deltaAnglesQ3[i] * 100.0;

      lidarx = POSITIONLIDARX + distance * sinFloat(angle);
      lidary = POSITIONLIDARY + distance * cosFloat(angle);

Et là j'ai un résultat intéressant : 

 bug.JPG   58,19 Ko   0 téléchargement(s)

Visiblement la valeur brut de correction angulaire lue varie bien fonction de la distance mesurée ... plus on est proche d'un obstacle et plus elle est faible ... 

Du coup j'en viens à douter des valeurs envoyées par notre lidar ! Et sur le coup semble bien plus judicieux de ne pas essayer de prendre en compte ces valeurs de corrections angulaire que de chercher à les prendre en compte ... 

Mais bon je suis curieux et je continue quand même l'investigation ... 

Je retourne sur le site de slamtec et je vais fouiller dans les ressources téléchargeable. 
On y trouve entre autre leur driver pour piloter le lidar : 

 rplidar_sdk_v1.6.0.zip   4,67 Mo   29 téléchargement(s)

en fouillant un peu on trouve un fichier assez intéressant : rplidar_driver.cpp 

" ctrl + f "  " Q3 " 

et on retrouve cette fonction : 

void     RPlidarDriverImplCommon::_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_t *nodebuffer, size_t &nodeCount)
{
    nodeCount = 0;
    if (_is_previous_capsuledataRdy) {
        int diffAngle_q8;
        int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF)<< 2);
        int prevStartAngle_q8 = ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2);

        diffAngle_q8 = (currentStartAngle_q8) - (prevStartAngle_q8);
        if (prevStartAngle_q8 >  currentStartAngle_q8) {
            diffAngle_q8 += (360<<8);
        }

        int angleInc_q16 = (diffAngle_q8 << 3);
        int currentAngle_raw_q16 = (prevStartAngle_q8 << 8);
        for (size_t pos = 0; pos < _countof(_cached_previous_capsuledata.cabins); ++pos)
        {
            int dist_q2[2];
            int angle_q6[2];
            int syncBit[2];

            dist_q2[0] = (_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0xFFFC);
            dist_q2[1] = (_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0xFFFC);

            int angle_offset1_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3)<<4));
            int angle_offset2_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3)<<4));

            angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3<<13))>>10);
            syncBit[0] =  (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0;
            currentAngle_raw_q16 += angleInc_q16;


            angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3<<13))>>10);
            syncBit[1] =  (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0;
            currentAngle_raw_q16 += angleInc_q16;

            for (int cpos = 0; cpos < 2; ++cpos) {

                if (angle_q6[cpos] < 0) angle_q6[cpos] += (360<<6);
                if (angle_q6[cpos] >= (360<<6)) angle_q6[cpos] -= (360<<6);

                rplidar_response_measurement_node_t node;

                node.sync_quality = (syncBit[cpos] | ((!syncBit[cpos]) << 1));
                if (dist_q2[cpos]) node.sync_quality |= (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);

                node.angle_q6_checkbit = (1 | (angle_q6[cpos]<<1));
                node.distance_q2 = dist_q2[cpos];

                nodebuffer[nodeCount++] = node;
             }

        }
    }

    _cached_previous_capsuledata = capsule;
    _is_previous_capsuledataRdy = true;
}

bon du premier coup d'oeil je ne comprend pas tout mais ... 

cette partie là du code :

int angle_offset1_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3)<<4));
int angle_offset2_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3)<<4));

ressemble beaucoup à s'y méprendre à la façon dont on essaye de s'y prendre ... mais ils ne s'embêtent pas avec le signe ... 

Effectivement j'ai utilisé la version 0.2 du document pour coder le truc et il n'y a pas le bug de nombre de bits. C'est la vielle version du datasheet posté chez Path qui nous à fait buguer.

J'ai aussi déjà fait la version Q3 il y a longtemps qui se trouve dans ton robot renommé en .q3 et le code est même posté au dessus...

Elle comporte la conversion de signe en mode "neuneu" identique à celle de Path obligatoire en JavaScript

J'ai aussi testé sans appliquer cette correction et ça donne aussi 3 degrés d'erreur avec un S quand t'es proche du lidar

 sans.png   38,52 Ko   0 téléchargement(s)

La vérité est ailleurs

Alors il y a d'autres pb. Tas check aussi la partie affichage ?

Alors comment t'explique que le même code d'affichage est parfait en mode 2K ?

Vu que mike a vu qu'ils ne gèrent pas le bit de signe dans le code d'origine j'ai fait la même chose, j'ai juste viré le signe de mon type deltaAnglesQ5, le bit de signe du lidar est toujours claqué sur mon bit de poid le plus fort mais vu que c'est un UINT et plus un INT ça ne passe jamais en négatif, et ça semble fonctionner ?!?!?! plus d'erreur de rotation ni de S ou cassure quand on approche !

 static uint8_t deltaAnglesQ5[RPLIDARNBSAMPLES];
 static uint16_t distances[RPLIDARNBSAMPLES];
 

 pasDeSigne.png   34,68 Ko   0 téléchargement(s)

Conclusion, ça fonctionne parfaitement après quelques heures de debug à deux j'optimise le code et le colle ici

Du coup Path t'as forcément un truc pourri étrange dans ton code:D

Pas besoin d'appliquer de signe à deltaAnglesQ3

J'ai plus besoin non plus de le passer en Q5:)

Je veux bien que tu m'expliques comment ils gèrent un signe avec ce code : 

            angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3<<13))>>10);
            syncBit[0] =  (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0;
            currentAngle_raw_q16 += angleInc_q16;


            angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3<<13))>>10);
            syncBit[1] =  (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0;
            currentAngle_raw_q16 += angleInc_q16;

Je m'intéresse surtout au " << 13  " et " >> 10 " car je ne vois pas comment ils gèrent le signe là dedans ... Par contre le SyncBit j'ai l'impression qu'il permet de détecter si on a dépasser les 360° ou pas. 

Il n'y a aucun signe c'est une erreur de la doc (y'en a eu d'autres) la preuve : le code d'origine n'en comporte pas et l'affichage fonctionne nickel avec un entier non signé...

Par contre maintenant on ouvre un autre sous sujet pour ce driver : détecter le passage de l'angle par 0 pour que les tableaux de stockage démarrent toujours à la même position, et la mike à trouvé un calcul bizarre pas indiqué dans la doc ou il faut comprendre ce qui se passe

On ne recopie pas des bits n'importe comment dans un type non strict, un int c'est 16 bits minimum, couramment 32 bits ou plus selon l'architecture... si tu balances un bit de signe en plein milieu d'un int au pif bah le nombre ne sera jamais négatif !

En fait c'est pas pour toi je peste contre leurs code moisi qui ne respecte pas du tout la doc et qui utilise des types non stricts (uint8_t int8_t etc...)

Bon moi je reprend leur code : 

void     RPlidarDriverImplCommon::_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_t *nodebuffer, size_t &nodeCount)
{
    nodeCount = 0;
    if (_is_previous_capsuledataRdy) {
        int diffAngle_q8;
        int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF)<< 2);
        int prevStartAngle_q8 = ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2);

        diffAngle_q8 = (currentStartAngle_q8) - (prevStartAngle_q8);
        if (prevStartAngle_q8 >  currentStartAngle_q8) {
            diffAngle_q8 += (360<<8);
        }

        int angleInc_q16 = (diffAngle_q8 << 3);
        int currentAngle_raw_q16 = (prevStartAngle_q8 << 8);
        for (size_t pos = 0; pos < _countof(_cached_previous_capsuledata.cabins); ++pos)
        {
            int dist_q2[2];
            int angle_q6[2];
            int syncBit[2];

            dist_q2[0] = (_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0xFFFC);
            dist_q2[1] = (_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0xFFFC);

            int angle_offset1_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3)<<4));
            int angle_offset2_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3)<<4));

            angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3<<13))>>10);
            syncBit[0] =  (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0;
            currentAngle_raw_q16 += angleInc_q16;


            angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3<<13))>>10);
            syncBit[1] =  (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0;
            currentAngle_raw_q16 += angleInc_q16;

            for (int cpos = 0; cpos < 2; ++cpos) {

                if (angle_q6[cpos] < 0) angle_q6[cpos] += (360<<6);
                if (angle_q6[cpos] >= (360<<6)) angle_q6[cpos] -= (360<<6);

                rplidar_response_measurement_node_t node;

                node.sync_quality = (syncBit[cpos] | ((!syncBit[cpos]) << 1));
                if (dist_q2[cpos]) node.sync_quality |= (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);

                node.angle_q6_checkbit = (1 | (angle_q6[cpos]<<1));
                node.distance_q2 = dist_q2[cpos];

                nodebuffer[nodeCount++] = node;
             }

        }
    }

    _cached_previous_capsuledata = capsule;
    _is_previous_capsuledataRdy = true;
}

avec cette formule : 

On regarde la ligne : 

int angleInc_q16 = (angleDiff_q8 << 3);     

=>  c'est l'équivalent de        (AngleDiffq8  << 8 ) / 32   = AngleDiffq8  << 8  >>5 = AngleDiffq8 << 3   
                                          =   AngleDiffq16   / 32                  

D'où pour reprendre la formule du dessus :
theta k _q16=  wi_q16 +    angleInc_q16 * k  - dtheta_q16(k)  ! 


une autre façon de multiplier par k c'est de sommer angleInc à chaque itération  ... 

Mathématiquement on peut écrire : 

theta k _q16=  (  wi_q16 +  k angleInc_q16  )    - dtheta_q16  (k )        

Ok mais pourquoi mettre en évidence cette partie ? 

wi_q16 est noté    :   prevStartAngle_q8 << 8; 

et   currentAngle_raw_q16   qu'on initialise à  wi_q16   en faisant currentAngle_raw_q16 = ( prevStartAngle_q8 << 8;  )   =   prevStartAngle_q16 ;  
    est la variable qui va stocker  au fur et à mesure :  wi_q16 +  k angleInc_q16      
 

currentAngle_raw_q16  ( k )  =   prevStartAngle_q16  +  k  angleInc_q16  

grâce à  la ligne :  currentAngle_raw_q16 += angleInc_q16;

D'où maintenant on peut écrire 

theta k_q16  =  currentAngle_raw_q16  ( k ) -  dtheta_q16 (k )   =  currentAngle_raw_q16  ( k ) - dtheta_q3 (k ) << 13   

ce qui est exactement faite grâce à cette ligne :

angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3<<13))>>10); 

sauf qu'en même temps  il transforment theta (k )q_16 en theta(k)q_6

car  angle_q6 =  angle_q16 >> 10  =  ( currentAngle_raw_q16  ( k ) -  dtheta_q16 (k ) ) >> 10   = ( currentAngle_raw_q16  ( k ) - dtheta_q3 (k ) << 13    ) >> 10 .