ATTENTION !! cette description est en cours de révision, ne fonctionne pas correctement avec le composant CMPS12

PILOTE PLASTIMO AT50 (S)

RESTAURATION DU COMPAS AVEC UNE BOUSSOLE ELECTRONIQUE

Le pilote Plastimo AT50 est à l'origine de conception simple et purement analogique :

On obtient ainsi un asservissement de type proportionnel : une erreur de cap se traduit par un déplacement proportionnel du vérin.

Les problèmes rencontrés sur cet équipement sont les suivants (expérience personnelle) :

  1. corrosion interne (la tige du vérin n'est pas étanche avec l'usure) : mauvais contacts, défauts électriques dans l'électronique, corrosion du moteur
  2. usure des balais du moteur
  3. détérioration des contacts des relais (problème supprimé avec l'utilisation de transistors MOS, version S)
  4. mauvaise tenue mécanique du compas sur le corps du pilote (2 petites vis dans une gorge en plastique fragile)
  5. sur un voilier mal équilibré à certaines allures (au près par vent fort = angle de barre requis) le cap suivi est imprécis
  6. toujours dans le cas d'un angle de barre important, la course limitée impose de décaler le pilote pour positionner la course utile du vérin du bon côté
  7. rupture des certaines pièces de structure en plastique
  8. usure et rupture de courroies du moteur

Ceci dit c'est un matériel simple accessible aux réparations, et avec 2 pilotes de même type, j'ai toujours pu me débrouiller pour en faire fonctionner un ! Traversée du l'Atlantique sous les alizés sans problème avec un voilier de 10.5m.

Le problème n°5 a été amélioré en insérant des condensateurs dans la connexion de sortie du potentiomètre de recopie : le comportement "proportionnel" est conservé à court terme, mais à long terme seule l'erreur de cap est prise en compte et il n'y a plus d'erreur même avec un angle de barre permanent. L'asservissement devient proportionnel + intégral.

Mais le pilote rescapé a perdu cet été 2020... son compas ! décroché à cause de détériorations de la fragile jupe de la cloche contenant le compas, il a fini sa carrière au fond de la mer...

REPARATION DU COMPAS

Il me restait un autre compas en piteux état (pièces plastique internes cassées, réservoir du fluide compas fendu donc vide, photorésistances HS...).

Plutôt que de tenter une réparation ou trouver une pièce neuve, j'ai eu l'idée de remplacer le compas magnétique par un circuit électronique, tel que celui utilisé sur les drones. Par exemple, un tel circuit, pour un prix inférieur à 30€, comporte un magnétomètre, un accéléromètre et un gyroscope, le tout en 3 axes, avec une intelligence capable de combiner toutes ces informations pour se calibrer, retrouver l'horizontale et la direction (et indiquer si il y a des incohérences).

C'est le cas du CMPS12 qui s'interface facilement avec une carte microprocesseur Arduino basique, ici une Nano largement suffisante.

Le principe adopté (au moins dans un premier temps) est de remplacer l'ancien compas sans aucune modification du reste du pilote : le nouveau compas devra :

La graduation supérieure reste valable et le principe de réglage identique (la direction se lit sur cette graduation vers l'avant du bateau).

En conséquence, on choisit une orientation du capteur nominale, (en fonction de la position du capteur par rapport à la graduation), par exemple le Nord, et le rôle de l'Arduino et de l'électronique associée, va être de calculer l'écart avec cette orientation nominale et de produire une tension variable centrée sur 4V.

La génération de cette dernière tension par l'Arduino passera, pour faire simple, par un signal PWM (signal numérique périodique de rapport cyclique variable) filtré énergiquement (la fréquence d'ondulation d'environ 500Hz devra être bien atténuée sans augmenter le retard au delà de 100ms), puis amplifié de 0 à 5V vers la gamme 0 à 8V.

La calibration du capteur CMPS12 requiert quelques mouvements à la mise sous tension, que l'on ne peut imposer à chaque mise en service du pilote. Heureusement on peut enregistrer cette calibration dans le capteur lui-même, on a donc prévu un poussoir interne effectuant cette enregistrement une fois pour toutes avant fermeture du pilote. Ensuite cette calibration est rechargée à chaque mise sous tension. Le CMPS12 peut indiquer si la calibration est cohérente, cette dernière peut être mise en défaut en particulier si un champ magnétique parasite vient perturber le champ terrestre.

On a donc prévu d'indiquer un défaut de calibration par une LED visible de l'extérieur, placée au centre du compas. Deux régimes de clignotement indiquent la sévérité de l'erreur de calibration. La LED est également utilisée en photodiode lorsqu'elle est éteinte pour moduler la durée d'éclairement et réduire l'éblouissement la nuit.

SCHEMA ELECTRIQUE

Le capteur CMPS12 est placé en mode "serial", et relié aux ports D2/D3, l'Arduino utilise une librairie permettant l'émulation d'un bus SPI.

Le filtre constitué de 3 cellules 22k/220nF atténue de 72dB l'ondulation vers 500Hz du signal PWM, qui s'établit en 60ms à 90%.

Le pont constitué de 2 résistances de 18k évite d'envoyer un signal d'erreur en butée à la mise sous tension (le port D11 est en entrée à la mise sous tension).

L'ampli OP amplifie le signal PWM depuis 0-5V vers 0-8V (le modèle utilisé n'est pas de type "rail-to-rail" et le comportement est un peu amélioré par la résistance de 5.1k).

La LED est isolée par la diode 1N4000, ce qui permet de lire la tension photoélectrique sur le port A0 et d'évaluer la lumière ambiante lorsque la LED est éteinte.

Le poussoir relié à D5 déclenche un enregistrement de la calibration, qui sera appliquée à chaque mise sous tension.

La connexion "POT" (reliée au port A1) n'est pas utilisée pour l'instant, elle pourrait permettre de sophistiquer l'asservissement en prenant en compte la position du vérin, dans ce cas il faudra déconnecter le potentiomètre de recopie de l'électronique d'origine et le relier à "POT".

 

LOGICIEL

#include <SoftwareSerial.h> //librairie pour bus SPI logiciel

#define CMPS_GET_ANGLE8 0x12  //constantes : adresses des données de la boussole
#define CMPS_GET_ANGLE16 0x13
#define CMPS_GET_PITCH 0x14
#define CMPS_GET_ROLL 0x15
#define GET_CALIBRATION_STATE 0x24
#define GET_ACCEL_RAW 0x20

SoftwareSerial CMPS12 = SoftwareSerial(2,3);  //création bus SPI sur les ports 2 et 3

unsigned char high_byte, low_byte, angle8, cal; 
char pitch, roll;
unsigned int angle16;
float cap;  //cap en degrés
int duty = 512; //rapport cyclique PWM 0 à 1023
int PWMpin = 11;  //port PWM
int LEDpin = 13;  //port LED
int plagedeg = 90; //plage totale d'erreur de cap en degrés 
int affserial = 0;  //sortie messages sur port série si 1, seulement accélération si 2
int ledperiod = 10; //période pour clignotement LED calibration
int iled = 0; //index pour affichage LED
int ledcal = 0; //mode affichage LED calibration : 0=OK pas d'affichage
// 1=calibration insuffisante clignotement 1/10
// 2=calibration mauvaise clignotement rapide
int analogLEDpin = 0; //port analogique tension LED
int analogPOTpin = 1; //port analogique tension potentiomètre de recopie
float Vled; //tension LED
int ncycle = 0;
int ncymax = 2; //nombre de loops avant application PWM
int accX, accY, accZ; // accélération 16 bits signés

void setup() //initialisation
{
  pinMode(LEDpin, OUTPUT);  //port LED en sortie    
  digitalWrite(LEDpin, LOW);  //LED éteinte
  pinMode(5,INPUT_PULLUP);   // port poussoir calibration
  pinMode(PWMpin,INPUT);   //port PWM en entrée pour éviter signal d'erreur en butée
  if (affserial) Serial.begin(9600);  // démarrage port série
  CMPS12.begin(9600); //démarrage communication avec boussole
  analogReference(DEFAULT); //référence mesures analogiques = 5V
  }

void loop()
{
  if (ncycle == ncymax)
    {
    pinMode(PWMpin,OUTPUT);   //port PWM en sortie
    analogWrite(PWMpin,512); //met la sortie PWM en service
    }
  CMPS12.write(CMPS_GET_ANGLE16);  // Request and read 16 bit angle
  while(CMPS12.available() < 2);  //attente retour 2 octets
  high_byte = CMPS12.read();  //lecture 2 octets
  low_byte = CMPS12.read();
  angle16 = high_byte;                // Calculate 16 bit angle
  angle16 <<= 8;  //valeur multipliée par 256
  angle16 += low_byte;  //ajout octet bas
  cap = (float) angle16/10; //angle en degrés
  
  CMPS12.write(CMPS_GET_PITCH);   // Request and read pitch value
  while(CMPS12.available() < 1);
  pitch = CMPS12.read();
  
  CMPS12.write(CMPS_GET_ROLL);    // Request and read roll value
  while(CMPS12.available() < 1);
  roll = CMPS12.read();

  CMPS12.write(GET_CALIBRATION_STATE);    //lecture de l'octet de calibration
  while(CMPS12.available() < 1);
  cal = CMPS12.read();  //octet: bits 0&1=magnétomètre 2&3=accéléromètre 4&5=gyroscope 6&7=global

  CMPS12.write(GET_ACCEL_RAW); //acquisition accélération brute
  while(CMPS12.available() < 6);  //attente retour 6 octets
  high_byte = CMPS12.read();  //lecture 2 octets X
  low_byte = CMPS12.read();
  accX = high_byte;  accX <<= 8;  accX += low_byte;
  high_byte = CMPS12.read();  //lecture 2 octets Y
  low_byte = CMPS12.read();
  accY = high_byte;  accY <<= 8;  accY += low_byte;
  high_byte = CMPS12.read();  //lecture 2 octets Z
  low_byte = CMPS12.read();
  accZ = high_byte;  accZ <<= 8;  accZ += low_byte;
  
  char calmag = cal & 0x3;  //calibration capteur magnétique
  char calacc = (cal>>2) & 0x3; //calibration accéléromètre
  char calgyr = (cal>>4) & 0x3; //calibration gyroscope
  char caltot = cal>>6; //calibration globale 0 à 3
  if (caltot>1) ledcal = 1; //si calibration globale au moins 1
  if (caltot==0) ledcal = 2;  //si calibration globale mauvaise
  if (caltot==3) ledcal = 0;  //si calibration globale maximale
  if (ledcal==0)   digitalWrite(LEDpin, LOW); //calibration globale max = LED éteinte
  if (ledcal==2)   digitalWrite(LEDpin, HIGH);  //calibration globale nulle = LED allumée
  if (ledcal==1)  //calibration globale insuffisante
    {
    iled++;
    if (iled>ledperiod) {iled=0; digitalWrite(LEDpin, HIGH);} //allumage LED périodique
    else digitalWrite(LEDpin, LOW); //sinon extinction LED
    }
  int d = (Vled*50-5);  //délai en ms en fonction de Vled (augmente avec l'éclairement)
  if (d<0) d=1; //délai mini = 1ms
  delay(d); //attente (allumage LED)
  digitalWrite(LEDpin, LOW);  //extinction LED
  d=100-d;  //délai complémentaire pour 100ms au total
  if (d<0) d=1; //minimum 1ms
  delay(d); //application du délai complémentaire

  float capsign;  //variable représentant l'erreur de cap par rapport au nord
  if (cap > 180) capsign = cap - 360; else capsign = cap; //négatif en dessous de 360°
  if (capsign > plagedeg/2) capsign = plagedeg/2; //limitation à +/- demi plage
  if (capsign < -plagedeg/2) capsign = -plagedeg/2;
  capsign = -capsign; //inversion du signal d'erreur (pilote à tribord)
  duty = (int)(255*(0.5+capsign/plagedeg)); //conversion erreur de cap en rapport cyclique 0 à 255
  if (duty>255) duty = 255;
  if (duty<0) duty = 0;
  if (ncycle >= ncymax) analogWrite(PWMpin,duty); //application du rapport cyclique
  Vled = (9*Vled+(float)5.0*analogRead(analogLEDpin)/1024)/10; //moyenne de Vled sur 10
  //on mesure Vled alors qu'elle est éteinte (effet photoélectrique)
  if (affserial==1)  //si sortie messages autorisée
    {
    Serial.print("roll=");            // Display roll data
    Serial.print(roll, DEC);
    
    Serial.print(" pitch=");          // Display pitch data
    Serial.print(pitch, DEC);
    
    Serial.print(" angle=");       // Display 16 bit angle with decimal place
    Serial.print(angle16 / 10, DEC);
    Serial.print(".");
    Serial.print(angle16 % 10, DEC);
    Serial.print(" cap=");
    Serial.print(cap);
    Serial.print(" cal=");
    Serial.print(cal, BIN);
    Serial.print(" duty=");
    Serial.print(duty);
    Serial.print(" ledcal=");
    Serial.print(ledcal);
    Serial.print(" Vled=");
    Serial.println(Vled);
    }
  if (affserial==2)  //si sortie accélération
    {
    Serial.print(" acceleration X =");
    Serial.print(accX, DEC);
    Serial.print(" Y =");
    Serial.print(accY, DEC);
    Serial.print(" Z =");
    Serial.println(accZ, DEC);
    }
  
  if (digitalRead(5)==LOW) //demande d'enregistrement de calibration (poussoir appuyé)
    {
    CMPS12.write(0xF0);
    delay(20);
    CMPS12.write(0xF5);
    delay(20);
    CMPS12.write(0xF6);
    delay(20);
   }
   if (ncycle < ncymax+1) ncycle++;
}

TESTS EN NAVIGATION

A ce jour (octobre 2020), seul un petit test sur rivière a été effectué :
  1. tenue du cap correcte, gain un peu trop élevé
  2. anomalies de calibration ponctuelles, et notamment en approchant un téléphone du pilote (champ magnétique du haut-parleur !!), cependant la tenue du cap ne semble en général pas affectée (les gyroscopes prennent le relais ?) 
  3. le pilote part en butée à chaque mise sous tension
Les anomalies 1 et 3 ont été corrigées au niveau matériel et logiciel, à vérifier.
L'anomalie 2 devra être approfondie en navigation, en particulier par mer agitée.
A suivre !!
 
contact (F6HHF) : 
Autres bidouilles