Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino

asked 2018-01-23 16:20:22 -0600

khansaadbinhasan gravatar image

updated 2018-01-24 14:30:28 -0600

I am running ros kinetic and my rosserial works fine for all programs except one( in which i get above error).

I have tried all the techniques suggested but even then it doesnt work.( baudrate is fine , i tried to plugin usb a lot of times, rosserial works fine for other programs) but still i am getting the same error for only one program. What is the solution for this?

Here is my code for arduino:

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif





#include <ros.h>
#include <sensor_msgs/Imu.h>

ros::NodeHandle nh;
sensor_msgs::Imu Imu_msg;
ros::Publisher pub_Imudata("Imu_data",&Imu_msg);







MPU6050 mpu;
MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;


#define LED_PIN 13 
bool blinkState = false;

// 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)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector

byte statusLed =13;
byte sensorInterrupt=0;
byte sensorPin=2;

double calibrationFactor=4.5;
volatile byte pulseCount;
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high



void dmpDataReady() 
{
  mpuInterrupt = true;
}



void setup() 
{



    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz)

    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    //changed this
    Serial.begin(115200);
    while (!Serial);

    nh.initNode();
    nh.advertise(pub_Imudata);


    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // wait for ready
    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer
    while (!Serial.available());                 // wait for data
    while (Serial.available() && Serial.read()); // empty buffer again


    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788);


    if (devStatus == 0) 
    {
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);


        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(1 , dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        packetSize = mpu.dmpGetFIFOPacketSize();
    } 

    else 
    {
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

}





void loop() 
{
    Imu_msg.header.seq++;
    Imu_msg.header.stamp = nh.now();


    if (!dmpReady) return;
    while (!mpuInterrupt && fifoCount < packetSize) {}

    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();


    fifoCount = mpu.getFIFOCount();

    if ((mpuIntStatus & 0x10) || fifoCount == 1024) 
    {
        mpu.resetFIFO();       
    } 

    else if (mpuIntStatus & 0x02) 
    {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        mpu.getFIFOBytes(fifoBuffer, packetSize);
        fifoCount -= packetSize;

        mpu.dmpGetQuaternion(&q, fifoBuffer);
        Imu_msg.orientation.w = q.w;
        Imu_msg.orientation.x = q.x;
        Imu_msg ...
(more)
edit retag flag offensive close merge delete

Comments

In my experience, this can be a software mismatch (as stated in the error) or a connection problem. I've never known it to be a problem with code.

jayess gravatar imagejayess ( 2018-01-24 14:56:23 -0600 )edit

I don't think its a software mismatch because i tried to run other codes and they run fine.

khansaadbinhasan gravatar imagekhansaadbinhasan ( 2018-01-24 15:02:20 -0600 )edit