I am still not quite understanding fixed point math. I have scaled all my constants by 10,000 and I am getting values for theta in the serial terminal but they are incorrect and the display is very slow. Can you talk more about this: "to do x = a * b you have to do something like x = (a * (int64_t)b) >> 16"? Does this mean that for every multiply and divide in my code I need to use this? What exactly does it mean?
Would another option be to use a separate cog to read the accelerometer values, a separate cog to read the gyro values, and the main cog to compute the math?
Of course one then has to really know ones algorithm. It's easy to overflow the fixed point number range one has selected.
Personally I would not want to tackle the problem of converting the Kalman filter code you presented to fixed point. I had enough headache creating a Fast Fourier Transform in assembler for the Propeller using fixed point maths.
Well it sounds like converting everything from float to fixed point is a bit over my head. What about using more cores? Would you bee able to show me how to do that?
If I recall correctly your Kalman filer is written in C.
A C compiler will not naturally distribute it's work load over multiple cores but you can get it to do so by using OpemMP (Google it) which is supported by prop-gcc.
This may provide some inspiration for spreading across multiple cogs: https://forums.parallax.com/discussion/167084/c-balancing-bot/p1
I wrote some C++ code for a balance bot using a Propeller and my PropWare library. I never got the PID loop tuned correctly, but the code all functions correctly so it might help you get the high-level design in place. Your project is reasonably similar, except instead of a PID loop in the final cog, you want to run a more complicated algorithm.
I am still not quite understanding fixed point math. I have scaled all my constants by 10,000 and I am getting values for theta in the serial terminal but they are incorrect and the display is very slow. Can you talk more about this: "to do x = a * b you have to do something like x = (a * (int64_t)b) >> 16"? Does this mean that for every multiply and divide in my code I need to use this? What exactly does it mean?
It depends, yes that is a typical operation. Shifting is a fast operation, multiply not so much, on the Prop1. Sometimes no extra runtime scaling is needed because the pre-scaled constants have it built in.
Even for integers, finding ways to remove as many multiplies and divides as possible is a common trick. If needed though, an integer multiply is the faster choice.
Google just gave me this - https://en.wikipedia.org/wiki/Libfixmath I'm surprised to see it is an actual Wikipedia page but looks like it might be just the ticket for you.
I'll be interested in best C support for this on the Prop2 since the Prop2 has hardware for a fast 16x16->32 multiply and also a slower 32x32->64 multiply. In both cases it is doing a type conversion that C doesn't understand.
As i mentioned in my other posts that besides making your calculations more efficient that immediately you could move your I/O routines to another cog so that the main loop only has to read hub variables. You only need one cog to handle the gyro and the accelerometer and then test your timing again to see if that made enough of a difference or whether your still need to use scaled integer maths.
I've tried offloading the accel and gyro readings into another cog but no luck. Most of the time the serial terminal is displaying nan. Other times it displays incorrect values. Could you take a look at my code below and let me know what you think is wrong?
void data();
while(1){
cog_run(data,128); // Start cog to get gyro and accel data
// Extended Kalman filter implmentation
/////////////////////
// Do Prediction Step, note: filter is not using accel measurements at this point, only gyro, so it drifts
int N = 1;
for(int i = 0; i < 1; i++){
////////////////////
// State derivatives
float f[2] = {
{gx+gy*sin(phi)*tan(theta)+gz*cos(phi)*tan(theta)},
{gy*cos(phi)-gz*sin(phi)}
};
///////////////////
// Linearization of state derivatives
float A[2][2] = {
{gy*cos(phi)*tan(theta)-gz*sin(phi)*tan(theta), gz*cos(phi)*(square(tan(theta))+1)+gy*sin(phi)*(square(tan(theta))+1)},
{-gz*cos(phi)-gy*sin(phi) , 0 }
};
//////////////////////
// Calculate xhat
// xhat = xhat + (Ts/N)*f
xhat[0]=xhat[0]+(Ts/N)*f[0];
xhat[1]=xhat[1]+(Ts/N)*f[1];
//phi=xhat[0];
//theta=xhat[1];
/////////////////////
// Calculate covariance
// P = P+(Ts/N)*(A*P+P*A'+Q);
P[0][0]=P[0][0]+(Ts/N)*(2*A[0][0]*P[0][0]+A[0][1]*P[0][1]+A[0][1]*P[1][0]+Q[0][0]);
P[0][1]=P[0][1]+(Ts/N)*(A[0][0]*P[0][1]+A[1][0]*P[0][0]+A[0][1]*P[1][1]+A[1][1]*P[0][1]+Q[0][1]);
P[1][0]=P[1][0]+(Ts/N)*(A[0][0]*P[1][0]+A[1][0]*P[0][0]+A[0][1]*P[1][1]+A[1][1]*P[1][0]+Q[1][0]);
P[1][1]=P[1][1]+(Ts/N)*(A[1][0]*P[0][1]+A[1][0]*P[1][0]+2*A[1][1]*P[1][1]+Q[1][1]);
//////////////////////
}
phi=xhat[0];
theta=xhat[1];
// Do Update Step
/////////////////////
// Declare vector of actual measurements
float y[3][1] = {
{ax*9.81},
{ay*9.81},
{az*-9.81}
};
////////////////////
// Define mathematical model of measurements based on xhat
float h[3][1] = {
{gy*Va*sin(theta)+gravity*sin(theta)},
{gz*Va*cos(theta)-gx*Va*sin(theta)-gravity*cos(theta)*sin(phi)},
{-gy*Va*cos(theta)-gravity*cos(theta)*cos(phi)}
};
///////////////////
// Linearization of mathematical model of measurements, note: this works and gets values from xhat above
float C[3][2] = {
{ 0, gravity*cos(theta)+Va*gy*cos(theta)},
{-gravity*cos(phi)*cos(theta), gravity*sin(phi)*sin(theta)-Va*gz*sin(theta)-Va*gx*cos(theta)},
{gravity*cos(theta)*sin(phi), Va*gy*sin(theta)+gravity*cos(phi)*sin(theta)}
};
///////////////////
// Kalman gain matrix
// L = P*C'*(C*P*C'+R)^-1
// Compute s=C*P*C'+R
s[0][0]=R[0][0] + C[0][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[0][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]);
s[0][1]=R[0][1] + C[1][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[1][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]);
s[0][2]=R[0][2] + C[2][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[2][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]);
s[1][0]=R[1][0] + C[0][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[0][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]);
s[1][1]=R[1][1] + C[1][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[1][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]);
s[1][2]=R[1][2] + C[2][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[2][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]);
s[2][0]=R[2][0] + C[0][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[0][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]);
s[2][1]=R[2][1] + C[1][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[1][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]);
s[2][2]=R[2][2] + C[2][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[2][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]);
det_s[0] = s[0][0]*s[1][1]*s[2][2]+s[0][1]*s[1][2]*s[2][0]+s[0][2]*s[1][0]*s[2][1] // Calculate determinant of d
-s[0][2]*s[1][1]*s[2][0]-s[0][1]*s[1][0]*s[2][2]-s[0][0]*s[1][2]*s[2][1];
adj_s[0][0] = s[1][1]*s[2][2]-s[1][2]*s[2][1]; // Calculate adjoint of d
adj_s[0][1] = s[0][2]*s[2][1]-s[0][1]*s[2][2];
adj_s[0][2] = s[0][1]*s[1][2]-s[1][1]*s[0][2];
adj_s[1][0] = s[2][0]*s[1][2]-s[1][0]*s[2][2];
adj_s[1][1] = s[0][0]*s[2][2]-s[2][0]*s[0][2]; // adjoint code messes up accel x data --> adj_s and S were initialized with wrong size
adj_s[1][2] = s[1][0]*s[0][2]-s[0][0]*s[1][2];
adj_s[2][0] = s[1][0]*s[2][1]-s[2][0]*s[1][1];
adj_s[2][1] = s[2][0]*s[0][1]-s[0][0]*s[2][1];
adj_s[2][2] = s[0][0]*s[1][1]-s[0][1]*s[1][0];
// Compute S=(C*P*C'+R)^-1
S[0][0]=adj_s[0][0]*1/det_s[0];
S[0][1]=adj_s[0][1]*1/det_s[0];
S[0][2]=adj_s[0][2]*1/det_s[0];
S[1][0]=adj_s[1][0]*1/det_s[0];
S[1][1]=adj_s[1][1]*1/det_s[0];
S[1][2]=adj_s[1][2]*1/det_s[0];
S[2][0]=adj_s[2][0]*1/det_s[0];
S[2][1]=adj_s[2][1]*1/det_s[0];
S[2][2]=adj_s[2][2]*1/det_s[0];
// Compute L = P*C'*(C*P*C'+R)^-1
L[0][0]=S[1][0]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][0]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][0];
L[0][1]=S[1][1]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][1]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][1];
L[0][2]=S[1][2]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][2]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][2];
L[1][0]=S[1][0]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][0]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][0];
L[1][1]=S[1][1]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][1]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][1];
L[1][2]=S[1][2]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][2]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][2];
//////////////////
// Define identity matrix the length of xhat (2x2)
float I[2][2] = {
{1,0},
{0,1}
};
/////////////////
// Covariance matrix updated with measurement information (2x2)
// P = (I-L*C)*P
P[0][0]=-P[1][0]*(C[0][1]*L[0][0]+C[1][1]*L[0][1]+C[2][1]*L[0][2])-P[0][0]*(C[1][0]*L[0][1]+C[2][0]*L[0][2]-1);
P[0][1]=-P[1][1]*(C[0][1]*L[0][0]+C[1][1]*L[0][1]+C[2][1]*L[0][2])-P[0][1]*(C[1][0]*L[0][1]+C[2][0]*L[0][2]-1);
P[1][0]=-P[1][0]*(C[0][1]*L[1][0]+C[1][1]*L[1][1]+C[2][1]*L[1][2]-1)-P[0][0]*(C[1][0]*L[1][1]+C[2][0]*L[1][2]);
P[1][1]=-P[1][1]*(C[0][1]*L[1][0]+C[1][1]*L[1][1]+C[2][1]*L[1][2]-1)-P[0][1]*(C[1][0]*L[1][1]+C[2][0]*L[1][2]);
//xhat=xhat+L*(y-h)
xhat[0]=xhat[0]-L[0][0]*(h[0][0]-y[0][0])-L[0][1]*(h[1][0]-y[1][0])-L[0][2]*(h[2][0]-y[2][0]);
xhat[1]=xhat[1]-L[1][0]*(h[0][0]-y[0][0])-L[1][1]*(h[1][0]-y[1][0])-L[1][2]*(h[2][0]-y[2][0]);
phi=xhat[0]; //Pitch
theta=xhat[1]; //Roll
float Error=theta*180/PI;//-(-65);
float Error_int=Error_int+Ts*Error;
float servo=kp*Error+ki*Error_int+kd*gx*180/PI;
// Add PWM control for motors
pwm_start(255); // Take outside of loop -> didn't work
if(servo<0){
low(1);
low(3);
pwm_set(2,0,-servo+4.); // when pwmstart = 100, set these to 1.8
pwm_set(4,1,-servo+3.);
}
else if(servo>0){
high(1);
high(3);
pwm_set(2,0,servo+4.);
pwm_set(4,1,servo+2);
}
}
}
// Math Functions Below: square
float square(float a){
b = a*a;
return b;
}
void data(){
// get gyroscope data
// x-axis
low(CS); //CS low selects chip
shift_out(SDA, SCL, MSBFIRST, 8, 0b10101000); //Send read register address (OUT_X_L)
xL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register
high(CS); //De-select chip with CS line high
low(CS);
shift_out(SDA, SCL, MSBFIRST, 8, 0b10101001); //Send read register address (OUT_X_H)
xH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register
high(CS);
gx = (short) (((xH<<8) | xL)-gxAvg)*.0175*PI/180;
// y-axis
low(CS); //CS low selects chip
shift_out(SDA, SCL, MSBFIRST, 8, 0b10101010); //Send read register address (OUT_Y_L)
yL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register
high(CS); //De-select chip with CS line high
low(CS);
shift_out(SDA, SCL, MSBFIRST, 8, 0b10101011); //Send read register address (OUT_Y_H)
yH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register
high(CS);
gy = (short) (((yH<<8) | yL)-gyAvg)*.0175*PI/180;
// z-axis
low(CS); //CS low selects chip
shift_out(SDA, SCL, MSBFIRST, 8, 0b11101100); //Send read register address (OUT_Z_L)
zL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register
high(CS); //De-select chip with CS line high
low(CS);
shift_out(SDA, SCL, MSBFIRST, 8, 0b10101101); //Send read register address (OUT_Z_H)
zH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register
high(CS);
gz = (short) (((zH<<8) | zL)-gzAvg)*.0175*PI/180; //2's complement operation, subtract zero rate offset, multiply by
//sensitivity value for 500deg/s, "short" needed for 2's complement
//print("gx = %f gy = %f gz = %f\n",gx,gy,gz);
pause(1); //Cycle at 20Hz
// get accel data
//x-axis
low(6); // CS low selects chip
shift_out(7, 8, MSBFIRST, 7, 0b0000110); // Send read register address, z-axis
shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value
x = shift_in(7, 8, MSBPRE, 8); // Get value from register
x = x-xOffset;
float ax = (float) x;
ax = ax/64;
high(6);
//y-axis
low(6); // CS low selects chip
shift_out(7, 8, MSBFIRST, 7, 0b0000111); // Send read register address, z-axis
shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value
y = shift_in(7, 8, MSBPRE, 8); // Get value from register
y = y-yOffset;
float ay = (float) y;
ay = ay/64;
high(6);
//z-axis
low(6); // CS low selects chip
shift_out(7, 8, MSBFIRST, 7, 0b0001000); // Send read register address, z-axis
shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value
z = shift_in(7, 8, MSBPRE, 8); // Get value from register
z = z-zOffset;
float az = (float) z;
az = az/64;
high(6); // De-select chip
// print("ax = %f ay = %f az = %f\n",ax,ay,az); // Display measurement
pause(1); // Loop at 20 Hz
}
What happens if you have two functions called data, which one does it use? This is a bad name to use anyway, call the I/O function read_gyros or something and your main function something appropriate. But why are you continually calling cog_run for the read_gyros when this needs to be started once and left running continuously updating the readings so that you can read them anytime. Can you post the complete code.
Is there any reason to do the whole thing twice? That "i" index is not used anywhere.
int N = 1;
Is there any reason to have this? It does nothing in your code.
I would be inclined not to declare all those variables in the main loop. Making them global would avoid having to create them on the stack all the time.
I think you have enough complicated calculation going on there that you had better make a version of it that runs on a PC where you can feed it known test data inputs and check the outputs are correct.
Move all that I/O interfacing stuff out into it's own module that you can test by itself on the Propeller.
Given that you are not using any variable indices into any of your arrays think about turning them into lots of suitably named scalar values. For example:
P[0][0] becomes P_0_0
I'm not sure but replacing all that array indexing with direct variable access might result in faster code.
That line of code is supposed to be for Euler integration to predict the next state or angle in between sensor measurements. N should be something like 10 but that just slows down the code further as you can imagine. I could remove that line completely since I'm just going with N=1. I also like what you are saying about replacing the array indexing.
Could you explain more about the PC version? What program would I use? How could I interface that program with my Prop to read in the sensor values? Thanks for the suggestions!
I don't understand the Kalman thing much but that "N" in your code does nothing anywhere.
For a PC version I would compile the code with the Microsoft C compiler on a Windows PC. Or compile it with GCC for a linux machine.
(Well, actually, on my Windows 10 machines I always do this kind of thing using GCC running under the Linux Subsystem for Windows).
That PC version would not interface to a Prop or any real sensors.
Rather one would provide "fake" input data to the algorithm so that one knows what the input is at all times and can check that the output is correct.
For this reason one should separate the sensor reading code from the kalman code. Then when running on the PC one can substitute the actual sensors for your test data.
With that in place one can tweak the kalman code, as an optimization for example, on the PC and re-run it with the same input data and check it still works as expected and produces the same output.
I don't understand the Kalman thing much but that "N" in your code does nothing anywhere.
For a PC version I would compile the code with the Microsoft C compiler on a Windows PC. Or compile it with GCC for a linux machine.
(Well, actually, on my Windows 10 machines I always do this kind of thing using GCC running under the Linux Subsystem for Windows).
That PC version would not interface to a Prop or any real sensors.
Rather one would provide "fake" input data to the algorithm so that one knows what the input is at all times and can check that the output is correct.
For this reason one should separate the sensor reading code from the kalman code. Then when running on the PC one can substitute the actual sensors for your test data.
With that in place one can tweak the kalman code, as an optimization for example, on the PC and re-run it with the same input data and check it still works as expected and produces the same output.
With complex formulas I often use a spreadsheet to break it down into small sections so that I can manually enter the sensor readings, see the intermediate results along with the final output, and verify that the results are reasonable. I find this can help simplify the calculations and help with the code. Often helps me spot areas where the speed of the code can be increased as well.
Here is my code so far. Please bear with me as I have not had a chance to implement all of the recommended changes so far. I am trying to read gyro and accel values from a different core but when I print the gyro values from the main cog, I get 0. Please help.
/* Implement the Extended Kalman Filter */
// Implement PWM motor control.
// Try using more cores
#include "simpletools.h"
#include "servo.h"
// Declare Math Functions
float square(float a); // Declare square function
// Variables for gyro
signed int whoAmI;
signed int xL, xH, yL, yH, zL, zH;
signed int statusReg;
float gx, gxSum, gxAvg, gy, gySum, gyAvg, gz, gzSum, gzAvg;
// Variables for accel
signed char x, y, z; // Z-axis value, *must be a signed char*
int axSum, axAvg, xOffset, aySum, ayAvg, yOffset, azSum, azAvg, zOffset;
// Variables for EKF
float phi = 0; //180*PI/180; // Changing initial angle makes angle change, but converges back to zero
float theta = 0;
float phi_unc = 45*PI/180; // Initial roll angle uncertainty is 5 deg
float theta_unc = 45*PI/180; // Initial pitch angle uncertainty is 5 deg
float sigma_noise_gyro = 0.0037; // Process noise (guess for now)
float sigma_noise_accel = 0.0001;//0.007;//0.001; // Measurement noise (guess for now), smaller value increases response but increases influence of lateral accels
float gx, gy, gz; // Initialize gyro for testing purposes
float ax, ay, az; // Initialize accel for testing purposes
float Va = 0; // Initialize forward velocity, m/s
float gravity = 9.81; //Initialize gravity, m/s^2
float Ts = 0.002;//0.01; // Sample time step (pause 10 ms in code)
float b; // Declare b for square operation
float s[3][3]; // [3][2] represents the size of the matrix (3x2)
float S[3][3];
float det_s[0];
float adj_s[3][3];
float L[2][3];
float kp=.2;//.06;
float ki=0;
float kd=0;//0.003;
int CS=11;
int SCL=10;
int SDA=9;
int main(){
// get gyroscope initializations
high(CS); //CS line high (SPI inactive)
low(SCL); //CLK line low (prepares chip to send brief high signals or clock pulses)
low(CS);
shift_out(SDA, SCL, MSBFIRST, 8, 0b10001111); //Send read register address (whoAmI)
whoAmI = shift_in(SDA, SCL, MSBPRE, 8);
high(CS);
if(whoAmI == 0b11010011){
print("Device ID Correct: %b\n", whoAmI);
}
else{
print("Device ID Incorrect: %b\n", whoAmI);
}
//Register 4 - Set 3-wire SPI
low(CS); //CS line low (start SPI)
shift_out(SDA, SCL, MSBFIRST, 8, 0b00100011); //Write MCTL register (register 4)to set 3-wire SPI
shift_out(SDA, SCL, MSBFIRST, 8, 0b10010001); //Value for MCTL register (register 4) to set 3-wire SPI
high(CS);
//Register 1 - Set 100Hz output rate, 25Hz cutoff freq, normal mode, enable all axes
low(CS);
shift_out(SDA, SCL, MSBFIRST, 8, 0b01100000); //Write MCTL register (register 1)
shift_out(SDA, SCL, MSBFIRST, 8, 0b00011111); //Value for MCTL register (register 1)
high(CS); //CS high(stop SPI)
pause(100);
//Get x-axis zero-rate offset (xAvg)
for(int i=0; i<100; i++){
low(CS); //CS low selects chip
shift_out(SDA, SCL, MSBFIRST, 8, 0b10101000); //Send read register address (OUT_X_L)
xL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register
high(CS); //De-select chip with CS line high
low(CS);
shift_out(SDA, SCL, MSBFIRST, 8, 0b10101001); //Send read register address (OUT_X_H)
xH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register
high(CS);
gx = (short) ((xH<<8) | xL);
gxSum += gx;
}
gxAvg = (gxSum/100)*PI/180;
print("gxAvg = %f\n", gxAvg);
//Get y-axis zero-rate offset (yAvg)
for(int i=0; i<100; i++){
low(CS); //CS low selects chip
shift_out(SDA, SCL, MSBFIRST, 8, 0b10101010); //Send read register address (OUT_Y_L)
yL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register
high(CS); //De-select chip with CS line high
low(CS);
shift_out(SDA, SCL, MSBFIRST, 8, 0b10101011); //Send read register address (OUT_Y_H)
yH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register
high(CS);
gy = (short) ((yH<<8) | yL);
gySum += gy;
}
gyAvg = (gySum/100)*PI/180;
print("gyAvg = %f\n", gyAvg);
//Get z-axis zero-rate offset (zAvg)
for(int i=0; i<100; i++){
low(CS); //CS low selects chip
shift_out(SDA, SCL, MSBFIRST, 8, 0b11101100); //Send read register address (OUT_Z_L)
zL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register
high(CS); //De-select chip with CS line high
low(CS);
shift_out(SDA, SCL, MSBFIRST, 8, 0b10101101); //Send read register address (OUT_Z_H)
zH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register
high(CS);
gz = (short) ((zH<<8) | zL);
gzSum += gz;
}
gzAvg = (gzSum/100)*PI/180;
print("gzAvg = %f\n", gzAvg);
// get accel initializations
high(6); // CS line high (inactive)
low(8); // CLK line low
low(6); // CS line low (start SPI)
shift_out(7, 8, MSBFIRST, 7, 0b1010110); // Write MCTL register
shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care bit
shift_out(7, 8, MSBFIRST, 8, 0b01100101); // Value for MCTL register
high(6); // CS line high (stop SPI)
pause(1);
//Get x-axis zero-rate offset (zAvg)
for(int i=0; i<100; i++){
low(6); // CS low selects chip
shift_out(7, 8, MSBFIRST, 7, 0b0000110); // Send read register address, z-axis
shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value
x = shift_in(7, 8, MSBPRE, 8); // Get value from register
high(6); // De-select chip
axSum += x;
}
axAvg = axSum/100;
xOffset = axAvg;
print("xOffset = %d\n", xOffset);
//Get y-axis zero-rate offset (zAvg)
for(int i=0; i<100; i++){
low(6); // CS low selects chip
shift_out(7, 8, MSBFIRST, 7, 0b0000111); // Send read register address, z-axis
shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value
y = shift_in(7, 8, MSBPRE, 8); // Get value from register
high(6); // De-select chip
aySum += y;
}
ayAvg = aySum/100;
yOffset = ayAvg;
print("yOffset = %d\n", yOffset);
//Get z-axis zero-rate offset (zAvg)
for(int i=0; i<100; i++){
low(6); // CS low selects chip
shift_out(7, 8, MSBFIRST, 7, 0b0001000); // Send read register address, z-axis
shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value
z = shift_in(7, 8, MSBPRE, 8); // Get value from register
high(6); // De-select chip
azSum += z;
}
azAvg = azSum/100;
zOffset = azAvg-64;
print("zOffset = %d\n", zOffset);
// Define EKF Initializiation Parameters
/////////////////////
// Define error covariance matrix
float P[2][2] = {
{square(phi_unc), 0},
{ 0, square(theta_unc)}
};
/////////////////////
// Define process noise covariance matrix
float Q[2][2] = {
{square(sigma_noise_gyro), 0},
{ 0, square(sigma_noise_gyro)}
};
/////////////////////
// Define measurement noise covariance matrix
float R[3][3] = {
{square(sigma_noise_accel), 0, 0},
{ 0, square(sigma_noise_accel), 0},
{ 0, 0, square(sigma_noise_accel)}
};
/////////////////////
// Define xhat
float xhat[2] = {
{phi},
{theta}
};
//phi=xhat[0];
//theta=xhat[1];
pause(1000); // Pause for 1 second before sampling data
high(26); // Set LED on pin 26 high to show data sampling has begun
//pwm_start(255);
void data();
//volatile float gx, gy, gz, ax, ay, az;
cog_run(data,128); // Start cog to get gyro and accel data
while(1){
//cog_run(data,128); // Start cog to get gyro and accel data
Just a quick look at the i/o section and i can see a common mistake in that you init the i/o in the main cog and then run the other cog to read it. This causes an i/o conflict so just put all the stuff to do with the i/o in the one cog, that is, move the init to the data() function.
Thanks for the tip, I didn't think of that. I've moved everything to do with the sensors into the data() function. Unfortunately I'm still getting 0 for the gyroscope data. My updated code is below:
/* Implement the Extended Kalman Filter */
// Implement PWM motor control.
// Try using more cores
#include "simpletools.h"
#include "servo.h"
// Declare Math Functions
float square(float a); // Declare square function
// Variables for EKF
float phi = 0; //180*PI/180; // Changing initial angle makes angle change, but converges back to zero
float theta = 0;
float phi_unc = 45*PI/180; // Initial roll angle uncertainty is 5 deg
float theta_unc = 45*PI/180; // Initial pitch angle uncertainty is 5 deg
float sigma_noise_gyro = 0.0037; // Process noise (guess for now)
float sigma_noise_accel = 0.0001;//0.007;//0.001; // Measurement noise (guess for now), smaller value increases response but increases influence of lateral accels
float gx, gy, gz; // Initialize gyro for testing purposes
float ax, ay, az; // Initialize accel for testing purposes
float Va = 0; // Initialize forward velocity, m/s
float gravity = 9.81; //Initialize gravity, m/s^2
float Ts = 0.002;//0.01; // Sample time step (pause 10 ms in code)
float b; // Declare b for square operation
float s[3][3]; // [3][2] represents the size of the matrix (3x2)
float S[3][3];
float det_s[0];
float adj_s[3][3];
float L[2][3];
float kp=.2;//.06;
float ki=0;
float kd=0;//0.003;
int main(){
// Define EKF Initializiation Parameters
/////////////////////
// Define error covariance matrix
float P[2][2] = {
{square(phi_unc), 0},
{ 0, square(theta_unc)}
};
/////////////////////
// Define process noise covariance matrix
float Q[2][2] = {
{square(sigma_noise_gyro), 0},
{ 0, square(sigma_noise_gyro)}
};
/////////////////////
// Define measurement noise covariance matrix
float R[3][3] = {
{square(sigma_noise_accel), 0, 0},
{ 0, square(sigma_noise_accel), 0},
{ 0, 0, square(sigma_noise_accel)}
};
/////////////////////
// Define xhat
float xhat[2] = {
{phi},
{theta}
};
//phi=xhat[0];
//theta=xhat[1];
pause(1000); // Pause for 1 second before sampling data
high(26); // Set LED on pin 26 high to show data sampling has begun
//pwm_start(255);
void data();
cog_run(data,128); // Start cog to get gyro and accel data
pause(5000);
while(1){
// Extended Kalman filter implmentation
/////////////////////
// Do Prediction Step, note: filter is not using accel measurements at this point, only gyro, so it drifts
int N = 1;
for(int i = 0; i < 1; i++){
////////////////////
// State derivatives
float f[2] = {
{gx+gy*sin(phi)*tan(theta)+gz*cos(phi)*tan(theta)},
{gy*cos(phi)-gz*sin(phi)}
};
///////////////////
// Linearization of state derivatives
float A[2][2] = {
{gy*cos(phi)*tan(theta)-gz*sin(phi)*tan(theta), gz*cos(phi)*(square(tan(theta))+1)+gy*sin(phi)*(square(tan(theta))+1)},
{-gz*cos(phi)-gy*sin(phi) , 0 }
};
//////////////////////
// Calculate xhat
// xhat = xhat + (Ts/N)*f
xhat[0]=xhat[0]+(Ts/N)*f[0];
xhat[1]=xhat[1]+(Ts/N)*f[1];
//phi=xhat[0];
//theta=xhat[1];
/////////////////////
// Calculate covariance
// P = P+(Ts/N)*(A*P+P*A'+Q);
P[0][0]=P[0][0]+(Ts/N)*(2*A[0][0]*P[0][0]+A[0][1]*P[0][1]+A[0][1]*P[1][0]+Q[0][0]);
P[0][1]=P[0][1]+(Ts/N)*(A[0][0]*P[0][1]+A[1][0]*P[0][0]+A[0][1]*P[1][1]+A[1][1]*P[0][1]+Q[0][1]);
P[1][0]=P[1][0]+(Ts/N)*(A[0][0]*P[1][0]+A[1][0]*P[0][0]+A[0][1]*P[1][1]+A[1][1]*P[1][0]+Q[1][0]);
P[1][1]=P[1][1]+(Ts/N)*(A[1][0]*P[0][1]+A[1][0]*P[1][0]+2*A[1][1]*P[1][1]+Q[1][1]);
//////////////////////
/*
// Print covariance matrix, P
print("P =\n");
for(int i = 0; i < 2; i++){
for(int k = 0; k < 2; k++){
print("%f ", P[i][k]*180/PI);
}
print("\n");
} */
}
phi=xhat[0];
theta=xhat[1];
// Do Update Step
/////////////////////
// Declare vector of actual measurements
float y[3][1] = {
{ax*9.81},
{ay*9.81},
{az*-9.81}
};
////////////////////
// Define mathematical model of measurements based on xhat
float h[3][1] = {
{gy*Va*sin(theta)+gravity*sin(theta)},
{gz*Va*cos(theta)-gx*Va*sin(theta)-gravity*cos(theta)*sin(phi)},
{-gy*Va*cos(theta)-gravity*cos(theta)*cos(phi)}
};
///////////////////
// Linearization of mathematical model of measurements, note: this works and gets values from xhat above
float C[3][2] = {
{ 0, gravity*cos(theta)+Va*gy*cos(theta)},
{-gravity*cos(phi)*cos(theta), gravity*sin(phi)*sin(theta)-Va*gz*sin(theta)-Va*gx*cos(theta)},
{gravity*cos(theta)*sin(phi), Va*gy*sin(theta)+gravity*cos(phi)*sin(theta)}
};
///////////////////
// Kalman gain matrix
// L = P*C'*(C*P*C'+R)^-1
// Compute s=C*P*C'+R
s[0][0]=R[0][0] + C[0][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[0][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]);
s[0][1]=R[0][1] + C[1][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[1][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]);
s[0][2]=R[0][2] + C[2][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[2][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]);
s[1][0]=R[1][0] + C[0][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[0][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]);
s[1][1]=R[1][1] + C[1][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[1][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]);
s[1][2]=R[1][2] + C[2][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[2][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]);
s[2][0]=R[2][0] + C[0][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[0][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]);
s[2][1]=R[2][1] + C[1][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[1][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]);
s[2][2]=R[2][2] + C[2][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[2][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]);
det_s[0] = s[0][0]*s[1][1]*s[2][2]+s[0][1]*s[1][2]*s[2][0]+s[0][2]*s[1][0]*s[2][1] // Calculate determinant of d
-s[0][2]*s[1][1]*s[2][0]-s[0][1]*s[1][0]*s[2][2]-s[0][0]*s[1][2]*s[2][1];
adj_s[0][0] = s[1][1]*s[2][2]-s[1][2]*s[2][1]; // Calculate adjoint of d
adj_s[0][1] = s[0][2]*s[2][1]-s[0][1]*s[2][2];
adj_s[0][2] = s[0][1]*s[1][2]-s[1][1]*s[0][2];
adj_s[1][0] = s[2][0]*s[1][2]-s[1][0]*s[2][2];
adj_s[1][1] = s[0][0]*s[2][2]-s[2][0]*s[0][2]; // adjoint code messes up accel x data --> adj_s and S were initialized with wrong size
adj_s[1][2] = s[1][0]*s[0][2]-s[0][0]*s[1][2];
adj_s[2][0] = s[1][0]*s[2][1]-s[2][0]*s[1][1];
adj_s[2][1] = s[2][0]*s[0][1]-s[0][0]*s[2][1];
adj_s[2][2] = s[0][0]*s[1][1]-s[0][1]*s[1][0];
// Compute S=(C*P*C'+R)^-1
S[0][0]=adj_s[0][0]*1/det_s[0];
S[0][1]=adj_s[0][1]*1/det_s[0];
S[0][2]=adj_s[0][2]*1/det_s[0];
S[1][0]=adj_s[1][0]*1/det_s[0];
S[1][1]=adj_s[1][1]*1/det_s[0];
S[1][2]=adj_s[1][2]*1/det_s[0];
S[2][0]=adj_s[2][0]*1/det_s[0];
S[2][1]=adj_s[2][1]*1/det_s[0];
S[2][2]=adj_s[2][2]*1/det_s[0];
// Compute L = P*C'*(C*P*C'+R)^-1
L[0][0]=S[1][0]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][0]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][0];
L[0][1]=S[1][1]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][1]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][1];
L[0][2]=S[1][2]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][2]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][2];
L[1][0]=S[1][0]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][0]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][0];
L[1][1]=S[1][1]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][1]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][1];
L[1][2]=S[1][2]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][2]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][2];
//////////////////
// Define identity matrix the length of xhat (2x2)
float I[2][2] = {
{1,0},
{0,1}
};
/////////////////
// Covariance matrix updated with measurement information (2x2)
// P = (I-L*C)*P
P[0][0]=-P[1][0]*(C[0][1]*L[0][0]+C[1][1]*L[0][1]+C[2][1]*L[0][2])-P[0][0]*(C[1][0]*L[0][1]+C[2][0]*L[0][2]-1);
P[0][1]=-P[1][1]*(C[0][1]*L[0][0]+C[1][1]*L[0][1]+C[2][1]*L[0][2])-P[0][1]*(C[1][0]*L[0][1]+C[2][0]*L[0][2]-1);
P[1][0]=-P[1][0]*(C[0][1]*L[1][0]+C[1][1]*L[1][1]+C[2][1]*L[1][2]-1)-P[0][0]*(C[1][0]*L[1][1]+C[2][0]*L[1][2]);
P[1][1]=-P[1][1]*(C[0][1]*L[1][0]+C[1][1]*L[1][1]+C[2][1]*L[1][2]-1)-P[0][1]*(C[1][0]*L[1][1]+C[2][0]*L[1][2]);
//xhat=xhat+L*(y-h)
xhat[0]=xhat[0]-L[0][0]*(h[0][0]-y[0][0])-L[0][1]*(h[1][0]-y[1][0])-L[0][2]*(h[2][0]-y[2][0]);
xhat[1]=xhat[1]-L[1][0]*(h[0][0]-y[0][0])-L[1][1]*(h[1][0]-y[1][0])-L[1][2]*(h[2][0]-y[2][0]);
phi=xhat[0]; //Pitch
theta=xhat[1]; //Roll
float Error=theta*180/PI;//-(-65);
float Error_int=Error_int+Ts*Error;
float servo=kp*Error+ki*Error_int+kd*gx*180/PI;
// Add PWM control for motors
if(servo<0){
low(1);
low(3);
pwm_set(2,0,-servo+5.); // when pwmstart = 100, set these to 1.8
pwm_set(4,1,-servo+5.);
}
else if(servo>0){
high(1);
high(3);
pwm_set(2,0,servo+5.);
pwm_set(4,1,servo+5);
}
print("gx=%f\n",gx*180/PI);
}
}
// Math Functions Below: square
float square(float a){
b = a*a;
return b;
}
I've confirmed that the second cog is reading the gyro and accelerometer by printing the values from that core. The problem is that my main cog is not using the sensor data from the second cog. The main cog just thinks all the values are zero.
But aren't you just trying to returning the values for the local variables gx gy etc? But your data function is defined as a void. I'm not a C programmer but it seems that you are not updating the gx gy gz global variables, only the locals. I may be totally wrong (I don't see anyone volunteering) but try removing gx gy gx from this line:
I copied and pasted your code into my own editor and started to take a look. The best advice I can give you is to break this huge problem into smaller bits and DO NOT try to tackle this all at once. By that, I mean create a program that just reads from the your gyroscope and test that on the Propeller. All it does is read from the gyro, and print numbers to the screen. You should have a function to make that easy, something with a signature like "short readGyro(int cs, int scl, int sda, unsigned char address)". Your main function would then just be:
int main () {
int CS = 1;
int SCL = 2;
int SDA = 3;
// Do gyro setup routines
while (1) {
short x, y, z;
x = readGyro(CS, SCL, SDA, 0b10101000);
y = readGyro(CS, SCL, SDA, 0b10101010);
z = readGyro(CS, SCL, SDA, 0b11101100);
print("x=%d\n", x);
print("y=%d\n", y);
print("z=%d\n", z);
pause(1);
}
}
I'd even create a reusable function for doing the gyro setup routines. It can have a similar signature, like void initializeGyro(int cs, int scl, int sda)". There's a lot of sensor initialization code that get in the way of understanding the meat of your program.
Once you've successfully tested your readGyro() function, do the same for reading from the accelerometer. Once you've tested that, put them together and test the two in a single cog. Once you've done that, put it aside.
Now see if you can get two different cogs each blinking an LED. Got it? Working? Great.
Next, try to get one cog blinking an LED and another cog reading the gyro and accel.
With that working, step toward inter-cog communication. Try to store the variables from the accelerometer and the gyro globally and print them to the serial terminal in the main cog.
Okay, put alllll of that aside and test your complex math algorithm stuff on a computer, with a real debugger and fast compiler and good editor. All that cool stuff. Fake some input data, do the math by hand, and make sure the output is correct. Cool.... now your complicated algorithm is complete.
Thheeeeeennnn you put it all together and test it on the Propeller.
The reason I'm breaking it down into such small steps is because
1) this is a large and complicated program you're writing. When I copied your two code snippets into my editor, I came up with almost 500 lines of code! That's a lot! That's WAY too much to test all at once.
2) It will help you break down logical pieces of the program into modular blocks and then turn those blocks into functions. Functions make your code more legible and make it easier for us to help you.
3) There are some pretty basic compilation failures (such as the data function having the incorrect signature). That says to me you're pretty new at this and therefore should start small and work your way up.
And finally, I've refactored a bit of your code with some reusable functions. Take a look at how much shorter the while-loop is inside the data() function and how much easier it is to read. You hardly even need comments anymore because the functions speak for themselves. In fact, if you extracted the address values for the axes (such as "int ACCELEROMETER_X_ADDRESS = 0b0000110"), there'd be absolutely no need for comments in that loop! Basic stuff like this make it shorter, more maintainable, contain fewer bugs, and make it more likely that I (and others?) will actually read the code you're posting instead of just posting generic and high-level answers to your questions.
uint16_t readGyro (int csPinNumber, int scl, int sda, uint8_t address);
uint8_t readAccelerometer (int csPinNumber, uint8_t address);
void data (void *par) {
// Variables for gyro
signed int whoAmI;
signed int statusReg;
float gxSum = 0, gxAvg, gySum = 0, gyAvg, gzSum = 0, gzAvg;
// Variables for accel
signed char x, y,
z; // Z-axis value, *must be a signed char*
int axSum = 0, axAvg, xOffset, aySum = 0, ayAvg, yOffset, azSum = 0, azAvg, zOffset;
int CS = 11;
int SCL = 10;
int SDA = 9;
// get gyroscope initializations
high(CS); //CS line high (SPI inactive)
low(SCL); //CLK line low (prepares chip to send brief high signals or clock pulses)
low(CS);
shift_out(SDA, SCL, MSBFIRST, 8, 0b10001111); //Send read register address (whoAmI)
whoAmI = shift_in(SDA, SCL, MSBPRE, 8);
high(CS);
/*
if(whoAmI == 0b11010011){
print("Device ID Correct: %b\n", whoAmI);
}
else{
print("Device ID Incorrect: %b\n", whoAmI);
}
*/
//Register 4 - Set 3-wire SPI
low(CS); //CS line low (start SPI)
shift_out(SDA, SCL, MSBFIRST, 8, 0b00100011); //Write MCTL register (register 4)to set 3-wire SPI
shift_out(SDA, SCL, MSBFIRST, 8, 0b10010001); //Value for MCTL register (register 4) to set 3-wire SPI
high(CS);
//Register 1 - Set 100Hz output rate, 25Hz cutoff freq, normal mode, enable all axes
low(CS);
shift_out(SDA, SCL, MSBFIRST, 8, 0b01100000); //Write MCTL register (register 1)
shift_out(SDA, SCL, MSBFIRST, 8, 0b00011111); //Value for MCTL register (register 1)
high(CS); //CS high(stop SPI)
pause(100);
//Get x-axis zero-rate offset (xAvg)
for (int i = 0; i < 100; i++) {
gxSum += readGyro(CS, SCL, SDA, 0b10101000);
}
gxAvg = (gxSum / 100) * PI / 180;
//print("gxAvg = %f\n", gxAvg);
//Get y-axis zero-rate offset (yAvg)
for (int i = 0; i < 100; i++) {
gySum += readGyro(CS, SCL, SDA, 0b10101010);
}
gyAvg = (gySum / 100) * PI / 180;
//print("gyAvg = %f\n", gyAvg);
//Get z-axis zero-rate offset (zAvg)
for (int i = 0; i < 100; i++) {
gzSum += readGyro(CS, SCL, SDA, 0b11101100);
}
gzAvg = (gzSum / 100) * PI / 180;
//print("gzAvg = %f\n", gzAvg);
// get accel initializations
high(6); // CS line high (inactive)
low(8); // CLK line low
low(6); // CS line low (start SPI)
shift_out(7, 8, MSBFIRST, 7, 0b1010110); // Write MCTL register
shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care bit
shift_out(7, 8, MSBFIRST, 8, 0b01100101); // Value for MCTL register
high(6); // CS line high (stop SPI)
pause(1);
//Get x-axis zero-rate offset (zAvg)
for (int i = 0; i < 100; i++) {
axSum += readAccelerometer(6, 0b0000110);
}
axAvg = axSum / 100;
xOffset = axAvg;
//print("xOffset = %d\n", xOffset);
//Get y-axis zero-rate offset (zAvg)
for (int i = 0; i < 100; i++) {
aySum += readAccelerometer(6, 0b0000110);
}
ayAvg = aySum / 100;
yOffset = ayAvg;
//print("yOffset = %d\n", yOffset);
//Get z-axis zero-rate offset (zAvg)
for (int i = 0; i < 100; i++) {
azSum += readAccelerometer(6, 0b0000110);
}
azAvg = azSum / 100;
zOffset = azAvg - 64;
//print("zOffset = %d\n", zOffset);
while (1) {
// get gyroscope data
gx = (short) (readGyro(CS, SCL, SDA, 0b10101000) - gxAvg) * .0175 * PI / 180;
gy = (short) (readGyro(CS, SCL, SDA, 0b10101010) - gyAvg) * .0175 * PI / 180;
gz = (short) (readGyro(CS, SCL, SDA, 0b11101100) - gzAvg) * .0175 * PI / 180;
//sensitivity value for 500deg/s, "short" needed for 2's complement
//print("gx = %f gy = %f gz = %f\n",gx,gy,gz);
pause(1); //Cycle at 20Hz
// get accel data
//x-axis
x = readAccelerometer(6, 0b0000110);
ax = (x - xOffset) / 64;
//y-axis
y = readAccelerometer(6, 0b0000111);
ay = (y - yOffset) / 64;
//z-axis
z = readAccelerometer(6, 0b0001000);
az = (z - zOffset) / 64;
//print("ax = %f ay = %f az = %f\n",ax,ay,az); // Display measurement
pause(1); // Loop at 20 Hz
}
}
uint16_t readGyro (int csPinNumber, int scl, int sda, uint8_t address) {
low(csPinNumber);
shift_out(sda, scl, MSBFIRST, 8, address);
uint8_t lowByte = shift_in(sda, scl, MSBPRE, 8);
shift_out(sda, scl, MSBFIRST, 8, address + 1);
uint8_t highByte = shift_in(sda, scl, MSBPRE, 8);
high(csPinNumber);
return ((uint16_t) (highByte) << 16) + lowByte;
}
uint8_t readAccelerometer (int csPinNumber, uint8_t address) {
low(csPinNumber);
shift_out(7, 8, MSBFIRST, 7, address);
shift_out(7, 8, MSBFIRST, 1, 0);
uint8_t temp = shift_in(7, 8, MSBPRE, 8);
high(csPinNumber);
return temp;
}
I hope you've found this at least somewhat useful
Good luck!
But aren't you just trying to returning the values for the local variables gx gy etc? But your data function is defined as a void. I'm not a C programmer but it seems that you are not updating the gx gy gz global variables, only the locals. I may be totally wrong (I don't see anyone volunteering) but try removing gx gy gx from this line:
You're reading that correctly. That was one of the things I caught while reading through his code too. The local gx/gy/gz and ax/ay/az variables are overriding the global ones, which is why the main cog isn't seeing any changes.
And the fact that there is a "return" statement in the data() function is one of the compiler errors I referred to in my post.
Transferring data between cores in Prop C should be done by using globals. Although you declared gx, gy,and gz as globals in your first listing, you redeclared them in data(). I'm not sure what that does. I think you should have gotten a warning.
I'm not sure that you declared all the global variables you need to send data back and forth.
One other thing to watch out for is calculating 2 related variables (like an x & y pair) in one core and using them in another core. When you do that the program must send the data to the hub. Each core has access to the hub for one variable in a rotating manner. I had a program that read raw x & y values from a bluetooth joystick and calculated the x & y in one core and then through globals sent them to a second core where they were used to position a device. The program originally didn't work correctly. It took x and paired it with y from a previous reading. Since they were 8 bit values I fixed the program by calculating x & y in the first core (as local variables), then combining them in a single global integer. There are other ways to make sure that the values are correctly related when they move between different cog functions (I haven't gotten that experienced).
You are still accessing the gyro data register by painful register instead of sending a register address with the auto-increment bit set so that all you have to do is read consecutive bytes of data while building up the 16-bit words after which you can deselect. However at present you are doing this:
Select - address reg $28 - read byte - deslect
Select - address reg $29 - read byte - deslect
Select - address reg $2A - read byte - deslect
Select - address reg $2B - read byte - deslect
Select - address reg $2C - read byte - deslect
Select - address reg $2D - read byte - deslect
When all you have to do is (with auto-increment M/S set):
Select - address reg $28 (0b11101000) - read 8 bytes - deselect
You can assemble those bytes into words and save them while you are reading them or you can wait until you deselect and do that or whatever.
It mightn't matter too much now that it is running in its own cog but it is the sensible and practical thing to do. You don't order your fries one at a time, just grab the whole packet.
Comments
Would another option be to use a separate cog to read the accelerometer values, a separate cog to read the gyro values, and the main cog to compute the math?
Thank you.
Think in binary. If you scale by 10,000 decimal that is a horribly long multiplication for the machine to do.
For example, you could scale by 256. That is only a shift by 8 bits. Which is just one machine instruction.
With such a 24:8 bit fixed point format one can do additions and subtractions as normal.
But after a multiply one has to shift the result right by 8 bits.
Personally I would not want to tackle the problem of converting the Kalman filter code you presented to fixed point. I had enough headache creating a Fast Fourier Transform in assembler for the Propeller using fixed point maths.
A C compiler will not naturally distribute it's work load over multiple cores but you can get it to do so by using OpemMP (Google it) which is supported by prop-gcc.
I used OpenMP to good effect to boost performance of my Fast Fourier Transform for the Propeller https://github.com/ZiCog/fftbench/blob/master/fftbench.c which also uses fixed point maths for speed.
However this is not trivial and requires one understanding ones algorithm. It took a lot of head scratching to get my parallel FFT to work.
I wrote some C++ code for a balance bot using a Propeller and my PropWare library. I never got the PID loop tuned correctly, but the code all functions correctly so it might help you get the high-level design in place. Your project is reasonably similar, except instead of a PID loop in the final cog, you want to run a more complicated algorithm.
Even for integers, finding ways to remove as many multiplies and divides as possible is a common trick. If needed though, an integer multiply is the faster choice.
Google just gave me this - https://en.wikipedia.org/wiki/Libfixmath I'm surprised to see it is an actual Wikipedia page but looks like it might be just the ticket for you.
void data(); while(1){ cog_run(data,128); // Start cog to get gyro and accel data // Extended Kalman filter implmentation ///////////////////// // Do Prediction Step, note: filter is not using accel measurements at this point, only gyro, so it drifts int N = 1; for(int i = 0; i < 1; i++){ //////////////////// // State derivatives float f[2] = { {gx+gy*sin(phi)*tan(theta)+gz*cos(phi)*tan(theta)}, {gy*cos(phi)-gz*sin(phi)} }; /////////////////// // Linearization of state derivatives float A[2][2] = { {gy*cos(phi)*tan(theta)-gz*sin(phi)*tan(theta), gz*cos(phi)*(square(tan(theta))+1)+gy*sin(phi)*(square(tan(theta))+1)}, {-gz*cos(phi)-gy*sin(phi) , 0 } }; ////////////////////// // Calculate xhat // xhat = xhat + (Ts/N)*f xhat[0]=xhat[0]+(Ts/N)*f[0]; xhat[1]=xhat[1]+(Ts/N)*f[1]; //phi=xhat[0]; //theta=xhat[1]; ///////////////////// // Calculate covariance // P = P+(Ts/N)*(A*P+P*A'+Q); P[0][0]=P[0][0]+(Ts/N)*(2*A[0][0]*P[0][0]+A[0][1]*P[0][1]+A[0][1]*P[1][0]+Q[0][0]); P[0][1]=P[0][1]+(Ts/N)*(A[0][0]*P[0][1]+A[1][0]*P[0][0]+A[0][1]*P[1][1]+A[1][1]*P[0][1]+Q[0][1]); P[1][0]=P[1][0]+(Ts/N)*(A[0][0]*P[1][0]+A[1][0]*P[0][0]+A[0][1]*P[1][1]+A[1][1]*P[1][0]+Q[1][0]); P[1][1]=P[1][1]+(Ts/N)*(A[1][0]*P[0][1]+A[1][0]*P[1][0]+2*A[1][1]*P[1][1]+Q[1][1]); ////////////////////// } phi=xhat[0]; theta=xhat[1]; // Do Update Step ///////////////////// // Declare vector of actual measurements float y[3][1] = { {ax*9.81}, {ay*9.81}, {az*-9.81} }; //////////////////// // Define mathematical model of measurements based on xhat float h[3][1] = { {gy*Va*sin(theta)+gravity*sin(theta)}, {gz*Va*cos(theta)-gx*Va*sin(theta)-gravity*cos(theta)*sin(phi)}, {-gy*Va*cos(theta)-gravity*cos(theta)*cos(phi)} }; /////////////////// // Linearization of mathematical model of measurements, note: this works and gets values from xhat above float C[3][2] = { { 0, gravity*cos(theta)+Va*gy*cos(theta)}, {-gravity*cos(phi)*cos(theta), gravity*sin(phi)*sin(theta)-Va*gz*sin(theta)-Va*gx*cos(theta)}, {gravity*cos(theta)*sin(phi), Va*gy*sin(theta)+gravity*cos(phi)*sin(theta)} }; /////////////////// // Kalman gain matrix // L = P*C'*(C*P*C'+R)^-1 // Compute s=C*P*C'+R s[0][0]=R[0][0] + C[0][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[0][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]); s[0][1]=R[0][1] + C[1][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[1][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]); s[0][2]=R[0][2] + C[2][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[2][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]); s[1][0]=R[1][0] + C[0][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[0][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]); s[1][1]=R[1][1] + C[1][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[1][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]); s[1][2]=R[1][2] + C[2][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[2][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]); s[2][0]=R[2][0] + C[0][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[0][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]); s[2][1]=R[2][1] + C[1][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[1][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]); s[2][2]=R[2][2] + C[2][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[2][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]); det_s[0] = s[0][0]*s[1][1]*s[2][2]+s[0][1]*s[1][2]*s[2][0]+s[0][2]*s[1][0]*s[2][1] // Calculate determinant of d -s[0][2]*s[1][1]*s[2][0]-s[0][1]*s[1][0]*s[2][2]-s[0][0]*s[1][2]*s[2][1]; adj_s[0][0] = s[1][1]*s[2][2]-s[1][2]*s[2][1]; // Calculate adjoint of d adj_s[0][1] = s[0][2]*s[2][1]-s[0][1]*s[2][2]; adj_s[0][2] = s[0][1]*s[1][2]-s[1][1]*s[0][2]; adj_s[1][0] = s[2][0]*s[1][2]-s[1][0]*s[2][2]; adj_s[1][1] = s[0][0]*s[2][2]-s[2][0]*s[0][2]; // adjoint code messes up accel x data --> adj_s and S were initialized with wrong size adj_s[1][2] = s[1][0]*s[0][2]-s[0][0]*s[1][2]; adj_s[2][0] = s[1][0]*s[2][1]-s[2][0]*s[1][1]; adj_s[2][1] = s[2][0]*s[0][1]-s[0][0]*s[2][1]; adj_s[2][2] = s[0][0]*s[1][1]-s[0][1]*s[1][0]; // Compute S=(C*P*C'+R)^-1 S[0][0]=adj_s[0][0]*1/det_s[0]; S[0][1]=adj_s[0][1]*1/det_s[0]; S[0][2]=adj_s[0][2]*1/det_s[0]; S[1][0]=adj_s[1][0]*1/det_s[0]; S[1][1]=adj_s[1][1]*1/det_s[0]; S[1][2]=adj_s[1][2]*1/det_s[0]; S[2][0]=adj_s[2][0]*1/det_s[0]; S[2][1]=adj_s[2][1]*1/det_s[0]; S[2][2]=adj_s[2][2]*1/det_s[0]; // Compute L = P*C'*(C*P*C'+R)^-1 L[0][0]=S[1][0]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][0]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][0]; L[0][1]=S[1][1]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][1]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][1]; L[0][2]=S[1][2]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][2]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][2]; L[1][0]=S[1][0]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][0]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][0]; L[1][1]=S[1][1]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][1]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][1]; L[1][2]=S[1][2]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][2]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][2]; ////////////////// // Define identity matrix the length of xhat (2x2) float I[2][2] = { {1,0}, {0,1} }; ///////////////// // Covariance matrix updated with measurement information (2x2) // P = (I-L*C)*P P[0][0]=-P[1][0]*(C[0][1]*L[0][0]+C[1][1]*L[0][1]+C[2][1]*L[0][2])-P[0][0]*(C[1][0]*L[0][1]+C[2][0]*L[0][2]-1); P[0][1]=-P[1][1]*(C[0][1]*L[0][0]+C[1][1]*L[0][1]+C[2][1]*L[0][2])-P[0][1]*(C[1][0]*L[0][1]+C[2][0]*L[0][2]-1); P[1][0]=-P[1][0]*(C[0][1]*L[1][0]+C[1][1]*L[1][1]+C[2][1]*L[1][2]-1)-P[0][0]*(C[1][0]*L[1][1]+C[2][0]*L[1][2]); P[1][1]=-P[1][1]*(C[0][1]*L[1][0]+C[1][1]*L[1][1]+C[2][1]*L[1][2]-1)-P[0][1]*(C[1][0]*L[1][1]+C[2][0]*L[1][2]); //xhat=xhat+L*(y-h) xhat[0]=xhat[0]-L[0][0]*(h[0][0]-y[0][0])-L[0][1]*(h[1][0]-y[1][0])-L[0][2]*(h[2][0]-y[2][0]); xhat[1]=xhat[1]-L[1][0]*(h[0][0]-y[0][0])-L[1][1]*(h[1][0]-y[1][0])-L[1][2]*(h[2][0]-y[2][0]); phi=xhat[0]; //Pitch theta=xhat[1]; //Roll float Error=theta*180/PI;//-(-65); float Error_int=Error_int+Ts*Error; float servo=kp*Error+ki*Error_int+kd*gx*180/PI; // Add PWM control for motors pwm_start(255); // Take outside of loop -> didn't work if(servo<0){ low(1); low(3); pwm_set(2,0,-servo+4.); // when pwmstart = 100, set these to 1.8 pwm_set(4,1,-servo+3.); } else if(servo>0){ high(1); high(3); pwm_set(2,0,servo+4.); pwm_set(4,1,servo+2); } } } // Math Functions Below: square float square(float a){ b = a*a; return b; } void data(){ // get gyroscope data // x-axis low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b10101000); //Send read register address (OUT_X_L) xL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101001); //Send read register address (OUT_X_H) xH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gx = (short) (((xH<<8) | xL)-gxAvg)*.0175*PI/180; // y-axis low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b10101010); //Send read register address (OUT_Y_L) yL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101011); //Send read register address (OUT_Y_H) yH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gy = (short) (((yH<<8) | yL)-gyAvg)*.0175*PI/180; // z-axis low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b11101100); //Send read register address (OUT_Z_L) zL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101101); //Send read register address (OUT_Z_H) zH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gz = (short) (((zH<<8) | zL)-gzAvg)*.0175*PI/180; //2's complement operation, subtract zero rate offset, multiply by //sensitivity value for 500deg/s, "short" needed for 2's complement //print("gx = %f gy = %f gz = %f\n",gx,gy,gz); pause(1); //Cycle at 20Hz // get accel data //x-axis low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0000110); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value x = shift_in(7, 8, MSBPRE, 8); // Get value from register x = x-xOffset; float ax = (float) x; ax = ax/64; high(6); //y-axis low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0000111); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value y = shift_in(7, 8, MSBPRE, 8); // Get value from register y = y-yOffset; float ay = (float) y; ay = ay/64; high(6); //z-axis low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0001000); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value z = shift_in(7, 8, MSBPRE, 8); // Get value from register z = z-zOffset; float az = (float) z; az = az/64; high(6); // De-select chip // print("ax = %f ay = %f az = %f\n",ax,ay,az); // Display measurement pause(1); // Loop at 20 Hz }for(int i = 0; i < 1; i++){ ...Is there any reason to do the whole thing twice? That "i" index is not used anywhere. Is there any reason to have this? It does nothing in your code.I would be inclined not to declare all those variables in the main loop. Making them global would avoid having to create them on the stack all the time.
I think you have enough complicated calculation going on there that you had better make a version of it that runs on a PC where you can feed it known test data inputs and check the outputs are correct.
Move all that I/O interfacing stuff out into it's own module that you can test by itself on the Propeller.
Given that you are not using any variable indices into any of your arrays think about turning them into lots of suitably named scalar values. For example:
P[0][0] becomes P_0_0
I'm not sure but replacing all that array indexing with direct variable access might result in faster code.
That line of code is supposed to be for Euler integration to predict the next state or angle in between sensor measurements. N should be something like 10 but that just slows down the code further as you can imagine. I could remove that line completely since I'm just going with N=1. I also like what you are saying about replacing the array indexing.
Could you explain more about the PC version? What program would I use? How could I interface that program with my Prop to read in the sensor values? Thanks for the suggestions!
For a PC version I would compile the code with the Microsoft C compiler on a Windows PC. Or compile it with GCC for a linux machine.
(Well, actually, on my Windows 10 machines I always do this kind of thing using GCC running under the Linux Subsystem for Windows).
That PC version would not interface to a Prop or any real sensors.
Rather one would provide "fake" input data to the algorithm so that one knows what the input is at all times and can check that the output is correct.
For this reason one should separate the sensor reading code from the kalman code. Then when running on the PC one can substitute the actual sensors for your test data.
With that in place one can tweak the kalman code, as an optimization for example, on the PC and re-run it with the same input data and check it still works as expected and produces the same output.
With complex formulas I often use a spreadsheet to break it down into small sections so that I can manually enter the sensor readings, see the intermediate results along with the final output, and verify that the results are reasonable. I find this can help simplify the calculations and help with the code. Often helps me spot areas where the speed of the code can be increased as well.
/* Implement the Extended Kalman Filter */ // Implement PWM motor control. // Try using more cores #include "simpletools.h" #include "servo.h" // Declare Math Functions float square(float a); // Declare square function // Variables for gyro signed int whoAmI; signed int xL, xH, yL, yH, zL, zH; signed int statusReg; float gx, gxSum, gxAvg, gy, gySum, gyAvg, gz, gzSum, gzAvg; // Variables for accel signed char x, y, z; // Z-axis value, *must be a signed char* int axSum, axAvg, xOffset, aySum, ayAvg, yOffset, azSum, azAvg, zOffset; // Variables for EKF float phi = 0; //180*PI/180; // Changing initial angle makes angle change, but converges back to zero float theta = 0; float phi_unc = 45*PI/180; // Initial roll angle uncertainty is 5 deg float theta_unc = 45*PI/180; // Initial pitch angle uncertainty is 5 deg float sigma_noise_gyro = 0.0037; // Process noise (guess for now) float sigma_noise_accel = 0.0001;//0.007;//0.001; // Measurement noise (guess for now), smaller value increases response but increases influence of lateral accels float gx, gy, gz; // Initialize gyro for testing purposes float ax, ay, az; // Initialize accel for testing purposes float Va = 0; // Initialize forward velocity, m/s float gravity = 9.81; //Initialize gravity, m/s^2 float Ts = 0.002;//0.01; // Sample time step (pause 10 ms in code) float b; // Declare b for square operation float s[3][3]; // [3][2] represents the size of the matrix (3x2) float S[3][3]; float det_s[0]; float adj_s[3][3]; float L[2][3]; float kp=.2;//.06; float ki=0; float kd=0;//0.003; int CS=11; int SCL=10; int SDA=9; int main(){ // get gyroscope initializations high(CS); //CS line high (SPI inactive) low(SCL); //CLK line low (prepares chip to send brief high signals or clock pulses) low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10001111); //Send read register address (whoAmI) whoAmI = shift_in(SDA, SCL, MSBPRE, 8); high(CS); if(whoAmI == 0b11010011){ print("Device ID Correct: %b\n", whoAmI); } else{ print("Device ID Incorrect: %b\n", whoAmI); } //Register 4 - Set 3-wire SPI low(CS); //CS line low (start SPI) shift_out(SDA, SCL, MSBFIRST, 8, 0b00100011); //Write MCTL register (register 4)to set 3-wire SPI shift_out(SDA, SCL, MSBFIRST, 8, 0b10010001); //Value for MCTL register (register 4) to set 3-wire SPI high(CS); //Register 1 - Set 100Hz output rate, 25Hz cutoff freq, normal mode, enable all axes low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b01100000); //Write MCTL register (register 1) shift_out(SDA, SCL, MSBFIRST, 8, 0b00011111); //Value for MCTL register (register 1) high(CS); //CS high(stop SPI) pause(100); //Get x-axis zero-rate offset (xAvg) for(int i=0; i<100; i++){ low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b10101000); //Send read register address (OUT_X_L) xL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101001); //Send read register address (OUT_X_H) xH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gx = (short) ((xH<<8) | xL); gxSum += gx; } gxAvg = (gxSum/100)*PI/180; print("gxAvg = %f\n", gxAvg); //Get y-axis zero-rate offset (yAvg) for(int i=0; i<100; i++){ low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b10101010); //Send read register address (OUT_Y_L) yL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101011); //Send read register address (OUT_Y_H) yH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gy = (short) ((yH<<8) | yL); gySum += gy; } gyAvg = (gySum/100)*PI/180; print("gyAvg = %f\n", gyAvg); //Get z-axis zero-rate offset (zAvg) for(int i=0; i<100; i++){ low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b11101100); //Send read register address (OUT_Z_L) zL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101101); //Send read register address (OUT_Z_H) zH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gz = (short) ((zH<<8) | zL); gzSum += gz; } gzAvg = (gzSum/100)*PI/180; print("gzAvg = %f\n", gzAvg); // get accel initializations high(6); // CS line high (inactive) low(8); // CLK line low low(6); // CS line low (start SPI) shift_out(7, 8, MSBFIRST, 7, 0b1010110); // Write MCTL register shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care bit shift_out(7, 8, MSBFIRST, 8, 0b01100101); // Value for MCTL register high(6); // CS line high (stop SPI) pause(1); //Get x-axis zero-rate offset (zAvg) for(int i=0; i<100; i++){ low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0000110); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value x = shift_in(7, 8, MSBPRE, 8); // Get value from register high(6); // De-select chip axSum += x; } axAvg = axSum/100; xOffset = axAvg; print("xOffset = %d\n", xOffset); //Get y-axis zero-rate offset (zAvg) for(int i=0; i<100; i++){ low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0000111); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value y = shift_in(7, 8, MSBPRE, 8); // Get value from register high(6); // De-select chip aySum += y; } ayAvg = aySum/100; yOffset = ayAvg; print("yOffset = %d\n", yOffset); //Get z-axis zero-rate offset (zAvg) for(int i=0; i<100; i++){ low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0001000); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value z = shift_in(7, 8, MSBPRE, 8); // Get value from register high(6); // De-select chip azSum += z; } azAvg = azSum/100; zOffset = azAvg-64; print("zOffset = %d\n", zOffset); // Define EKF Initializiation Parameters ///////////////////// // Define error covariance matrix float P[2][2] = { {square(phi_unc), 0}, { 0, square(theta_unc)} }; ///////////////////// // Define process noise covariance matrix float Q[2][2] = { {square(sigma_noise_gyro), 0}, { 0, square(sigma_noise_gyro)} }; ///////////////////// // Define measurement noise covariance matrix float R[3][3] = { {square(sigma_noise_accel), 0, 0}, { 0, square(sigma_noise_accel), 0}, { 0, 0, square(sigma_noise_accel)} }; ///////////////////// // Define xhat float xhat[2] = { {phi}, {theta} }; //phi=xhat[0]; //theta=xhat[1]; pause(1000); // Pause for 1 second before sampling data high(26); // Set LED on pin 26 high to show data sampling has begun //pwm_start(255); void data(); //volatile float gx, gy, gz, ax, ay, az; cog_run(data,128); // Start cog to get gyro and accel data while(1){ //cog_run(data,128); // Start cog to get gyro and accel data// Extended Kalman filter implmentation ///////////////////// // Do Prediction Step, note: filter is not using accel measurements at this point, only gyro, so it drifts int N = 1; for(int i = 0; i < 1; i++){ //////////////////// // State derivatives float f[2] = { {gx+gy*sin(phi)*tan(theta)+gz*cos(phi)*tan(theta)}, {gy*cos(phi)-gz*sin(phi)} }; /////////////////// // Linearization of state derivatives float A[2][2] = { {gy*cos(phi)*tan(theta)-gz*sin(phi)*tan(theta), gz*cos(phi)*(square(tan(theta))+1)+gy*sin(phi)*(square(tan(theta))+1)}, {-gz*cos(phi)-gy*sin(phi) , 0 } }; ////////////////////// // Calculate xhat // xhat = xhat + (Ts/N)*f xhat[0]=xhat[0]+(Ts/N)*f[0]; xhat[1]=xhat[1]+(Ts/N)*f[1]; //phi=xhat[0]; //theta=xhat[1]; ///////////////////// // Calculate covariance // P = P+(Ts/N)*(A*P+P*A'+Q); P[0][0]=P[0][0]+(Ts/N)*(2*A[0][0]*P[0][0]+A[0][1]*P[0][1]+A[0][1]*P[1][0]+Q[0][0]); P[0][1]=P[0][1]+(Ts/N)*(A[0][0]*P[0][1]+A[1][0]*P[0][0]+A[0][1]*P[1][1]+A[1][1]*P[0][1]+Q[0][1]); P[1][0]=P[1][0]+(Ts/N)*(A[0][0]*P[1][0]+A[1][0]*P[0][0]+A[0][1]*P[1][1]+A[1][1]*P[1][0]+Q[1][0]); P[1][1]=P[1][1]+(Ts/N)*(A[1][0]*P[0][1]+A[1][0]*P[1][0]+2*A[1][1]*P[1][1]+Q[1][1]); ////////////////////// /* // Print covariance matrix, P print("P =\n"); for(int i = 0; i < 2; i++){ for(int k = 0; k < 2; k++){ print("%f ", P[i][k]*180/PI); } print("\n"); } */ } phi=xhat[0]; theta=xhat[1]; // Do Update Step ///////////////////// // Declare vector of actual measurements float y[3][1] = { {ax*9.81}, {ay*9.81}, {az*-9.81} }; //////////////////// // Define mathematical model of measurements based on xhat float h[3][1] = { {gy*Va*sin(theta)+gravity*sin(theta)}, {gz*Va*cos(theta)-gx*Va*sin(theta)-gravity*cos(theta)*sin(phi)}, {-gy*Va*cos(theta)-gravity*cos(theta)*cos(phi)} }; /////////////////// // Linearization of mathematical model of measurements, note: this works and gets values from xhat above float C[3][2] = { { 0, gravity*cos(theta)+Va*gy*cos(theta)}, {-gravity*cos(phi)*cos(theta), gravity*sin(phi)*sin(theta)-Va*gz*sin(theta)-Va*gx*cos(theta)}, {gravity*cos(theta)*sin(phi), Va*gy*sin(theta)+gravity*cos(phi)*sin(theta)} }; /////////////////// // Kalman gain matrix // L = P*C'*(C*P*C'+R)^-1 // Compute s=C*P*C'+R s[0][0]=R[0][0] + C[0][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[0][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]); s[0][1]=R[0][1] + C[1][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[1][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]); s[0][2]=R[0][2] + C[2][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[2][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]); s[1][0]=R[1][0] + C[0][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[0][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]); s[1][1]=R[1][1] + C[1][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[1][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]); s[1][2]=R[1][2] + C[2][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[2][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]); s[2][0]=R[2][0] + C[0][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[0][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]); s[2][1]=R[2][1] + C[1][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[1][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]); s[2][2]=R[2][2] + C[2][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[2][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]); det_s[0] = s[0][0]*s[1][1]*s[2][2]+s[0][1]*s[1][2]*s[2][0]+s[0][2]*s[1][0]*s[2][1] // Calculate determinant of d -s[0][2]*s[1][1]*s[2][0]-s[0][1]*s[1][0]*s[2][2]-s[0][0]*s[1][2]*s[2][1]; adj_s[0][0] = s[1][1]*s[2][2]-s[1][2]*s[2][1]; // Calculate adjoint of d adj_s[0][1] = s[0][2]*s[2][1]-s[0][1]*s[2][2]; adj_s[0][2] = s[0][1]*s[1][2]-s[1][1]*s[0][2]; adj_s[1][0] = s[2][0]*s[1][2]-s[1][0]*s[2][2]; adj_s[1][1] = s[0][0]*s[2][2]-s[2][0]*s[0][2]; // adjoint code messes up accel x data --> adj_s and S were initialized with wrong size adj_s[1][2] = s[1][0]*s[0][2]-s[0][0]*s[1][2]; adj_s[2][0] = s[1][0]*s[2][1]-s[2][0]*s[1][1]; adj_s[2][1] = s[2][0]*s[0][1]-s[0][0]*s[2][1]; adj_s[2][2] = s[0][0]*s[1][1]-s[0][1]*s[1][0]; // Compute S=(C*P*C'+R)^-1 S[0][0]=adj_s[0][0]*1/det_s[0]; S[0][1]=adj_s[0][1]*1/det_s[0]; S[0][2]=adj_s[0][2]*1/det_s[0]; S[1][0]=adj_s[1][0]*1/det_s[0]; S[1][1]=adj_s[1][1]*1/det_s[0]; S[1][2]=adj_s[1][2]*1/det_s[0]; S[2][0]=adj_s[2][0]*1/det_s[0]; S[2][1]=adj_s[2][1]*1/det_s[0]; S[2][2]=adj_s[2][2]*1/det_s[0]; // Compute L = P*C'*(C*P*C'+R)^-1 L[0][0]=S[1][0]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][0]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][0]; L[0][1]=S[1][1]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][1]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][1]; L[0][2]=S[1][2]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][2]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][2]; L[1][0]=S[1][0]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][0]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][0]; L[1][1]=S[1][1]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][1]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][1]; L[1][2]=S[1][2]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][2]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][2]; ////////////////// // Define identity matrix the length of xhat (2x2) float I[2][2] = { {1,0}, {0,1} }; ///////////////// // Covariance matrix updated with measurement information (2x2) // P = (I-L*C)*P P[0][0]=-P[1][0]*(C[0][1]*L[0][0]+C[1][1]*L[0][1]+C[2][1]*L[0][2])-P[0][0]*(C[1][0]*L[0][1]+C[2][0]*L[0][2]-1); P[0][1]=-P[1][1]*(C[0][1]*L[0][0]+C[1][1]*L[0][1]+C[2][1]*L[0][2])-P[0][1]*(C[1][0]*L[0][1]+C[2][0]*L[0][2]-1); P[1][0]=-P[1][0]*(C[0][1]*L[1][0]+C[1][1]*L[1][1]+C[2][1]*L[1][2]-1)-P[0][0]*(C[1][0]*L[1][1]+C[2][0]*L[1][2]); P[1][1]=-P[1][1]*(C[0][1]*L[1][0]+C[1][1]*L[1][1]+C[2][1]*L[1][2]-1)-P[0][1]*(C[1][0]*L[1][1]+C[2][0]*L[1][2]); //xhat=xhat+L*(y-h) xhat[0]=xhat[0]-L[0][0]*(h[0][0]-y[0][0])-L[0][1]*(h[1][0]-y[1][0])-L[0][2]*(h[2][0]-y[2][0]); xhat[1]=xhat[1]-L[1][0]*(h[0][0]-y[0][0])-L[1][1]*(h[1][0]-y[1][0])-L[1][2]*(h[2][0]-y[2][0]); phi=xhat[0]; //Pitch theta=xhat[1]; //Roll ///////////////// /* phi_hat_unc = ; theta_hat_unc = ; */ /* // Print covariance matrix, P print("y-h =\n"); for(int i = 0; i < 3; i++){ for(int k = 0; k < 1; k++){ print("%f ", y[i][k]-h[i][k]); } print("\n"); } */ //print("phi=%f phi_dot=%f\n",phi*180/PI,gy*180/PI); //print("phi=%f theta=%f\n",phi*180/PI,theta*180/PI); //print("%f\n",phi*180/PI); //PI w/ rate feedback //float phi=(phi+PI)%(2*PI)-PI; float phi_mod = fmod(phi+PI,2*PI)-PI; float theta_mod = fmod(theta+PI,2*PI)-PI; //print("%f\n",phi_mod*180/PI); /*float Error=phi*180/PI;//-(-65); float Error_int=Error_int+Ts*Error; float servo=kp*Error+ki*Error_int-kd*gy*180/PI; */ float Error=theta*180/PI;//-(-65); float Error_int=Error_int+Ts*Error; float servo=kp*Error+ki*Error_int+kd*gx*180/PI; // Add PWM control for motors pwm_start(255); // Take outside of loop -> didn't work /* // Left Motor if(servo < 0){ pwm_set(3,0,-servo); pwm_set(1,1,-servo); } else if(servo > 0){ pwm_set(4,0,servo); pwm_set(2,1,servo); } */ if(servo<0){ low(1); low(3); pwm_set(2,0,-servo+4.); // when pwmstart = 100, set these to 1.8 pwm_set(4,1,-servo+3.); } else if(servo>0){ high(1); high(3); pwm_set(2,0,servo+4.); pwm_set(4,1,servo+2); } /* // Right Motor if(servo < 0){ pwm_set(1,1,-servo); } else if(servo > 0){ pwm_set(2,1,servo); } */ //servo_speed(12,-servo); //servo_speed(13,servo); //print("error=%f\n",Error); //print("Error_int=%f\n",Error_int); //print("servo=%f\n",servo); //print("theta=%f\n",theta*180/PI); print("gx=%f\n",gx*180/PI); //print("gx=%f\n",gx*180/PI); //print("%f\n",phi*180/PI); } } // Math Functions Below: square float square(float a){ b = a*a; return b; } void data(){ while(1){ // get gyroscope data // x-axis low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b10101000); //Send read register address (OUT_X_L) xL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101001); //Send read register address (OUT_X_H) xH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gx = (short) (((xH<<8) | xL)-gxAvg)*.0175*PI/180; // y-axis low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b10101010); //Send read register address (OUT_Y_L) yL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101011); //Send read register address (OUT_Y_H) yH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gy = (short) (((yH<<8) | yL)-gyAvg)*.0175*PI/180; // z-axis low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b11101100); //Send read register address (OUT_Z_L) zL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101101); //Send read register address (OUT_Z_H) zH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gz = (short) (((zH<<8) | zL)-gzAvg)*.0175*PI/180; //2's complement operation, subtract zero rate offset, multiply by //sensitivity value for 500deg/s, "short" needed for 2's complement //print("gx = %f gy = %f gz = %f\n",gx,gy,gz); pause(1); //Cycle at 20Hz // get accel data //x-axis low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0000110); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value x = shift_in(7, 8, MSBPRE, 8); // Get value from register x = x-xOffset; float ax = (float) x; ax = ax/64; high(6); //y-axis low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0000111); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value y = shift_in(7, 8, MSBPRE, 8); // Get value from register y = y-yOffset; float ay = (float) y; ay = ay/64; high(6); //z-axis low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0001000); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value z = shift_in(7, 8, MSBPRE, 8); // Get value from register z = z-zOffset; float az = (float) z; az = az/64; high(6); // De-select chip //print("ax = %f ay = %f az = %f\n",ax,ay,az); // Display measurement pause(1); // Loop at 20 Hz } }Thanks for the tip, I didn't think of that. I've moved everything to do with the sensors into the data() function. Unfortunately I'm still getting 0 for the gyroscope data. My updated code is below:
/* Implement the Extended Kalman Filter */ // Implement PWM motor control. // Try using more cores #include "simpletools.h" #include "servo.h" // Declare Math Functions float square(float a); // Declare square function // Variables for EKF float phi = 0; //180*PI/180; // Changing initial angle makes angle change, but converges back to zero float theta = 0; float phi_unc = 45*PI/180; // Initial roll angle uncertainty is 5 deg float theta_unc = 45*PI/180; // Initial pitch angle uncertainty is 5 deg float sigma_noise_gyro = 0.0037; // Process noise (guess for now) float sigma_noise_accel = 0.0001;//0.007;//0.001; // Measurement noise (guess for now), smaller value increases response but increases influence of lateral accels float gx, gy, gz; // Initialize gyro for testing purposes float ax, ay, az; // Initialize accel for testing purposes float Va = 0; // Initialize forward velocity, m/s float gravity = 9.81; //Initialize gravity, m/s^2 float Ts = 0.002;//0.01; // Sample time step (pause 10 ms in code) float b; // Declare b for square operation float s[3][3]; // [3][2] represents the size of the matrix (3x2) float S[3][3]; float det_s[0]; float adj_s[3][3]; float L[2][3]; float kp=.2;//.06; float ki=0; float kd=0;//0.003; int main(){ // Define EKF Initializiation Parameters ///////////////////// // Define error covariance matrix float P[2][2] = { {square(phi_unc), 0}, { 0, square(theta_unc)} }; ///////////////////// // Define process noise covariance matrix float Q[2][2] = { {square(sigma_noise_gyro), 0}, { 0, square(sigma_noise_gyro)} }; ///////////////////// // Define measurement noise covariance matrix float R[3][3] = { {square(sigma_noise_accel), 0, 0}, { 0, square(sigma_noise_accel), 0}, { 0, 0, square(sigma_noise_accel)} }; ///////////////////// // Define xhat float xhat[2] = { {phi}, {theta} }; //phi=xhat[0]; //theta=xhat[1]; pause(1000); // Pause for 1 second before sampling data high(26); // Set LED on pin 26 high to show data sampling has begun //pwm_start(255); void data(); cog_run(data,128); // Start cog to get gyro and accel data pause(5000); while(1){ // Extended Kalman filter implmentation ///////////////////// // Do Prediction Step, note: filter is not using accel measurements at this point, only gyro, so it drifts int N = 1; for(int i = 0; i < 1; i++){ //////////////////// // State derivatives float f[2] = { {gx+gy*sin(phi)*tan(theta)+gz*cos(phi)*tan(theta)}, {gy*cos(phi)-gz*sin(phi)} }; /////////////////// // Linearization of state derivatives float A[2][2] = { {gy*cos(phi)*tan(theta)-gz*sin(phi)*tan(theta), gz*cos(phi)*(square(tan(theta))+1)+gy*sin(phi)*(square(tan(theta))+1)}, {-gz*cos(phi)-gy*sin(phi) , 0 } }; ////////////////////// // Calculate xhat // xhat = xhat + (Ts/N)*f xhat[0]=xhat[0]+(Ts/N)*f[0]; xhat[1]=xhat[1]+(Ts/N)*f[1]; //phi=xhat[0]; //theta=xhat[1]; ///////////////////// // Calculate covariance // P = P+(Ts/N)*(A*P+P*A'+Q); P[0][0]=P[0][0]+(Ts/N)*(2*A[0][0]*P[0][0]+A[0][1]*P[0][1]+A[0][1]*P[1][0]+Q[0][0]); P[0][1]=P[0][1]+(Ts/N)*(A[0][0]*P[0][1]+A[1][0]*P[0][0]+A[0][1]*P[1][1]+A[1][1]*P[0][1]+Q[0][1]); P[1][0]=P[1][0]+(Ts/N)*(A[0][0]*P[1][0]+A[1][0]*P[0][0]+A[0][1]*P[1][1]+A[1][1]*P[1][0]+Q[1][0]); P[1][1]=P[1][1]+(Ts/N)*(A[1][0]*P[0][1]+A[1][0]*P[1][0]+2*A[1][1]*P[1][1]+Q[1][1]); ////////////////////// /* // Print covariance matrix, P print("P =\n"); for(int i = 0; i < 2; i++){ for(int k = 0; k < 2; k++){ print("%f ", P[i][k]*180/PI); } print("\n"); } */ } phi=xhat[0]; theta=xhat[1]; // Do Update Step ///////////////////// // Declare vector of actual measurements float y[3][1] = { {ax*9.81}, {ay*9.81}, {az*-9.81} }; //////////////////// // Define mathematical model of measurements based on xhat float h[3][1] = { {gy*Va*sin(theta)+gravity*sin(theta)}, {gz*Va*cos(theta)-gx*Va*sin(theta)-gravity*cos(theta)*sin(phi)}, {-gy*Va*cos(theta)-gravity*cos(theta)*cos(phi)} }; /////////////////// // Linearization of mathematical model of measurements, note: this works and gets values from xhat above float C[3][2] = { { 0, gravity*cos(theta)+Va*gy*cos(theta)}, {-gravity*cos(phi)*cos(theta), gravity*sin(phi)*sin(theta)-Va*gz*sin(theta)-Va*gx*cos(theta)}, {gravity*cos(theta)*sin(phi), Va*gy*sin(theta)+gravity*cos(phi)*sin(theta)} }; /////////////////// // Kalman gain matrix // L = P*C'*(C*P*C'+R)^-1 // Compute s=C*P*C'+R s[0][0]=R[0][0] + C[0][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[0][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]); s[0][1]=R[0][1] + C[1][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[1][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]); s[0][2]=R[0][2] + C[2][0]*(C[0][0]*P[0][0] + C[0][1]*P[1][0]) + C[2][1]*(C[0][0]*P[0][1] + C[0][1]*P[1][1]); s[1][0]=R[1][0] + C[0][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[0][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]); s[1][1]=R[1][1] + C[1][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[1][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]); s[1][2]=R[1][2] + C[2][0]*(C[1][0]*P[0][0] + C[1][1]*P[1][0]) + C[2][1]*(C[1][0]*P[0][1] + C[1][1]*P[1][1]); s[2][0]=R[2][0] + C[0][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[0][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]); s[2][1]=R[2][1] + C[1][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[1][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]); s[2][2]=R[2][2] + C[2][0]*(C[2][0]*P[0][0] + C[2][1]*P[1][0]) + C[2][1]*(C[2][0]*P[0][1] + C[2][1]*P[1][1]); det_s[0] = s[0][0]*s[1][1]*s[2][2]+s[0][1]*s[1][2]*s[2][0]+s[0][2]*s[1][0]*s[2][1] // Calculate determinant of d -s[0][2]*s[1][1]*s[2][0]-s[0][1]*s[1][0]*s[2][2]-s[0][0]*s[1][2]*s[2][1]; adj_s[0][0] = s[1][1]*s[2][2]-s[1][2]*s[2][1]; // Calculate adjoint of d adj_s[0][1] = s[0][2]*s[2][1]-s[0][1]*s[2][2]; adj_s[0][2] = s[0][1]*s[1][2]-s[1][1]*s[0][2]; adj_s[1][0] = s[2][0]*s[1][2]-s[1][0]*s[2][2]; adj_s[1][1] = s[0][0]*s[2][2]-s[2][0]*s[0][2]; // adjoint code messes up accel x data --> adj_s and S were initialized with wrong size adj_s[1][2] = s[1][0]*s[0][2]-s[0][0]*s[1][2]; adj_s[2][0] = s[1][0]*s[2][1]-s[2][0]*s[1][1]; adj_s[2][1] = s[2][0]*s[0][1]-s[0][0]*s[2][1]; adj_s[2][2] = s[0][0]*s[1][1]-s[0][1]*s[1][0]; // Compute S=(C*P*C'+R)^-1 S[0][0]=adj_s[0][0]*1/det_s[0]; S[0][1]=adj_s[0][1]*1/det_s[0]; S[0][2]=adj_s[0][2]*1/det_s[0]; S[1][0]=adj_s[1][0]*1/det_s[0]; S[1][1]=adj_s[1][1]*1/det_s[0]; S[1][2]=adj_s[1][2]*1/det_s[0]; S[2][0]=adj_s[2][0]*1/det_s[0]; S[2][1]=adj_s[2][1]*1/det_s[0]; S[2][2]=adj_s[2][2]*1/det_s[0]; // Compute L = P*C'*(C*P*C'+R)^-1 L[0][0]=S[1][0]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][0]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][0]; L[0][1]=S[1][1]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][1]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][1]; L[0][2]=S[1][2]*(C[1][0]*P[0][0]+C[1][1]*P[0][1])+S[2][2]*(C[2][0]*P[0][0]+C[2][1]*P[0][1])+C[0][1]*P[0][1]*S[0][2]; L[1][0]=S[1][0]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][0]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][0]; L[1][1]=S[1][1]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][1]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][1]; L[1][2]=S[1][2]*(C[1][0]*P[1][0]+C[1][1]*P[1][1])+S[2][2]*(C[2][0]*P[1][0]+C[2][1]*P[1][1])+C[0][1]*P[1][1]*S[0][2]; ////////////////// // Define identity matrix the length of xhat (2x2) float I[2][2] = { {1,0}, {0,1} }; ///////////////// // Covariance matrix updated with measurement information (2x2) // P = (I-L*C)*P P[0][0]=-P[1][0]*(C[0][1]*L[0][0]+C[1][1]*L[0][1]+C[2][1]*L[0][2])-P[0][0]*(C[1][0]*L[0][1]+C[2][0]*L[0][2]-1); P[0][1]=-P[1][1]*(C[0][1]*L[0][0]+C[1][1]*L[0][1]+C[2][1]*L[0][2])-P[0][1]*(C[1][0]*L[0][1]+C[2][0]*L[0][2]-1); P[1][0]=-P[1][0]*(C[0][1]*L[1][0]+C[1][1]*L[1][1]+C[2][1]*L[1][2]-1)-P[0][0]*(C[1][0]*L[1][1]+C[2][0]*L[1][2]); P[1][1]=-P[1][1]*(C[0][1]*L[1][0]+C[1][1]*L[1][1]+C[2][1]*L[1][2]-1)-P[0][1]*(C[1][0]*L[1][1]+C[2][0]*L[1][2]); //xhat=xhat+L*(y-h) xhat[0]=xhat[0]-L[0][0]*(h[0][0]-y[0][0])-L[0][1]*(h[1][0]-y[1][0])-L[0][2]*(h[2][0]-y[2][0]); xhat[1]=xhat[1]-L[1][0]*(h[0][0]-y[0][0])-L[1][1]*(h[1][0]-y[1][0])-L[1][2]*(h[2][0]-y[2][0]); phi=xhat[0]; //Pitch theta=xhat[1]; //Roll float Error=theta*180/PI;//-(-65); float Error_int=Error_int+Ts*Error; float servo=kp*Error+ki*Error_int+kd*gx*180/PI; // Add PWM control for motors if(servo<0){ low(1); low(3); pwm_set(2,0,-servo+5.); // when pwmstart = 100, set these to 1.8 pwm_set(4,1,-servo+5.); } else if(servo>0){ high(1); high(3); pwm_set(2,0,servo+5.); pwm_set(4,1,servo+5); } print("gx=%f\n",gx*180/PI); } } // Math Functions Below: square float square(float a){ b = a*a; return b; }void data(){ // Variables for gyro signed int whoAmI; signed int xL, xH, yL, yH, zL, zH; signed int statusReg; float gx, gxSum, gxAvg, gy, gySum, gyAvg, gz, gzSum, gzAvg; // Variables for accel signed char x, y, z; // Z-axis value, *must be a signed char* int axSum, axAvg, xOffset, aySum, ayAvg, yOffset, azSum, azAvg, zOffset; int CS=11; int SCL=10; int SDA=9; // get gyroscope initializations high(CS); //CS line high (SPI inactive) low(SCL); //CLK line low (prepares chip to send brief high signals or clock pulses) low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10001111); //Send read register address (whoAmI) whoAmI = shift_in(SDA, SCL, MSBPRE, 8); high(CS); /* if(whoAmI == 0b11010011){ print("Device ID Correct: %b\n", whoAmI); } else{ print("Device ID Incorrect: %b\n", whoAmI); } */ //Register 4 - Set 3-wire SPI low(CS); //CS line low (start SPI) shift_out(SDA, SCL, MSBFIRST, 8, 0b00100011); //Write MCTL register (register 4)to set 3-wire SPI shift_out(SDA, SCL, MSBFIRST, 8, 0b10010001); //Value for MCTL register (register 4) to set 3-wire SPI high(CS); //Register 1 - Set 100Hz output rate, 25Hz cutoff freq, normal mode, enable all axes low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b01100000); //Write MCTL register (register 1) shift_out(SDA, SCL, MSBFIRST, 8, 0b00011111); //Value for MCTL register (register 1) high(CS); //CS high(stop SPI) pause(100); //Get x-axis zero-rate offset (xAvg) for(int i=0; i<100; i++){ low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b10101000); //Send read register address (OUT_X_L) xL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101001); //Send read register address (OUT_X_H) xH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gx = (short) ((xH<<8) | xL); gxSum += gx; } gxAvg = (gxSum/100)*PI/180; //print("gxAvg = %f\n", gxAvg); //Get y-axis zero-rate offset (yAvg) for(int i=0; i<100; i++){ low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b10101010); //Send read register address (OUT_Y_L) yL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101011); //Send read register address (OUT_Y_H) yH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gy = (short) ((yH<<8) | yL); gySum += gy; } gyAvg = (gySum/100)*PI/180; //print("gyAvg = %f\n", gyAvg); //Get z-axis zero-rate offset (zAvg) for(int i=0; i<100; i++){ low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b11101100); //Send read register address (OUT_Z_L) zL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101101); //Send read register address (OUT_Z_H) zH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gz = (short) ((zH<<8) | zL); gzSum += gz; } gzAvg = (gzSum/100)*PI/180; //print("gzAvg = %f\n", gzAvg); // get accel initializations high(6); // CS line high (inactive) low(8); // CLK line low low(6); // CS line low (start SPI) shift_out(7, 8, MSBFIRST, 7, 0b1010110); // Write MCTL register shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care bit shift_out(7, 8, MSBFIRST, 8, 0b01100101); // Value for MCTL register high(6); // CS line high (stop SPI) pause(1); //Get x-axis zero-rate offset (zAvg) for(int i=0; i<100; i++){ low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0000110); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value x = shift_in(7, 8, MSBPRE, 8); // Get value from register high(6); // De-select chip axSum += x; } axAvg = axSum/100; xOffset = axAvg; //print("xOffset = %d\n", xOffset); //Get y-axis zero-rate offset (zAvg) for(int i=0; i<100; i++){ low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0000111); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value y = shift_in(7, 8, MSBPRE, 8); // Get value from register high(6); // De-select chip aySum += y; } ayAvg = aySum/100; yOffset = ayAvg; //print("yOffset = %d\n", yOffset); //Get z-axis zero-rate offset (zAvg) for(int i=0; i<100; i++){ low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0001000); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value z = shift_in(7, 8, MSBPRE, 8); // Get value from register high(6); // De-select chip azSum += z; } azAvg = azSum/100; zOffset = azAvg-64; //print("zOffset = %d\n", zOffset); while(1){ // get gyroscope data // x-axis low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b10101000); //Send read register address (OUT_X_L) xL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101001); //Send read register address (OUT_X_H) xH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gx = (short) (((xH<<8) | xL)-gxAvg)*.0175*PI/180; // y-axis low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b10101010); //Send read register address (OUT_Y_L) yL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101011); //Send read register address (OUT_Y_H) yH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gy = (short) (((yH<<8) | yL)-gyAvg)*.0175*PI/180; // z-axis low(CS); //CS low selects chip shift_out(SDA, SCL, MSBFIRST, 8, 0b11101100); //Send read register address (OUT_Z_L) zL = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); //De-select chip with CS line high low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10101101); //Send read register address (OUT_Z_H) zH = shift_in(SDA, SCL, MSBPRE, 8); //Get value from register high(CS); gz = (short) (((zH<<8) | zL)-gzAvg)*.0175*PI/180; //2's complement operation, subtract zero rate offset, multiply by //sensitivity value for 500deg/s, "short" needed for 2's complement //print("gx = %f gy = %f gz = %f\n",gx,gy,gz); pause(1); //Cycle at 20Hz // get accel data //x-axis low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0000110); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value x = shift_in(7, 8, MSBPRE, 8); // Get value from register x = x-xOffset; float ax = (float) x; ax = ax/64; high(6); //y-axis low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0000111); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value y = shift_in(7, 8, MSBPRE, 8); // Get value from register y = y-yOffset; float ay = (float) y; ay = ay/64; high(6); //z-axis low(6); // CS low selects chip shift_out(7, 8, MSBFIRST, 7, 0b0001000); // Send read register address, z-axis shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care value z = shift_in(7, 8, MSBPRE, 8); // Get value from register z = z-zOffset; float az = (float) z; az = az/64; high(6); // De-select chip //print("ax = %f ay = %f az = %f\n",ax,ay,az); // Display measurement pause(1); // Loop at 20 Hz return gx, gy, gz, ax, ay, az; } }int main () { int CS = 1; int SCL = 2; int SDA = 3; // Do gyro setup routines while (1) { short x, y, z; x = readGyro(CS, SCL, SDA, 0b10101000); y = readGyro(CS, SCL, SDA, 0b10101010); z = readGyro(CS, SCL, SDA, 0b11101100); print("x=%d\n", x); print("y=%d\n", y); print("z=%d\n", z); pause(1); } }I'd even create a reusable function for doing the gyro setup routines. It can have a similar signature, like void initializeGyro(int cs, int scl, int sda)". There's a lot of sensor initialization code that get in the way of understanding the meat of your program.
Once you've successfully tested your readGyro() function, do the same for reading from the accelerometer. Once you've tested that, put them together and test the two in a single cog. Once you've done that, put it aside.
Now see if you can get two different cogs each blinking an LED. Got it? Working? Great.
Next, try to get one cog blinking an LED and another cog reading the gyro and accel.
With that working, step toward inter-cog communication. Try to store the variables from the accelerometer and the gyro globally and print them to the serial terminal in the main cog.
Okay, put alllll of that aside and test your complex math algorithm stuff on a computer, with a real debugger and fast compiler and good editor. All that cool stuff. Fake some input data, do the math by hand, and make sure the output is correct. Cool.... now your complicated algorithm is complete.
Thheeeeeennnn you put it all together and test it on the Propeller.
The reason I'm breaking it down into such small steps is because
1) this is a large and complicated program you're writing. When I copied your two code snippets into my editor, I came up with almost 500 lines of code! That's a lot! That's WAY too much to test all at once.
2) It will help you break down logical pieces of the program into modular blocks and then turn those blocks into functions. Functions make your code more legible and make it easier for us to help you.
3) There are some pretty basic compilation failures (such as the data function having the incorrect signature). That says to me you're pretty new at this and therefore should start small and work your way up.
And finally, I've refactored a bit of your code with some reusable functions. Take a look at how much shorter the while-loop is inside the data() function and how much easier it is to read. You hardly even need comments anymore because the functions speak for themselves. In fact, if you extracted the address values for the axes (such as "int ACCELEROMETER_X_ADDRESS = 0b0000110"), there'd be absolutely no need for comments in that loop! Basic stuff like this make it shorter, more maintainable, contain fewer bugs, and make it more likely that I (and others?) will actually read the code you're posting instead of just posting generic and high-level answers to your questions.
uint16_t readGyro (int csPinNumber, int scl, int sda, uint8_t address); uint8_t readAccelerometer (int csPinNumber, uint8_t address); void data (void *par) { // Variables for gyro signed int whoAmI; signed int statusReg; float gxSum = 0, gxAvg, gySum = 0, gyAvg, gzSum = 0, gzAvg; // Variables for accel signed char x, y, z; // Z-axis value, *must be a signed char* int axSum = 0, axAvg, xOffset, aySum = 0, ayAvg, yOffset, azSum = 0, azAvg, zOffset; int CS = 11; int SCL = 10; int SDA = 9; // get gyroscope initializations high(CS); //CS line high (SPI inactive) low(SCL); //CLK line low (prepares chip to send brief high signals or clock pulses) low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b10001111); //Send read register address (whoAmI) whoAmI = shift_in(SDA, SCL, MSBPRE, 8); high(CS); /* if(whoAmI == 0b11010011){ print("Device ID Correct: %b\n", whoAmI); } else{ print("Device ID Incorrect: %b\n", whoAmI); } */ //Register 4 - Set 3-wire SPI low(CS); //CS line low (start SPI) shift_out(SDA, SCL, MSBFIRST, 8, 0b00100011); //Write MCTL register (register 4)to set 3-wire SPI shift_out(SDA, SCL, MSBFIRST, 8, 0b10010001); //Value for MCTL register (register 4) to set 3-wire SPI high(CS); //Register 1 - Set 100Hz output rate, 25Hz cutoff freq, normal mode, enable all axes low(CS); shift_out(SDA, SCL, MSBFIRST, 8, 0b01100000); //Write MCTL register (register 1) shift_out(SDA, SCL, MSBFIRST, 8, 0b00011111); //Value for MCTL register (register 1) high(CS); //CS high(stop SPI) pause(100); //Get x-axis zero-rate offset (xAvg) for (int i = 0; i < 100; i++) { gxSum += readGyro(CS, SCL, SDA, 0b10101000); } gxAvg = (gxSum / 100) * PI / 180; //print("gxAvg = %f\n", gxAvg); //Get y-axis zero-rate offset (yAvg) for (int i = 0; i < 100; i++) { gySum += readGyro(CS, SCL, SDA, 0b10101010); } gyAvg = (gySum / 100) * PI / 180; //print("gyAvg = %f\n", gyAvg); //Get z-axis zero-rate offset (zAvg) for (int i = 0; i < 100; i++) { gzSum += readGyro(CS, SCL, SDA, 0b11101100); } gzAvg = (gzSum / 100) * PI / 180; //print("gzAvg = %f\n", gzAvg); // get accel initializations high(6); // CS line high (inactive) low(8); // CLK line low low(6); // CS line low (start SPI) shift_out(7, 8, MSBFIRST, 7, 0b1010110); // Write MCTL register shift_out(7, 8, MSBFIRST, 1, 0b0); // Send don't care bit shift_out(7, 8, MSBFIRST, 8, 0b01100101); // Value for MCTL register high(6); // CS line high (stop SPI) pause(1); //Get x-axis zero-rate offset (zAvg) for (int i = 0; i < 100; i++) { axSum += readAccelerometer(6, 0b0000110); } axAvg = axSum / 100; xOffset = axAvg; //print("xOffset = %d\n", xOffset); //Get y-axis zero-rate offset (zAvg) for (int i = 0; i < 100; i++) { aySum += readAccelerometer(6, 0b0000110); } ayAvg = aySum / 100; yOffset = ayAvg; //print("yOffset = %d\n", yOffset); //Get z-axis zero-rate offset (zAvg) for (int i = 0; i < 100; i++) { azSum += readAccelerometer(6, 0b0000110); } azAvg = azSum / 100; zOffset = azAvg - 64; //print("zOffset = %d\n", zOffset); while (1) { // get gyroscope data gx = (short) (readGyro(CS, SCL, SDA, 0b10101000) - gxAvg) * .0175 * PI / 180; gy = (short) (readGyro(CS, SCL, SDA, 0b10101010) - gyAvg) * .0175 * PI / 180; gz = (short) (readGyro(CS, SCL, SDA, 0b11101100) - gzAvg) * .0175 * PI / 180; //sensitivity value for 500deg/s, "short" needed for 2's complement //print("gx = %f gy = %f gz = %f\n",gx,gy,gz); pause(1); //Cycle at 20Hz // get accel data //x-axis x = readAccelerometer(6, 0b0000110); ax = (x - xOffset) / 64; //y-axis y = readAccelerometer(6, 0b0000111); ay = (y - yOffset) / 64; //z-axis z = readAccelerometer(6, 0b0001000); az = (z - zOffset) / 64; //print("ax = %f ay = %f az = %f\n",ax,ay,az); // Display measurement pause(1); // Loop at 20 Hz } } uint16_t readGyro (int csPinNumber, int scl, int sda, uint8_t address) { low(csPinNumber); shift_out(sda, scl, MSBFIRST, 8, address); uint8_t lowByte = shift_in(sda, scl, MSBPRE, 8); shift_out(sda, scl, MSBFIRST, 8, address + 1); uint8_t highByte = shift_in(sda, scl, MSBPRE, 8); high(csPinNumber); return ((uint16_t) (highByte) << 16) + lowByte; } uint8_t readAccelerometer (int csPinNumber, uint8_t address) { low(csPinNumber); shift_out(7, 8, MSBFIRST, 7, address); shift_out(7, 8, MSBFIRST, 1, 0); uint8_t temp = shift_in(7, 8, MSBPRE, 8); high(csPinNumber); return temp; }I hope you've found this at least somewhat useful
Good luck!
You're reading that correctly. That was one of the things I caught while reading through his code too. The local gx/gy/gz and ax/ay/az variables are overriding the global ones, which is why the main cog isn't seeing any changes.
And the fact that there is a "return" statement in the data() function is one of the compiler errors I referred to in my post.
I'm not sure that you declared all the global variables you need to send data back and forth.
One other thing to watch out for is calculating 2 related variables (like an x & y pair) in one core and using them in another core. When you do that the program must send the data to the hub. Each core has access to the hub for one variable in a rotating manner. I had a program that read raw x & y values from a bluetooth joystick and calculated the x & y in one core and then through globals sent them to a second core where they were used to position a device. The program originally didn't work correctly. It took x and paired it with y from a previous reading. Since they were 8 bit values I fixed the program by calculating x & y in the first core (as local variables), then combining them in a single global integer. There are other ways to make sure that the values are correctly related when they move between different cog functions (I haven't gotten that experienced).
Hope this helps a little.
Tom
Select - address reg $28 - read byte - deslect
Select - address reg $29 - read byte - deslect
Select - address reg $2A - read byte - deslect
Select - address reg $2B - read byte - deslect
Select - address reg $2C - read byte - deslect
Select - address reg $2D - read byte - deslect
When all you have to do is (with auto-increment M/S set):
Select - address reg $28 (0b11101000) - read 8 bytes - deselect
You can assemble those bytes into words and save them while you are reading them or you can wait until you deselect and do that or whatever.
It mightn't matter too much now that it is running in its own cog but it is the sensible and practical thing to do. You don't order your fries one at a time, just grab the whole packet.