219 réponses à ce sujet

[Path]

Voilà quelque nouvelles du projet. Je voulais explorer ce monde de la robotique amateur. Je suis servi. Je fais beaucoup de détours et de parenthèses. Elles sont passionnantes mais quant à l'atteinte de l'objectif et du kiffe qui va avec, il faut savoir garder la foi.

 

Quand je trouve un moment tranquille pour m'en occuper, je me rend compte que je mets 2h pour me replonger dedans. C'est pas immédiat. Ça réduit pas mal mon efficacité sur le projet. Bon c'est pas chiant du tout mais ça ralenti encore quand on a pas beaucoup de temps à la base.

 

Je suis passé par une petite recherche de suppression du pid. Ou une solution pour contourner le réglage des 3 constantes du pid. http://www.robot-maker.com/forum/topic/12192-se-passer-de-pid/

ça vaut ce que ça vaut. C'est à dire un compromis de plus. Mine rien, je commence à les accumuler ces compromis. Et ça fait baisser mon kiffe pour ce robot !!! C'est pas bon. J'aime pas ça les compromis. Il y a ce contrôle de vitesse sans pid, la communication série en ascii, le sur-dimensionnement des composants un câblage avec breadboard. Si un jour j'ai une version finale d'un robot, je commanderai un PCB quelque part Idem, pour le sur-dimensionnement, je mettrai peut-être la logique et le câblage des 2 nanos dans la mega. Cela simplifiera le robot.

 

J'ai failli avoir un compromis de plus avec l'alimentation. Du moins, il m'est arrivé un truc que j'attendais pas. Le robot est câblé à 100%. Mon alim (une batterie 12V et un conv. CC 12-5V 16A) fournit du 5V aux 3 arduinos, aux 4 encodeurs, à l'écran et à l'imu. Pour alimenter le raspberry et le lidar, je comptais reprendre le 5V qui passe 2 breadboard, un nano, une nappe pour changer d'étage. Mais surprise, je n'ai plus 5v mais 3,8 V ... J'ai omis ce que je crois être une bonne pratique : repiquer le courant à la source, le plus près possible de la source. Je tire un câble 5V en sortie du convertisseur CC. Rien à faire, la tension n'est pas suffisante pour alimenter le raspberry. Sachant que le convertisseur peut en théorie sortir 16A, je n'ai toujours pas compris. Je veux bien que mes câbles aient une résistance interne mais quand même. Le plus long mesure moins de 20 cm. J'ai re-contrôlé le câblage à la recherche de court-circuit. Rien. Je suis remonté à la source 12V et j'ai ajouté un petit UBEC du shop. Et là c'est bon. J'ai pas compris. Je suis pas doué pour alimenter un robot autonome. Voir Emile ^^ Le compromis dont je parlais c'est que j'ai failli ajouter une 2eme batterie pour alimenter le raspberry et le lidar.

 

Le niveau de kiffe est supérieur à la somme des anti-kiffes. Bref, je suis toujours dessus.

 

 

[Serveurperso]

Après les compromis sont obligatoires t'en a partout, la nature même de l'électronique force à un agrégat de compromis.

Après ce que tu n'aime pas c'est la bouïne. Effectivement un protocole ASCII entre 2 machines qui peuvent parler le binaire couramment c'est de la bouïne:)

T'as des compromis bouïneux et des compromis classe. lol

 

[Path]

Pascal, je voudrais partager cette réflection.

 

Tu dis que tout le code (sauf évidemment le parser du lidar) tiendrait sur un seul arduino 8 bits 16Mhz. Je veux bien te croire. Cela veux dire que une mega peut tenir avec une boucle async la lecture des 4 interruptions, les commandes arrivant sur le serial l'affichage et l'accès à l'imu. En fait maintenant que j'ai le code complet, je pense que oui. Cela simplifierait le robot en retirant les 2 nanos, la comm série qui va avec et la complexité d'accès asynchrone aux compteurs détenus par les nanos.

 

Je peux aussi remplacer mon convertisseur qui prend une place folle par un petit UBEC.

 

Quitte a faire ce refactoring, je me demande si je pourrais pas remplacer la mega, les 2 nanos et la raspberry par une arduino due ? Je sais que tu vas préférer la PIC32 mais avoue qu'elles ont des caractéristiques similaires et la due est plus facile d'accès. Cela simplifierait radicalement mon robot !! J'ai vérifié tous mes composants sont compatibles 3,3V.

 

Quand tu dis que le parser du lidar, l'odométrie et le code de contrôle du robot tient dans le pic32, c'est avec quels genres de compromis ?

 

Après il va me manquer un canal de communication radio avec l'ordi. La réflexion est en cours.

[Oracid]

J'ai re-contrôlé le câblage à la recherche de court-circuit. Rien. Je suis remonté à la source 12V et j'ai ajouté un petit UBEC du shop. Et là c'est bon. J'ai pas compris. Je suis pas doué pour alimenter un robot autonome.

Peux-tu nous de quoi il s'agit ? Un lien, peut-être ?

[Path]

Je parle du fameux truc qui a une sortie qui ressemble à un connecteur de servo  http://www.robot-maker.com/shop/alimentation/74-convertisseur-5v-7a.html

[Path]

@Pascal, j'ai retrouvé le compromis qu'il faut faire avec un PIC32 et un rplidar. C'est les 2K. Pour avoir les 4K, il faut du CPU comme celui d'un raspberry.

[Serveurperso]

@Pascal, j'ai retrouvé le compromis qu'il faut faire avec un PIC32 et un rplidar. C'est les 2K. Pour avoir les 4K, il faut du CPU comme celui d'un raspberry.

 

Je viens de finir mon avancement du cloud pour aujourd'hui ! premier fonctionnement avec succès maintenant plus que des détails...

 

Pourquoi un compromis avec le PIC32 ? On fais tout fonctionner dessus et en 4K SVP d'ou le fameux thread de dev C d'un driver optimisé RPLIDAR spécial 4K lol

 

Mais le Due doit être aussi bien j'ai pris le PIC32 car plus de périphériques il me semble puis surtout a l'époque je codais dessus direct en MPLAB sans framework Arduino du coup j'ai continué dessus lol

 

Pourquoi tu étudies pas notre code C avec jojo ? D'ailleurs ça doit se porter sur n'importe puce sans trop forcer sous réserve de port série avec de gros FIFO

car j'en ai chier avec le datasheet PIC32 pour coder le driver UART interrupt avec FIFO custom.

[Path]

Ha mince alors désolé pour le 4K !! Mais bonne nouvelle !!
Alors je peux simplifier Hector avec un unique arduino due. Sous réserve de gros fifo tu dis. Je vais aller trouver le fil dont tu parles.

[Path]

En attendant que la Due arrive, mon nouveau jouet Je commence à étudier la théorie. A priori, il y a plein de trucs qui changent par rapport aux arduino que je connais. Tous des 8bits.

 

C'est en 32 bits. Sur le Due, un int c'est 4 octets comme le long en 8 bits. A priori, le long sur Due fait aussi 4 bytes.

On pourrait changer la fréquence des 12 pin PWM indépendamment d'une fréquence d'horloge qui affecterait la fonction delay(). <- Il faut que je check ça !!

Tous les pins numériques peuvent servir d'interruption matériels.

Les ports ne fonctionnent plus de la même façon. On parle de ports parallèles (PIO). Il y en a 4 PIOA, PIOB, PIOC et PIOD. Les ports sont sur 32 bits et permettent d'accéder à tous les pins : https://www.arduino....PinMappingSAM3X

 

Par exemple,

 

pour set le pin 13 (la led), soit le bit 27 du port B : 

PIOB->PIO_SODR=1<<27;

pour clear le même pin :

PIOB->PIO_CODR=1<<27;

Si on a la flemme de lire le gros tableau donné en lien, pour connaître le port (A, B, C ou D) d'un pin :

Pio *p = digitalPinToPort(13);

Si on est allergique aux décalages de bits :

uint32_t b13 = digitalPinToBitMask(13);

On utilise ces 2 fonctions comme ça :

p->PIO_SODR = b13; // set
p->PIO_CODR = b13; // clear

pour lire le même pin :

boolean p13=!!(PIOB->PIO_PDSR & (1<<27));

Pour lire un port x en entier :

uint32_t port = REG_PIOx_PDSR;

Pour écrire un port en entier :

REG_PIOx_ODSR = a32bitsValue;

Où x vaut A, B, C ou D.

 

Elle vont être belles mes interruptions !!

 

La Due peut aussi faire de la musique en stéréo avec ses 2 DAC mais osef pour Hector

 

 

PS. Merci Mike pour la rapidité d'envoi ^^

[Path]

J'ai pas trouvé beaucoup d'info sur le changement de fréquence PWM sur le due.

La meilleure ressource que j'ai trouvé est celle-ci http://www.kerrywong.com/2014/09/21/on-arduino-due-pwm-frequency/

 

J'ai hâte de l'avoir en main pour tester tout ça. Et je crois bien que je suis parti pour faire un 2eme portage du driver ^^

[Serveurperso]

J'ai pas trouvé beaucoup d'info sur le changement de fréquence PWM sur le due.

La meilleure ressource que j'ai trouvé est celle-ci http://www.kerrywong.com/2014/09/21/on-arduino-due-pwm-frequency/

 

J'ai hâte de l'avoir en main pour tester tout ça. Et je crois bien que je suis parti pour faire un 2eme portage du driver ^^

 

Top ça m'intéresse de voir cette plateforme (le Due)

[Path]

Tu tombe bien Pascal, dis, si tu as l'info sans chercher. À titre de comparaison, pour préparer l'archi du code. Il est de quelle taille le buffer du serial sur le pic32 ? Plus il est petit, plus il faut le lire souvent.

[Path]

Bon !! Nouveau jouet bien reçu !!

 

J'ai découvert la lib Scheduler pour les processeurs SAM. Elle permet d'avoir plusieurs loop et de gérer le passage de l'une à l'autre avec yield(). Un embryon de système multi-tâche.

#include <Scheduler.h>

uint32_t timestamp;
uint32_t timestamp1;

void setup() {
  Serial.begin(115200);
  timestamp = timestamp1 = millis();
  Scheduler.startLoop(loop1);
}

void loop() {
  Serial.println("loop");
  if(timestamp + 100 < millis()) {
    timestamp = millis();
    Serial.println("===============Timer");
  }
  yield();
}

void loop1() {
  Serial.println("loop1");
  if(timestamp1 + 50 < millis()) {
    timestamp1 = millis();
    Serial.println("===============Timer1");
  }
  yield();
}

Sur moniteur série :

Spoiler

loop
loop1
loop
loop1
===============Timer1
loop
===============Timer
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
===============Timer1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
loop
loop1
===============Timer1

 

Je vais le tester pour prioriser la lecture du port du lidar sur le reste du code. Pour lire aussi souvent que nécessaire le petit buffer du port série qui est de 128 octets. On peut saucissonner les grosses fonctions mais je trouve ça plus classe.

[Serveurperso]

C'est la base, c'est la même chose que la lib Simple timer sur Arduino, d’ailleurs c'est pas propre a une architecture c'est un design de base en embarqué

http://playground.arduino.cc/Code/SimpleTimer

[Path]

Je connais ^^ Mais y a pas le yield !! Ça oblige à découper les fonction à lendroit même où on place cette commande. Je trouve ça joli.

Jai vu quon pouvait aussi utiliser des interruptions sur l'uart. Je vais jeter un oeil.

[Serveurperso]

Ah tiens oui le yield permet de faire du temps réel caca:) pas eu besoin de ça moi:) par contre je perd de la puissance

[Path]

Ce matin, une petite exploration du PWM de la Due. Le RPLidar a besoin d'une fréquence à 25kHz. La fréquence par défaut qu'on obtient avec analogWrite() est de 1kHz.

 

L'arduino Due fonctionne avec 8 canaux hardware indépendants pour lesquels on peut configurer la fréquence. Ces canaux sont reliés physiquement à d'autres pins que ceux désignés sur la carte (pin 2 à 13). On peut donc obtenir un signal PWM hardware sur d'autres pins (si l'envie nous prend de). Le paramétrage de ces canaux est un peu compliqué. voir https://forum.arduin...?topic=453208.0 ou http://www.kerrywong.com/2014/09/21/on-arduino-due-pwm-frequency/

 

Je suis tombé sur une lib faite par des étudiants https://github.com/antodom/pwm_lib qui fait ça pour nous. Il suffit de poser le code dans un sous dossier pwm_lib le library arduino.

 

Petit programme de test qui fait cohabiter analogWrite et la lib. On obtient un signal à 1kHz sur le pin 12 et un autre à 25kHz sur le pin 6.

#include "pwm_lib.h"

using namespace arduino_due::pwm_lib;

// pour le lidar -> 25kHz et 60% duty
// PWML7_PC24  // PWM_CH7 -> Le pin est le seul sur ce channel -> pin6
#define PWM_PERIOD_PIN_6 4000 // hundredth of usecs (1e-8 secs)
#define PWM_DUTY_PIN_6 2400 // hundredth of usecs (1e-8 secs)

pwm<pwm_pin::PWML7_PC24> pwm_pin6;

void setup() {
  pwm_pin6.start(PWM_PERIOD_PIN_6,PWM_DUTY_PIN_6);
  pinMode(12, OUTPUT);
  analogWrite(12, 100);
}

void loop() {}

[Serveurperso]

ça c'est top car sur le PIC32 j'ai pas réussi à configurer un PWM différent pour le MOTOCTL du lidar. car 25K pour les mosfets des contrôleurs moteur c'est trop et j'en ai déjà fait fumé un à cause de ça (tout à 25KHz) et le MOTOCTL ne fonctionne pas à 20KHz les tanches !

 

Et un mosfet qui traîne à la commutation sur un H-bridge ça met le feu direct ! heureusement j'ai des fusibles partout maintenant lol

[Path]

Voilà, j'ai un premier jet de mon pilote du RPLidar pour l'Arduino Due.

C'est brut, pas encore testé ni optimisé. (Il me manque un outil de debug pour visualiser la forme des points captés.) Mais je résiste pas à partager pour échanger sur ma compréhension de ce fameux bit de signe en cpp 

 

A la base, cette valeur est donnée en Q3 sur 6 bits (5 bits de données plus le bit de signe). Je la mets directe en Q5 pour donner son sens au bit de signe dans un entier signé. Quand le cpu repasse à un entier de 32bits, le signe est propagé sur les 3 octets qu'il ajoute en poids fort.

int8_t deltaAngle1Q5 = ((byte0 & B00000011) << 6) | ((byte4 & 0x0F) << 2);

Le code complet :

Spoiler

#include <Scheduler.h>
#include "pwm_lib.h"

using namespace arduino_due::pwm_lib;

// pour le lidar -> 25kHz et 60% duty
// PWML7_PC24  // PWM_CH7 -> Le pin est le seul sur ce channel -> pin6
#define PWM_PERIOD_PIN_6 4000 // hundredth of usecs (1e-8 secs)
#define PWM_DUTY_PIN_6 2400 // hundredth of usecs (1e-8 secs)

// pour le driver moteur
pwm<pwm_pin::PWML7_PC24> pwm_pin6;

#define RESPONSE_HEAD_SIZE 7
#define CABIN_HEAD_SIZE 4
#define CABINS_SIZE 80

uint8_t RXResponseBuffer[RESPONSE_HEAD_SIZE];
uint8_t RXHeadBuffer[CABIN_HEAD_SIZE];
uint8_t RXCabinsBuffer[CABINS_SIZE];

const uint8_t requestStop[2] = {0xA5, 0x25};
const uint8_t requestExpressScan[9] = {0xA5, 0x82, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x22};
const uint8_t responseHead[RESPONSE_HEAD_SIZE] = {0xA5, 0x5A, 0x54, 0x00, 0x00, 0x40, 0x82};

uint8_t bytesRead = 0;

struct HeadStruct {
	int8_t sync1; // New packet marker
	int8_t sync2;
	float startAngle; // current packet's startAngle and next's one are needed for current cabins
	uint8_t checkSum; // current packet's checksum
	uint8_t sum; // current head's sum 
	bool startFlag; // if true, current packet should be ignored
} head;

float startAngle;
uint8_t checkSum;
uint8_t sum;

struct CabinStruct {
	uint16_t distance1;
	uint16_t distance2;
	float deltaAngle1;
	float deltaAngle2;
} cabin;

struct ScanStruct {
	uint16_t distance;
	float angle;
	int16_t x;
	int16_t y;
};

ScanStruct scans[32];

void stopAll(String message) {
	// Arrête le moteur du lidar
	pwm_pin6.stop();
	// arrête le scan du lidar
	Serial1.write((const char *)requestStop);
	// print debug message
	Serial.println(message);
	//blink led
	digitalWrite(LED_BUILTIN, HIGH);
	// NB. avec le scheduler, le delay agit comme un yield.
	while(1);
}

void setup() {
	// Pour alerter
	pinMode(LED_BUILTIN, OUTPUT);
	digitalWrite(LED_BUILTIN, LOW);

	// Serial : 0 (RX) and 1 (TX) => Debug USB
	Serial.begin(115200);
	// Serial 1: 19 (RX) and 18 (TX) => RP Lidar
	Serial1.begin(115200);
	while(!Serial1);
	// Start PWM RP Lidar rotor
	pwm_pin6.start(PWM_PERIOD_PIN_6,PWM_DUTY_PIN_6);
	// Send express scan header
	Serial1.write((const char *)requestExpressScan);
	Serial1.setTimeout(3000);
	// read response
	bytesRead = Serial1.readBytes(RXResponseBuffer, RESPONSE_HEAD_SIZE);
	// test number of bytes read
	if(bytesRead != RESPONSE_HEAD_SIZE) stopAll("Timeout while reading express scan response head");
	// test response head value
	for(uint8_t i = 0; i++; i < RESPONSE_HEAD_SIZE) {
		if(responseHead[i] != RXResponseBuffer[i]) stopAll("Wrong express scan response head");
	}
	// read 1st cabin head
	bytesRead = Serial1.readBytes(RXHeadBuffer, CABIN_HEAD_SIZE);
	if(bytesRead != CABIN_HEAD_SIZE) stopAll("Timeout while reading cabin head");
	parseHead();
	if(head.sync1 != 0xA || head.sync2 != 0x5) stopAll("First two bytes should be a new packet");

	Scheduler.startLoop(lidarRXParserLoop);
}

void parseHead() {
	uint8_t byte0 = RXHeadBuffer[0];
	uint8_t byte1 = RXHeadBuffer[1];
	uint8_t byte2 = RXHeadBuffer[2];
	uint8_t byte3 = RXHeadBuffer[3];

	head.sync1 = (byte0 & 0xF0) >> 4;
	head.sync2 = (byte1 & 0xF0) >> 4;
	head.checkSum = ((byte1 & 0x0F) << 4) | (byte0 & 0x0F);
	head.startAngle = (((byte3 & B01111111) << 8) | byte2) / 64;
	head.startFlag = !!byte3 & B10000000;
	head.sum = byte2 ^ byte3;
}

void parseCabin(uint8_t cabinNumber) {
	uint8_t byte0 = RXCabinsBuffer[5*cabinNumber+0];
	uint8_t byte1 = RXCabinsBuffer[5*cabinNumber+1];
	uint8_t byte2 = RXCabinsBuffer[5*cabinNumber+2];
	uint8_t byte3 = RXCabinsBuffer[5*cabinNumber+3];
	uint8_t byte4 = RXCabinsBuffer[5*cabinNumber+4];

	sum ^= byte0;
	sum ^= byte1;
	sum ^= byte2;
	sum ^= byte3;
	sum ^= byte4;

	cabin.distance1 = (byte1 << 6) | ((byte0 & B11111100) >> 2);
	cabin.distance2 = (byte3 << 6) | ((byte2 & B11111100) >> 2);

	int8_t deltaAngle1Q5 = ((byte0 & B00000011) << 6) | ((byte4 & 0x0F) << 2);
	int8_t deltaAngle2Q5 = ((byte2 & B00000011) << 6) | ((byte4 & 0xF0) >> 2);

	cabin.deltaAngle1 = deltaAngle1Q5 / 32;
	cabin.deltaAngle2 = deltaAngle2Q5 / 32;
}

float angleDiff(float angle1, float angle2) {
	if (angle1 <= angle2) return (angle2 - angle1);
	return (360 + angle2 - angle1);
}

float computeAngle(uint8_t k, float angle1, float angle2, float delta) {
	return angle1 - delta + (angleDiff(angle1, angle2) * k / 32);
}

void lidarRXParserLoop() {
	
	if(Serial1.available() >= CABINS_SIZE + CABIN_HEAD_SIZE) {

		// Let's light the led while lidar reading and computing
		digitalWrite(LED_BUILTIN, HIGH);		

		// read cabins
		bytesRead = Serial1.readBytes(RXCabinsBuffer, CABINS_SIZE);
		if(bytesRead != CABINS_SIZE) stopAll("Timeout while reading cabin");
		
		// read next head needed by angle computation
		bytesRead = Serial1.readBytes(RXHeadBuffer, CABIN_HEAD_SIZE);
		if(bytesRead != CABIN_HEAD_SIZE) stopAll("Timeout while reading cabin head");
		
		// preserve current values from head
		startAngle = head.startAngle;
		checkSum = head.checkSum;
		sum = head.sum;

		// parse next header
		parseHead();
		if(head.sync1 != 0xA || head.sync2 != 0x5) stopAll("First two bytes of head should be a new packet");
		// Now head contains next header values.
		
		// Let's ignore previous cabins if start flag is reset
		if(head.startFlag == false) {

			for(uint8_t cabinNumber = 0; cabinNumber++; cabinNumber < 16) {

				parseCabin(cabinNumber);
				
				ScanStruct* scanPtr = &scans[cabinNumber*2];
				scanPtr->distance = cabin.distance1;
				scanPtr->angle = computeAngle(cabinNumber*2, startAngle, head.startAngle, cabin.deltaAngle1);
				scanPtr->x = scanPtr->distance * cos(scanPtr->angle * 3.14159/180);
				scanPtr->y = scanPtr->distance * sin(scanPtr->angle * 3.14159/180);

				Serial.print("SCAN:");
				Serial.print(scanPtr->x, 2);
				Serial.print(":");
				Serial.println(scanPtr->y, 2);

				scanPtr = &scans[cabinNumber*2+1];
				scanPtr->distance = cabin.distance2;
				scanPtr->angle = computeAngle(cabinNumber*2+1, startAngle, head.startAngle, cabin.deltaAngle2);
				scanPtr->x = scanPtr->distance * cos(scanPtr->angle * 3.14159/180);
				scanPtr->y = scanPtr->distance * sin(scanPtr->angle * 3.14159/180);

				Serial.print("SCAN:");
				Serial.print(scanPtr->x, 2);
				Serial.print(":");
				Serial.println(scanPtr->y, 2);
			}

			// test checkSum
			if(sum != checkSum) stopAll("Wrong checkSum");
		}

		digitalWrite(LED_BUILTIN, LOW);
	}

	yield(); // On rend la main seulement après les calculs du lidar
}

uint32_t timestamp = 0;
void loop() {
	// debug "loop" toutes le secondes
	if(timestamp == 0) timestamp = millis();
	if(timestamp + 1000 < millis()) {
		timestamp = millis();
		Serial.println("=================== loop ===================");
	}
	yield();
	yield();
	yield();
	yield();
}

[Serveurperso]

Tu peux voir que j'ai fait la même chose sous une forme différente pour la récupération directe du signe du deltaAnglesQ3 :

On fait un benchmark ? lol

 

Spoiler

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;

 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;

      // Ici détection du passage par l'angle zéro
      // Ici remise à zéro de l'index du tableau de stockage final

     }
     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

      //
      // Ici distances[i] et angle sont OK !
      // Ici arithmétique entière et stockage des résultats dans un tableau global
      //

     }

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

  }
 }

}

Modifié par Path, 05 août 2018 - 04:58 .
Ajout de la balise [spoiler] [/spoiler]