Reading Pitch Roll from CHR6DM

Using Arduino Programming Questions
1

Good Day all

Im trying to read from a CHRobotics 6dm using my arduino uno and Im unable to read the values for some reason. Ive transferred my code from Visual C# and am running no errors so Im not sure why Im not getting anything from the sensor. Ive also tried code from a separate forum post with no luck as well, http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1286541868 , Ive attached my code and if you see anything out of the ordinary any advice would be great.

Thanks

void setup() 
{
 Serial.begin(115200);

}

void loop() {
  int CHRtoRead = 17;
  
  while ( Serial.available() > 0)
  {
    int i =0;
    char CHRbuffer[17];
    
      CHRbuffer[i] = Serial.read();
      if (CHRbuffer[i] == 's')
        {
        i = i + 7;
        int YawVal = (int)((((CHRbuffer[i++] << 8) | CHRbuffer[i++]) << 16) >> 16);
        double YawCalc = YawVal * 0.0109863;
                                                                       
        int PitchVal = (int)((((CHRbuffer[i++] << 8) | CHRbuffer[i++]) << 16) >> 16);                        
        double PitchCalc = PitchVal * 0.0109863;        
                                                
        int RollVal = (int)((((CHRbuffer[i++] << 8) | CHRbuffer[i++]) << 16) >> 16);                           
        double RollCalc = RollVal * 0.0109863;        
        
        Serial.print("$CTAP,R,");
        Serial.print(RollCalc);
        Serial.print(",P,");
        Serial.print(PitchCalc);
        Serial.print(",Y,");
        Serial.println(YawCalc);
        }
        else
        {
          i++;
        }
   
    
  }
 
delay(350);
}
2

Im trying to read from a CHRobotics 6dm

Got a link to this device?

How is it connected to the Arduino?

Ive transferred my code from Visual C#

What code? How were you reading from this device on the PC?

3 
if (CHRbuffer[i] == 's')
        {
        i = i + 7;
        int YawVal = (int)((((CHRbuffer[i++] << 8) |

Are you forgetting something?
Or is your code prescient?

4

http://www.chrobotics.com/index.php?main_page=product_info&products_id=2

This is the device. It uses the UART onboard my Arduino. pin 0 of the digital pins.

I was previously using a Netduino Plus but some of its functionality has stopped. Here is the code for the Netduino. I took most of it over to the Arduino UNO and modified it to the point where theres no errors but I guess there is something I am forgetting.....

if (CHRtoRead > 12)
                {
                    int i = 0;
                    //blink the LED to show we got data
                    led.Write(true);
                    //Thread.Sleep(100);
                    led.Write(false);
                    // get the waiting data
                    byte[] CHRbuffer = new byte[CHRtoRead];
                    
                    serialPort2.Read(CHRbuffer, 0, CHRbuffer.Length);
                    serialPort1.Write(CHRbuffer, 0, CHRbuffer.Length);

                    if (CHRbuffer[i] == 's')
                    {
                        //Print final result
                        i = i + 7;
                        
                        int YawVal = (int)((((CHRbuffer[i++] << 8) | CHRbuffer[i++]) << 16) >> 16);
                        String ShowYaw = YawVal.ToString();                        
                        double YawCalc = YawVal * 0.0109863;
                        String ShowCalc = YawCalc.ToString();
                        //Debug.Print("Yaw");
                        //Debug.Print(ShowCalc);
                        
                        int PitchVal = (int)((((CHRbuffer[i++] << 8) | CHRbuffer[i++]) << 16) >> 16);
                        String ShowPitch = PitchVal.ToString();                        
                        double PitchCalc = PitchVal * 0.0109863;
                        String ShowPCalc = PitchCalc.ToString();
                        //Debug.Print("Pitch");
                        //Debug.Print(ShowPCalc);
                        
                        int RollVal = (int)((((CHRbuffer[i++] << 8) | CHRbuffer[i++]) << 16) >> 16);
                        String ShowRoll = RollVal.ToString();                        
                        double RollCalc = RollVal * 0.0109863;
                        String ShowRCalc = RollCalc.ToString();
                        //Debug.Print("Roll");
                        //Debug.Print(ShowRCalc);
                        
                        string CTAP = "$CTAP,Y," + YawCalc + ",P," + PitchCalc + ",R," + RollCalc + "\r\n";
                        Debug.Print(CTAP);
                        sw.Write(CTAP);
                    }
                    else
                    {
                        i++;
                    }
5 
while ( Serial.available() > 0)
  {
    int i =0;
    char CHRbuffer[17];
    
      CHRbuffer[i] = Serial.read();
      if (CHRbuffer[i] == 's')

Each time through the loop, you reset i. Is that correct?

6

I have changed the code a little but still no data......

while ( Serial.available() > 0)
  {
    int i = 0;
    char CHRbuffer[17];
    while (i != 17)
    {
      CHRbuffer[i]= Serial.read();
      i++;
    }
    i = 0;
    
    while (i != 17)
    {    
      if (CHRbuffer[i] == 's')
        {
        i = i + 7;
        int YawVal = (int)((((CHRbuffer[i++] << 8) | CHRbuffer[i++]) << 16) >> 16);
        double YawCalc = YawVal * 0.0109863;
                                                                       
        int PitchVal = (int)((((CHRbuffer[i++] << 8) | CHRbuffer[i++]) << 16) >> 16);                        
        double PitchCalc = PitchVal * 0.0109863;        
                                                
        int RollVal = (int)((((CHRbuffer[i++] << 8) | CHRbuffer[i++]) << 16) >> 16);                           
        double RollCalc = RollVal * 0.0109863;        
        
        Serial.print("$CTAP,R,");
        Serial.print(RollCalc);
        Serial.print(",P,");
        Serial.print(PitchCalc);
        Serial.print(",Y,");
        Serial.println(YawCalc);
        }
        else
        {
          i++;
        }
    }
    
  }
7 
while ( Serial.available() > 0)
  {
    int i = 0;
    char CHRbuffer[17];
    while (i != 17)
    {
      CHRbuffer[i]= Serial.read();

So now you're reading data that may not be there.

8

The index into the array that defines where to store the data needs to be either global or static, and needs to be defined/initialized outside of the while loop.

9

Okay so Ive gone back to the basics and just read from the port and then output that data back so I know the unit is working and just outputting data. Now I need to figure out how to put it in a variable and then parse that data to get the pitch roll values like I want. Do I use the while loop like I had or is there a more efficient way to do this?

10

Even when I run this code written by Honk as a test I get 0 values on the pitch roll yaw. I have the TX pin of the unit connected to the RX pin of the digital pin of the arduino so its not a connectivity problem.

float CHR_yawClean;
float CHR_pitchClean;
float CHR_rollClean;
//float CHR_AccelZ;

#define CHR_MESSAGE_LENGTH 16
#define BYTE1 0
#define BYTE2 1
#define BYTE3 2
#define PTByte 3
#define NBYTE 4
#define Data_BYTE1 5
#define Data_BYTE2 6
#define YAWBYTE1 7
#define YAWBYTE2 8
#define PITCHBYTE1 9
#define PITCHBYTE2 10
#define ROLLBYTE1 11
#define ROLLBYTE2 12
#define ACCELZBYTE1 13
#define ACCELZBYTE2 14
#define CHKBYTE1 15
#define CHKBYTE2 16

#define YAW_FACT 0.0109863F
#define Pitch_Fact 0.0109863F
#define Roll_Fact 0.0109863F
//#define Z_Fact 0.106812F

void setup()
  {
    Serial.begin(115200);
    
  }
  
void loop(){
  readCHR6DM();
  Serial.print("YAW: ");
  Serial.print(CHR_yawClean);
  Serial.print(" Pitch: ");
  Serial.print(CHR_pitchClean);
  Serial.print(" Roll: ");
  Serial.print(CHR_rollClean);
  delay (350);
}


void readCHR6DM(){
  int CHR_yawWord, CHR_pitchWord, CHR_rollWord;
  unsigned int CHR_chkWord, calc_chkWord;
  int i, j;
  unsigned char buffer[CHR_MESSAGE_LENGTH+2]; //17 bytes
  unsigned long sumBytes;
  
  while (Serial.available() >0){
    start:
    i = 0;
      buffer[i] = Serial.read();
      if(buffer[i] == 's'){
        i++;
        buffer[i] = Serial.read();
      }
      else {goto start;}
      
      if (buffer[i] =='n'){
        i++;
        buffer[i] = Serial.read();
      }
      else {goto start;}
      
      if (buffer[i] =='p'){
        i++;
        buffer[i] = Serial.read();
      }
      else {goto start;}
      
      if(buffer[i] == 0xB7){
        i++;
        buffer[i] = Serial.read();
      }
      else {goto start;}
      
      if(buffer[i] == 0x0A){
        i++;
        buffer[i] = Serial.read();
      }
      else {goto start;}
      
      if(buffer[i] == 0xE0){
        i++;
        buffer[i] = Serial.read();
      }
      else {goto start;}
      
      if(buffer[i] == 0x02){
        i++;
        buffer[i] = Serial.read();
      
        i++;
        buffer[i] = Serial.read();
        
        i++;
        buffer[i] = Serial.read();
        
        i++;
        buffer[i] = Serial.read();
        
        i++;
        buffer[i] = Serial.read();
        
        i++;
        buffer[i] = Serial.read();
        
        i++;
        buffer[i] = Serial.read();
        
        i++;
        buffer[i] = Serial.read();
        
        i++;
        buffer[i] = Serial.read();
        
        i++;
        buffer[i] = Serial.read();
      }
      
      else{Serial.println("Sensor not connected");}
      
      CHR_yawWord = (int)buffer[YAWBYTE1]<<8;
      CHR_yawWord |= buffer[YAWBYTE2];
      
      CHR_pitchWord = (int)buffer[PITCHBYTE1]<<8;
      CHR_pitchWord |= buffer[PITCHBYTE2];
      
      CHR_rollWord = (int)buffer[ROLLBYTE1]<<8;
      CHR_rollWord |= buffer[ROLLBYTE2];
      
      CHR_yawClean = (CHR_yawWord * YAW_FACT);
      CHR_pitchClean = (CHR_pitchWord * Pitch_Fact);
      CHR_rollClean = (CHR_rollWord * Roll_Fact);
  }
}
11

'goto'?
Run for the hills!

I'm sorry, but you're still doing reads when you don't know if there's data there or no.

Can you give us some idea of what the 17 characters you're expecting look like?

12

Yeah I wasnt too happy with it but I needed somewhere to start even though I still get 0's for the data stream.

My test with this code shows the data flowing freely and quickly at 115200

void setup()  
{
  Serial.begin(115200);
   
}

void loop()                    
{

  if (Serial.available()) {
      Serial.print((char)Serial.read());
  }
}
13

My test with this code shows the data flowing freely and quickly at 115200

For those of us who don't have that device laying around, what data are you seeing from the device, and what do you need to parse from the data stream?

14

You really need to consider the structure of the message, and what damage you could do if your receiver starts working part-way through a message.
What are the message delimiters?
Ideally, you should just read and discard characters until you have either seen tha last of the previous message, or seen the start of a new one.

15

According to the CHR6dm data sheet the data is output in byte chunks starting with an 's' as the beginning of the packet burst and follows with 'n' 'p'. There is two 8 bit byte values following that describes the packet type and another 8 bit byte value to tell how long the packet is. Following these values are the registers in 8 bit format that are the actual pitch roll yaw values each taking up two registers so there would be a Pitch1/Pitch2 Roll1/Roll2 Yaw1/Yaw2 that when put together give the values for Pitch Roll and Yaw. There are accelerometer packets following these but Im not interested in those values. Which is where the calculation "int YawVal = (int)((((CHRbuffer[i++] << 8) | CHRbuffer[i++]) << 16) >> 16);" comes into play to decipher the byte registers.

When I view the data in the serial monitor I see the "snp" start bits and everything after is the register data looks like "snp· ÿþ<O" in the serial window but I know is valid data but the serial window doesnt decipher the registers.

16

OK, so just loop reading and discarding input until you find an 's'.
Then read your message in one character at a time, or, having seen the 's', wait until available returns 16, and then read them in.

17

Yes ideally I am waiting for the 's" byte and then 7 bytes after that are the values I am looking for.

18 
while (Serial.read () != 's') ;
19

Yes ideally I am waiting for the 's" byte and then 7 bytes after that are the values I am looking for.

According to the CHR6dm data sheet the data is output in byte chunks starting with an 's' as the beginning of the packet burst and follows with 'n' 'p'. There is two 8 bit byte values following that describes the packet type and another 8 bit byte value to tell how long the packet is. Following these values are the registers in 8 bit format that are the actual pitch roll yaw values each taking up two registers so there would be a Pitch1/Pitch2 Roll1/Roll2 Yaw1/Yaw2 that when put together give the values for Pitch Roll and Yaw.

So, the 7 bytes of interest are the 'n', 'p', 2 for packet type, 1 for packet size, and 2 for pitch?

20

Sorry for the mistype PaulS its the 'snp' PacketType and PacketNumber are discarded. From my previous coding it was the first 7 bytes that are discarded and everything after those 7 bytes are what Im looking for. I just use the 's' as the start of the packet.