Vu que tu parles de conversion du polaire en cartésien je te colle la totalité de du driver qui la comprend ainsi que le calage des array[0] au Theta zéro du lidar. Tu parles aussi de pb d’ordonnancement de mon côté j'ai au pire une seule "méta"cabine à traiter en integermaths ce qui ne foire pas l'ordonnancement. Je suis curieux de voir ton avancement.
cos16() et sin16() fonctions qui exploitent une lookup de 16 bits de taille sur 16 bits d'amplitude directement en mémoire flash pour les maths integer.
#define PI16 0x8000
#define PISURDEUX16 0x4000
#define UN16 0x8000
const uint16_t sinTable16[] {
0x8000, 0x8003, 0x8006, 0x8009, 0x800C, 0x800F, 0x8012, 0x8015, 0x8019, 0x801C, 0x801F, 0x8022, 0x8025, 0x8028, 0x802B, 0x802F,
0x8032, 0x8035, 0x8038, 0x803B, 0x803E, 0x8041, 0x8045, 0x8048, 0x804B, 0x804E, 0x8051, 0x8054, 0x8057, 0x805B, 0x805E, 0x8061,
... sinus ...
0x7F9E, 0x7FA1, 0x7FA4, 0x7FA8, 0x7FAB, 0x7FAE, 0x7FB1, 0x7FB4, 0x7FB7, 0x7FBA, 0x7FBE, 0x7FC1, 0x7FC4, 0x7FC7, 0x7FCA, 0x7FCD,
0x7FD0, 0x7FD4, 0x7FD7, 0x7FDA, 0x7FDD, 0x7FE0, 0x7FE3, 0x7FE6, 0x7FEA, 0x7FED, 0x7FF0, 0x7FF3, 0x7FF6, 0x7FF9, 0x7FFC, 0x8000
};
int16_t sin16(uint16_t angle) {
return sinTable16[angle] - UN16;
}
int16_t cos16(uint16_t angle) {
return sin16(PISURDEUX16 - angle);
}
lidarsx[j] et y[i] // Cartésiennes dans le plan du ROBOT
lidarsCartex[j] et y[i] // Cartésiennes dans le plan de la CARTE après filtrage des points qui sont DANS le robot !!!
pointOdometrie.x et .y // Odométrie absolue et corrigée du robot (sortie de boucle de correcteur d'estimation).
void readLidar() {
static bool init = false;
uint8_t current;
static uint8_t n = 0;
static uint8_t checksum;
static uint8_t sum = 0;
static uint16_t startAngleQ6;
bool start;
static uint16_t diffAngleQ6;
static uint16_t oldStartAngleQ6 = 0;
int32_t diffAngleTotalQ6;
int32_t angleBrutQ6;
static int32_t oldAngleBrutQ6;
int32_t angle;
static uint8_t deltaAnglesQ3[NBMESURESCABINES];
static uint16_t distances[NBMESURESCABINES];
static uint8_t m = 0;
static uint8_t s = 0;
static uint16_t j = 0;
if(!up) {
init = false;
return;
}
while(RXLIDAR.available()) {
current = RXLIDAR.read();
switch(n) {
case 0: // Début de réception de l'en-tête
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;
startAngleQ6 = current;
n = 3;
break;
case 3:
sum ^= current;
startAngleQ6 |= (current & 0x7F) << 8;
start = current >> 7; // Fin de réception de l'en-tête
if(start)
TXDEBUG.println("start");
if(!init) { // Ne pas calculer sans cabines
init = true;
n = 4;
break;
}
diffAngleQ6 = startAngleQ6 - oldStartAngleQ6; // Calculer l'angle entre deux mesures de référence
if(oldStartAngleQ6 > startAngleQ6)
diffAngleQ6 += UNTOURQ6;
diffAngleTotalQ6 = 0;
for(uint8_t i = 0; i < NBMESURESCABINES; i++) {
angleBrutQ6 = (oldStartAngleQ6 + diffAngleTotalQ6 / NBMESURESCABINES) % UNTOURQ6; // Calculer l'angle non compensé
diffAngleTotalQ6 += diffAngleQ6;
if(oldAngleBrutQ6 > angleBrutQ6) { // Détection du passage par zéro de l'angle non compensé
lidarReady = true;
lidarsx[j] = MESURENULLE;
lidarsy[j] = MESURENULLE;
lidarsCartex[j] = MESURENULLE;
lidarsCartey[j] = MESURENULLE;
j = 0;
}
oldAngleBrutQ6 = angleBrutQ6;
if(distances[i]) { // Si la lecture est valide
angle = angleBrutQ6 - (deltaAnglesQ3[i] << 3); // Calculer l'angle compensé
angle = angle * 65536 / UNTOURQ6; // Remise à l'échelle de l'angle
// Passage du plan du lidar au plan du robot
lidarsx[j] = POSITIONLIDARX + distances[i] * sin16(angle) / UN16; // Enregistrement cartésien
lidarsy[j] = POSITIONLIDARY + distances[i] * cos16(angle) / UN16;
if(lidarsx[j] > POSITIONSROBOTXMAX || lidarsx[j] < POSITIONSROBOTXMIN ||
lidarsy[j] > POSITIONSROBOTYMAX || lidarsy[j] < POSITIONSROBOTYMIN) { // Si la lecture est hors du robot
// Passage du plan du robot au plan de la carte
lidarsCartex[j] = pointOdometrie.x + (lidarsx[j] * cos16(theta16) - lidarsy[j] * sin16(theta16)) / UN16;
lidarsCartey[j] = pointOdometrie.y + (lidarsx[j] * sin16(theta16) + lidarsy[j] * cos16(theta16)) / UN16;
if(j < NBMESURESMAX - 1) // Si il reste de la place dans les tableaux
j++;
else
lidarReady = false;
}
}
}
oldStartAngleQ6 = startAngleQ6;
n = 4;
break;
default: // Début de réception des cabines
sum ^= current;
switch(m) {
case 0:
deltaAnglesQ3[s] = (current & 0b11) << 4;
distances[s] = current >> 2;
m = 1;
break;
case 1:
distances[s] |= current << 6;
m = 2;
break;
case 2:
deltaAnglesQ3[s + 1] = (current & 0b11) << 4;
distances[s + 1] = current >> 2;
m = 3;
break;
case 3:
distances[s + 1] |= current << 6;
m = 4;
break;
case 4:
deltaAnglesQ3[s] |= current & 0b1111;
deltaAnglesQ3[s + 1] |= current >> 4;
m = 0;
s += 2;
if(s == NBMESURESCABINES) {
if(sum != checksum) {
TXDEBUG.println("checksum");
init = false; // Ne pas faire les calculs pour ces cabines
}
n = 0;
s = 0;
sum = 0;
}
break;
}
break;
}
}
}