void Send(uint8_t * out_buf) {
#ifndef NDEBUG
// Check Header
fprintf(stdout, "Message header:\n");
for (int i=0; i<HEADER_SIZE; i++) {
fprintf(stdout, " %02x",out_buf[i]);
}
fprintf(stdout, "\n\n");
#endif
int N = out_buf[N_INDEX];
int L = out_buf[L_INDEX]+1;
#ifndef NDEBUG
fprintf(stdout, "Message body:\n");
for (int i=0; i<N; i++) {
for (int j=0; j<L; j++) {
fprintf(stdout, " %02x",out_buf[HEADER_SIZE+i*L+j]);
}
fprintf(stdout, "\n");
}
fprintf(stdout, "END\n");
#endif
int message_size = HEADER_SIZE + N*L;
memcpy(output_buffer,out_buf,sizeof(uint8_t)*message_size);
ardu.write( output_buffer, message_size ) ;
}
void Serial_Send_Message(string message)
{
char output_buffer[64];
int i,message_length;
message_length=message.length();
for(i=0;i<message_length;i++)
{
output_buffer[i]=message[i];
}
serial_port.write(output_buffer,message_length);
}
void write(SerialStream& serial_port, const std::string& data)
{
serial_port.write(data.c_str(), data.size());
}