Forum Rule: Always post complete source code & details to reproduce any issue!
Results 1 to 16 of 16

Thread: UAVcan for Teensy 3.x Proof of Concept

  1. #1
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    8,229

    UAVcan for Teensy 3.x Proof of Concept

    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

  2. #2
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    8,229
    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

  3. #3
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Santa Fe, NM
    Posts
    764
    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.

  4. #4
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    8,229
    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.

  5. #5
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    8,229
    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.

  6. #6
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    8,229
    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

  7. #7
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    8,229

    Using a GIMBAL with UAVcan - Intial Tests

    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.

    Code:
    #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.
    Attached Files Attached Files

  8. #8
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    8,229
    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.

  9. #9
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    8,229

    Using a GIMBAL with UAVcan - Live

    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
    Attached Thumbnails Attached Thumbnails Click image for larger version. 

Name:	20180902_093559.png 
Views:	131 
Size:	697.3 KB 
ID:	14638  

  10. #10
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    8,229
    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:

    Code:
    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

  11. #11
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Santa Fe, NM
    Posts
    764
    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

  12. #12
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    8,229
    Quote Originally Posted by brtaylor View Post
    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

  13. #13
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Santa Fe, NM
    Posts
    764
    Quote Originally Posted by mjs513 View Post
    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.

  14. #14
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    8,229
    Quote Originally Posted by brtaylor View Post
    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

  15. #15
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Santa Fe, NM
    Posts
    764
    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

  16. #16
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    8,229
    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.

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •