Pages: [1] 2 3   Go Down
Author Topic: Reading Pitch Roll from CHR6DM  (Read 3207 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Newbie
*
Karma: 0
Posts: 35
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
Code:
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);
}
Logged

Seattle, WA USA
Online Online
Brattain Member
*****
Karma: 548
Posts: 46013
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
Im trying to read from a CHRobotics 6dm
Got a link to this device?

How is it connected to the Arduino?

Quote
Ive transferred my code from Visual C#
What code? How were you reading from this device on the PC?
Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 238
Posts: 24321
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
if (CHRbuffer[i] == 's')
        {
        i = i + 7;
        int YawVal = (int)((((CHRbuffer[i++] << 8) |
Are you forgetting something?
Or is your code prescient?
« Last Edit: February 23, 2011, 09:23:40 am by AWOL » Logged

"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

0
Offline Offline
Newbie
*
Karma: 0
Posts: 35
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.....

Code:
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++;
                    }
Logged

Seattle, WA USA
Online Online
Brattain Member
*****
Karma: 548
Posts: 46013
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
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?
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 35
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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


Code:
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++;
        }
    }
    
  }
Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 238
Posts: 24321
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
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.
Logged

"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

Seattle, WA USA
Online Online
Brattain Member
*****
Karma: 548
Posts: 46013
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 35
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 35
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.


Code:
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);
  }
}
Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 238
Posts: 24321
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

'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?
« Last Edit: February 25, 2011, 09:19:05 am by AWOL » Logged

"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

0
Offline Offline
Newbie
*
Karma: 0
Posts: 35
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
Code:
void setup() 
{
  Serial.begin(115200);
   
}

void loop()                   
{

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

Seattle, WA USA
Online Online
Brattain Member
*****
Karma: 548
Posts: 46013
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
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?
Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 238
Posts: 24321
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

0
Offline Offline
Newbie
*
Karma: 0
Posts: 35
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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++] << smiley-cool | 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.
Logged

Pages: [1] 2 3   Go Up
Jump to: