Feuille1

/* Mesure du niveau de carburant (méthanol) dans le réservoir par mesures capacitives

Compatible via R = 4.7K avec le S_PORT des récepteurs Frsky des séries : X, S, ...
Compatible avec les récepteurs de la série D sur la sortie PWM,
ajouter un filtre passe-bas fc = 100 Hz (R = 4.7K, C = 2.2µF) et raccorder à l'entrée A2 du récepteur
Utilisation d'un GY 521 MPU 6050 pour faire une mesure d'attitude de l'avion et mesurer le niveau du carbu
Possibilité de mesurer le temps de la boucle principale pour le rafraichissement des données du S_PORT (
Les données relatives à chaque avion doivent être définies dans le fichier config.h
Suivre les commentaires dans le code pour explorer les différentes fonctions : calibration, PWM, S_PORT e
Un script LUA complémentaire est disponible sur le site https://superair78.wordpress.com/ pour visualiser le

Avionic78
version : Test72_SPORT_3108

Sur les idées de :

New beta version of openXsensor (=openXvario)
https://openrcforums.com/forum/viewtopic.php?f=86&t=6080&p=145190&hilit=openxsensor#p145190
Capacimètre spécial faibles valeurs, de 0.1pF à 100pF de Christophe Juillet
https://forum.arduino.cc/index.php?topic=517276.0
Accéléromètre et gyroscope MPU6050
http://gilles.thebault.free.fr/spip.php?article32
FrSky S-Port telemetry library - easy to use and configurable
https://www.rcgroups.com/forums/showthread.php?2245978-FrSky-S-Port-telemetry-library-easy-to-use-an

Branchements :

Arduino pin | MPU6050

Vcc (5V) | Vcc
GND | GND
A4 | SDA
A5 | SCL
2 | INT

Arduino pin | Rx
4 (via 4.7K) | S_PORT
6 (via filtre) | PWM
Vcc | Vcc
GND | GND

Si NiMH avec 4 éléments, alimenter l'Arduino par la broche Vcc plutôt que par la broche RAW (diminue la c

Arduino pin | Tank

A0 (via 2M) | Wire_1
GND | Wire_2

Le condensateur est relié à la masse et à la broche A0

Arduino pin | R1 (2 M)
A0 | R1 (one of the two pins) to be connected to Wire_1
A1 | R1 (the other pin)

La résistance R1 est reliée à la broche A0 et à la broche A1

A0 broche de lecture du condensateur

Page 1
 Feuille1
A1 broche de charge du condensateur
Seules les broches A0 et A1 sont utilisées
*/

// déclaration des libairies

#include "config.h"
#include "Wire.h" // Arduino Wire library
#include "I2Cdev.h" //Installer ces 2 librairies
#include "MPU6050.h"
#include "math.h"
#include "FrSkySportSensor.h"
#include "FrSkySportSensorFcs.h"
#include "FrSkySportSingleWireSerial.h"
#include "FrSkySportTelemetry.h"

FrSkySportSensorFcs fcs; // Create FCS-40A sensor with default ID (use ID8 for FCS-150A)
FrSkySportTelemetry telemetry;

MPU6050 accelgyro;
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69

// déclaration des constantes

const float R1 = 2000.; // valeur de la résistance de charge en KOhms, ici 2 MOhms
const float K = 0.059; // valeur de la constante de temps (à ajuster éventuellement)
const char pwm_pin = 6; // sortie pwm sur la pin 6, f = 980 Hz
const float tension_max = 3.;

// déclaration des variables pour le calcul du temps de boucle

float t_initial = 0;
float t_final = 0;
float duree = 0;

// déclaration des variables pour l'IMU

int ax = 0;
int ay = 0;
int az = 0;
int gx = 0;
int gy = 0;
int gz = 0;
float angleInitialX = 0.;
float angleInitialY = 0.;
float angleResultX = 0.;
float angleResultY = 0.;

//déclaration des variables pour la mesure de capacité

float capa = 0.; // valeur de la capacité mesurée
float tension_sortie = 0.; // tension de sortie VFAS (3.3V max)
int attitude = 0; // angles de roulis et de tangage compatibles pour la mesure de niveau dans le réservoir

void setup()
{
Serial.begin(115200);

Page 2
 Feuille1
initialize_DMP();
initialize_capa();
//calibration();
//printData_calibration(); // offset et capacité parasite de calibration à indiquer dans config.h
initialize_sport();
}

void loop()
{
//t_initial = millis();
//capa_value(); // pour debug
angle_value();
test_angle();
sport_output();
//pwm_output();
//printData_capa(); // pour debug
//printData_DMP(); // pour debug
//delay(10); // pour debug (mettre à 500 pour mesure de capacités sinon 10 maxi pour mesure d'angles)
//t_final = millis();
//time_loop();
}

void initialize_DMP()
{
Wire.begin(); //I2C bus
while (!Serial) {;}// wait for serial port to connect. Needed for native USB (LEONARDO)
// initialize device
accelgyro.initialize();
}

void initialize_capa()
{
TCCR1A = 0; // Timer 1 en mode compteur
TCCR1B = 1; // prédiviseur du timer 1 à 1
pinMode(A1, OUTPUT);
}

void calibration()
{
float offset;
offset = mesure(65536L);
offset /= 16;
capa_fil = offset / (float) R1 * K;
}

void initialize_sport()
{
// Configure the telemetry serial port and sensors (remember to use & to specify a pointer to sensor)
telemetry.begin(FrSkySportSingleWireSerial::SOFT_SERIAL_PIN_4, &fcs); // sortie SPORT sur la pin 4
}

void angle_value()
{

Page 3
 Feuille1
static float angleX = 0.;
static float angleY = 0.;
static float currentTime = 0.;
float previousTime = 0.;
float elapsedTime = 0.;
const float elapsed_time = 5000.;
const float conv_rad_deg = 180./PI;

previousTime = currentTime;
currentTime = millis();
elapsedTime = (currentTime - previousTime)/1000.;

accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

angleX=0.98*(angleX+float(gx)*elapsedTime/131) + 0.02*atan2((double)ay,(double)az)*conv_rad_deg; // m
angleY=0.98*(angleY+float(gy)*elapsedTime/131) + 0.02*atan2((double)ax,(double)az)*conv_rad_deg; // m

if (currentTime <= elapsed_time)

{
angleInitialX = angleX;
angleInitialY = angleY;
}
if (currentTime > elapsed_time)
{
angleResultX = diffAngle(angleX, angleInitialX);
angleResultY = diffAngle(angleY, angleInitialY);
}
}

void capa_value()
{
const float elapsed_time = 5000.;
const float tension_ini = 3.3;
const float coef = tension_max / capa_max;
const unsigned imax = 2048;
const unsigned jmax = 4; // on séquence le calcul pour les contraintes de temps réel de la boucle principale
const int kmax = imax/jmax;
static int k = 1;
static long cumul = 0;
static float tension_pic = 0.; // tension avec le niveau de carburant en phase d'initialisation qui ne peut qu

long tt = mesure(jmax);
if (k < kmax)
{
cumul += tt;
k++;
}
else
{
tt = cumul;
tt <<= 1; // décalage à gauche de 1 pour imax = 2048, ce qui revient à multiplier par 2
capa = tt / (float) R1 * K - capa_fil;
cumul = 0;
k = 0;

Page 4
 Feuille1
}

if (millis() < elapsed_time) // attente de 5s

{
tension_sortie = tension_ini; // initialisation à la mise sous tension (tension_sortie max = 3.3 V
tension_pic = tension_max * (capa - capa_min) / (capa_max - capa_min); // niveau de carbur
tension_pic = 3.;

if (tension_pic < 0)
{
tension_pic = 0.;
}
if (tension_pic > tension_max)
{
tension_pic = tension_max;
}
}
else
{
//tension_sortie = capa * coef; // tension de sortie pour debug
tension_sortie = tension_max * (capa - capa_min) / (capa_max - capa_min); // tension de sor

if (tension_sortie < 0)
{
tension_sortie = 0.;
}

if (tension_sortie > tension_max)

{
tension_sortie = tension_max;
}

if (tension_sortie > tension_pic)

{
tension_sortie = tension_pic;
}

if (tension_sortie <= tension_pic)

{
tension_pic = tension_sortie; // le niveau de carburant ne peut que décroître a
}
}
}

long mesure(long jmax)

{
long tt = 0;
int t;

pinMode(A0, OUTPUT);
delayMicroseconds(100); // décharge le condensateur
for (long j = 0; j < jmax; j++) // on répète la mesure afin d'améliorer la précision
{

Page 5
 Feuille1
pinMode(A0, INPUT);
byte oldSREG = SREG;
cli();
bitSet(PORTC, 1); // broche A1 HIGH
TCNT1 = 0;
asm volatile( "rjmp boucle\
n
);
t = TCNT1;
SREG = oldSREG;
digitalWrite(A1, LOW); // décharge le condensateur
pinMode(A0, OUTPUT);
tt += t / 3;
}
pinMode(A0, INPUT);
return tt;
}

float diffAngle(float x,float y)

{
float res = 0.;
float delta = 0.;
float delta_c = 0.;
delta = abs(x -y);
delta_c = 360 - delta;
if (delta < delta_c)
{
res = delta;
}
else
{
res = delta_c;
}
return res;
}

void test_angle()
{
attitude = 0;
const float angleMax = 30.;

if ((angleResultX < angleMax) && (angleResultY < angleMax))

{
attitude = 1;
capa_value();
}
else
{
pinMode(A0, OUTPUT); //on maintient déchargé le condensateur
digitalWrite(A1, LOW); // 0V aux bornes de R1
delayMicroseconds(100);
}
}

Page 6
 Feuille1

void sport_output()
{
fcs.setData(attitude,tension_sortie); // Tension de sortie
telemetry.send(); // Send the telemetry data, note that the data will only be sent for sensors that are being p
}

void pwm_output()
{
float tension_alim = 5.;
byte duty_cycle = 0;

duty_cycle = byte(tension_sortie / tension_alim * 255);

analogWrite(pwm_pin, duty_cycle);
}

void time_loop() // pour debug

{
static int l = 1;
int lmax = 1000;
if (l < lmax)
{
duree = duree + (t_final - t_initial);
l++;
}
else
{
duree = duree / lmax;
l = 0;
Serial.print(F("Durée boucle (ms) = "));
Serial.print(duree, 2);
Serial.println();
}
}

void printData_calibration()
{

Serial.print(F("Capacité parasite = "));

Serial.print(capa_fil, 1);
Serial.println(F(" pF"));
Serial.println();
}

void printData_capa()
{
Serial.print(F("Attitude = "));
Serial.print(attitude);
Serial.println();

Serial.print(F("Capacite (pF) = "));

Serial.print(capa,1);
Serial.println();

Page 7
 Feuille1

Serial.print(F("Tension_sortie (V) = "));

Serial.println(tension_sortie,2);
Serial.println();
}

void printData_DMP()
{
Serial.print(F("Angles (deg) : "));
Serial.print(F("angleInitialX = "));
Serial.print(angleInitialX);
Serial.print(F(", "));
Serial.print(F("angleInitialY = "));
Serial.print(angleInitialY);
Serial.println();
Serial.print(F("angleResultX = "));
Serial.print(angleResultX);
Serial.print(F(", "));
Serial.print(F("angleResultY = "));
Serial.println(angleResultY);
Serial.println();
}

Page 8
 Feuille1

à l'entrée A2 du récepteur
vion et mesurer le niveau du carburant dans la plage d'inclinaisons autorisées
ement des données du S_PORT (timing critique)

ons : calibration, PWM, S_PORT et debug : affichage, delay, mesure non séquencée de la capacité, temps de boucle
.wordpress.com/ pour visualiser le niveau de carburant testé sur une Taranis X9D Plus

&hilit=openxsensor#p145190

t-telemetry-library-easy-to-use-and-configurable

e par la broche RAW (diminue la chute de tension du régulateur)

Page 9
 Feuille1

iveau dans le réservoir

Page 10
 Feuille1

quer dans config.h

10 maxi pour mesure d'angles)

LEONARDO)

specify a pointer to sensor)

s); // sortie SPORT sur la pin 4

Page 11
 Feuille1

ay,(double)az)*conv_rad_deg; // mesure de l'angle X

ax,(double)az)*conv_rad_deg; // mesure de l'angle Y

temps réel de la boucle principale (mettre à 2048 pour debug capacité sinon 4)

hase d'initialisation qui ne peut que décroître

ui revient à multiplier par 2

Page 12
 Feuille1

ension (tension_sortie max = 3.3 V)

x - capa_min); // niveau de carburant lors de la phase d'itialisation

max - capa_min); // tension de sortie normalisée

carburant ne peut que décroître au cours du temps

Page 13
 Feuille1

e sent for sensors that are being polled at given moment

Page 14
 Feuille1

emps de boucle

Page 15