delay function not evaluating properly inside loop condition

Mauvai

Using the following code:

int dt, t;
dt = CLKFREQ*2;
 while(1)
  {
     t = CNT;

//do stuff
    //print("\n%d, %d", CNT-t, dt);
    while(CNT-t < dt)
    {
      pause(1);
    }
}

I expect the code '//do stuff' to execute (assuming it was actual code), and then delay until the loop has run for a full 2 seconds (i.e. CLKFREQ*2) - but it doesnt - as far as i can tell it doesn't even enter the loop (tested by printing text from inside it - never happened).

What's interesting is that if i uncomment the print line, and print CNT-t and dt, not only can i see that CNT-t is much smaller than dt (and thus the loop should be entered), but the loop now enters, and the code behaves as expected.

I suspect that CNT is not being rechecked on each iteration of the loop, though really I have no idea. Ideas?

Dave Hein

I compiled the code with the "-S" option that saves the generated assembly file, and I found that the "CNT-t < dt" comparison is being done with CMP instead of CMPS. So it is doing an unsigned comparison instead of a signed comparison. I'm guessing that CNT is defined as an unsigned int, and this is causing the comparison do be done on unsigned ints. If you change "while(CNT-t < dt)" to "while((int)CNT-t < dt)" the compiler generates a CMPS instruction, and a signed comparison will be done.

Mauvai

No Joy I tried redefining t and dt as unsigned ints, and forced CNT to an unsigned int also, to no avail.

It was my understanding that CNT is internally stored as an unsigned long. Is this wrong?

Dave Hein

Well you didn't do what I suggested, so of course you still get the same result. Change "while(CNT-t < dt)" to "while((int)CNT-t < dt)" like I suggested and it will work.

Mauvai

Apologies, I was unclear: I tried both what you said, and what I said. Neither worked

Dave Hein

Actually, I just realized that it shouldn't matter whether you do a signed comparison or unsigned, as long as CNT-t is less than 2 billion or so. Post all of your code. There must be some other issue with it.

Mauvai

As i said before, uncommenting the line
//print("\n%d, %d", CNT-t, dt);
makes the code behave exactly as expected. I dont think the issue is quite this simple.

There is quite a bit of code... the dofSetup function works, so I'm assuming it can be ignored...

/*Full Attitude and Heading Reference System - based on  sparkfun 9dof 
(obviously running for parallax propeller)


uses a second order complementary filter on the roll and picth, using gryo and
accelerometer data to feed it. 
*/


#include <stdio.h>            // Recommended over iostream for saving space
#include <propeller.h>        // Propeller-specific functions
#include "simpletools.h"


const int AccelAddr  = 0x53;
const int GyroAddr  = 0x68;
const int MagAddr  = 0x1E;


i2c *DofBus;           //create pointer to i2c bus object
const int DofSCL = 2;  //devine SDA and SDL pins for i2c
const int DofSDA = 3;
const float pi = 3.14159265359;
//function declarations


inline int twos2signed(char MSB, char LSB);
void dofSetup();   //cant start function name with 9 :(












//accelerometer calibration - needs to be redone!
const int Bx = 19;
const int By = 4;
const int Bz = 0;


const float Sx = 1.0/263;  //perform division now, as division is lower than multiplication
const float Sy = 1.0/264;
const float Sz = 1.0/256;






//gryroscope calibration. needs to be redone!
const float xGyroConv =  1.0/ 14.375 / 1.065;
const float yGyroConv =  1.0/ 14.375 / 1.0;    //<- this is intentional
const float zGyroConv =  1.0/ 14.375 / 1.035;  // LSBs per (degree per second)
                                     //hyperterminal case 7 tests indicate that the 
                                     //quoted scaling factor is 3.5% too high
                                     
//At zero rotational velocity, the gyro outputs these values 
//(these are register values, not degrees per second)
//these values should be SUBTRACTED from the readback
const int xGyroDrift = -5;
const int yGyroDrift = -42;
const int zGyroDrift = -5;






int main()
{
  DofBus = i2c_newbus(DofSCL, DofSDA, 0);


  dofSetup();






  int ax, ay, az;
  int wx, wy, wz;
  int mx, my, mz;
  char buffer[6];
  float angle;


  float theta_raw;   //raw pitch and roll, straight from accelerometer
  float phi_raw;


  float yMag_h, xMag_h, Heading;


  int dt, t;            //stores timing values: t holds a clock value, dt is compared to it
  dt = CLKFREQ*2;    // 0.5Hz internal sampling  (dt = no of clock cyckes in 1/0.5 seconds)


  while(1)
  {
     t = CNT;
    i2c_in(DofBus, AccelAddr, 0x32, 1, (unsigned char*)buffer, 6);
   
    ax = (twos2signed(buffer[1], buffer[0])-Bx)*Sx;
    ay = (twos2signed(buffer[3], buffer[2])-By)*Sy;
    az = (twos2signed(buffer[5], buffer[4])-Bz)*Sz;
    print("\n ax: %f, ay: %f, az: %f", ax, ay, az);
    
    theta_raw = atan(-ax/(sqrt(ay*ay + az*az)));    //based on the 1-2-3 rotations - see cillian's report
    phi_raw = atan(ay/az);
    




    i2c_in(DofBus, GyroAddr, 0x1D, 1, (unsigned char*)buffer, 6);   //read 6 registers starting at 0x1D - data registers
    wx = twos2signed(buffer[0], buffer[1]) - xGyroDrift;    //convert 2s compliment split into 2 registers into signed
    wy = twos2signed(buffer[2], buffer[3]) - yGyroDrift;    //and subtract gyro drift
    wz = twos2signed(buffer[4], buffer[5]) - zGyroDrift;








    i2c_in(DofBus, MagAddr, 0x03, 1, (unsigned char*)buffer, 6);
    mx = twos2signed(buffer[0], buffer[1]);
    my = twos2signed(buffer[4], buffer[5]);
    mz = twos2signed(buffer[2], buffer[3]);
           
    //xMag_h and so on are the magntic componants  in the  and y directions and a plane perpendicular to the earths surface. 
    //see  "Applications of Magnetic Sensors for Low Cost Compass Systems"
    xMag_h = mx * cos(theta_raw) + my * sin(theta_raw) * sin(phi_raw) + mz * cos(phi_raw) * sin(theta_raw);  
    //note z is flipped to what the papper says, as our z axis is up, not down -  and we still want clockwise positive (looking down from top)
    yMag_h = my * cos(phi_raw) - mz * sin(phi_raw);
    Heading = atan2(yMag_h, xMag_h);
 
    //print("\n Th: %f, Phi: %f, wx: %d, wy: %d, wz: %d, Heading: %f", theta_raw, phi_raw, wx, wy, wz, Heading);
    //print("\n ax: %f, ay: %f, az: %f", ax, ay, az);
    //print("\nHeeeeeeeelp me");
    //print("\n%d, %d", CNT-t, dt);
    while((int)CNT-t < dt)
    {
      //print("delayed");
      pause(1);
    }
}




  return 0;
}














inline int twos2signed(char MSB, char LSB)
{
   if (MSB >= 0x80)  //negative
   {
     return 0xFFFF0000 | (   ((MSB<<8) & 0xFF00)   |   LSB & 0xFF   );   
     //left pad from 16 bits to 32 bits with 1s
     //Do this because prop ints are 32 bits
     //assigning this value to a signed int results a negative value (as the leftmost bit is 1)
   }   
   return (   ((MSB<<8) & 0xFF00)   |   LSB & 0xFF   );
}  






void dofSetup()
{
  
  
   
     char reg_in[1];       //char to store single byte read in by I2C
    char reg_out[1];      //char to store single byte to be written to part with I2C
    char buffer[6];       //buffer which stores 56 register read backs from a multibyte i2c readback
    
    
    //accelerometer setup
    reg_out[0] = 0x08; //Power control: Measure mode
    i2c_out(DofBus, AccelAddr, 0x2D, 1, (unsigned char*)reg_out, 1);
    i2c_in(DofBus, AccelAddr, 0x2D, 1, (unsigned char*)reg_in, 1);
    print("Power control: 0x%x\n", reg_in[0]);
    
    reg_out[0] = 0x00;// bypass mode, no FIFO
    i2c_out(DofBus, AccelAddr, 0x38, 1, (unsigned char*)reg_out, 1);
    i2c_in(DofBus, AccelAddr, 0x38, 1, (unsigned char*)reg_in, 1);
    print("FIFO Setup: 0x%x\n", reg_in[0]);
    
    reg_out[0] = 0x00;// 10 bit mode, 2g range
    i2c_out(DofBus, AccelAddr, 0x31, 1, (unsigned char*)reg_out, 1);
    i2c_in(DofBus, AccelAddr, 0x31, 1, (unsigned char*)reg_in, 1);
    print("Data Format: 0x%x\n", reg_in[0]);
    
    reg_out[0] = 0x0B;// 200Hz sample rate
    i2c_out(DofBus, AccelAddr, 0x2C, 1, (unsigned char*)reg_out, 1);
    i2c_in(DofBus, AccelAddr, 0x2C, 1, (unsigned char*)reg_in, 1);
    print("Sample rate: 0x%x\n", reg_in[0]);
    
    
    
    //gryo setup
    reg_out[0] = 0x01; //Power control: Set clk to xGyro, all gyros on
    i2c_out(DofBus, GyroAddr, 0x3E, 1, (unsigned char*)reg_out, 1);
    i2c_in(DofBus, GyroAddr, 0x3E, 1, (unsigned char*)reg_in, 1);
    print("Power control: 0x%x\n", reg_in[0]);
    
    reg_out[0] = 0x09; //Sample rate divider: set to 9, gives x/10 = sample output rate, where x is internal rate
       i2c_out(DofBus, GyroAddr, 0x15, 1, (unsigned char*)reg_out, 1);
    i2c_in(DofBus, GyroAddr, 0x15, 1, (unsigned char*)reg_in, 1);
    print("Rate divider: 0x%x\n", reg_in[0]);
    
    reg_out[0] = 0x1A; //scale selection and digital filter: bits 4&5: = 0b11: full scale. bits 2 10: 
    i2c_out(DofBus, GyroAddr, 0x16, 1, (unsigned char*)reg_out, 1);
    i2c_in(DofBus, GyroAddr, 0x16, 1, (unsigned char*)reg_in, 1);
    print("Digital Filter: 0x%x\n", reg_in[0]);
    
     
    
    //magnetometer setup
    reg_out[0] = 0x38; //2 sample averaging, 75Hx sample rate, normal measure mode (no self tiest bias)
    i2c_out(DofBus, MagAddr, 0x00, 1, (unsigned char*)reg_out, 1);     
    i2c_in(DofBus, MagAddr, 0x00, 1, (unsigned char*)reg_in, 1);
    print("CRA: 0x%x\n", reg_in[0]);
    
    reg_out[0] = 0x20; //Gain, default (1090 LSB per Gauss)
    i2c_out(DofBus, MagAddr, 0x01, 1, (unsigned char*)reg_out, 1);
    i2c_in(DofBus, MagAddr, 0x01, 1, (unsigned char*)reg_in, 1);
    print("CRB: 0x%x\n", reg_in[0]);
    
    reg_out[0] = 0x00; //Continuous measure mode
    i2c_out(DofBus, MagAddr, 0x02, 1, (unsigned char*)reg_out, 1);
    i2c_in(DofBus, MagAddr, 0x02, 1, (unsigned char*)reg_in, 1);
    print("MR: 0x%x\n", reg_in[0]);
    
    print("Setup finished");
    pause(3000); //allow to stabalise
    
   
    
}

Dave Hein

This code works for me.

#include <stdio.h>
#include <simpletools.h>

int main()
{
int dt, t;
dt = CLKFREQ*2;
 while(1)
  {
     t = CNT;

//do stuff
    print("\n%d, %d", CNT-t, dt);
    while(CNT-t < dt)
    {
      pause(1);
    }
  }  
}

How do you know the delay isn't working in your code if you don't have a print in the loop?

Mauvai

The full code i just posted has a print in the main while loop. It prints 5-10 times per second, whereas it should print every 2 seconds.

/the above works for me too. However i suspect that if you comment out the line that prints CNT and t (include a different print, for instance), i suspect it wont work -it doesn't for me

Mauvai

I have since discovered that my code works if the i2c reads are performed inside a function.... I also tried commenting out a lot of lines and got really, really strange results.
For instance, the lines:

ax = (twos2signed(buffer[1], buffer[0])-Bx)*Sx;    
ay = (twos2signed(buffer[3], buffer[2])-By)*Sy;    
az = (twos2signed(buffer[5], buffer[4])-Bz)*Sz;

Will prevent it from working correctly, but if I comment out any one of them - it doesn't matter which - it works perfectly. I have no earthly idea why this would happen.

Dave Hein

How large is your program? Maybe you are having stack problems because your program is too large.

Mauvai

Not that large. I posted the code above. Like i said, nothing i observed conformed to c++ coding results. I moved all the code to a class based system, it seems to have fixed the issue (and created a host of other ones, but thats for another thread....)

Dave Hein

I compiled the program you posted, and it is 30100 bytes without the printf's and 30792 with the printf's. This almost uses up all of the 32K hub RAM, but you still have about 2K available for the stack, which should be more than enough. I looked at the I2C code to see if it uses a lot of stack, but it should only use a few dozen bytes on the stack.

I'll look at the code a bit more to see if I can find the problem.

Mauvai

the code for me compiles to 17KB, not 30KB. Im compiling using CMM and OS size optimization, loading to RAM - i dont know if it would make that much of a difference? Either way i have resolved the issue as i mentioned above by using classes - i dont why it fixes it, but it does.

Dave Hein

I printed out the value of 't' in the loop, and it is always zero. I think the optimizer is causing some problems with updating the value of t. It works OK if I change the declaration of 't' to "volatile int t;".