2

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

Here is the result using Timer (stop at about 0.5 second): enter image description here

2
  • Using Serial.print inside the timer interrupt routine is not advised. It also doesn't look like it is doing what you actually want, and that is to measure every 10 ms. It just reports the last measurement every 10 ms (which might be stale). For now try increasing the baud-rate of the serial connection, and see if that helps.
    – Gerben
    Feb 21, 2017 at 20:08
  • I increased baud-rate, it still stops in the middle (but the program can print results longer). Can you suggest any modification of the code to achieve my objective? (I tried using millis() but it still stops in the middle). Feb 22, 2017 at 9:56

1 Answer 1

0

As noted in the last part of Nick Gammon's answer to the question How does the Arduino handle serial buffer overflow?, if the serial output buffer fills up, serial output blocks, waiting for an interrupt telling it it's ok to send the next byte out the serial port. When interrupts are turned off, that interrupt is never processed, so the program deadlocks. Don't do Serial prints in an ISR.

Looking at your code, it looks like a typical message has about 20 characters. Sending at 38400 bps, or 384 characters per second, it seems to me the buffer shouldn't fill, even if it's small (16 bytes) instead of large (64 bytes). However, I don't know of another explanation for the behavior you describe.

Summary: In your ISR, just store the current time and other relevant data in volatile variables, set a flag in a volatile variable, and in your main loop print stuff and clear the flag (or clear the flag and print stuff) whenever you see that the flag has been set.


Edit 1: I've struck out some erroneous numbers above. Here is a correction: 100 sends per second at 20 characters each is 2000 characters per second. 38400 bps of asynchronous data at 10 bits per transmitted byte is 3840 characters per second. So the raw data rate should be fast enough that the buffer ordinarily doesn't overfill.


In the summary above, I suggested that the ISR store the current time and other relevant data in volatile variables and set a flag in a volatile variable. That is, read millis() and store it, perhaps as follows:

// at file level ...
volatile unsigned long isrMillis;
volatile byte isrFlag;

// in ISR ...
  isrMillis = millis();
  isrFlag = true;

// in main loop ...
  unsigned long mlMillis;
  ...
  // when flag is true...
  cli();
  mlMillis = isrMillis;
  isrFlag = false;
  sei();
  // Now do stuff with mlMillis
  ...

However, looking at your ISR, perhaps it should go like the following instead:

// at file level ...
volatile float xf, yf, xfc, yfc;
volatile unsigned long t1, t2, tdelt;
volatile byte isrflag;

// in ISR ...
void myHandler(){
  tdelt = t2-t1;
  xfc = xf;
  yfc = yf;   
  isrFlag = true;    
}

// in main loop ...
  // when flag is true...
  isrFlag = false;
  // Now do stuff with tdelt, xfc, and yfc
  ...

Note that if an interrupt occurs while xf and yf are in the middle of being computed via your Kalman filter or whatever, the statements xfc = xf; and yfc = yf; may pick up partially-stored results. You can avoid that problem at least in part by protecting stores of t1, t2, xf, yf in the main loop:

  cli();
  xf = someXResult;
  yf = someYResult;  
  t1 = someT1Result;
  t2 = someT2Result;  
  sei(); 

In the above I've pointed out a few issues with races and concurrency and suggested technical solutions. Those suggestions may be quite wide of the mark rather than appropriate. In general I don't have a precise understanding of what you are trying to do with the software and don't know what redesign should be done to make it coherent.

2
  • As you said, I will set the timer for 10ms, and print stuff in loop() or another function. But my typical message is about 20 characters, which mean each time printing, it will take me ~5.2ms to print all. I'm wondering if I have to adjust the time setting for Timer or not? I want to be as close to 10ms as possible to match my sensor reading with another external reading. Feb 22, 2017 at 9:44
  • @NguyễnThanhVũ, see edit Feb 22, 2017 at 18:15

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.