delay function not evaluating properly inside loop condition
Mauvai
Posts: 45
Using the following code:
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?
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?
Comments
It was my understanding that CNT is internally stored as an unsigned long. Is this wrong?
//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 }#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?/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
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.
I'll look at the code a bit more to see if I can find the problem.