Typecasting bytes properly for writing to a serial handle in C++

Question

Background

I'm doing an Arduino port of code into C++ and having issues with the data being sent as unsigned char. I need to send an 8 byte packet of unsigned chars consisting of:

Byte 1 - Header - 0xFF
Byte 2 - Right Joy Vertical - value between -100 to 100 shifted up by 128
Byte 3 - Right Joy Horizontal - value between -100 to 100 shifted up by 128
Byte 4 - Left Joy Vertical - value between -100 to 100 shifted up by 128
Byte 5 - Right Joy Horizontal - value between -100 to 100 shifted up by 128
Byte 6 - Button Values - 8 bit byte where each bit is mapped to a button
Byte 7 - Extended Inst. - "0" not needed
Byte 8 - Checksum

The arduino code had code written as:

Serial.write((unsigned char)right_V);
Serial.write((unsigned char)buttons);

This obviously isn't as easy in C++.

Problems:

1) How do I typecast properly to send 8 bytes of unsigned char by my 'sp->write()' function?

2) What printf modifiers do I use to display the data as unsigned char bytes in HEX?

Code clarification

btn is a struct consisting of double precision joystick values

cmd[] is an unsigned char array of size 8

sp->write(const char * data, int length) is a wrapper for the fstream write function. I've included the definition just in case.

ROS_INFO is a wrapper for printf for outputting to a ROS Console

CODE

// *****************************************************************************
// Create the Commander unsigned char packet as per the Arbotix Commander specifications 
int PhantomXROS::arbotixCmdPackage(cont_value btn)
{

int d_cmd=0;
unsigned char st_code;

std::stringstream ss;
ss << std::hex << "0xFF";
ss >> st_code;

//Mapping joystick values to the servo values of the robot arm
int right_V=this->map(btn.rightV, -1, 1, -100, 100)+128;
int right_H=this->map(btn.rightH, -1, 1, -100, 100)+128;
int left_V=this->map(btn.leftV, -1, 1, -100, 100)+128;
int left_H=this->map(btn.leftH, -1, 1, -100, 100)+128;



//Bytes 1-5 Convert integers to bytes
this->cmd[0] = (unsigned char)st_code;
this->cmd[1] = (unsigned char)right_V;
this->cmd[2] = (unsigned char)right_H;
this->cmd[3] = (unsigned char)left_V;
this->cmd[4] = (unsigned char)left_H;

//Byte 6 - Assign a button ON/OFF to its corresponding bit
d_cmd += (btn.R1 > 0) ? 1 : 0;
d_cmd += (btn.R2 > 0) ? 2 : 0;
d_cmd += (btn.R3 > 0) ? 4 : 0;
d_cmd += (btn.L4 > 0) ? 8 : 0;
d_cmd += (btn.L5 > 0) ? 16 : 0;
d_cmd += (btn.L6 > 0) ? 32 : 0;
d_cmd += (btn.RT > 0) ? 64 : 0;
d_cmd += (btn.LT > 0) ? 128 : 0;
this->cmd[5] = (unsigned char)d_cmd;

//Byte 7 - Extended instruction - none, therefore 0
this->cmd[6] = (unsigned char)0;

//Byte 8 - Checksum
this->cmd[7] = (unsigned char)((255 - (right_V+right_H+left_V+left_H+d_cmd)%256));

//Reinterpret the cmd byte array to an 8 char string
std::string buf(reinterpret_cast<const char*>(cmd), 8);

//write to the arbotix serial handle
try{
    sp_->write(buf.c_str(),buf.size()); 

}
catch(cereal::Exception& e){ 
    ROS_INFO("Could not write to Arbotix:");
    ROS_BREAK();
    return(-1);
}


//Output data to the ROS console
if(this->timer > 500)
{


    ROS_INFO("Arbotix Cmd right_v has written: %d", right_V);
    ROS_INFO("Arbotix Cmd right_h has written: %d", right_H);
    ROS_INFO("Arbotix Cmd left_v has written: %d", left_V);
    ROS_INFO("Arbotix Cmd left_h has written: %d", left_H);
    ROS_INFO("Arbotix Cmd d_cmd has written: %d", d_cmd);
    ROS_INFO("String command: %s",buf.c_str());

    this->timer = 0;

}


return 0;


}

Below is the write code

int cereal::CerealPort::write(const char * data, int length)
00146 {
00147         int len = length==-1 ? strlen(data) : length;
00148 
00149         // IO is currently non-blocking. This is what we want for the more cerealon read case.
00150         int origflags = fcntl(fd_, F_GETFL, 0);
00151         fcntl(fd_, F_SETFL, origflags & ~O_NONBLOCK); // TODO: @todo can we make this all work in non-blocking?
00152         int retval = ::write(fd_, data, len);
00153         fcntl(fd_, F_SETFL, origflags | O_NONBLOCK);
00154 
00155         if(retval == len) return retval;
00156         else CEREAL_EXCEPT(cereal::Exception, "write failed");
00157 }

1) i believe it's "SERIAL", not "CEREAL", but 2) I'm sure I'm not understanding your situation, but do you have control over ::write? why not const unsigned char * data? — im so confused
also google printf, and you will see the either x or X modifier for hexadecimal display of numbers — im so confused
You should really try to narrow it down to as few lines of code as possible, I'm fairly sure that in doing so you will probably find that it's as easy in C++ as it was in C. The process of deconstructing the problem is usually a great way to actually solve it. — kfsone
Also, instead of fcntl wrapped around ::write just use ::send(fd_, data, len, origflags & ~O_NONBLOCK); see linux.die.net/man/2/send — kfsone
@AK4749 I'm using the ROS system, and a cereal is the name of the serial comm. package within it. I don't have control over it, but I did copy its code over and tried set the data input parameter as const unsigned char* - it gave errors. — ramabrahma

Captain Obvlious Captain Obvlious · Accepted Answer · 2013-06-26T19:50:04

Your typecasting is correct but using std:string is not necessary here. You can use the cmd buffer directly in your call to sp_->write()

//write to the arbotix serial handle
try
{
    sp_->write(reinterpret_cast<const char*>(cmd), 8);
}
catch(cereal::Exception& e){ 
    ROS_INFO("Could not write to Arbotix:");
    ROS_BREAK();
    return(-1);
}

To print the data in hex format you can use the %02x or %02X format specifiers like so

ROS_INFO("Arbotix Cmd right_v has written: %02X", right_V);

This tells the printf function (used by ROS_INFO) to print at least two hexadecimal digits and prepend a zero character if less than two digits are output.

Typecasting bytes properly for writing to a serial handle in C++

1 Answers