This thread has been locked.

If you have a related question, please click the "Ask a related question" button in the top right corner. The newly created question will be automatically linked to this question.

MSP430F5xx 9-Axis Sensor Fusion App-Note (DCM)

Other Parts Discussed in Thread: MSP430F5529, DRV8843, MSP-TS430PN80USB

Hi there,

just stumbled over SLAA518 App Note: Nine-Axis Sensor Fusion Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family (http://www.ti.com/mcu/docs/litabsmultiplefilelist.tsp?sectionId=96&tabId=1502&literatureNumber=slaa518&docCategoryId=1&familyId=342).

Will TI release the hardware presented in this AppNote as EVM? Were to find more data on this hardware (i.e. Eagle design files (prefered), Gerber files, ..)?

Rgds
aBUGSworstnightmare

  • Hi -

    Thanks for your interest in the app note!  We will release an update to the App note on Feb 20th that will include a software update and the Gerber files for the board.

    Regards,

    Mark Saxton

  • Hi Mark!

      I just finished designing a board with an MSP430F5529, HMC5883L Magnetometer, ITG3200 Gyros, MMA8451QT Accelerometers and an XBee radio and then BAM you beat me to it!

    I'd love to see your code but the link in the app note is broken.

    Also what was the USB stack size that you ended up with?

     

    Great work!

    Regards,

    Chris Lewis.

  • Chris - 

    Appreciate your interest and thanks for your comments.  Credit goes to the App note authors and the Open Source community!

    Sorry about the link - I've had a few pings on that today, I'm working to get it fixed.  In the meantime, try the below, please - 

    http://www.ti.com/mcu/docs/litabsmultiplefilelist.tsp?sectionId=96&tabId=1502&literatureNumber=slaa518a&docCategoryId=1&familyId=342

    Erick Macias (one of the authors) wrote the code, I'll ask him to chime in on the USB stack specifics.

    Regards,

    Mark

  • Chris,

    The USB CDC stack resides in flash memory and its footprint was around 4.5 kB.

    Let me know if you have more questions.

    -Erick

  • Hi Erik,

      Thanks for the reply.

    I've been trying out your code on my hardware (which is very similar to yours) and have gotten everything going except that I have some very strange things happening inside the magnetometer calibrate routine.

    Because it looks to me that some of the variables aren't getting copied correctly (eg x to mx in the line " if (x>mx) mx=x; ") I thought that it might be heap related.

    Can you tell me what your stack and heap sizes are?

    Also I found that the compiler option --printf_option was set to minimal which prevented successful comms to the GUIs because printf didn't process the floats.

     

    Cheers,

     

    Chris Lewis.

     

  • Hello Chris,

    The stack size is 160 bytes and the heap size is 160 bytes, these are the default values for CCS 5.1.0.09 and for IAR Embedded Workbench 5.20.1.

    I have a couple of questions to better assist you in narrowing down this issue.

    chris lewis said:

    I've been trying out your code on my hardware (which is very similar to yours)  

    Q1. What is the package for the MSP430F5529?

    Q2. What are your delays like between each read of the magnetometer inside the calibrate function?

    I have seen something similar to what you are describing, and the root cause of the problem was my delays were not 100 mS between each read of the magnetometer inside the calibrate function ( HMC58x3.c).

    Best regards,
    Erick

  • Hi Erik,

      Thanks for getting back to me.

    In the mean time I've sorted the problem & it all works now.

    The root cause was me doing something dumb. I had inverted the magnetometer data on the x & y channels to compensate for my placement of the device & not considered the impact on the calibrate routine.

    The issue with variables not copying properly seems to be related to the IDE (CCS 5.1.1.00031) not reporting correctly but I haven't chased this further.

    My configuration is as follows:

    MSP430F5529 in the 80 lead QFP

     HMC5883L Magnetometer

     ITG3200 Gyros

     MMA8451QT Accelerometers

    XBee radio

    2 x DRV8843 to drive up to 4 motors

    2 x quadrature motor encoder inputs

    4 x model servo channels

    MaxSonar MB1010 ultrasonic range finder

    Sharp 2Y0A02 optical range finder

    As you have probably guessed its a balancing robot and I'm using your code to jump start the project.

     

    All the best,

    Chris.

  • Chris,

    I'm glad to hear you were able to get the code running.

    chris lewis said:

    The root cause was me doing something dumb. I had inverted the magnetometer data on the x & y channels to compensate for my placement of the device & not considered the impact on the calibrate routine.

    The issue with variables not copying properly seems to be related to the IDE (CCS 5.1.1.00031) not reporting correctly but I haven't chased this further.

    Check if you make the variables that are not updating correctly type volatiles, and check if the memory window updates them correclty.  

    Let me know if you have any other questions.

    Best regards,

    Erick

  • Hi Erick

    i have a MSP-TS430PN80USB with M430F5529 and I want to implement the HID sensor fusion with this platform.

    May I have the info of the sensor fusion firmware code implementation?

    I have an example of HID descriptor for Keyboard/Mouse and it's interface protocol is 1 and 2. 

    But, What's the HID descriptor and Interface Protocol code of Sensor Collection and each sensor ?

    Ambient Light, Magnetometer, Gyroscope, and Accelerometer...? Thanks!

  • Hello all.

    Could anybody repost the links for the app note and the zip file with the code? I have started to mess with the sensor hub, and I think this note will be very interesting.

    In fact, I come from here:

    Thank you.

    Regards

  • Erick,

    In looking at this Application Note, I am trying to wrap my head around the DCM Algorithm. In section 2, this document explains that the accelerometer can be used as an orientation reference in the X and Z axis. From my understanding, isn't rotation about the Z axis traditionally what we consider "yaw"? If so, and if the board is on a flat surface, the acceleration due to the forces of gravity would not change as this board is rotated around Z.

    Also, this paragraph states that "The heading of the system used as a reference vector in the Y axis (yaw error)".

    My question is, based on the software written for this PCB, what orientation is this board meant to be? Is rotation about the Z axis what you consider "yaw"? Is the Y axis the reference for direction (compass needle)?

    thanks

    craig

  • PAk SY,

    Did you test this fusion code result ? did it get the correct yaw, pitch & roll rotation? 

    Which version (Fixed/Float) you test on MSP430?  could got the correct result of sin/cos/atan2 caculation?

  • JackRose Kuo said:

    PAk SY,

    Did you test this fusion code result ? did it get the correct yaw, pitch & roll rotation? 

    Which version (Fixed/Float) you test on MSP430?  could got the correct result of sin/cos/atan2 caculation?

    No I didn't. I couldn't download the app note, later I did not have the time to test it.

    Sorry.

  • Hi TI     ( )

    I check the "MSP430_AHRS_9_Axis_DCM_Float" example code, but just can't caculate the correct fusion result.

    (p.s. the authors are E. Macias / D. Torres / S. Ravindran.)

    6562.Magnetometer.c
    /** 
     * @file Magnetometer.c
     * 
     * @detail Description : 
     * Magnetometer function calls
     * 
     * @author E. Macias / D. Torres / S. Ravindran
     * @author Texas Instruments, Inc
     * @date December, 2011
     * @version 1.0 - Initial version
     * @note Built with IAR Embedded Workbench: 5.20.1 and CCS Version 5.1.0.09000
     **/
    
    /* ******************************************************* */
    /* I2C code for HMC5583 magnetometer                       */
    /*                                                         */
    /* ******************************************************* */
    
    #include "Magnetometer.h"
    
    /**
    * @brief <b>Function Name</b>:     : Init_Compass                                               
    * @brief  <b>Description</b>: This function initializes the magnetometer, by 
    * enabling the continous-measurement mode and setting the gain configuration to
    * +- .88 Ga ( Gain value = 0). Also, it calculates the scaled values for each 
    * axes with calibrate().
    * as the clock reference.
    * @param Input Parameters: None
    * @return Return Values: None
    **/ 
    void Init_Compass()
    {
      // Magnetometer
      I2C_Set_Slave_Address(HMC58X3_ADDR);
      Sleep(20);
      
      /*************************** Initialization *********************************/
      global_tx_buffer[0] = HMC58X3_R_CONFA;	// Register
      global_tx_buffer[1] = 0x70; 	              // 8 samples averaged per measurement
      I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
      
      global_tx_buffer[0] = HMC58X3_R_CONFB;	// Register
      global_tx_buffer[1] = 0xA0; 	              // Gain configurated to +-4.7 Ga
      I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
      
      global_tx_buffer[0] = HMC58X3_R_MODE;	// Register
      global_tx_buffer[1] = 0x00; 	        // Continuous-measurement mode enabled
      I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
      
      // Calibrate HMC using self test, not recommended to change the gain after calibration.
      calibrate(0); // Use gain 1=default, valid 0-7, 7 not recommended.
      
      // Single mode conversion was used in calibration, now set continuous mode
      global_tx_buffer[0] = HMC58X3_R_MODE; // Register
      global_tx_buffer[1] = 0x00; // Set Mode to 0x00
      I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
    }
    
    /**
    * @brief <b>Function Name</b>:     : Read_Compass                                               
    * @brief  <b>Description</b>: This function calls getRaw(), to read the magnetometer's 
    * raw x, y, and z value, stores it in the sen_data structure. Afterwards it 
    * stores the float values in the sen_data structure by dividing the raw value by
    * the scale values for each axes. The offset for each axes is then subtracted from
    * the scaled raw values. Soft calibration is applied to the hard calibrated values
    * and the heading of the AHRS board is calculated calling Compass_Heading(). 
    * @param Input Parameters: None
    * @return Return Values: None
    **/ 
    void Read_Compass(void)
    {
        
              float magRot_x;
              float magRot_y;
              float magRot_z;          
              
              
              getRaw(&sen_data.magnetom_x_raw,&sen_data.magnetom_y_raw,&sen_data.magnetom_z_raw);
              
              // Scale Raw data
              sen_data.magnetom_x = ((float) sen_data.magnetom_x_raw) / x_scale;
              sen_data.magnetom_y = ((float) sen_data.magnetom_y_raw) / y_scale;
              sen_data.magnetom_z = ((float) sen_data.magnetom_z_raw) / z_scale;
    
              //===============================================
              // Apply Hard iron compensation 
              // This removes effects of permanetly magnetic material
              // that is on the sensor platform
              // (offset X,Y,and Z of raw data)
              
              sen_data.magnetom_x -= sen_offset.magnetom_offset[0];
              sen_data.magnetom_y -= sen_offset.magnetom_offset[1];
              sen_data.magnetom_z -= sen_offset.magnetom_offset[2];
              
              
              
              //===============================================
              // Apply Soft iron compensation
              // This removes the distortion caused by ferrous 
              // material that is on the sensor platform
              // (turn the elipse into a circle)
       
              //===============================================
              //Apply correction to the X axis
              
              //rotate the XY vector
              magRot_x = sen_data.magnetom_x * cos(sen_offset.magnetom_XY_Theta) + sen_data.magnetom_y *sin(sen_offset.magnetom_XY_Theta);
              magRot_y = sen_data.magnetom_y * cos(sen_offset.magnetom_XY_Theta) - sen_data.magnetom_x *sin(sen_offset.magnetom_XY_Theta);
              
              //Scale the X
              sen_data.magnetom_x = magRot_x/sen_offset.magnetom_XY_Scale;
              
              //rotate the XY vector back
              sen_data.magnetom_x = sen_data.magnetom_x * cos(-sen_offset.magnetom_XY_Theta) + magRot_y * sin(-sen_offset.magnetom_XY_Theta);
              sen_data.magnetom_y = magRot_y * cos(-sen_offset.magnetom_XY_Theta) + sen_data.magnetom_x * sin(-sen_offset.magnetom_XY_Theta);
              
              
              
              //===============================================
              //Apply correction to the Z axis
              
              //rotate the YZ vector
              magRot_z = sen_data.magnetom_z * cos(sen_offset.magnetom_YZ_Theta) + sen_data.magnetom_y *sin(sen_offset.magnetom_YZ_Theta);
              magRot_y = sen_data.magnetom_y * cos(sen_offset.magnetom_YZ_Theta) - sen_data.magnetom_z *sin(sen_offset.magnetom_YZ_Theta);
              
              //Scale the Z
              sen_data.magnetom_z = magRot_z/sen_offset.magnetom_YZ_Scale;
              
              //rotate the YZ vector back
              sen_data.magnetom_z = sen_data.magnetom_z * cos(-sen_offset.magnetom_YZ_Theta) + magRot_y * sin(-sen_offset.magnetom_YZ_Theta);
              sen_data.magnetom_y = magRot_y * cos(-sen_offset.magnetom_YZ_Theta) + sen_data.magnetom_z * sin(-sen_offset.magnetom_YZ_Theta);
              
              
              //===============================================
              // Now that the magnetometer data is corrected,
              // we can calculate our magnetic heading
              Compass_Heading(); // Calculate magnetic heading 
    }
    
    
    /**
    * @brief <b>Function Name</b>:     : Compass_Heading                                               
    * @brief  <b>Description</b>: This function calculates the heading of the AHRS
    * board using the hard and soft calibrated magnetometer values. Then stores the
    * result in the sen_data structure. 
    * @param Input Parameters: None
    * @return Return Values: None
    **/ 
    void Compass_Heading(void)
    {
      float MAG_X;
      float MAG_Y;
      float cos_roll;
      float sin_roll;
      float cos_pitch;
      float sin_pitch;
      
      cos_roll = cos(roll);
      sin_roll = sin(roll);
      cos_pitch = cos(pitch);
      sin_pitch = sin(pitch);
      
      // Tilt compensated Magnetic field X:
      MAG_X = sen_data.magnetom_x*cos_pitch + sen_data.magnetom_y*sin_roll*sin_pitch + sen_data.magnetom_z*cos_roll*sin_pitch;
      
      // Tilt compensated Magnetic field Y:
      MAG_Y = sen_data.magnetom_y*cos_roll - sen_data.magnetom_z*sin_roll;
      
      // Magnetic Heading
      sen_data.magnetom_heading = atan2(-1*MAG_Y,MAG_X);
      
      if(sen_data.magnetom_heading < 0) 
        sen_data.magnetom_heading += 2 * M_PI;
     
    }
    

    Please help to check the Mag code, it calculate the heading result for DCM matrix update, but result seems incorrect since there are 0 as initial value. And not sure if this variables been update during rotation?

    0, // Magnetometer XY Theta
    0, // Magnetometer XY Scale
    0, // Magnetometer YZ Theta
    0};// Magnetometer YZ Scale

    Any master can help to check it? Any master get the correct rotation behavior by this code?

      

  • Hi, I'm interesting in MSP430F5xx 9-Axis Sensor Fusion, I will develop a sensor hub, I want to know where I can download the SCH and source code.
  • Do we have any documents related to the sample code MSP430 AHRS 9-Axis Sensor Fusion. I need more materials to understand the more well, for example the author comment something like 

    Vector_Scale_q15(&temp[0][0], &D_Matrix[1][0], err); // (Q2.14*Q1.15 --> Q2.14)
    Vector_Scale_q15(&temp[1][0], &D_Matrix[0][0], err); //eq.19

    But i dont understand Q2.14*Q1.15 is?

    Thanks

**Attention** This is a public forum