Skip to content
Snippets Groups Projects
Commit 28e48569 authored by Tommy Persson's avatar Tommy Persson
Browse files

Adding network byte order.

parent 51df12be
No related branches found
No related tags found
No related merge requests found
......@@ -53,16 +53,6 @@ float bytesToFloat(unsigned char b0, unsigned char b1, unsigned char b2, unsign
return result;
}
double bytesToDouble(unsigned char b0, unsigned char b1, unsigned char b2, unsigned char b3,
unsigned char b4, unsigned char b5, unsigned char b6, unsigned char b7) {
//unsigned char byte_array[] = { b0, b1, b2, b3, b4, b5, b6, b7 };
unsigned char byte_array[] = { b7, b6, b5, b4, b3, b2, b1, b0 }; //netw byte order
double result;
std::copy(reinterpret_cast<const char*>(&byte_array[0]),
reinterpret_cast<const char*>(&byte_array[8]),
reinterpret_cast<char*>(&result));
return result;
}
class udp_server {
public:
......@@ -75,6 +65,25 @@ public:
}
private:
double bytesToDouble(unsigned int i) {
//unsigned char byte_array[] = { b0, b1, b2, b3, b4, b5, b6, b7 };
unsigned char byte_array[] = { recv_buffer_[i+7],
recv_buffer_[i+6],
recv_buffer_[i+5],
recv_buffer_[i+4],
recv_buffer_[i+3],
recv_buffer_[i+2],
recv_buffer_[i+1],
recv_buffer_[i+0]}; //netw byte order
double result;
std::copy(reinterpret_cast<const char*>(&byte_array[0]),
reinterpret_cast<const char*>(&byte_array[8]),
reinterpret_cast<char*>(&result));
return result;
}
void start_receive() {
socket_.async_receive_from(boost::asio::buffer(recv_buffer_), remote_endpoint_,
boost::bind(&udp_server::handle_receive, this,
......@@ -87,30 +96,25 @@ private:
if (!error || error == boost::asio::error::message_size) {
// cerr << "Got message: " << bytes_transferred << " - " << recv_buffer_[0] << endl;
// float time = bytesToFloat(recv_buffer_[1], recv_buffer_[2], recv_buffer_[3], recv_buffer_[4]);
double time = bytesToDouble(recv_buffer_[1], recv_buffer_[2], recv_buffer_[3], recv_buffer_[4],
recv_buffer_[5], recv_buffer_[6], recv_buffer_[7], recv_buffer_[8]);
double gtime = bytesToDouble(1);
//float ax, ay, az, gx, gy, gz;
double ax, ay, az, gx, gy, gz, v0, v1, v2;
v0 = bytesToDouble(recv_buffer_[9], recv_buffer_[10], recv_buffer_[11], recv_buffer_[12],
recv_buffer_[13], recv_buffer_[14], recv_buffer_[15], recv_buffer_[16]);
v1 = bytesToDouble(recv_buffer_[17], recv_buffer_[18], recv_buffer_[19], recv_buffer_[20],
recv_buffer_[21], recv_buffer_[22], recv_buffer_[23], recv_buffer_[24]);
v2 = bytesToDouble(recv_buffer_[25], recv_buffer_[26], recv_buffer_[27], recv_buffer_[28],
recv_buffer_[29], recv_buffer_[30], recv_buffer_[31], recv_buffer_[32]);
if (recv_buffer_[0] == 'A') {
have_acc_flag = true;
ax = v0;
ay = v1;
az = v2;
ROS_ERROR("ACCELEROMETER: %f - %f %f %f", time, ax, ay, az);
}
double ax, ay, az, gx, gy, gz;
gx = bytesToDouble(9);
gy = bytesToDouble(17);
gz = bytesToDouble(25);
double atime = bytesToDouble(33);
ax = bytesToDouble(41);
ay = bytesToDouble(49);
az = bytesToDouble(57);
if (recv_buffer_[0] == 'G') {
have_gyro_flag = true;
gx = v0;
gy = v1;
gz = v2;
ROS_ERROR(" GYRO: %f - %f %f %f", time, gx, gy, gz);
}
if (recv_buffer_[32] == 'A') {
have_acc_flag = true;
ROS_ERROR("ACCELEROMETER: %f - %f %f %f", time, ax, ay, az);
}
if (!have_gyro_flag) {
start_receive();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment