Aller au contenu


Photo
* * * * * 1 note(s)

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

SLAMTEC RPLIDAR A2 4K Embarqué Integer FPU FIxed point

  • Veuillez vous connecter pour répondre
57 réponses à ce sujet

#21 Path

Path

    Made By Humans

  • Modérateur
  • PipPipPipPipPip
  • 2 459 messages
  • Gender:Male
  • Location:Paris

Posté 17 avril 2018 - 10:36

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.


Podcast Made By Humans

Je cherche des volontaires de tous niveaux pour nos petites conversations entre hobbyistes.

Accès aux salles secrètes

 


#22 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 411 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 17 avril 2018 - 10:52

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;

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


Accès officiel aux robots : https://www.vigibot.com
News : https://www.vigirobotics.com
 
Mon serveur de développement : https://www.serveurperso.com
 

#23 Path

Path

    Made By Humans

  • Modérateur
  • PipPipPipPipPip
  • 2 459 messages
  • Gender:Male
  • Location:Paris

Posté 17 avril 2018 - 11:10

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

Podcast Made By Humans

Je cherche des volontaires de tous niveaux pour nos petites conversations entre hobbyistes.

Accès aux salles secrètes

 


#24 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 8 231 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 18 avril 2018 - 01:32

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

 

EDIT : La version la plus récente :  Fichier joint  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 : 

Fichier joint  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 : 

Fichier joint  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 il me 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 : 

Fichier joint  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 ... et ils font des opérations que j'ai encore besoin de comprendre : 
 


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 

 

Les réalisations de Mike118  

 

 

 


#25 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 411 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 18 avril 2018 - 06:00

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

 

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

 

La vérité est ailleurs

 


Accès officiel aux robots : https://www.vigibot.com
News : https://www.vigirobotics.com
 
Mon serveur de développement : https://www.serveurperso.com
 

#26 Path

Path

    Made By Humans

  • Modérateur
  • PipPipPipPipPip
  • 2 459 messages
  • Gender:Male
  • Location:Paris

Posté 18 avril 2018 - 06:23

On peut aussi préciser que tout ce code de fou est une chose optionnelle pour ceux qui voudraient utiliser le rplidar :). Ils fournissent un driver ^^

Podcast Made By Humans

Je cherche des volontaires de tous niveaux pour nos petites conversations entre hobbyistes.

Accès aux salles secrètes

 


#27 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 411 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 18 avril 2018 - 06:25

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 ?

:D


Accès officiel aux robots : https://www.vigibot.com
News : https://www.vigirobotics.com
 
Mon serveur de développement : https://www.serveurperso.com
 

#28 Path

Path

    Made By Humans

  • Modérateur
  • PipPipPipPipPip
  • 2 459 messages
  • Gender:Male
  • Location:Paris

Posté 18 avril 2018 - 06:30

Ok ok :)
Courage

Podcast Made By Humans

Je cherche des volontaires de tous niveaux pour nos petites conversations entre hobbyistes.

Accès aux salles secrètes

 


#29 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 411 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 18 avril 2018 - 06:34

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];
 

 

 

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


Accès officiel aux robots : https://www.vigibot.com
News : https://www.vigirobotics.com
 
Mon serveur de développement : https://www.serveurperso.com
 

#30 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 411 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 18 avril 2018 - 07:17

Conclusion, ça fonctionne parfaitement après quelques heures de debug à deux :D 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:)


Accès officiel aux robots : https://www.vigibot.com
News : https://www.vigirobotics.com
 
Mon serveur de développement : https://www.serveurperso.com
 

#31 Path

Path

    Made By Humans

  • Modérateur
  • PipPipPipPipPip
  • 2 459 messages
  • Gender:Male
  • Location:Paris

Posté 18 avril 2018 - 07:48

Moi c'était pourri avant dappliquer cette inversion de signe. Il y a suffisamment de différences entre ces 2 languages notamment sur le typage pour tromper notre intuition ;)

Podcast Made By Humans

Je cherche des volontaires de tous niveaux pour nos petites conversations entre hobbyistes.

Accès aux salles secrètes

 


#32 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 8 231 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 18 avril 2018 - 09:16

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. 


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 

 

Les réalisations de Mike118  

 

 

 


#33 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 411 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 18 avril 2018 - 06:09

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é...


Accès officiel aux robots : https://www.vigibot.com
News : https://www.vigirobotics.com
 
Mon serveur de développement : https://www.serveurperso.com
 

#34 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 411 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 18 avril 2018 - 06:17

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


Accès officiel aux robots : https://www.vigibot.com
News : https://www.vigirobotics.com
 
Mon serveur de développement : https://www.serveurperso.com
 

#35 Path

Path

    Made By Humans

  • Modérateur
  • PipPipPipPipPip
  • 2 459 messages
  • Gender:Male
  • Location:Paris

Posté 18 avril 2018 - 06:34

Du coup je comprends pas votre histoire de signe :) La doc a été révisée et elle indique toujours un bit de signe.
http://www-numi.fnal.gov/offline_software/srt_public_context/WebDocs/Companion/cxx_crib/shift.html

Podcast Made By Humans

Je cherche des volontaires de tous niveaux pour nos petites conversations entre hobbyistes.

Accès aux salles secrètes

 


#36 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 411 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 18 avril 2018 - 06:46

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 !


Accès officiel aux robots : https://www.vigibot.com
News : https://www.vigirobotics.com
 
Mon serveur de développement : https://www.serveurperso.com
 

#37 Path

Path

    Made By Humans

  • Modérateur
  • PipPipPipPipPip
  • 2 459 messages
  • Gender:Male
  • Location:Paris

Posté 18 avril 2018 - 06:52

Je sais pas répondre pour toi. Il se peut que mike ait raison :) « <<13 »

Podcast Made By Humans

Je cherche des volontaires de tous niveaux pour nos petites conversations entre hobbyistes.

Accès aux salles secrètes

 


#38 Serveurperso

Serveurperso

    Membre passionné

  • Membres
  • PipPipPip
  • 411 messages
  • Gender:Male
  • Location:Paris
  • Interests:Systèmes/Réseaux/Dev/Hardware/RF/Optique/Lasers...

Posté 18 avril 2018 - 06:57

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...)


Accès officiel aux robots : https://www.vigibot.com
News : https://www.vigirobotics.com
 
Mon serveur de développement : https://www.serveurperso.com
 

#39 Mike118

Mike118

    Staff Robot Maker

  • Administrateur
  • PipPipPipPipPip
  • 8 231 messages
  • Gender:Male
  • Location:Anglet
  • Interests:Robotique, Entrepreneuriat, Innovation, Programmation, Résolution de problème, Recherche de solutions, Mécanique, Electronique, Créer, Concevoir

Posté 18 avril 2018 - 07:30

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 : 

post-1264-0-42266400-1523992351.png

 

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 . 


 

 

 

 


Si mon commentaire vous a plus laissez nous un avis  !  :thank_you:

Nouveau sur Robot Maker ? 

Jetez un oeil aux blogs, aux tutoriels, aux ouvrages, au robotscope  aux articles,  à la boutique  et aux différents services disponible !
En attendant qu'une bibliothèque de fichiers 3D soit mise en place n'hésitez pas à demander si vous avez besoin du fichier 3D d'un des produits de la boutique... On l'a peut être ! 

 

Les réalisations de Mike118  

 

 

 


#40 Path

Path

    Made By Humans

  • Modérateur
  • PipPipPipPipPip
  • 2 459 messages
  • Gender:Male
  • Location:Paris

Posté 18 avril 2018 - 07:36

Perso, jai pas du tout suivi leur code. Jai codé selon ma compréhension de la doc et jai eu le code de validation grace à Pascal ^^

Podcast Made By Humans

Je cherche des volontaires de tous niveaux pour nos petites conversations entre hobbyistes.

Accès aux salles secrètes

 





0 utilisateur(s) li(sen)t ce sujet

0 members, 0 guests, 0 anonymous users