I'm writing a code to print/update the value of sensors for each 10 ms, and I decide to use Timer instead of delay function (I'm afraid it's not exactly 10ms with delay function).But when I watch the result on terminal, it stops running at about 0.5s. I guess there is a problem with my code in using Timer because when I remove timer, it prints normally. I debug all day but still cannot find any errors. I guess this is related to memory or pointer or print function.
Please help me :( !!
Here is the code. The code is very long but you can ignore all the unnecessary things:
#include <DueTimer.h>
int mod_11k = 12;
int mod_17k = 13;
int analog1 = 0; //output pin of signal 1
int analog2 = 1; //output pin of signal 2
int analog3 = 2; //output pin of signal 3
float xf; float yf;
unsigned long t1;
unsigned long t2;
void myHandler(){
Serial.print(t2-t1);
Serial.print(" ");
Serial.print(xf,3);
Serial.print(" ");
Serial.println(yf,3);
}
void setup() {
Serial.begin(38400);
pinMode(mod_11k,OUTPUT);
pinMode(mod_17k,OUTPUT);
Timer1.attachInterrupt(myHandler).start(10000);
digitalWrite(mod_17k,LOW);
digitalWrite(mod_11k,HIGH);
}
void loop() {
afficherSig(analog1,analog2,analog3);
}
void afficherSig(int pin1, int pin2, int pin3){
unsigned int i;
float mes[3];
float mesflt[3];
float x; float y;
int nb_ech = 10000;
float res = 0.0049;
float Te;
float Tbeg;
float QR1[2],QR2[2] ;
float *sol1;float *sol2;float *sol3;
float P1[4];float P2[4];float P3[4];
float xprec1[2];float xprec2[2];float xprec3[2];
float *s1_11k;
float *s1f_11k;
float H = 80;
//------------------------------>>>
QR1[0] = 1;
QR1[1] = 10;
QR2[0] = 1;
QR2[1] = 100;
P1[0] =QR1[0]; P1[1] =0; P1[2] =0; P1[3] =QR1[0];
P2[0] =QR1[0]; P2[1] =0; P2[2] =0; P2[3] =QR1[0];
P3[0] =QR1[0]; P3[1] =0; P3[2] =0; P3[3] =QR1[0];
xprec1[0] =0; xprec1[1] =0;
xprec2[0] =0; xprec2[1] =0;
xprec3[0] =0; xprec3[1] =0;
//------------------------------>>>
t1 = millis();
Tbeg= millis();
mes[0] = res*analogRead(analog1);
mes[1] = res*analogRead(analog2);
mes[2] = res*analogRead(analog3);
xprec1[0] = mes[0];
xprec2[0] = mes[1];
xprec3[0] = mes[2];
Te = (millis() - Tbeg)/1000;
Tbeg = millis();
sol1 = Fkalman(xprec1,mes[0],P1,QR1,Te);
sol2 = Fkalman(xprec2,mes[1],P2,QR1,Te);
sol3 = Fkalman(xprec3,mes[2],P3,QR1,Te);
//------------------------------->>>
for(i=1;i<nb_ech;i++) {
P1[0] =sol1[2]; P1[1] =sol1[3]; P1[2] =sol1[4]; P1[3] =sol1[5];
P2[0] =sol2[2]; P2[1] =sol2[3]; P2[2] =sol2[4]; P2[3] =sol2[5];
P3[0] =sol3[2]; P3[1] =sol3[3]; P3[2] =sol3[4]; P3[3] =sol3[5];
xprec1[0] =sol1[0]; xprec1[1] =sol1[1];
xprec2[0] =sol2[0]; xprec2[1] =sol2[1];
xprec3[0] =sol3[0]; xprec3[1] =sol3[1];
free(sol1);
free(sol2);
free(sol3);
mes[0] = res*analogRead(analog1);
mes[1] = res*analogRead(analog2);
mes[2] = res*analogRead(analog3);
Te = (millis() - Tbeg)/1000;
Tbeg = millis();
sol1 = Fkalman(xprec1,mes[0],P1,QR1,Te);
sol2 = Fkalman(xprec2,mes[1],P2,QR1,Te);
sol3 = Fkalman(xprec3,mes[2],P3,QR1,Te);
mesflt[0] = sol1[0];
mesflt[1] = sol2[0];
mesflt[2] = sol3[0]; // Envoie des résultats à la sortie série
//Traitment du signal
s1_11k = traitement1(mes[0],mes[1],mes[2]);
s1f_11k = traitement1(mesflt[0],mesflt[1],mesflt[2]);
x = H*s1_11k[0];
xf = H*s1f_11k[0];
y = -H*s1_11k[1];
yf = -H*s1f_11k[1];
t2 = millis();
}
}
float *Fkalman(float xprec[2],float ymes,float P[4],float QR[2] ,float Te){
// Filtre proprement dit
float *sol = NULL ;
sol = (float*) malloc (sizeof(float) * 6);
float xpred[2];
float Ppred[4] ;
float Kf[2] ;
int q = QR[0];
int r = QR[1];
xpred[0] = xprec[0] + Te*xprec[1];
xpred[1] = xprec[1];
Ppred[0] = q*q + P[0]+ Te*(P[2] +P[1]+Te*P[3]);
Ppred[1] = P[1] + Te*P[3];
Ppred[2] = P[2] + Te*P[3] ;
Ppred[3] = q*q+ P[3];
Kf[0] = (Ppred[0]/(r+Ppred[0]));
Kf[1] = (Ppred[2]/(r+Ppred[0]));
sol[0] = xpred[0] + Kf[0]*(ymes-xpred[0]);
sol[1] = xprec[1] + Kf[1]*(ymes-xprec[0]);
sol[2] = Ppred[0]*(1-Kf[0]);
sol[3] = Ppred[1]*(1-Kf[0]);
sol[4] = -Kf[1]*Ppred[0]+Ppred[2];
sol[5] = -Kf[1]*Ppred[1]+Ppred[3];
return sol;
}
float *traitement1(float mes1,float mes2,float mes3)
{
float *sol11 = NULL ;
sol11 = (float*) malloc (sizeof(float) * 2);
sol11[0] = (mes1-mes2)/(mes1+mes2);
sol11[1] = (((mes1+mes2)/2)-mes3)/(((mes1+mes2)/2)+mes3);
return sol11;
}