Not reading Subscribed ImuData

[mjs513]

Hi all.
Know this may be premature with the examples sketches but wanted to see if I could get it working with the Arduino style library I put together. I modified the teensy_body_control example to only publish ImuData from the phoenix custom message (yes I ran it through the compiler to generate the .hpp file). This seemed to be working. Using a custom message sniffer on a second Teensy 3.5 I was able to see the following messages come across, not sure if they are correct yet:

 LEN: 8 EXT: 1 REMOTE: 0 TS: 14979 ID: 269716326 Buffer: 52 23 A2 5D 4B 51 0 93 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 15114 ID: 269716326 Buffer: 0 0 B0 1D B0 9D 8D 33 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 23168 ID: 269716326 Buffer: 91 93 9F 7D 4B 51 0 94 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 23305 ID: 269716326 Buffer: 0 0 8D 1C 8D 9C 0 34 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 23443 ID: 269716326 Buffer: 0 1F 25 AE 27 8D 1C 14 
  LEN: 7 EXT: 1 REMOTE: 0 TS: 23581 ID: 269716326 Buffer: A0 5D 0 C4 40 BC 74 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 32005 ID: 269716326 Buffer: 25 B0 24 A0 4B 51 0 95 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 32143 ID: 269716326 Buffer: 0 0 B0 9D D4 1A 0 35 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 41857 ID: 269716326 Buffer: 11 F6 A2 C6 4B 51 0 96 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 41993 ID: 269716326 Buffer: 0 0 D4 9E 8D 18 8D 36 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 42129 ID: 269716326 Buffer: 18 1F 25 1F 21 D4 9E 16 
  LEN: 7 EXT: 1 REMOTE: 0 TS: 42266 ID: 269716326 Buffer: A0 5D 0 C4 40 BC 76 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 51707 ID: 269716326 Buffer: 6B 48 1C ED 4B 51 0 97 

So at least I know something is getting sent. I then created a subscriber only sketch, and this is where I get fuzzy on the right format. But any way it seems that the subscriber node gets fired but no messages get printed in the callback and it appears the callback never gets fired. Really would like to get this example working.

Here are the two associated files:

Main sketch

#include "Arduino.h"

// CAN Node settings
static constexpr uint32_t nodeID = 100;
static constexpr uint8_t swVersion = 1;
static constexpr uint8_t hwVersion = 1;
static const char* nodeName = "org.phoenix.imu.data";

// application settings
static constexpr float framerate = 1000;

#include "phoenix_can_shield.h"
#include "Subscriber.h"

// this is important for UAVCAN to compile
namespace std {
  void __throw_bad_alloc()
  {
    Serial.println("Unable to allocate memory");
  }

  void __throw_length_error( char const*e )
  {
    Serial.print("Length Error :");
    Serial.println(e);
  }

  void __throw_bad_function_call()
  {
    Serial.println("Bad function call!");
  }
}

void setup() {
  //get EEPROM Parameters
  //readParamsFromEEPROM();
  delay(3000);
  Serial.begin(115200);
  Serial.println("Setup");
  
  // init LEDs
  initLeds();

  // create a node
  systemClock = &initSystemClock();
  canDriver = &initCanDriver();
  node = new Node<NodeMemoryPoolSize>(*canDriver, *systemClock);
  bool nodeStart = initNode(node, nodeID, nodeName, swVersion, hwVersion);
  
  // init subscriber
  initSubscriber(node);


  // set up filters
  configureCanAcceptanceFilters(*node);


  // start up node
  node->setModeOperational();

  Serial.println("Node Setup Finished");


}

uint32_t t_ = 0;
void loop() {
  // wait in cycle
  //uint32_t t = micros();
  //Serial.print("CPU Load: ");
  //Serial.println((float)(t-t_)/100);
  cycleWait(framerate);
  //t_ = micros();

  // get RC data, high level commands, motor telemetry rear motors
  cycleNode(node);
  
  // toggle heartbeat
  toggleHeartBeat();
}

Subscriber.cpp file:

#ifndef	SUBSCRIBER_HPP
#define	SUBSCRIBER_HPP

#include <uavcan/uavcan.hpp>
#include <uavcan/dsdlc/phoenix_msgs/ImuData.hpp>

using namespace uavcan;
using namespace phoenix_msgs;

Subscriber<ImuData> *imu_data_Subscriber;

typedef struct {
  float lin_acc[3];
  float gyro[3];
  float euler[3];
} imu_t;

imu_t imu;

void imu_data_callback(const ImuData& msg)
{
  // TODO

  imu.lin_acc[0] = msg.lin_acceleration[0];
  imu.lin_acc[1] = msg.lin_acceleration[1];
  imu.lin_acc[2] = msg.lin_acceleration[2];

  imu.gyro[0] = msg.rate_gyro[0];
  imu.gyro[1] = msg.rate_gyro[1];
  imu.gyro[2] = msg.rate_gyro[2];

  imu.euler[0] = msg.euler[0];
  imu.euler[1] = msg.euler[1];
  imu.euler[2] = msg.euler[2];
  
  Serial.print("Orientation: ");
  Serial.print(imu.euler[0]); Serial.print(", ");
  Serial.print(imu.euler[1]); Serial.print(", ");
  Serial.println(imu.euler[2]);
}


void initSubscriber(Node<NodeMemoryPoolSize> *node)
{
  // create a subscriber
  imu_data_Subscriber = new Subscriber<ImuData>(*node);

  if(imu_data_Subscriber->start(imu_data_callback) < 0)
  {
    Serial.println("Unable to start subscriber!");
  } else {
    Serial.println("Subscriber Started");
  }
}

#endif

[mjs513]

Just as a check I put a couple serial prints in the driver proper and it is receiving messages.

[mjs513]

@fabolhak
Ok.. As a test I decided to just use an example from UAVcan on sending and receiving Sideslip messages using your sketch format. When I did that it worked perfectly. It decoded the sideslip messages no problem. Maybe its a problem with the imudata message itself or multiple messages?

[fabolhak]

From the sources you provided everything looks fine to me. If you would provide the full source code for the Subscriber as well as Publisher I could have a second look at it.

Unfortunately, I currently don't have access to hardware. Therefore, I can not test it :(.

If your budget allows it, I would also recommend buying the Zubax Babel. It is very handy for debugging UAVCAN communication.

[mjs513]

@fabolhak
Thanks for getting back to me on this. Think the sketches may be working fine. There may be something with the driver. I posted on the UAVcan Google group since I knew you were busy: https://groups.google.com/forum/#!topic/uavcan/gK6vNSfmQG8. You may want to check out what Pavel was saying.

Right now I am trying to see what's actually coming in versus what is going out. For the sideslip case its working no problem whats being sent is what's being received. For the ImuData message that's another case. I will keep you posted on that one if you don't mind.

Anyway, here is the publisher sketch.
teensy_uavcan_imu_publish.zip

I will definitely check out the Zubax Babel.

Thanks
Mike

[mjs513]

@fabolhak
Did some checking and using a different sniffer, the message supposedly being sent looks like this:

 LEN: 8 EXT: 1 REMOTE: 0 FrameID: 2417199974 Buffer: EC F 57 79 EA 1C 0 99 
 LEN: 8 EXT: 1 REMOTE: 0 FrameID: 2417199974 Buffer: 0 0 8D 98 8D 18 0 39 
 LEN: 8 EXT: 1 REMOTE: 0 FrameID: 2417199974 Buffer: 0 1F 25 1F A9 8D 98 19 
 LEN: 7 EXT: 1 REMOTE: 0 FrameID: 2417199974 Buffer: A0 5D 40 C2 C0 BD 79 

Which I believe is the correct format and what should be sent. Using the second teensy as a sniffer, this is what I am seeing:

  LEN: 8 EXT: 1 REMOTE: 0 TS: 2226 ID: 269716326 Buffer: BA D2 35 49 EA 1C 0 98 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 2361 ID: 269716326 Buffer: 0 0 8D 94 8D 14 0 38 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 14547 ID: 269716326 Buffer: EC F 57 79 EA 1C 0 99 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 14684 ID: 269716326 Buffer: 0 0 8D 98 8D 18 0 39 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 14822 ID: 269716326 Buffer: 0 1F 25 1F A9 8D 98 19 
  LEN: 7 EXT: 1 REMOTE: 0 TS: 14960 ID: 269716326 Buffer: A0 5D 40 C2 C0 BD 79 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 27099 ID: 269716326 Buffer: BB D0 60 AA EA 1C 0 9A 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 27237 ID: 269716326 Buffer: 0 0 0 0 0 0 8D 3A 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 27380 ID: 269716326 Buffer: 18 1F 25 1F A5 0 0 1A 
  LEN: 7 EXT: 1 REMOTE: 0 TS: 27520 ID: 269716326 Buffer: A0 5D 40 C2 C0 BD 7A 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 39300 ID: 269716326 Buffer: 7B 23 A DA EA 1C 0 9B 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 39437 ID: 269716326 Buffer: 0 0 8D 14 8D 14 8D 3B 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 53236 ID: 269716326 Buffer: F4 80 7A 10 EB 1C 0 9C 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 53372 ID: 269716326 Buffer: 0 0 8D 14 D4 9A 8D 3C 
  LEN: 8 EXT: 1 REMOTE: 0 TS: 53508 ID: 269716326 Buffer: 18 1F 21 1F A1 8D 14 1C 
  LEN: 7 EXT: 1 REMOTE: 0 TS: 53647 ID: 269716326 Buffer: A0 5D 40 C2 C0 BD 7C 

It looks like it loosing packets that should be transferred. See the message that I referenced.

Thanks
Mike

[fabolhak]

Hello Mike,

unfortunately, I can not find any problems with your code at a first glance. However, I can give you two bits of advice:

Maybe you want to check out the official definition of an IMU message and see if that works (we wrote our own custom message type to save some bytes we don't use):
https://uavcan.org/Specification/7._List_of_standard_data_types/#rawimu

Working with the raw CAN messages can sometimes be confusing. Maybe I could be helpful to write a CAN message decoder. I did that some time ago for our own CAN sniffer. Maybe it is useful for you too:
https://github.com/tum-phoenix/drive_helper_logicport_parser

However, I general it is not a good sign that some message types get transferred correctly and some others not. It should not make a difference. This could mean that the driver implementation is not stable.

[mjs513]

Maybe it is useful for you too: https://github.com/tum-phoenix/drive_helper_logicport_parser

Thanks, this will be very useful going forward. Anyway, you must be reading my mind. I just got it working a little while ago but I had to change the flexcan driver. I updated to this version: https://forum.pjrc.com/threads/45017-Fix-for-FlexCAN-library-provided-with-Teensyduino?highlight=nmea2000 but had to make a few minor modifications to make it equivalent to what was there. If you are interested I can post the changed files.

Once I updated the driver it worked like a charm and printed out the euler angles since that was all I was asking for as a test :)

Now I am working on another test message (Fix.hpp) just to be sure but ran into another issue that I think is my own doing. Publisher not working - CPU is like 330000 percent so I must have messed something up

[mjs513]

Just wanted to let you know found the error. All is good. Driver works fine.
Thanks for your help. Let me know about the driver
Mike

[mjs513]

@fabolhak I am attaching the updated flexcan library files if you would like to use them.
FlexCan.zip

[fabolhak]

Hello Mike,
I'm very glad to hear that you got it working. Thank you for uploading your version of the FlexCAN library. I did compare both files with the ones included in our driver implementation. The only real difference I can spot is that the IRQ priority is different and a bug (?) that I fixed in order to get in working for us.

Anyways, as long as it works for both of us, everything is fine :).

[mjs513]

I will keep both changes in mind just in case. I am running primarily on a Teensy 3.5 and 3.6. Just did a few tests on the Teensy 3.2.