Getting wrong byte values from CHR UM6 in my library but sketch works fine

I’ve searched many ways and have not found this issue.

I am writing a partial library to get data from the CH Robotics UM6 IMU. The code, which is based on code from a post by JonRussell works in a sketch, but when I break it out into a library, I am getting the wrong values. Here is the relevant fragment from the library:

boolean CHRUM6::readPacket()
{
	boolean returnVal = false;
	Serial.println("Reading packet...");
	 n = Serial1.available();
	 Serial.print("n is ");
	 Serial.println(n, DEC);
	  if (n > 0)
	  {
	    c = Serial1.read();
	    Serial.print("c=");
	    Serial.println(c, HEX);
	    switch(nState){
	      case STATE_ZERO : // Begin. Look for 's'.
	        // Start of new packet...
	        Reset();
	        if (c == 's'){
				Serial.println("State_S");
	          nState = STATE_S;
	        } else {
	          nState = STATE_ZERO;
	        }
	        break;
                case STATE_S : // Have 's'. Look for 'n'.
	        if (c == 'n'){
				Serial.println("State_SN");
	          nState = STATE_SN;
	        } else {
	          nState = STATE_ZERO;
	        }
	        break;
	      case STATE_SN : // Have 'sn'. Look for 'p'.
	        if (c == 'p'){
	          nState = STATE_SNP;
	        } else {
	          nState = STATE_ZERO;
	        }
	        break;
	      case STATE_SNP : // Have 'snp'. Read PacketType and calculate DataLength.
	        UM6_Packet.HasData = 1 && (c & PT_HAS_DATA);
	        UM6_Packet.IsBatch = 1 && (c & PT_IS_BATCH);
	        UM6_Packet.BatchLength = ((c >> 2) & 0b00001111);
	        UM6_Packet.CommFail = 1 && (c & PT_COMM_FAIL);
	        nState = STATE_PT;
	        if (UM6_Packet.IsBatch){
	          UM6_Packet.DataLength = UM6_Packet.BatchLength * 4;
	       //  Serial.print("Is Batch and batch length is ");
	       //   Serial.println(UM6_Packet.BatchLength, DEC);
	        } else {
	          UM6_Packet.DataLength = 4;
	        //  Serial.println("Is not a batch");
	        }
	        break;
	      case STATE_PT : // Have PacketType. Read Address.
	        UM6_Packet.Address = c;
	        nDataByteCount = 0;
	        nState = STATE_READ_DATA;
	        break;
	      case STATE_READ_DATA : // Read Data. (UM6_PT.BatchLength * 4) bytes.
	        aPacketData[nDataByteCount] = c;
	        nDataByteCount++;
	        if (nDataByteCount >= UM6_Packet.DataLength){
	          nState = STATE_CHK1;
	        }
	        break;
	      case STATE_CHK1 : // Read Checksum 1
	        UM6_Packet.Checksum1 = c;
	        nState = STATE_CHK0;
	        break;
	      case STATE_CHK0 : // Read Checksum 0
	        UM6_Packet.Checksum0 = c;
	        nState = STATE_DONE;
	        break;
	      case STATE_DONE : // Entire packet consumed. Process packet
	        ProcessPacket();
	        nState = STATE_ZERO;
	        break;
	      }
	      returnVal= true;
	    }
	    else
	    {
			Serial.println("Data not available on Serial1");
			returnVal = false;
		}
	   return returnVal;

}

Here is the relevant code from my test sketch:

#include <CHRUM6.h>
#define BAUD 57600
CHRUM6 myIMU;

void setup()
{
  Serial.begin(BAUD);
  myIMU.begin(BAUD);
  
}

void loop()
{
  myIMU.readPacket();

Here is the output to Serial Monitor:

Reading packet...
n is 0
Data not available on Serial1
Roll is 0.000
Pitch is 0.000
Yaw is 0.000
Reading packet...
n is 0
Data not available on Serial1
Roll is 0.000
Pitch is 0.000
Yaw is 0.000
Reading packet...
n is 1
c=BF
Roll is 0.000
Pitch is 0.000
Yaw is 0.000
Reading packet...
n is 1
c=F5
Roll is 0.000
Pitch is 0.000
Yaw is 0.000
Reading packet...

I have checked the Rx/Tx from the IMU with a logic analyzer, and it always sends packets starting with 0x73, 0x6E, 0x70 (‘s’ ‘n’ ‘p’) as I expect, but for some reason the Serial1.read() is not getting that. The same code in a sketch works fine as well.

Any thoughts? Are these fragments sufficient or should I posts the complete header and source files?

Are these fragments sufficient or should I posts the complete header and source files?

No and yes. For instance, the type of c is undefined. Why what should be a local variable is a class member or global is a mystery.

Posting the code that works would be good, too.

Ok, here is the complete header file:

#ifndef CHRUM6_h
#define CHRUM6_h

#include "Arduino.h"



#define STATE_ZERO         0
#define STATE_S            1
#define STATE_SN           2
#define STATE_SNP          3
#define STATE_PT           4
#define STATE_READ_DATA    5
#define STATE_CHK1         6
#define STATE_CHK0         7
#define STATE_DONE         8


#define UM6_GET_DATA       0xAE
#define UM6_REG_EULER_ROLL_PITCH  0x62
#define UM6_REG_EULER_YAW  0x63
#define UM6_EULER_SCALAR  0.0109863

#define PT_HAS_DATA  0b10000000
#define PT_IS_BATCH  0b01000000
#define PT_COMM_FAIL 0b00000001

#define DATA_BUFF_LEN  16



typedef struct {
  boolean HasData;
  boolean IsBatch;
  byte BatchLength;
  boolean CommFail;
  byte Address;
  byte Checksum1;
  byte Checksum0;
  byte DataLength;
} UM6_PacketStruct ;



class CHRUM6
{
    public:
    CHRUM6();

    void begin(int baud);
    boolean readPacket();


    float getRoll();
    float getPitch();
    float getYaw();

    private:

    byte aPacketData[DATA_BUFF_LEN];
    int n;
    byte c;
    int nDataByteCount;
    int nState;
    float roll;
    float pitch;
    float yaw;
    void ProcessPacket();
    void Reset();
    UM6_PacketStruct UM6_Packet;
};

#endif

It all doesn't fit so I will have to post it in a couple of messages.

Here is the complete source file:

#include "Arduino.h"
#include "CHRUM6.h"

CHRUM6::CHRUM6()
{
	roll = 0.0;
	pitch = 0.0;
	yaw = 0.0;
	n = 0;
	c = 0;
	nDataByteCount = 0;
	nState = 0;
	Reset();

}

void CHRUM6::begin(int baud)
{
	Serial1.begin(baud);

}

boolean CHRUM6::readPacket()
{
	boolean returnVal = false;
	Serial.println("Reading packet...");
	 n = Serial1.available();
	 Serial.print("n is ");
	 Serial.println(n, DEC);
	  if (n > 0)
	  {
	    c = Serial1.read();
	    Serial.print("c=");
	    Serial.println(c, HEX);
	    switch(nState){
	      case STATE_ZERO : // Begin. Look for 's'.
	        // Start of new packet...
	        Reset();
	        if (c == 's'){
				Serial.println("State_S");
	          nState = STATE_S;
	        } else {
	          nState = STATE_ZERO;
	        }
	        break;
	      case STATE_S : // Have 's'. Look for 'n'.
	        if (c == 'n'){
				Serial.println("State_SN");
	          nState = STATE_SN;
	        } else {
	          nState = STATE_ZERO;
	        }
	        break;
	      case STATE_SN : // Have 'sn'. Look for 'p'.
	        if (c == 'p'){
	          nState = STATE_SNP;
	        } else {
	          nState = STATE_ZERO;
	        }
	        break;
	      case STATE_SNP : // Have 'snp'. Read PacketType and calculate DataLength.
	        UM6_Packet.HasData = 1 && (c & PT_HAS_DATA);
	        UM6_Packet.IsBatch = 1 && (c & PT_IS_BATCH);
	        UM6_Packet.BatchLength = ((c >> 2) & 0b00001111);
	        UM6_Packet.CommFail = 1 && (c & PT_COMM_FAIL);
	        nState = STATE_PT;
	        if (UM6_Packet.IsBatch){
	          UM6_Packet.DataLength = UM6_Packet.BatchLength * 4;
	       //  Serial.print("Is Batch and batch length is ");
	       //   Serial.println(UM6_Packet.BatchLength, DEC);
	        } else {
	          UM6_Packet.DataLength = 4;
	        //  Serial.println("Is not a batch");
	        }
	        break;
	      case STATE_PT : // Have PacketType. Read Address.
	        UM6_Packet.Address = c;
	        nDataByteCount = 0;
	        nState = STATE_READ_DATA;
	        break;
	      case STATE_READ_DATA : // Read Data. (UM6_PT.BatchLength * 4) bytes.
	        aPacketData[nDataByteCount] = c;
	        nDataByteCount++;
	        if (nDataByteCount >= UM6_Packet.DataLength){
	          nState = STATE_CHK1;
	        }
	        break;
	      case STATE_CHK1 : // Read Checksum 1
	        UM6_Packet.Checksum1 = c;
	        nState = STATE_CHK0;
	        break;
	      case STATE_CHK0 : // Read Checksum 0
	        UM6_Packet.Checksum0 = c;
	        nState = STATE_DONE;
	        break;
	      case STATE_DONE : // Entire packet consumed. Process packet
	        ProcessPacket();
	        nState = STATE_ZERO;
	        break;
	      }
	      returnVal= true;
	    }
	    else
	    {
			Serial.println("Data not available on Serial1");
			returnVal = false;
		}
	   return returnVal;

}

float CHRUM6::getRoll()
{
	return roll;
}

float CHRUM6::getPitch()
{
	return pitch;
}

float CHRUM6::getYaw()
{
	return yaw;
}

void CHRUM6::ProcessPacket()
{
	Serial.println("Processing packet...");
	float DataA = 0;
  	float DataB = 0;
  	float DataC = 0;
  	float DataD = 0;
  	int i = 0;

	if (UM6_Packet.HasData && !UM6_Packet.CommFail)
	{
		i = (aPacketData[0] << 8) | aPacketData[1];
		DataA = i * UM6_EULER_SCALAR;
		i = (aPacketData[2] << 8) | aPacketData[3];
		DataB = i * UM6_EULER_SCALAR;
		if (UM6_Packet.DataLength > 4)
		{
		  i = (aPacketData[4] << 8) | aPacketData[5];
		  DataC = i * UM6_EULER_SCALAR;
		  i = (aPacketData[6] << 8) | aPacketData[7];
		  DataD = i * UM6_EULER_SCALAR;
		}
  	}
  	roll = DataA;
  	pitch = DataB;
  	yaw = DataC;
}

void CHRUM6::Reset()
{
	UM6_Packet.HasData = false;
	UM6_Packet.IsBatch = false;
	UM6_Packet.BatchLength = 0;
	UM6_Packet.CommFail = false;
	UM6_Packet.Address = 0;
	UM6_Packet.Checksum1 = 0;
	UM6_Packet.Checksum0 = 0;
	UM6_Packet.DataLength = 0;
}

Here is the sample sketch:

#include <CHRUM6.h>
#define BAUD 57600
CHRUM6 myIMU;

void setup()
{
  Serial.begin(BAUD);
  myIMU.begin(BAUD);
  
}

void loop()
{
  myIMU.readPacket();
  //{
    Serial.print("Roll is ");
    Serial.println(myIMU.getRoll(), 3);
    Serial.print("Pitch is ");
    Serial.println(myIMU.getPitch(), 3);
    Serial.print("Yaw is ");
    Serial.println(myIMU.getYaw(), 3);
  /*}
  else
  {
    Serial.println("No data from CHRUM6");
  }*/
}

Here is the output from this sketch:

Reading packet...
n is 0
Data not available on Serial1
Roll is 0.000
Pitch is 0.000
Yaw is 0.000
Reading packet...
n is 0
Data not available on Serial1
Roll is 0.000
Pitch is 0.000
Yaw is 0.000
Reading packet...
n is 1
c=BF
Roll is 0.000
Pitch is 0.000
Yaw is 0.000
Reading packet...
n is 1
c=F5
Roll is 0.000
Pitch is 0.000
Yaw is 0.000
Reading packet...

Here is the rest.

Here is the working sketch I used to create library:

int nState = 0;
#define STATE_ZERO         0
#define STATE_S            1
#define STATE_SN           2
#define STATE_SNP          3
#define STATE_PT           4
#define STATE_READ_DATA    5
#define STATE_CHK1         6
#define STATE_CHK0         7
#define STATE_DONE         8 

#define BAUD 57600
#define UM6_GET_DATA       0xAE
#define UM6_REG_EULER_ROLL_PITCH  0x62
#define UM6_REG_EULER_YAW  0x63
#define UM6_EULER_SCALAR  0.0109863

#define PT_HAS_DATA  0b10000000
#define PT_IS_BATCH  0b01000000
#define PT_COMM_FAIL 0b00000001

#define DATA_BUFF_LEN  16

byte aPacketData[DATA_BUFF_LEN];
int n = 0;
byte c = 0;
int nDataByteCount = 0;

typedef struct {
  boolean HasData;
  boolean IsBatch;
  byte BatchLength;
  boolean CommFail;
  byte Address;
  byte Checksum1;
  byte Checksum0;
  byte DataLength;
} UM6_PacketStruct ;

UM6_PacketStruct UM6_Packet;

void setup(){
  Serial1.begin(BAUD);
  Serial.begin(BAUD);
}

void loop(){
 
  n = Serial1.available();
  if (n > 0){
    c = Serial1.read();
    //Serial.print("c=");
    //Serial.println(c, HEX);
    switch(nState){
      case STATE_ZERO : // Begin. Look for 's'.
        // Start of new packet...
        Reset();
        if (c == 's'){
          nState = STATE_S;
        } else {
          nState = STATE_ZERO;
        }
        break;
      case STATE_S : // Have 's'. Look for 'n'.
        if (c == 'n'){
          nState = STATE_SN; 
        } else {
          nState = STATE_ZERO;
        }
        break;
      case STATE_SN : // Have 'sn'. Look for 'p'.
        if (c == 'p'){
          nState = STATE_SNP; 
        } else {
          nState = STATE_ZERO;
        }
        break;
      case STATE_SNP : // Have 'snp'. Read PacketType and calculate DataLength.
        UM6_Packet.HasData = 1 && (c & PT_HAS_DATA);
        UM6_Packet.IsBatch = 1 && (c & PT_IS_BATCH);
        UM6_Packet.BatchLength = ((c >> 2) & 0b00001111);
        UM6_Packet.CommFail = 1 && (c & PT_COMM_FAIL);
        nState = STATE_PT;
        if (UM6_Packet.IsBatch){
          UM6_Packet.DataLength = UM6_Packet.BatchLength * 4;
       //  Serial.print("Is Batch and batch length is ");
       //   Serial.println(UM6_Packet.BatchLength, DEC);
        } else {
          UM6_Packet.DataLength = 4;
        //  Serial.println("Is not a batch");
        }
        break;
      case STATE_PT : // Have PacketType. Read Address.
        UM6_Packet.Address = c;
        nDataByteCount = 0;
        nState = STATE_READ_DATA; 
        break;
      case STATE_READ_DATA : // Read Data. (UM6_PT.BatchLength * 4) bytes.
        aPacketData[nDataByteCount] = c;
        nDataByteCount++;
        if (nDataByteCount >= UM6_Packet.DataLength){
          nState = STATE_CHK1;
        }
        break;
      case STATE_CHK1 : // Read Checksum 1
        UM6_Packet.Checksum1 = c;
        nState = STATE_CHK0;
        break;
      case STATE_CHK0 : // Read Checksum 0
        UM6_Packet.Checksum0 = c;
        nState = STATE_DONE;
        break;
      case STATE_DONE : // Entire packet consumed. Process packet
        ProcessPacket();
        nState = STATE_ZERO;
        break;
      }
    }
    else
    {
      Serial.println("Data not available");
    }
  }
  
void ProcessPacket(){
  float DataA = 0;
  float DataB = 0;
  float DataC = 0;
  float DataD = 0;
  int i = 0;
  
  if(UM6_Packet.Address == UM6_REG_EULER_ROLL_PITCH) {
    Serial.print("Millis since startup: ");
    Serial.println(millis());
    Serial.print("Euler roll and pitch: ");
    if (UM6_Packet.HasData && !UM6_Packet.CommFail){
        i = (aPacketData[0] << 8) | aPacketData[1];
        DataA = i * UM6_EULER_SCALAR;
        i = (aPacketData[2] << 8) | aPacketData[3];
        DataB = i * UM6_EULER_SCALAR;
        if (UM6_Packet.DataLength > 4){
          i = (aPacketData[4] << 8) | aPacketData[5];
          DataC = i * UM6_EULER_SCALAR;
          i = (aPacketData[6] << 8) | aPacketData[7];
          DataD = i * UM6_EULER_SCALAR;
        }
      }
      PrintDebugFloatABCD(DataA,DataB,DataC,DataD);
  }
  else if(UM6_Packet.Address ==UM6_REG_EULER_YAW) {
    Serial.print("Euler yaw: ");
    if (UM6_Packet.HasData && !UM6_Packet.CommFail){
        i = (aPacketData[0] << 8) | aPacketData[1];
        DataA = i * UM6_EULER_SCALAR;
        i = (aPacketData[2] << 8) | aPacketData[3];
        DataB = i * UM6_EULER_SCALAR;
        if (UM6_Packet.DataLength > 4){
          i = (aPacketData[4] << 8) | aPacketData[5];
          DataC = i * UM6_EULER_SCALAR;
          i = (aPacketData[6] << 8) | aPacketData[7];
          DataD = i * UM6_EULER_SCALAR;
        }
      }
      PrintDebugFloatABCD(DataA,DataB,DataC,DataD);
  }
  else
  {
    Serial.println("Unknown data");
  }
}
  
void Reset(){
  UM6_Packet.HasData = false;
  UM6_Packet.IsBatch = false;
  UM6_Packet.BatchLength = 0;
  UM6_Packet.CommFail = false;
  UM6_Packet.Address = 0;
  UM6_Packet.Checksum1 = 0;
  UM6_Packet.Checksum0 = 0;
  UM6_Packet.DataLength = 0;
}

void PrintDebugFloatABCD(float a, float b, float c, float d){
 // Serial.print("N = ");
 // Serial.print(n,DEC);
  Serial.print(" Roll = ");
  Serial.print(a,3);
  Serial.print(" Pitch = ");
  Serial.print(b,3);
  Serial.print(" Yaw = ");
  Serial.println(c,3);
 // Serial.print(" D = ");
 // Serial.print(d,DEC);
  //Serial.println(".");
}

And here is the output from that sketch:

Data not available
Unknown data
Unknown data
Data not available
Data not available
Millis since startup: 20
Euler roll and pitch:  Roll = 81.870 Pitch = -3.043 Yaw = 44.758
Unknown data
Unknown data
Unknown data
Data not available
Data not available
Millis since startup: 50
Euler roll and pitch:  Roll = 81.980 Pitch = -2.988 Yaw = 44.813
Unknown data
Unknown data
Unknown data
Data not available
Data not available
Millis since startup: 79
Euler roll and pitch:  Roll = 82.090 Pitch = -2.922 Yaw = 44.879
Unknown data
Unknown data
Data not available
Unknown data
Data not available
Millis since startup: 109
Euler roll and pitch:  Roll = 82.331 Pitch = -2.780 Yaw = 45.000
Unknown data
Unknown data
Data not available
Unknown data
Data not available
Millis since startup: 139
Euler roll and pitch:  Roll = 82.419 Pitch = -2.736 Yaw = 45.022
Unknown data
Unknown data
Unknown data
Data not available
Data not available
Millis since startup: 168
Euler roll and pitch:  Roll = 82.551 Pitch = -2.703 Yaw = 45.055
Unknown data

I’ve run the library test sketch for a considerable amount of time and never got the bytes I was looking for, yet the working sketch only gets wrong data a couple of times over a 30 millisecond period.

The logic analyzer only shows packets with the correct bytes, so the problem isn’t the sensor, it’s in the code somewhere. I don’t understand is that the code is copied and pasted directly from the working sketch.

Any ideas?

In the sketch that works, the amount of serial data to read is important, with respect to the messages printed. The loop() function may be called thousands of times between the times when there is serial data.

That is not the case in the library version. The time it takes to output all the serial data you are printing may be causing the serial buffer to overflow, resulting in lost data.