Regarding the serial buffer, there definitely is one, else your code would be limited by how fast the serial transmission happens, byte by byte. Length, references, etc. I'll leave to those who know the libraries, I'm just a user.
Regarding binary, the data you're sending, if sent as binary
4 bytes per float, times 4
2 bytes per int, times 6.
Sum total 28 bytes, but usually a protocol is implemented to package it, so more likely 30 or so.
At worst, you could send the largest negative value a float can hold x 4, plus 32767 x 6, for a total char count including separators of: -3.4028235E+38 * 4 + -32768*4
or,60+27 (plus line ending) or 88 characters.
Now, that's worst case. Let's look at your best case.
4 * 0.00 + 6 * 0 would give you 32 bytes, including a single line ending. So at your level best, your process will only slightly exceed the binary performance. Any time your floats grow, or your ints deviate from zero, your protocol slows down.
Now, I grant you, it's easier for you to understand the process, if you keep it all as ASCII. But efficient, it isn't.
Natural binary coded data is transmitted faster than ASCII coded data (see post#19) .
1. Assume you data are:
float qx = 1234.56; float qy = 2345.67; float qz = 3456.78; float qw = 4567.89;
int accAx = 0x4567; int accAy = 0x5678; int accAz = 0x6789;
int gyGx = 0x1234; int gyGy = 0x2345; int gyGz = 0x3456;
2. Let us send the data of Step-1 in natural binary from Arduini UNO (Sender) to Arduino Arduino NANO (Receiver) at Bd = 9600 using SUART (2, 3) (Software UART Port). The format of one tarsmission frame is:
0x12 0x34 0x45 //Start mark = synchronization pattern
qx qy qz qw //quaternion data; each 4-byte of binary32 format
accAx accAy accAz //acceleration data; each is 2-byte
gxGx gzGy gxGz //Gyro data; each is 2-byte
Sender Sketch: (just sending one float number for demonstration)
#include<SoftwareSerial.h>
SoftwareSerial SUART(2, 3); //SRX = 2, STX = 3
float qx = 1234.56;
float qy = 2345.67;
float qz = 3456.78;
float qw = 4567.89;
int accAx = 0x4567;
int accAy = 0x5678;
int accAz = 0x6789;
int gyGx = 0x1234;
int gyGy = 0x2345;
int gyGz = 0x3456;
byte *ptr;
void setup()
{
Serial.begin(9600);
SUART.begin(9600);
}
void loop()
{
SUART.write(0x12);
SUART.write(0x34);
SUART.write(0x56);
txFloat(qx);
delay(2000);
}
void txFloat(float y)
{
ptr = (byte*)&y;
for (int i = 0; i < 4; i++)
{
byte z = *ptr;
SUART.write(z);
ptr++;
}
}
Receiver Sketch:
#include<SoftwareSerial.h>
SoftwareSerial SUART(2, 3); //SRX = 2, STX = 3
float qx, qy, qz, qw;
int accAx, accAy, accAz;
int gyGx, gyGy, gyGz;
byte syncData[3];
long int syncPatt; //123456
byte myData[4];
bool flag = false;
void setup()
{
Serial.begin(9600);
SUART.begin(9600);
}
void loop()
{
byte n = SUART.available();
if (n == 3)
{
for (int i = 0; i < 3; i++)
{
syncData[i] = SUART.read();
Serial.println(syncData[i], HEX); //debug
}
syncPatt = (long int)syncData[0] << 16 | (long int)syncData[1] << 8 | (long int)syncData[2];
Serial.println(syncPatt, HEX); //debug
if (syncPatt == 0x123456)
{
SUART.readBytes(myData, 4);
memcpy(&qx, &myData, 4);
Serial.println(qx, 2); //shows: 1234.56 debug
}
}
}
Output:
12
34
56
123456 //sync pattern
1234.56 //qx data
Is BCD faster than directly writing the floating point bytes? (Also could you please tell me how to find post 19? I'm very new here)
If I also take the extra step of converting the floating point number to fixed point then I can send it as an integer so instead of taking 4 bytes it will now take 2. Is this a good idea?
Also I understand that directly writing bytes will be faster but given that the dmp is only outputting 2 decimal points values, 1.12 for example will is already taking up 4 bytes as ascii and floating point will anyways take up 4 bytes, why is writing binary faster.
Lastly is serial.writ()e slower than serial.print()?
gyro return values between -1.0 and 1.0, isn't it?
Not only is writing binary faster, potentially much faster, it's periodic. Your string lengths will vary, and depending on values involved, could vary drastically. The binary transmission is the same length every time. Every time. Every time. Get it?
But, I worry that you don't understand enough yet to begin to use a binary method. It would require coding on the receiving end that I suspect you're not ready for. So, I'd suggest you stick with your ASCII method. You'll probably get more improvement in your process by tuning your Arduino code.
Best of luck, I'm out for the day.
2 Likes
Thank you. I will still go ahead with the binary part nonetheless and learn from failing 
many thanks to everyone here. Fortunate to have learnt so much from so many knowledgeable people.
1.1234567890123456789 is still 4 bytes in float. how much it takes as ASCII ?
1.12 * 100 = 112 this is 1 byte, savvy?
thank you the example made it clearer.
Everything past the 1.123456 is uncertain in the 6-7 digit precision of a 4 byte float:
void setup() {
float x = 1.1234567890123456789;
Serial.begin(115200);
Serial.print(x, 40);
Serial.print(" Size:");
Serial.print(sizeof(x));
}
void loop() {
}
...prints:
1.1234568357467651367187500000000000000000 Size:4
ETA: If you modify it to add powers of 10:
1.1234568357467651367187500000000000000000 Size:4
2.1234569549560546875000000000000000000000 Size:4
11.1234569549560546875000000000000000000000 Size:4
101.1234588623046875000000000000000000000000 Size:4
1001.1234741210937500000000000000000000000000 Size:4
10001.1230468750000000000000000000000000000000 Size:4
100001.1250000000000000000000000000000000000000 Size:4
1000001.1250000000000000000000000000000000000000 Size:4
10000001.0000000000000000000000000000000000000000 Size:4
100000000.0000000000000000000000000000000000000000 Size:4
I've managed to implement the binary part and convert it equivalently on the receiver side. And I've managed to decrease the amount of data I'm sending more by converting the quaternion data to fixed point representation by multiplying them by 100. So all in all I might be sending 21 bytes (2 * 10 + 1 for return). But what's happening now (and this was happening previously as well) is that the Arduino has started to "hang"? I've tried it with miniterm via miniterm -e /dev/ttyUSB0 57600, I ask it to send the imu data and send its it along just fine. Then I exit out of miniterm and then re-run it again, asking for imu data, and it just hangs, not printing anything at all. I suspect that whatever is causing this is also whats causing the receiver to time out as it is not receiving any data. This is what the read function prints out.
if(Serial.availableForWrite() > 20)
{
// you can print
Serial.write((byte*)&iqx, sizeof(iqx));
Serial.write((byte*)&iqy, sizeof(iqy));
Serial.write((byte*)&iqz, sizeof(iqz));
Serial.write((byte*)&iqw, sizeof(iqw));
Serial.write((byte*)&ax, sizeof(ax));
Serial.write((byte*)&ay, sizeof(ay));
Serial.write((byte*)&az, sizeof(az));
Serial.write((byte*)&gx, sizeof(gx));
Serial.write((byte*)&gy, sizeof(gy));
Serial.write((byte*)&gz, sizeof(gz));
Serial.println("");
}
And this is on the receiver side:
int read_imu_data(int16_t* qx, int16_t* qy, int16_t* qz, int16_t* qw, int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz)
{
serial_conn_.FlushIOBuffers(); // Just in case
serial_conn_.Write("i\r");
uint8_t imu_data[21];
try
{
for(int i = 0; i < 22; i++)
serial_conn_.ReadByte(imu_data[i], 1000);
*qx = (int16_t&)imu_data;
*qy = (int16_t&)imu_data[2];
*qz = (int16_t&)imu_data[4];
*qw = (int16_t&)imu_data[6];
*ax = (int16_t&)imu_data[8];
*ay = (int16_t&)imu_data[10];
*az = (int16_t&)imu_data[12];
*gx = (int16_t&)imu_data[14];
*gy = (int16_t&)imu_data[16];
*gz = (int16_t&)imu_data[18];
}
catch(const LibSerial::ReadTimeout&)
{
*qx = 0;
*qy = 0;
*qz = 0;
*qw = 0;
*ax = 0;
*ay = 0;
*az = 0;
*gx = 0;
*gy = 0;
*gz = 0;
}
return -1;
}
I have 0 problems with the receiver side, it works just fine every time. But the Arduino keeps getting stuck and I'm not sure what the issue is.
@camsysca @kolaha sorry for pinging but I would love to have your opinions on why its doing this
The arduino code is from the ros_arduino_bridge in the firmware folder in the repo (I've linked it in post 1).
Its running on an Arduino Mega, and I have no sensors connected to it besides the MPU6050. Code's made for an Arduino Uno but for what I'm doing you don't need it and it won't make a difference. You can use miniterm to connect to it and send it data. All you have to type is 'i'.
I've modified ROSArduinoBridge.ino to the following:
/*********************************************************************
* ROSArduinoBridge
A set of simple serial commands to control a differential drive
robot and receive back sensor and odometry data. Default
configuration assumes use of an Arduino Mega + Pololu motor
controller shield + Robogaia Mega Encoder shield. Edit the
readEncoder() and setMotorSpeed() wrapper functions if using
different motor controller or encoder method.
Created for the Pi Robot Project: http://www.pirobot.org
and the Home Brew Robotics Club (HBRC): http://hbrobotics.org
Authors: Patrick Goebel, James Nugen
Inspired and modeled after the ArbotiX driver by Michael Ferguson
Software License Agreement (BSD License)
Copyright (c) 2012, Patrick Goebel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following
disclaimer in the documentation and/or other materials provided
with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#define USE_BASE // Enable the base controller code
//#undef USE_BASE // Disable the base controller code
/* Define the motor controller and encoder library you are using */
#ifdef USE_BASE
/* The Pololu VNH5019 dual motor driver shield */
//#define POLOLU_VNH5019
/* The Pololu MC33926 dual motor driver shield */
//#define POLOLU_MC33926
/* The RoboGaia encoder shield */
//#define ROBOGAIA
/* Encoders directly attached to Arduino board */
#define ARDUINO_ENC_COUNTER
/* L298 Motor driver*/
#define L298_MOTOR_DRIVER
#endif
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#define INTERRUPT_PIN 18 // use pin 2 on Arduino Uno & most boards
MPU6050 mpu;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint8_t fifoBuffer[64]; // FIFO storage buffer
Quaternion q;
int16_t ax, ay, az;
int16_t gx, gy, gz;
int16_t iqx, iqy, iqz, iqw;
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
//#define USE_SERVOS // Enable use of PWM servos as defined in servos.h
#undef USE_SERVOS // Disable use of PWM servos
/* Serial port baud rate */
#define BAUDRATE 57600
/* Maximum PWM signal */
#define MAX_PWM 255
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
/* Include definition of serial commands */
#include "commands.h"
/* Sensor functions */
#include "sensors.h"
/* Include servo support if required */
#ifdef USE_SERVOS
#include <Servo.h>
#include "servos.h"
#endif
#ifdef USE_BASE
/* Motor driver function definitions */
#include "motor_driver.h"
/* Encoder driver function definitions */
#include "encoder_driver.h"
/* PID parameters and functions */
#include "diff_controller.h"
/* Run the PID loop at 30 times per second */
#define PID_RATE 30 // Hz
/* Convert the rate into an interval */
const int PID_INTERVAL = 1000 / PID_RATE;
/* Track the next time we make a PID calculation */
unsigned long nextPID = PID_INTERVAL;
/* Stop the robot if it hasn't received a movement command
in this number of milliseconds */
#define AUTO_STOP_INTERVAL 2000
long lastMotorCommand = AUTO_STOP_INTERVAL;
#endif
/* Variable initialization */
// A pair of varibles to help parse serial commands (thanks Fergs)
int arg = 0;
int index = 0;
// Variable to hold an input character
char chr;
// Variable to hold the current single-character command
char cmd;
// Character arrays to hold the first and second arguments
char argv1[16];
char argv2[16];
// The arguments converted to integers
long arg1;
long arg2;
/* Clear the current command parameters */
void resetCommand() {
cmd = NULL;
memset(argv1, 0, sizeof(argv1));
memset(argv2, 0, sizeof(argv2));
arg1 = 0;
arg2 = 0;
arg = 0;
index = 0;
}
/* Run a command. Commands are defined in commands.h */
int runCommand() {
int i = 0;
char *p = argv1;
char *str;
int pid_args[4];
arg1 = atoi(argv1);
arg2 = atoi(argv2);
switch(cmd) {
case GET_BAUDRATE:
Serial.println(BAUDRATE);
break;
case ANALOG_READ:
Serial.println(analogRead(arg1));
break;
case DIGITAL_READ:
Serial.println(digitalRead(arg1));
break;
case ANALOG_WRITE:
analogWrite(arg1, arg2);
Serial.println("OK");
break;
case DIGITAL_WRITE:
if (arg2 == 0) digitalWrite(arg1, LOW);
else if (arg2 == 1) digitalWrite(arg1, HIGH);
Serial.println("OK");
break;
case PIN_MODE:
if (arg2 == 0) pinMode(arg1, INPUT);
else if (arg2 == 1) pinMode(arg1, OUTPUT);
Serial.println("OK");
break;
case PING:
Serial.println(Ping(arg1));
break;
#ifdef USE_SERVOS
case SERVO_WRITE:
servos[arg1].setTargetPosition(arg2);
Serial.println("OK");
break;
case SERVO_READ:
Serial.println(servos[arg1].getServo().read());
break;
#endif
case IMU_READ:
// Serial.print(Serial.available());
// Serial.print(" ");
if(Serial.availableForWrite() > 20)
{
// you can print
Serial.write((byte*)&iqx, sizeof(iqx));
Serial.write((byte*)&iqy, sizeof(iqy));
Serial.write((byte*)&iqz, sizeof(iqz));
Serial.write((byte*)&iqw, sizeof(iqw));
Serial.write((byte*)&ax, sizeof(ax));
Serial.write((byte*)&ay, sizeof(ay));
Serial.write((byte*)&az, sizeof(az));
Serial.write((byte*)&gx, sizeof(gx));
Serial.write((byte*)&gy, sizeof(gy));
Serial.write((byte*)&gz, sizeof(gz));
Serial.println("");
}
// Serial.print(iqx);
// Serial.print(" ");
// Serial.print(iqy);
// Serial.print(" ");
// Serial.print(iqz);
// Serial.print(" ");
// Serial.print(iqw);
// Serial.print(" ");
// Serial.print(ax);
// Serial.print(" ");
// Serial.print(ay);
// Serial.print(" ");
// Serial.print(az);
// Serial.print(" ");
// Serial.print(gx);
// Serial.print(" ");
// Serial.print(gy);
// Serial.print(" ");
// Serial.println(gz);
break;
#ifdef USE_BASE
case READ_ENCODERS:
Serial.print(readEncoder(LEFT));
Serial.print(" ");
Serial.println(readEncoder(RIGHT));
break;
case RESET_ENCODERS:
resetEncoders();
resetPID();
Serial.println("OK");
break;
case MOTOR_SPEEDS:
/* Reset the auto stop timer */
lastMotorCommand = millis();
if (arg1 == 0 && arg2 == 0) {
setMotorSpeeds(0, 0);
resetPID();
moving = 0;
}
else moving = 1;
leftPID.TargetTicksPerFrame = arg1;
rightPID.TargetTicksPerFrame = arg2;
Serial.println("OK");
break;
case MOTOR_RAW_PWM:
/* Reset the auto stop timer */
lastMotorCommand = millis();
resetPID();
moving = 0; // Sneaky way to temporarily disable the PID
setMotorSpeeds(arg1, arg2);
Serial.println("OK");
break;
case UPDATE_PID:
while ((str = strtok_r(p, ":", &p)) != '\0') {
pid_args[i] = atoi(str);
i++;
}
Kp = pid_args[0];
Kd = pid_args[1];
Ki = pid_args[2];
Ko = pid_args[3];
Serial.println("OK");
break;
#endif
default:
Serial.println("Invalid Command");
break;
}
}
/* Setup function--runs once at startup. */
void setup() {
Serial.begin(BAUDRATE);
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
mpu.setDMPEnabled(true);
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
dmpReady = true;
// Initialize the motor controller if used */
#ifdef USE_BASE
#ifdef ARDUINO_ENC_COUNTER
//set as inputs
DDRD &= ~(1<<LEFT_ENC_PIN_A);
DDRD &= ~(1<<LEFT_ENC_PIN_B);
DDRC &= ~(1<<RIGHT_ENC_PIN_A);
DDRC &= ~(1<<RIGHT_ENC_PIN_B);
//enable pull up resistors
PORTD |= (1<<LEFT_ENC_PIN_A);
PORTD |= (1<<LEFT_ENC_PIN_B);
PORTC |= (1<<RIGHT_ENC_PIN_A);
PORTC |= (1<<RIGHT_ENC_PIN_B);
// tell pin change mask to listen to left encoder pins
PCMSK2 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
// tell pin change mask to listen to right encoder pins
PCMSK1 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
// enable PCINT1 and PCINT2 interrupt in the general interrupt mask
PCICR |= (1 << PCIE1) | (1 << PCIE2);
#endif
initMotorController();
resetPID();
#endif
/* Attach servos if used */
#ifdef USE_SERVOS
int i;
for (i = 0; i < N_SERVOS; i++) {
servos[i].initServo(
servoPins[i],
stepDelay[i],
servoInitPosition[i]);
}
#endif
}
/* Enter the main loop. Read and parse input from the serial port
and run any valid commands. Run a PID calculation at the target
interval and check for auto-stop conditions.
*/
void loop() {
while (Serial.available() > 0) {
// Read the next character
chr = Serial.read();
// Terminate a command with a CR
if (chr == 13) {
if (arg == 1) argv1[index] = NULL;
else if (arg == 2) argv2[index] = NULL;
runCommand();
resetCommand();
}
// Use spaces to delimit parts of the command
else if (chr == ' ') {
// Step through the arguments
if (arg == 0) arg = 1;
else if (arg == 1) {
argv1[index] = NULL;
arg = 2;
index = 0;
}
continue;
}
else {
if (arg == 0) {
// The first arg is the single-letter command
cmd = chr;
}
else if (arg == 1) {
// Subsequent arguments can be more than one character
argv1[index] = chr;
index++;
}
else if (arg == 2) {
argv2[index] = chr;
index++;
}
}
}
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
{
mpu.dmpGetQuaternion(&q, fifoBuffer);
// we want the quaternion values in integer format
iqx = q.x * 100;
iqy = q.y * 100;
iqz = q.z * 100;
iqw = q.w * 100;
}
// If we are using base control, run a PID calculation at the appropriate intervals
#ifdef USE_BASE
if (millis() > nextPID) {
updatePID();
nextPID += PID_INTERVAL;
}
// Check to see if we have exceeded the auto-stop interval
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
setMotorSpeeds(0, 0);
moving = 0;
}
#endif
// Sweep servos
#ifdef USE_SERVOS
int i;
for (i = 0; i < N_SERVOS; i++) {
servos[i].doSweep();
}
#endif
}
@Delta_G Do you want a simpler version of the code? And is it possible that the MPU 6050 DMP is causing the Arduino to hang or it's blocking it until data is available?
This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.