背景
我正在将 Arduino 代码移植到 C++ 中，并且在将数据作为无符号字符发送时遇到问题。我需要发送一个 8 字节的无符号字符包，其中包括：
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
arduino 代码的代码编写为：
Serial.write((unsigned char)right_V);
Serial.write((unsigned char)buttons);
这显然在 C++ 中并不容易。
问题：
1) 如何正确类型转换以通过我的“sp->write()”函数发送 8 个字节的无符号字符？
2) 我使用什么 printf 修饰符将数据显示为 HEX 中的无符号字符字节？
代码说明
btn是一个由双精度操纵杆值组成的结构
cmd[]是一个大小为 8 的无符号字符数组
sp->write(const char * data, int length)是 fstream 写入函数的包装器。我已经包含了定义以防万一。
ROS_INFO是 printf 的包装器，用于输出到 ROS 控制台
代码
// *****************************************************************************
// 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;
}
下面是写代码
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 }