Is there a faster way to read sensor data?

amalfiCoast · 2018-11-23 23:42

I am reading all three axes of a gyroscope and accelerometer module using my own code below. The update rate using this code is about 70 Hz. However, I tried using my LSM9DS1 IMU module with the supplied library and the update rate is 500 Hz! My question is, what is the Parallax library doing that makes the update rate so fast and what about my code below makes it so slow? Thank you.

// 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
                                        
    // get accel data
    //x-axis
    low(6);                                                                  // CS low selects chip
    shift_out(7, 8, MSBFIRST, 8, 0b00001100);                                  // Send read register address, z-axis    
    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, 8, 0b00001110);                                  // Send read register address, z-axis    
    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, 8, 0b00010000);                                  // Send read register address, z-axis    
    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

T Chap · 2018-11-24 00:24

You can search “fastspin” and it will automatically convert to PASM. Then it will be a lot faster. There are several ways to convert it so you have to do a little learning on it.

Clock Loop · 2018-11-24 00:26

Well, do you know what settings the parallax example uses?

The LSM9DS1 is a versatile, motion-sensing system-in-a-chip. It houses a 3-axis accelerometer, 3-axis gyroscope, and 3-axis magnetometer – nine degrees of freedom (9DOF) in a single IC! Each sensor in the LSM9DS1 supports a wide range of…ranges: the accelerometer’s scale can be set to ± 2, 4, 8, or 16 g, the gyroscope supports ± 245, 500, and 2000 °/s, and the magnetometer has full-scale ranges of ± 2, 4, 12, or 16 gauss.

Since the chip has so many axis, and so many scales it can do, is your code is probably using the device at maximum , 16g, 2000 °/s, and 16 gauss?

This creates much more data to transfer and causes the lsm chip to take longer in its response.
After a quick look over the chip .h file --->https://www.parallax.com/sites/default/files/files/prod/docs/l/Learn-Folder-Updated-2018.10.02.zip (its deep in the sensors folder)

It shows that they use a data transmit method that looks like it would be faster than the SHIFT_OUT routine that you use.
Why don't you just include the lsm9ds1.h file and use it?

I didn't look at the parallax code to see what precision they are setting for the lsm9ds1 module in their example. But that is probably where you need to look. And the shift out is probably slow.

amalfiCoast · 2018-11-24 00:57

Hi T Chap,

Thanks for your response. I'd prefer to stick with C because that's what I'm used to and the rest of my program is also in C but if I can't get anything else to work then I will look further into PASM.

amalfiCoast · 2018-11-24 01:02

Hi Clock Loop,

Thanks for your response. I'll need to check to see if my settings are what Parallax is using. Thanks for that suggestion. Also, I thought I saw in the library's imu_read and imu_write functions that they use the shift_in and shift_out commands. I do think these commands are what is slowing down my code but I don't know of another way to do it yet. I feel like there should be something obvious going on here since the Parallax loop is running more than five times as fast as mine.

Clock Loop · 2018-11-24 01:06

When I used the shift out method to talk old serial lcd displayshttps://forums.parallax.com/discussion/81312/nokia-5160-cell-lcd-bs2, they had a cs need and were a shift clock interface also, I found I could talk to the display faster, by bit bashing it.

It takes a long time to write out a serial stream using that method, but that would let you avoid the shift out/in method.
AND as a bonus, if you do it this way, you can EAISLY use PASM. (but one of the prop pasm c gurus here need to convert this c into (c pasm) for us.)

low(SDA);     '(0)data output
low(SCL);      'clock the data into the chip.
high(SCL);    'clock the data into the chip.

high(SDA);   '(1)data output
low(SCL);      'clock the data into the chip.
high(SCL);    'clock the data into the chip.


zL = 0;         '(0) clear data input buffer
low(SCL);      'clock the data into the chip.
high(SCL);    'clock the data into the chip.
zL = SDA;     '(0 or 1)data input  (i don't know how to write correct c language for pin input bit state to variable)

etc... have fun?

amalfiCoast · 2018-11-24 01:21

Could you expand on the 'bit bashing' approach a little bit? I'm not quite sure I understand what it entails. Maybe I need to understand what the shift_in and shift_out commands actually do first.

Thanks,
David

Clock Loop · 2018-11-24 01:33

Well, i can by slowly typing out the data stream that results from these two lines of code.

shift_out(7, 8, MSBFIRST, 8, 0b00001110);                                  // Send read register address, z-axis

But even that will take me a long time to show you.

Then you would need input data also.

y = shift_in(7, 8, MSBPRE, 8);                                            // Get value from register

Which takes even longer, because you need to reconstruct the data into the proper sized variable instead of a bit stream of 1/0 and a variable with just a 1 or a 0.

Currently your method stores 32bits in a variable.

I am talking about storing 1 bit in a variable.
So you can see how LONG that will take to write out and then take all those bits and put them into a single variable.

Those two single lines of code end up being hundreds of lines using bit bashing.

Clock Loop · 2018-11-24 01:46

amalfiCoast wrote: »

Could you expand on the 'bit bashing' approach a little bit? I'm not quite sure I understand what it entails. Maybe I need to understand what the shift_in and shift_out commands actually do first.

Thanks,
David

Thats exaclty what i would need to go do because I do not know or remember how that method works exactly.

"SPI / I2C serial interfaces "

Shift out would use the spi interface, i think... but without any bells and whistles...(those might be in the i2c)

Data sheet is teh!
https://www.st.com/resource/en/datasheet/lsm9ds1.pdf

The LSM9DS1 includes an I 2 C serial bus
interface supporting standard and fast mode
(100 kHz and 400 kHz) and an SPI serial
standard interface.

amalfiCoast · 2018-11-24 02:47

Okay, here's something interesting. Parallax is using the LMM Main RAM memory model and I was using the CMM Main RAM Compact memory model. It seems like that is why there is such a difference in speed! However, now with all my other code, I'm getting an error: "Your program is too big for the memory model selected in the project." My hub overflowed by 3472 bytes. Is there any way to decrease the size of my program so it will fit in the LMM Main RAM memory model? I really need the speed that comes with that memory model.

Thanks,
David

Clock Loop · 2018-11-24 05:01

You must not be showing all your code, because your code has so little.
I still haven't installed simple ide because I am trying to compile it and haven't gotten it to work yet.. gotta catch up...

amalfiCoast · 2018-11-24 05:23

Clock Loop,

You're right I wasn't showing all my code. I was able to get the size down to where it would fit in the LMM memory model by hard coding values, pre-calculating values such as sensor offsets in a separate script, etc. Now the code runs extremely fast. Almost too fast to the point where it messes with the pwm_set() function. I use pwm_set() after a state estimate propagation and then again after a new state estimate from a correction. Those details don't matter but for some reason it only is able to handle one of these pwm_set() functions. I even tried passing the data to a separate function that would handle the pwm_set(), but that didn't work. My code is below, with the separate function I tried commented out. Sorry if this post is confusing.

/* Implement the Extended Kalman Filter */
// Implement PWM motor control.
// Try to hold Kalman gain constant to speed up code.
// Try to propagate estimates better.
// Implement new motor controller.
// Current standard.
// 23 Nov 2018
// Rev 24 cleaned up.
// Use LMM RAM memory model.
// Add servo function.
#include "simpletools.h"
#include "propeller.h"

// Declare Servo Function
motor(servo);
float servo;

// Declare PID Variables
float Error_prev = 0;
float dt = .0108;

// Declare Timing Variable
float startTime;
float delta = 0.005; // Initial guess of loop elapsed time
float start;

// 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;
float ax, ay, az;

// Variables for EKF
float phi = 0; // Changing initial angle makes angle change, but converges back to zero
float theta = 0;
float Va = 0; // Initialize forward velocity, m/s
float gravity = 9.81; //Initialize gravity, m/s^2
float L[2][3];
float kp=12;//for n=1:12;//for n=1:13;//for n=2:8;//160;//.06; 30
float ki=120;//for n=1:120;//for n=1:100;//for n=2:25;
float kd=.3;//for n=1:0.3;//for n=1:0.3;//for n=2:0.2;//-2;//0.003; 2.5
float motorOffset=20;
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)

  //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);
  
  gxAvg = -.61;
  gyAvg = .43;
  gzAvg = -.4;
  
  
  // 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, 8, 0b10101100);                                    // Write MCTL register
  shift_out(7, 8, MSBFIRST, 8, 0b01100101);                                   // Value for MCTL register
  
  high(6);                                                                    // CS line high (stop SPI)
  pause(1);
  
  xOffset = -7;
  yOffset = -19;
  zOffset = 3;
  
  // Define EKF Initializiation Parameters
  
  /////////////////////
  // Define xhat
  float xhat[2] = {
    {phi},
    {theta}
  };  
  
    L[0][0]=0;
    L[0][1]=-0.000742;
    L[0][2]=0;
    L[1][0]=0.000742;
    L[1][1]=0;
    L[1][2]=0;
  
  pwm_start(255);
  pause(1000); // Pause for 1 second before sampling data
  high(26);    // Set LED on pin 26 high to show data sampling has begun
  
  while(1){
  start = CNT;
    // 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
    // get accel data
    //x-axis
    low(6);                                                                  // CS low selects chip
    shift_out(7, 8, MSBFIRST, 8, 0b00001100);                                  // Send read register address, z-axis    
    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, 8, 0b00001110);                                  // Send read register address, z-axis    
    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, 8, 0b00010000);                                  // Send read register address, z-axis    
    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
  
    // Extended Kalman filter implmentation
    ///////////////////// 
    
    // Do Prediction Step, note: filter is not using accel measurements at this point, only gyro, so it drifts
    startTime = CNT; // Remember starting time
   
    int N = 1;
    for(int i = 0; i < N; i++){
      
    ////////////////////
    // State derivatives
    float f[2] = {
      {gy+gx*sin(phi)*tan(theta)+gz*cos(phi)*tan(theta)},
      {gx*cos(phi)-gz*sin(phi)}
    };  
   
    //////////////////////
    // Calculate xhat
    // xhat = xhat + (Ts/N)*f
    xhat[0]=xhat[0]+(delta/N)*f[0];
    xhat[1]=xhat[1]+(delta/N)*f[1];
    phi=xhat[0];
    theta=xhat[1];
    
    float Error=theta*180/PI;
    float Error_int = Error_int+dt*Error;
    float servo=kp*Error+ki*Error_int+kd*gx*180/PI;
    print("%f\n",theta*180/PI);
//    motor(servo);
   
    if(servo<0){
      //Motor 1
      high(0);
      low(1);
      pwm_set(2,0,-servo+motorOffset); // when pwmstart = 100, set these to 1.8
      //Motor 2
      high(3);
      low(4);
      pwm_set(5,1,-servo+motorOffset);
    }
    else if(servo>0){
      //Motor 1
      high(1);
      low(0);
      pwm_set(2,0,servo+motorOffset);
      //Motor 2
      high(4);
      low(3);
      pwm_set(5,1,servo+motorOffset);
    }   
            
     delta = (CNT-startTime)/80000000; // Number of elapsed clock cycles divided by 80MHz clock
  } 
    // print("delta = %f\n",delta);

    // 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] = {
      {gx*Va*sin(theta)+gravity*sin(theta)},
      {gz*Va*cos(theta)-gy*Va*sin(theta)-gravity*cos(theta)*sin(phi)},
      {-gx*Va*cos(theta)-gravity*cos(theta)*cos(phi)}
    };
    
    ///////////////////
    //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+dt*Error;
    float servo=kp*Error+ki*Error_int+kd*gx*180/PI;
//    motor(servo);
     
    if(servo<0){
      //Motor 1
      high(0);
      low(1);
      pwm_set(2,0,-servo+motorOffset); // when pwmstart = 100, set these to 1.8
      //Motor 2
      high(3);
      low(4);
      pwm_set(5,1,-servo+motorOffset);
    }
    else if(servo>0){
      //Motor 1
      high(1);
      low(0);
      pwm_set(2,0,servo+motorOffset);
      //Motor 2
      high(4);
      low(3);
      pwm_set(5,1,servo+motorOffset);
    }           
    
    dt = (CNT-start)/80000000; // Number of elapsed clock cycles divided by 80MHz clock
    print("%f\n",theta*180/PI);  
//    print("dt = %f\n",dt);  
  }  
}    
/*
motor(servo){
if(servo<0){
      //Motor 1
      high(0);
      low(1);
      pwm_set(2,0,-servo+motorOffset); // when pwmstart = 100, set these to 1.8
      //Motor 2
      high(3);
      low(4);
      pwm_set(5,1,-servo+motorOffset);
    }
    else if(servo>0){
      //Motor 1
      high(1);
      low(0);
      pwm_set(2,0,servo+motorOffset);
      //Motor 2
      high(4);
      low(3);
      pwm_set(5,1,servo+motorOffset);
    }   
  }    
  */

avsa242 · 2018-11-24 12:30

I'd suggest reading the sensor data several registers at a time. Right now, at least for the gyro data, you're reading each msb & lsb reg separately. You should be able to read all of them in at once (into a large enough var, of course) using something like:

shift_out(SDA, SCL, MSBFIRST, 8, 0b10101000)
y_raw = shift_in(7, 8, MSBPRE, 48);

and cull the bytes out afterwards. Might not save a whole lot, but should be something. Reading all of the data first and doing the processing afterwards would ensure you're working on the same "frame" of data, as well (it could very well be that everything happens fast enough that this wouldn't be an issue).

Cheers,
Jesse

amalfiCoast · 2018-11-24 16:27

I am still having an issue with the pwm_set() function not being able to handle the fast data rates but for now I am satisfied. I am only sending one pwm_set() command per loop iteration now. Ideally it should be two but I guess it's okay, because it works.

My next question would be, why does the LMM memory model speed up the code so significantly? It's amazing how much faster it runs.

For anyone interested, the code for my working balancing robot running an extended Kalman filter is below. I have pre-computed and fixed the Kalman gain matrix which is not optimal, but it works to speed up the code and ensure the program is able to fit inside the LMM memory model. Any questions, please ask. I'd be happy to help anyone with a similar project if I can.

/* Implement the Extended Kalman Filter */
// Implement PWM motor control.
// Try to hold Kalman gain constant to speed up code.
// Try to propagate estimates better.
// Implement new motor controller.
// Current standard.
// 23 Nov 2018
// Rev 24 cleaned up.
// Use LMM RAM memory model.
// Get rid of zero-rate offset.
// Only print/update servo at update step.
#include "simpletools.h"
#include "propeller.h"

// Declare PID Variables
float Error_prev = 0;
float dt = .0088;

// Declare Timing Variable
float startTime;
float delta = 0.00257; // Initial guess of loop elapsed time
float start;

// Variables for gyro
signed int xL, xH, yL, yH, zL, zH;
float gx, gxSum, gxAvg, gy, gySum, gyAvg, gz, gzSum, gzAvg;
int CS=11;
int SCL=10;
int SDA=9;

// 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;
float ax, ay, az;

// Variables for EKF
float phi = 0; // Changing initial angle makes angle change, but converges back to zero
float theta = 0;
float Va = 0; // Initialize forward velocity, m/s
float gravity = 9.81; //Initialize gravity, m/s^2
float L[2][3];
float kp=12;//for n=1:12;//for n=1:13;//for n=2:8;//160;//.06; 30
float ki=120;//for n=1:120;//for n=1:100;//for n=2:25;
float kd=.3;//for n=1:0.3;//for n=1:0.3;//for n=2:0.2;//-2;//0.003; 2.5
float motorOffset=35;

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)

  //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 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, 8, 0b10101100);                                    // Write MCTL register
  shift_out(7, 8, MSBFIRST, 8, 0b01100101);                                   // Value for MCTL register
  
  high(6);                                                                    // CS line high (stop SPI)
  pause(1);
  
  xOffset = -7;   // Found from separate script
  yOffset = -19;  // Found from separate script
  zOffset = 3;    // Found from separate script
  
  // Define EKF Initializiation Parameters
  
  /////////////////////
  // Define xhat
  float xhat[2] = {
    {phi},
    {theta}
  };  
  
  // Define Kalman gain matrix
    L[0][0]=0;                        // phi=0;
    L[0][1]=-0.000742;         // theta=0;
    L[0][2]=0;                        // phi_unc=5*PI/180;
    L[1][0]=0.000742;           // theta_unc=5*PI/180;
    L[1][1]=0;                        // sigma_noise_gyro=0.0037;
    L[1][2]=0;                        // sigma_noise_accel=10;
  
  pwm_start(255);
  pause(1000); // Pause for 1 second before sampling data
  high(26);    // Set LED on pin 26 high to show data sampling has begun
  
  while(1){
  start = CNT;
    // 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)*.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)*.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)*.0175*PI/180;     //2's complement operation, subtract zero rate offset, multiply by 
                                                          //sensitivity value for 500deg/s, "short" needed for 2's complement
    // get accel data
    //x-axis
    low(6);                                                                  // CS low selects chip
    shift_out(7, 8, MSBFIRST, 8, 0b00001100);                                  // Send read register address, z-axis    
    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, 8, 0b00001110);                                  // Send read register address, z-axis    
    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, 8, 0b00010000);                                  // Send read register address, z-axis    
    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
  
    // Extended Kalman filter implmentation
    ///////////////////// 
    
    // Do Prediction Step, note: filter is not using accel measurements at this point, only gyro, so it drifts
    startTime = CNT; // Remember starting time
   
    int N = 1;
    for(int i = 0; i < N; i++){
      
    ////////////////////
    // State derivatives
    float f[2] = {
      {gy+gx*sin(phi)*tan(theta)+gz*cos(phi)*tan(theta)},
      {gx*cos(phi)-gz*sin(phi)}
    };  
   
    //////////////////////
    // Calculate xhat
    // xhat = xhat + (Ts/N)*f
    xhat[0]=xhat[0]+(delta/N)*f[0];
    xhat[1]=xhat[1]+(delta/N)*f[1];
    phi=xhat[0];
    theta=xhat[1];           
    
    delta = (CNT-startTime)/80000000; // Number of elapsed clock cycles divided by 80MHz clock
  } 
//    print("delta = %f\n",delta);

    // 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] = {
      {gx*Va*sin(theta)+gravity*sin(theta)},
      {gz*Va*cos(theta)-gy*Va*sin(theta)-gravity*cos(theta)*sin(phi)},
      {-gx*Va*cos(theta)-gravity*cos(theta)*cos(phi)}
    };
    
    ///////////////////
    //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;
    float Error_int = Error_int+dt*Error;
    float servo=kp*Error+ki*Error_int+kd*gx*180/PI;
     
    if(servo<0){
      //Motor 1 (right motor)
      high(0);
      low(1);
      pwm_set(2,0,-servo+motorOffset+3);
      //Motor 2 (left motor)
      high(3);
      low(4);
      pwm_set(5,1,-servo+motorOffset);
    }
    else if(servo>0){
      //Motor 1 (right motor)
      high(1);
      low(0);
      pwm_set(2,0,servo+motorOffset+3);
      //Motor 2 (left motor)
      high(4);
      low(3);
      pwm_set(5,1,servo+motorOffset);
    }           
    
    dt = (CNT-start)/80000000; // Number of elapsed clock cycles divided by 80MHz clock
    print("%f\n",theta*180/PI);  
//    print("dt = %f\n",dt);  
  }  
}

pmrobert · 2018-11-24 23:44

Some of the referenced modes are not available. Cog, LMM and CMM are the ones whose descriptions you want to peruse.
https://github.com/parallaxinc/propgcc-docs/blob/master/doc/Memory.md

amalfiCoast · 2018-11-25 16:29

Thanks very much pmrobert.

MattMatz · 2018-11-26 09:20

Hi!

I wrote/stole that library from the Arduino/Sparkfun one for the same chip. I used shift-in and shift-out just like you did, so I don't think the speed difference is there. Just a quick glance at your code in the original post shows a lot of float operations, and a lot of casts from int to float.

Floating point on the Prop, especially in C is time-consuming. I avoid it until the very end, or simply don't use it at all. Try doing all of your math with integers right up to the end - that may help.

Is there a faster way to read sensor data?

Comments