UAVcan for Teensy 3.x Proof of Concept

[mjs513]

After some work and a lot of help from @KurtE was able to reformat libuavcan (https://github.com/UAVCAN/libuavcan) into a Arduino style library that can compile using Arduino IDE/Teensyduino. The teensy driver and examples are taken from the UAVcan site and tum-phoenix (https://github.com/tum-phoenix/drive_teensy_main and https://github.com/tum-phoenix/drive_teensy_libuavcan) and modified appropriately. It seems to work. Again this is just a proof of concept. Still figuring out all the ins and outs of UAVcan but the library will give everyone something to play with.

The library is in a GITHUB repository for your use if interested: https://github.com/mjs513/libuavcan-Teensy-3.x . The example sketches show an example for publishing and subscribing to sideslip data from a sensor.

Cheers
Mike

[mjs513]

Ok. I just updated the Flexcan library to resolve issues with multi line messages. I also updated the library with a GNSS Fix test message as well as a example of using a custom message for IMU data. The IMU example uses a BNO055 from adafruit and transmits live data to the subscribing node. So I guess the library is now more than a proof of concept as it looks like it works.

the flexcan library used was a slightly modified version found on this thread: https://forum.pjrc.com/threads/45017...light=nmea2000. thanks @ttlappalainen. Worked like a charm.

Mike

[brtaylor]

Is there a good overview of the UAVcan protocol? I've implemented their hardware design methodology for all of the buses on my flight controller, but am using my own messaging protocol. I wonder if it would be worth using the UAVcan protocol over I2C in anticipation of switching to a CAN bus at some point, but I'd like to evaluate the overhead and error handling of the UAVcan protocol compared to our in-house design.

[mjs513]

Don't know if there is any simple overview out there. If your go to https://uavcan.org/ it pretty much details the specification, implementation and provides a bunch of tutorials.

[mjs513]

Just update the library so that Ostream with operate correctly with Teensy, i.e., printf works. See this thread for explanation: https://forum.pjrc.com/threads/53423-Printf-revisited.

[mjs513]

Ok. Added a new GitHub repository for a Teensy CANbus monitor that can be used with UAVcan applications and the UAVcan GUI tool (https://github.com/UAVCAN/gui_tool). The repository is at: https://github.com/mjs513/Teensy_Canbus_Monitor_IFCT

It does use tonton81's IFCT library mcp_can emulation since the original application that I modified was for a MCP2515 and the CANbus shield. So this version should now work with any CAN transceiver.

Cheers
Mike

[mjs513]

In prep for a little more challenging project with UAVcan and Teensies I picked up a inexpensive simpleBGC (clone) 2 axis gimbal. Unfortunately had to flash it with a different software version of simpleBGC and had to slightly modify their library but I did manage to get it working. Attached is the library I modified for the gimbal. it does come with some sample sketches but wrote my own for just moving the gimbal in pitch and printing out the angles.

#include <inttypes.h>
#include <SBGC.h>
#include <SBGC_Arduino.h>

////////////////////////////////////////////////
// Serial baud rate should match with the rate, configured for the SimpleBGC controller
#define SERIAL_SPEED 115200

// delay between commands, ms
#define SBGC_CMD_DELAY 10

// Set serial port where SBGC32 is connected
#define serial Serial1

////////////////////////////////////////
static SBGC_cmd_realtime_data0_t rt_data0;
static SBGC_cmd_realtime_data_t rt_data;
static SBGC_cmd_getAngles_data_t rt_angle_data;

#define MAX_WAIT_TIME_MS 2000  // time to wait for incoming commands to be in CONNECTED state
#define REALTIME_DATA_REQUEST_INTERAL_MS 5 // interval between reatime data requests

static uint16_t cur_time_ms, last_cmd_time_ms; //last_bt_connect_ms;
static uint8_t is_connected = 0;

elapsedMillis dataRate;

void setup()
{
    serial.begin(SERIAL_SPEED);
    Serial.begin(115200);
    Serial.println("Start\n");
    
    SBGC_Demo_setup(&serial);
       
}

void loop()
{
    //mode; speedROLL; angleROLL; speedPITCH; anglePITCH; speedYAW; angleYAW;
    SBGC_cmd_control_t c = { 0, 0, 0, 0, 0, 0, 0 };

    // Move camera to initial position (all angles are zero)
    // Set speed 3 degree/sec
    c.mode = SBGC_CONTROL_MODE_ANGLE;
    c.speedROLL = c.speedPITCH = 10 * SBGC_SPEED_SCALE; 
    SBGC_cmd_control_send(c, sbgc_parser);
    delay(SBGC_CMD_DELAY);

    for(int i = -17; i < 40; i++){
		  c.anglePITCH = SBGC_DEGREE_TO_ANGLE(i);
		  SBGC_cmd_control_send(c, sbgc_parser);
      delay(SBGC_CMD_DELAY);

      cur_time_ms = millis();

      ////////// Request realtime data
      readAngles();
      delay(100);
    }
    
    ////////// Request realtime data
    readAngles();
    Serial.println("--------------");    
    delay(5000);

}

void readAngles(){
      SerialCommand cmd;
      if(is_connected) {
        cmd.init(SBGC_CMD_GET_ANGLES);
      } else { // Set version request to init connection
        cmd.init(SBGC_CMD_BOARD_INFO);
      }
      sbgc_parser.send_cmd(cmd, 0);
    delay(REALTIME_DATA_REQUEST_INTERAL_MS);
    process_in_queue();
}

// Process incoming commands. Call it as frequently as possible, to prevent overrun of serial input buffer.
void process_in_queue() {
  while(sbgc_parser.read_cmd()) {
    SerialCommand &cmd = sbgc_parser.in_cmd;
    last_cmd_time_ms = cur_time_ms;
    if(!is_connected) set_connected();
    
    uint8_t error = 0;
    switch(cmd.id) {
    // Receive realtime data
    case SBGC_CMD_GET_ANGLES:
      error = SBGC_cmd_getangles_data_unpack(rt_angle_data, cmd);
      //Serial.print("ERROR: ");Serial.println(error);
      if(!error) {
        Serial.print("Pitch: "); 
        Serial.print(SBGC_ANGLE_TO_DEGREE_INT(rt_angle_data.angleData[PITCH].angle));
        Serial.print(", "); Serial.print("Roll: ");  
        Serial.println(SBGC_ANGLE_TO_DEGREE_INT(rt_angle_data.angleData[ROLL].angle));
       } else {
        sbgc_parser.onParseError(error);
      }
      break;         
    case SBGC_CMD_REALTIME_DATA:
      error = SBGC_cmd_realtime_data0_unpack(rt_data0, cmd);
      //Serial.print("ERROR: ");Serial.println(error);
      if(!error) {
        Serial.print("Pitch: "); 
        Serial.print(SBGC_ANGLE_TO_DEGREE_INT(rt_data0.camera_angle[PITCH]));
        Serial.print(", "); Serial.print("Roll: ");  
        Serial.println(SBGC_ANGLE_TO_DEGREE_INT(rt_data0.camera_angle[ROLL]));
      } else {
        sbgc_parser.onParseError(error);
      }
      break;    
      
    case SBGC_CMD_REALTIME_DATA_3:
    case SBGC_CMD_REALTIME_DATA_4:
      error = SBGC_cmd_realtime_data_unpack(rt_data, cmd);
      Serial.print("ERROR: ");Serial.println(error);
      if(!error) {
        // Extract some usefull data
        // Average stabilization error (0.001 degree)
      } else {
        sbgc_parser.onParseError(error);
      }
      break;    
    }
  }
  
  // If no commands for a long time, set connected state to false
  if(is_connected && (uint16_t)(cur_time_ms - last_cmd_time_ms) > MAX_WAIT_TIME_MS) {
    is_connected = 0;
  }
}

// Called once on a connection established
void set_connected() {
  is_connected = 1;
}

The output isn't very impressive just a list of pitch and roll angles. Hopes this helps someone else if they are playing around with gimbals.

Will post more once I get it incorporated it into a UAVcan sketch.

[mjs513]

Before getting back to work I incorporated tonton81's new IFCT FlexCAN library into UAVcan. Thanks to tonton81 for all his help in getting this merged and updating his library. I also updated the examples to ensure no bus crashes when banging the CAN bus at loop speeds. This happened with the old library and the new library as well. All changes have been posted to GITHUB master branch.

[mjs513]

The Teensy 3.2 is the main controller and sends serial commands I entered to the node over the CAN bus. The Teensy 3.6 is the Gimbal node and powered from the battery for the Gimbal/controller. Notice no USB cable.

In the demo 4 commands are implemented Pxx for Pitch(deg), R for Roll, Z for return to 0 and L to do a Pitch -16 degrees. Here is quick video of it in action: https://youtu.be/eOAdpgBizgQ. Again this is just a demo of in action. Attached is photo of the test rig.


Cheers

[mjs513]

I've added another example to the GITHUB repository showing how you can connect an IMU node along with a GNSS node to the third Teensy that subscribes to both nodes. In this case I did it to illustrate the use of UAVcan acceptance filters. The output of the sketch looks like this:

Setup
Application node created
GNSS Subscriber Started

Reconfiguring acceptance filters ...
IMU Subscriber Started

Configuration with AcceptAnonymousMessages input and two subscribers:
config.ID [0]= 2147483648
config.MK [0]= 3758096639
config.ID [1]= 2147509376
config.MK [1]= 3758129024
config.ID [2]= 2148764416
config.MK [2]= 3774873472
config.ID [3]= 2147755008
config.MK [3]= 3774873472


Applying new configuration, IMU is accepted now...
Node Setup Finished
Orientation: 0.69, -7.13, 0.31

GNSS Fix: 2, 0, 40.76, -73.83
0, 0, 1.50, 1.50, 1.50
9, 3, 1.10
Orientation: 0.69, -7.13, 0.31

GNSS Fix: 2, 0, 40.76, -73.83
0, 0, 1.50, 1.50, 1.50
9, 3, 1.10
Orientation: 0.69, -7.13, 0.31

GNSS Fix: 2, 0, 40.76, -73.83
0, 0, 1.50, 1.50, 1.50
9, 3, 1.10
Orientation: 0.69, -7.13, 0.31

GNSS Fix: 2, 0, 40.76, -73.83
0, 0, 1.50, 1.50, 1.50
9, 3, 1.10
Orientation: 0.69, -7.13, 0.31

For the test I used a T3.5 reading IMU data from a BNO055, a T3.6 sending test GNSS data and a T3.2 subscribing to both messages.

Cheers
Mike

[brtaylor]

Hi Mike,

I'm taking a look at this library; I'd like to update it to use DroneCAN, which is basically UAV CAN version 0:
https://github.com/dronecan/libuavcan

I'd also like to use FlexCAN_T4 and ensure that all files are licensed MIT. Am I correct in thinking that everything done to make this work for Teensy is in the uavcan_teensy folder?
https://github.com/mjs513/libuavcan-.../uavcan_teensy

Hard to believe that it's 3 years later and I'm finally getting around to playing with DroneCAN / UAV CAN. That's how it goes sometimes I guess.

Thanks,
Brian

[mjs513]

Hi Mike,

I'm taking a look at this library; I'd like to update it to use DroneCAN, which is basically UAV CAN version 0:
https://github.com/dronecan/libuavcan

I'd also like to use FlexCAN_T4 and ensure that all files are licensed MIT. Am I correct in thinking that everything done to make this work for Teensy is in the uavcan_teensy folder?
https://github.com/mjs513/libuavcan-.../uavcan_teensy

Hard to believe that it's 3 years later and I'm finally getting around to playing with DroneCAN / UAV CAN. That's how it goes sometimes I guess.

Thanks,
Brian

Can't believe its been 3 years either since I worked on that one. Think someone also did a recent port of UAVCAN with Flexcan_t4 on plaformIO: https://forum.pjrc.com/threads/69213...ghlight=UAVCAN but haven't really looked at it

[brtaylor]

Can't believe its been 3 years either since I worked on that one. Think someone also did a recent port of UAVCAN with Flexcan_t4 on plaformIO: https://forum.pjrc.com/threads/69213...ghlight=UAVCAN but haven't really looked at it

Thanks, I saw that post, but didn't look too closely at it, since I was assuming that they were getting UAV CAN v1 ported and Drone CAN is more similar to UAV CAN v0. I think I'm just going to try to define similar wrapper functions as you and see if I can't tie together DroneCAN and FlexCAN_T4 into something usable.

[mjs513]

Thanks, I saw that post, but didn't look too closely at it, since I was assuming that they were getting UAV CAN v1 ported and Drone CAN is more similar to UAV CAN v0. I think I'm just going to try to define similar wrapper functions as you and see if I can't tie together DroneCAN and FlexCAN_T4 into something usable.

That probably the best approach. Be curious what you come up with. Never seem to be able to tie all these projects together - keep getting distracted

[brtaylor]

Still a work in progress, but moving forward with implementing DroneCAN on Teensy 3.x and 4.x:
https://github.com/bolderflight/dronecan

It requires our modified version of FlexCAN_T4 (for filter and mask on mailboxes):
https://github.com/bolderflight/flexcan

Also requires our circular buffer:
https://github.com/bolderflight/circle_buf

Publisher and subscriber is implemented and tested working on Teensy 3.x and 4.x (specifically used Teensy 3.6 and 4.1 for testing). The restart microservice is also implemented and buried in the library. Simple example publisher and subscriber are in the examples folder. Also was tested by publishing data to a PX4 running a beta version of firmware, super neat that the PX4 automatically detected the sensor and started using the data.

To do / known issues:
1. Documentation
2. This only works with CAN2.0, need to implement and test with CAN FD
3. Only tested using 1 CAN interface so far, need to test how multiple interfaces work, especially trying to use multiple interfaces for reliability
4. I don't love the way the library objects are instantiated, especially would like to avoid needing to use dynamic memory allocation (i.e. new)
5. Want to implement more microservices (i.e. time sync, etc)
6. Could probably do more error checking and handling
7. Want to figure out implementing custom messages
8. Want to explore cleaning up API

[mjs513]

Nice work, you made a lot of progress on DroneCAN. Did a bit of reading. Think they said DroneCAN is based off of UAVCAN v0 which is why its looks so familiar looking at the library.

In terms of implementing custom messages think I was playing with that when I was doing the UAVCan library. Had to use python to either updated existing messages or create a new one: Might check this out specific to DroneCAN which is like the one I used for UAVCan: https://github.com/dronecan/dronecan_dsdlc

You know you are going to make me want to pull out my CAN setup again.