CAN bus write

Status
Not open for further replies.

Fox95

Active member
Is there a very basic example of a CAN bus write program somewhere for the teensy 3.6

I found this on the sparkfun examples and i have made it work with the sparkfun can bus shield and uno.

but i need to move over to the teensy 3.6 and run the same code on it and i am not sure what all i need to change to make it work correctly.

and i like to run at 1M not 500 kbps, I found that the sparkfun can bus shield has no library to allow it to run at 1M on 500 kbps.

thanks!







/****************************************************************************
CAN Write Demo for the SparkFun CAN Bus Shield.

Written by Stephen McCoy.
Original tutorial available here: http://www.instructables.com/id/CAN-Bus-Sniffing-and-Broadcasting-with-Arduino
Used with permission 2016. License CC By SA.

Distributed as-is; no warranty is given.
*************************************************************************/

#include <Canbus.h>
#include <defaults.h>
#include <global.h>
#include <mcp2515.h>
#include <mcp2515_defs.h>

//********************************Setup Loop*********************************//

void setup() {
Serial.begin(9600);
Serial.println("CAN Write - Testing transmission of CAN Bus messages");
delay(1000);

if(Canbus.init(CANSPEED_500)) //Initialise MCP2515 CAN controller at the specified speed
Serial.println("CAN Init ok");
else
Serial.println("Can't init CAN");

delay(1000);
}

//********************************Main Loop*********************************//

void loop()
{
tCAN message;

message.id = 0x640; //formatted in HEX
message.header.rtr = 0;
message.header.length = 8; //formatted in DEC
message.data[0] = 0x3e;
message.data[1] = 0xfe;
message.data[2] = 0x7a;
message.data[3] = 0x00; //formatted in HEX
message.data[4] = 0x37;
message.data[5] = 0x00;
message.data[6] = 0x00;
message.data[7] = 0x00;

mcp2515_bit_modify(CANCTRL, (1<<REQOP2)|(1<<REQOP1)|(1<<REQOP0), 0);
mcp2515_send_message(&message);

delay(1000);
}
 
maybe if i ask like this, I'd like to be able to send messages in hex like the code on the right. does anyone know the line command to be able to do this with flexcan? the code on the left is from flexcan example, the code on the right is the mcp2515 example. the code on the left i am not sure what the data value is its not hex though

cantest.JPG
 
Not sure which library you want to use with the T3.6. If you tell us maybe we can help better. I am not really familiar with the library used in the instructable you referenced

If you use IFCT think you will be better off which pretty much looks like the code on the left.
 
Not sure which library you want to use with the T3.6. If you tell us maybe we can help better. I am not really familiar with the library used in the instructable you referenced

If you use IFCT think you will be better off which pretty much looks like the code on the left.


I have ifct downloaded. Can you describe how to transmit the values in the right side of the pic using ifct?
 
Actually was just reviewing @tonton81's FlexCan_t4 library and looks like that's a replacement for IFCT. But if you want to give IFCT a try:

Code:
#include <IFCT.h>

void setup() {
  pinMode(2, OUTPUT); // for the transceiver enable pin, use the pin for your transceiver
  Can0.setBaudRate(1000000);
  Can0.enableFIFO();
}

void loop() {
  CAN_message_t msg;
  if ( Can0.read(msg) ) canSniff(msg);

  msg.flags.extended = 0;
  msg.flags.remote = 0;
  msg.len = 8;
  msg.id = 0x351;   //your other example had 0x640
  msg.buf[0] = random(0, 255);
  msg.buf[1] = random(0, 255);
  msg.buf[2] = random(0, 255);
  msg.buf[3] = random(0, 255);
  msg.buf[4] = random(0, 255);
  msg.buf[5] = random(0, 255);
  msg.buf[6] = random(0, 255);
  msg.buf[7] = random(0, 255);
  CAN.write(msg);


}

void canSniff(const CAN_message_t &msg) {
  Serial.print("MB "); Serial.print(msg.mb);
  Serial.print("  LEN: "); Serial.print(msg.len);
  Serial.print(" EXT: "); Serial.print(msg.flags.extended);
  Serial.print(" REMOTE: "); Serial.print(msg.rtr);
  Serial.print(" TS: "); Serial.print(msg.timestamp);
  Serial.print(" ID: "); Serial.print(msg.id, HEX);
  Serial.print(" Buffer: ");
  for ( uint8_t i = 0; i < msg.len; i++ ) {
    Serial.print(msg.buf[i], HEX); Serial.print(" ");
  } Serial.println();
}

Its been quite a while since i played with FlexCan. Will have to redo the setup with a T3.x and give it a try :)
 
Just looking at FlexCAN_t4 and looks like this might work for you as well - haven't tested it yet so no gurantees :)
Code:
// -------------------------------------------------------------
// CANtest for Teensy 4.0 using CAN2 and CAN2 bus
#include <FlexCAN_T4.h>

#define debug(msg) Serial.print("["); Serial.print(__FILE__); Serial.print("::"); Serial.print(__LINE__);  Serial.print("::"); Serial.print(msg); Serial.println("]");
void debug_pause(void)
{
  Serial.print("Paused...");
  while (!Serial.available());
  while (Serial.available()) Serial.read();
  Serial.println("Restarted.");
}

FlexCAN_T4<CAN0, RX_SIZE_256, TX_SIZE_16> Can0;


static CAN_message_t msg;
static uint8_t hex[17] = "0123456789abcdef";

// -------------------------------------------------------------
static void hexDump(uint8_t dumpLen, uint8_t *bytePtr)
{
  uint8_t working;
  while( dumpLen-- ) {
    working = *bytePtr++;
    Serial.write( hex[ working>>4 ] );
    Serial.write( hex[ working&15 ] );
  }
  Serial.write('\r');
  Serial.write('\n');
}


// -------------------------------------------------------------
void setup(void)
{
  Serial.begin(115200);
  int iSerialTimeout = 1000000;
  delay(100);
  while (!Serial && (iSerialTimeout-- != 0));  
  debug (F("start setup"));

  Can0.begin();  
  Can0.setBaudRate(1000000);   // I like to set the baud rates just to be on the safe side

  // t4 missing msg.ext = 0;
  msg.id = 0x100;  //pick one
  msg.len = 8;
  msg.flags.extended = 0;
  msg.flags.remote   = 0;
  msg.flags.overrun  = 0;
  msg.flags.reserved = 0;
  msg.buf[0] = 10;
  msg.buf[1] = 20;
  msg.buf[2] = 0;
  msg.buf[3] = 100;
  msg.buf[4] = 128;
  msg.buf[5] = 64;
  msg.buf[6] = 32;
  msg.buf[7] = 16;
  debug (F("setup complete"));
  //debug_pause();
}


// -------------------------------------------------------------
void loop(void)
{
  msg.buf[0]++;       //since you are in a loop i just incremented each time in stead of doing it 5 times
  Can0.write(msg);    //you could do it your way as well
  //Serial.println("cantest - Repeat: Read bus0, Write bus1");
  CAN_message_t inMsg;
  if (Can0.read(inMsg)!=0)
  {
    Serial.print("W RD bus 0: "); hexDump(8, inMsg.buf);
  }
  delay(20);
}
 
values.JPG

the values circled in the pic taken from your examples are not hex. I do not know what they are or where they come from.

so can you tell me how to modify that code to simply transmit hex values?

the hex values i need to transmit are:

[0] = 0x38;
[1] = 0xfe;
[2] = 0x7a;
[3] = 0x00;
[4] = 0x37;
[5] = 0x00;
[6] = 0x00;
[7] = 0x00;

on address 0x640

that's all i need to do, nothing else, just transmit those values.
 
Ok obvious answer just change the message id to what you want and message to the hex values that you want instead of ints, thats it. Here you go:
Code:
#include <IFCT.h>

void setup() {
  pinMode(2, OUTPUT); // for the transceiver enable pin, use the pin for your transceiver
  Can0.setBaudRate(1000000);
  Can0.enableFIFO();
}

void loop() {
  CAN_message_t msg;
  if ( Can0.read(msg) ) canSniff(msg);

  msg.flags.extended = 0;
  msg.flags.remote = 0;
  msg.len = 8;
[COLOR="#FF0000"]  msg.id = 0x640;   //changed ID to 0x640
[/COLOR]  msg.buf[0] = random(0, 255);
  msg.buf[1] = 0xfe;
  msg.buf[2] = 0x7a;
  msg.buf[3] = 0x00;
  msg.buf[4] = 0x37;
  msg.buf[5] = 0x00;
  msg.buf[6] = 0x00;
  msg.buf[7] = 0x00;
  CAN.write(msg);


}

void canSniff(const CAN_message_t &msg) {
  Serial.print("MB "); Serial.print(msg.mb);
  Serial.print("  LEN: "); Serial.print(msg.len);
  Serial.print(" EXT: "); Serial.print(msg.flags.extended);
  Serial.print(" REMOTE: "); Serial.print(msg.rtr);
  Serial.print(" TS: "); Serial.print(msg.timestamp);
  Serial.print(" ID: "); Serial.print(msg.id, HEX);
  Serial.print(" Buffer: ");
  for ( uint8_t i = 0; i < msg.len; i++ ) {
    Serial.print(msg.buf[i], HEX); Serial.print(" ");
  } Serial.println();
}
I left the cansniff their in case you wanted to try receiving messages. otherwise you can delete it.
 
Ok obvious answer just change the message id to what you want and message to the hex values that you want instead of ints, thats it. Here you go:
Code:
#include <IFCT.h>

void setup() {
  pinMode(2, OUTPUT); // for the transceiver enable pin, use the pin for your transceiver
  Can0.setBaudRate(1000000);
  Can0.enableFIFO();
}

void loop() {
  CAN_message_t msg;
  if ( Can0.read(msg) ) canSniff(msg);

  msg.flags.extended = 0;
  msg.flags.remote = 0;
  msg.len = 8;
[COLOR="#FF0000"]  msg.id = 0x640;   //changed ID to 0x640
[/COLOR]  msg.buf[0] = random(0, 255);
  msg.buf[1] = 0xfe;
  msg.buf[2] = 0x7a;
  msg.buf[3] = 0x00;
  msg.buf[4] = 0x37;
  msg.buf[5] = 0x00;
  msg.buf[6] = 0x00;
  msg.buf[7] = 0x00;
  CAN.write(msg);


}

void canSniff(const CAN_message_t &msg) {
  Serial.print("MB "); Serial.print(msg.mb);
  Serial.print("  LEN: "); Serial.print(msg.len);
  Serial.print(" EXT: "); Serial.print(msg.flags.extended);
  Serial.print(" REMOTE: "); Serial.print(msg.rtr);
  Serial.print(" TS: "); Serial.print(msg.timestamp);
  Serial.print(" ID: "); Serial.print(msg.id, HEX);
  Serial.print(" Buffer: ");
  for ( uint8_t i = 0; i < msg.len; i++ ) {
    Serial.print(msg.buf[i], HEX); Serial.print(" ");
  } Serial.println();
}
I left the cansniff their in case you wanted to try receiving messages. otherwise you can delete it.

it errors when compiling:

the line: CAN.write (msg); is the error line and says 'CAN' was not declared in this scope
 
it errors when compiling:

the line: CAN.write (msg); is the error line and says 'CAN' was not declared in this scope

i figured it out it needs to be this:

Can0.write(msg);

and now it works correctly! thank you for the help this is just what i needed, perfect!
 
i figured it out it needs to be this:

Can0.write(msg);

and now it works correctly! thank you for the help this is just what i needed, perfect!

Good glad it worked. Thats was my fault - thats what happens when you do copy and pastes.
 
Good glad it worked. Thats was my fault - thats what happens when you do copy and pastes.

i need to ask one more question, i need to apply the above can transmit to a code file that runs a IMU but i am unsure where to put it in the file, i pasted it into the main loop but its not sending out any CAN data. could you look at this and tell me if its in the correct spot to transmit?




Code:
/*************************************
                INCLUDES
 *************************************/
#include <Wire.h> 
#include <IFCT.h>

/*************************************
                DEFINITIONS
 *************************************/
#define nrf52
//#define Simblee       
//#define Nano

#define Led                       13              // Led  (Arduino nano = 13) to measure loop duration (10) nrf52 = 17
#define btn_TARE                  A1              // input pin for button TARE
#define btn_CAL                   6               // input pin for button TARE
int plot_interval                 = 1000;         // plot interval in ms
unsigned long plot_timer          = 0;

#define BNO_ADDRESS               0x4A            // Device address when SA0 Pin 17 = GND; 0x4B SA0 Pin 17 = VDD

uint8_t cargo[25]; 

float Q0,Q1,Q2,Q3,H_est;                                     
uint8_t stat_;                                     // Status (0-3)
float yaw,pitch,roll;

const uint8_t quat_report        = 0x05;          // defines kind of rotation vector (0x05), geomagnetic (0x09), AR/VR (0x28)
                                                  // without magnetometer : game rotation vector (0x08), AR/VR Game (0x29)
const int reporting_frequency    = 400;           // reporting frequency in Hz  // note that serial output strongly reduces data rate

uint32_t rate              = 1000000 / reporting_frequency; 
uint8_t B0_rate            = rate & 0xFF;                       //calculates LSB (byte 0)
uint8_t B1_rate            = (rate >> 8) & 0xFF; 
uint8_t B2_rate            = (rate >> 16) & 0xFF; 
uint8_t B3_rate            = rate >> 24;                        //calculates MSB (byte 3) 

/******* Conversions *************/

#define QP(n)                       (1.0f / (1 << n))                   // 1 << n ==  2^-n
#define radtodeg                    (180.0f / PI)



/*************************************
                 SETUP
*************************************/

void setup() {

// pin definitions
  pinMode(Led, OUTPUT);
  digitalWrite(Led, HIGH);               // initial state of LED = on
  pinMode(btn_TARE, INPUT_PULLUP);
  pinMode(btn_CAL, INPUT_PULLUP); 

  //pinMode(2, OUTPUT); // for the transceiver enable pin, use the pin for your transceiver
  Can0.setBaudRate(1000000);
  Can0.enableFIFO();


// communication 
  Serial.begin(115200);                  // 115200 baud
  
  #ifdef Simblee 
  Wire.speed = 400;  // select 400 kbps (100, 250)   // 
  Wire.beginOnPins(5,6);                 // (SCLpin,SDApin)
  #endif
 
  #ifdef nrf52
  Wire.begin();                          // no need to define pins. they are declared in variants.h
  Wire.setClock(400000L);
  #endif
 
  #ifdef Nano
  Wire.begin();                          // start I2C communication     
  Wire.setClock(400000L);                // set I2C to 400kHz
  #endif
  
  Serial.println ("*********************************************************************");
  Serial.println (" Rock bottom code for  BNO080 on Atmega 328p; Cortex M0 (Simblee) and M4 (nrf52)  V1.1 2018-12-30");
  Serial.println ("*********************************************************************");

// BNO settings
  Wire.beginTransmission(BNO_ADDRESS);
  while (Wire.endTransmission() != 0);         //wait until device is responding (32 kHz XTO running)
  Serial.println("BNO found");

 /*
  for (byte i = 8; i < 120; i++)              //I2C Scanner for debug purpose
  {
    Wire.beginTransmission (i);
    if (Wire.endTransmission () == 0)
      {
      Serial.print ("Found address: ");
      Serial.print (i, DEC);
      Serial.print (" (0x");
      Serial.print (i, HEX);
      Serial.println (")");
      } 
  } 
  */
 
  // On system startup, the SHTP control application sends its full advertisement response, unsolicited, to the host (chapter 5.1ff SHTP v1.7) 
  // remark: commands are only accepted after that  alternatively add a delay(200);
 
  Serial.print("SHTP advertising: ");
  cargo[0] = 1;                                       // start condition
  while (cargo[0] != 0)                               // repeat if length is still > 0
  {   
    Wire.requestFrom(BNO_ADDRESS,32);                 
    int i=0; 
      while (Wire.available())
      {
      cargo[i] = Wire.read();
      i++;
      } 
      for(int j = 4 ; j <33; j++)  // skip first 4 bytes 
      {
      Serial.print (cargo[j],HEX); 
      Serial.print(",");
      }   
   } 
   Serial.println(); 
 
 
  get_Product_ID();                      
  set_feature_cmd_QUAT();                // set the required feature report data rate  are generated  at preset report interval
  delay(200);
  save_periodic_DCD();                   // saves DCD every 5 minutes ( only if cal = 3)
 
 
  ME_cal(1,1,1,0);                       // default after reset is (1,0,1,0); switch autocal on @ booting (otherwise gyro is not on)

}


  /*************************************
                     LOOP
   *************************************/
void loop() {

 get_QUAT();                           // get actual QUAT data (if new are available)

        
  
 if (millis() - plot_timer > plot_interval){ 
        plot_timer = millis();
        Serial.print ("S "); Serial.print (stat_);
        Serial.print ("; E "); Serial.print (H_est + 0.05f,1);                   // including rounding
        Serial.print ("; q0 "); Serial.print (Q0 + 0.00005f,4);                  // = qw (more digits to find out north direction (y axis N --> q0 = 1)
        Serial.print ("; q1 "); Serial.print (Q1 + 0.0005f,3);
        Serial.print ("; q2 "); Serial.print (Q2 + 0.0005f,3);
        Serial.print ("; q3 "); Serial.println (Q3 + 0.0005f,3);
        Serial.print ("ROLL = "); Serial.println (roll,1);
        Serial.print ("PITCH = "); Serial.println (pitch,1);
        Serial.print ("YAW = "); Serial.println (yaw,1);
        Serial.print ("STAT = "); Serial.println (stat_);
        Serial.println();
        
        
  

 }
 
  //if (stat_ == 3)  digitalWrite(Led,HIGH);                              // indicate cal status 3
  if (stat_ == 3)  digitalWrite(Led, !digitalRead(Led));                  // blink Led every loop run (--> to measure loop duration);             
  else digitalWrite(Led, LOW);                                            // status Led 


// *************  buttons *******************************
  
  if(digitalRead(btn_TARE) == LOW ){                        // button pressed  stores actual phi as mean value and saves actual calibration to flash
    delay(200);
    while(digitalRead(btn_TARE) == LOW);                    // wait for button release
    //actions follow  here
    TARE();                                                 
   }    


   if(digitalRead(btn_CAL) == LOW){
     delay(200);
     while(digitalRead(btn_CAL) == LOW );            // wait for button release
    
     //actions follow here
     save_DCD();                                     // store cal in flash
     delay(200);
    // ME_cal(0,0,1,0);                              //autocal acc + gyro stop; magnetometer  cal continues
    }
 //*******************************************************

}                                                    // loop ends here

/*************************************
             Subroutines
/*************************************/

/*******************************************************************************************************************
/* This code reads quaternions 
 * kind of reporting (quat_report) is defined above
 */


void get_QUAT(){ 
 int16_t q0,q1,q2,q3,h_est;                        // quaternions q0 = qw 1 = i; 2 = j; 3 = k;  h_est = heading accurracy estimation
 float a,b,c,d,norm;
 
 if (quat_report == 0x08 || quat_report == 0x29){
    Wire.requestFrom(BNO_ADDRESS,21);
    int i=0; 
    while (Wire.available()){
      cargo[i] = Wire.read();
      i++;
    }
  }
   
  else{ 
    Wire.requestFrom(BNO_ADDRESS,23);
    int i=0; 
      while (Wire.available()){
      cargo[i] = Wire.read();
      i++;
      }
  }
      if(cargo[9] == quat_report)                                             // check for report 
         
        {
        q1 = (((int16_t)cargo[14] << 8) | cargo[13] ); 
        q2 = (((int16_t)cargo[16] << 8) | cargo[15] );
        q3 = (((int16_t)cargo[18] << 8) | cargo[17] );
        q0 = (((int16_t)cargo[20] << 8) | cargo[19] ); 

     // patch for  rarely occurring wrong data from BNO of unknown reasons. --> Check if q(0,1,2,3) is not a unity vector
        a = q0 * QP(14); b = q1 * QP(14); c = q2 * QP(14); d = q3 * QP(14);    // apply Q point (quats are already unity vector)
        norm = sqrtf(a * a + b * b + c * c + d * d);
        if(abs(norm - 1.0f) > 0.0015) return;                                   // skip faulty quats
        
        Q0 = a; Q1 = b; Q2 = c; Q3 = d;                                        // here orientation may be changed b=x; c=y, d = z
       stat_ = cargo[11] & 0x03;                                               // bits 1:0 contain the status (0,1,2,3)  
    
       if (quat_report == 0x05 || quat_report == 0x09 || quat_report == 0x28 ){  // heading accurracy only in some reports available
        h_est = (((int16_t)cargo[22] << 8) | cargo[21] );                        // heading accurracy estimation  
        H_est = h_est * QP(12);                                                  // apply Q point this is 12  not 14
        H_est *= radtodeg;                                                       // convert to degrees                
       }

         // calculate euler angles                
        yaw   =  atan2f(Q1 * Q2 + Q0 * Q3, Q0 * Q0 + Q1 * Q1 - 0.5f);
        float argument = 2.0f * (Q1 * Q3 - Q0 * Q2);                               // may be > 1 due to rounding errors --> undefined
        if(abs(argument) > 1.0f) argument = 1.0f; 
        pitch =  asinf(argument); 
        roll  =  atan2f(Q0 * Q1 + Q2 * Q3, Q0 * Q0 + Q3 * Q3 - 0.5f);
  
        yaw += PI * 0.5f; // correction of the y axis direction
        if(yaw < 0) yaw += 2.0f * PI;
        if(yaw > 2.0f *PI) yaw -= 2.0f * PI;  
  
        yaw   *= radtodeg;
        yaw    = 360.0f - yaw;
        pitch *= radtodeg;
        roll  *= radtodeg;

        CAN_message_t msg;
        msg.flags.extended = 0;
        msg.flags.remote = 0;
        msg.len = 8;
        msg.id = 0x640;   //changed ID to 0x640
        msg.buf[0] = 0x3e;
        msg.buf[1] = 0xfe;
        msg.buf[2] = 0x7a;
        msg.buf[3] = 0x00;
        msg.buf[4] = 0x37;
        msg.buf[5] = 0x00;
        msg.buf[6] = 0x00;
        msg.buf[7] = 0x00;
        Can0.write(msg);
      }

        
      
}

//************************************************************************
//                COMMANDS
//************************************************************************

       





// This code activates quaternion output  at defined rate

void set_feature_cmd_QUAT(){                                 // quat_report determines the kind of quaternions (see data sheets)
  uint8_t quat_setup[21] = {21,0,2,0,0xFD,quat_report,0,0,0,B0_rate,B1_rate,B2_rate,B3_rate,0,0,0,0,0,0,0,0};  
   Wire.beginTransmission(BNO_ADDRESS);   
   Wire.write(quat_setup, sizeof(quat_setup));            
   Wire.endTransmission();
}

//***********************************************************************************************************************************************
/* This code tares  and stores results in flash after correct positioning and calibration (follow Tare procedure Hillcrest BNO080 Tare function Usage Guide 1000-4045)
* Make sure to run  rotation vector (0x05) and watch h_est and stat (should be << 10 deg and 3 respectively)
* NOTE: tare and calibration are different things
* NOTE: to undo tare persist requires calibration of the device followed by another tare persist with the device oriented y = north bfz = vertical before. 
*/

void TARE(){
  uint8_t RV;               // rotation vector used to tare defined in quat_report
  if(quat_report == 0x05) RV = 0x00;
  if(quat_report == 0x08) RV = 0x01; 
  if(quat_report == 0x09) RV = 0x02; 
  uint8_t tare_now[16] = {16,0,2,0,0xF2,0,0x03,0,0x07,RV,0,0,0,0,0,0};                //0x07 means all axes 0x04 = Z axis only; based on rotation vector
  uint8_t tare_persist[16] = {16,0,2,0,0xF2,0,0x03,0x01,RV,0,0,0,0,0,0,0};
  Wire.beginTransmission(BNO_ADDRESS);
  Wire.write(tare_now, sizeof(tare_now));
  //Wire.write(tare_persist, sizeof(tare_persist));                                  // uncomment  for tare persist;
  Wire.endTransmission();              
 
 }

//***********************************************************************************************************************************************
/* The calibration data in RAM is always updated in background. 'Save DCD'  push the RAM record into the FLASH for reload @ next boot.
   The algorithm will only update the calibration data when it feels the current accuracy is good. You don't need to care about the status or heading error.
   Save  before power off or whenever you would like to do. (Hillcrest information)  
 */

void save_DCD(){                                             
  uint8_t save_dcd[16] = {16,0,2,0,0xF2,0,0x06,0,0,0,0,0,0,0,0,0};
  Wire.beginTransmission(BNO_ADDRESS);  
  Wire.write(save_dcd, sizeof(save_dcd));
  Wire.endTransmission();    
}

//***********************************************************************************************************************************************
/* This code disables the calibration running in the background of the accelerometer gyro and magnetometer.  
 * sensors can be set individually on and off (chapter 6.4.7.1)
 * P0 = accelerometer, P1 = gyro; P2 =magnetometer; P4 = planar accelerometer (without Z axis)
 * 0 = disable; 1 = enable
 */

void ME_cal(uint8_t P0, uint8_t P1, uint8_t P2, uint8_t P4){
  uint8_t me_cal[16] = {16,0,2,0,0xF2,0,0x07,P0,P1,P2,0,P4,0,0,0,0};
  Wire.beginTransmission(BNO_ADDRESS);              
  Wire.write(me_cal, sizeof(me_cal));
  Wire.endTransmission();  
}

//***********************************************************************************************************************************************
// Saves dynamic cal data periodically every 5 minutes (default)  

void save_periodic_DCD(){                                             
  uint8_t periodic_dcd[16] = {16,0,2,0,0xF2,0,0x09,0,0,0,0,0,0,0,0,0};
  Wire.beginTransmission(BNO_ADDRESS);  
  Wire.write(periodic_dcd, sizeof(periodic_dcd));
  Wire.endTransmission();    
}

//***********************************************************************************************************************************************
/* This code reads  the product ID
 */
void get_Product_ID(){
  uint8_t get_PID[6] = {6,0,2,0,0xF9,0};
  //uint8_t get_PID[6] = {6,0,2,0,0xF9,0};
  Wire.beginTransmission(BNO_ADDRESS);                         // requesting product ID (chapter 6.3.1)
  Wire.write(get_PID, sizeof(get_PID));
  Wire.endTransmission();  
  delay(200);                                                  // wait until response is available
  

   while (cargo[4] != 0xF8)                            // wait for command response
  {   
    Wire.requestFrom(BNO_ADDRESS,25);                 // fetching response(chapter 6.4.7.3)
    int i=0; 
      while (Wire.available())
      {
      cargo[i] = Wire.read();
      i++;
      }
   }  
        Serial.print ("Product ID response: ");
        Serial.print ("Reset cause: ");
        Serial.print (cargo[5],HEX); 
        Serial.print (" SW Major: ");
        Serial.print (cargo[6],HEX); 
        Serial.print (" SW Minor: "); 
        Serial.println (cargo[7],HEX);
   
       
       
}
//***********************************************************************************************************************************************

/* Utilities (Quaternion mathematics)
 
  q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3  = 1
   
 // Gravity vector from quaternion
  
  gx = q1 * q3 - q0 * q2;
  gy = q0 * q1 + q2 * q3;
  gz = q0 * q0 + q3 * q3 -0.5f; 
  norm = sqrtf(gx * gx + gy * gy + gz * gz);                                                              
  norm = 1.0f / norm;
  gx *= norm; gy *= norm; gz *= norm;
 
 
 # Rotation matrix (half)
  (R00 R10 R20)
 =(R01 R11 R21)
  (R02 R12 R22)
  
 R00 = q0 * q0 + q1 * q1 -0.5f; // = 0.5f - q2 * q2 - q3 * q3;
 R01 = q1 * q2 - q0 * q3;
 R02 = q1 * q3 + q0 * q2;
 R10 = q1 * q2 + q0 * q3;
 R11 = q0 * q0 + q2 * q2 -0.5; // = 0.5f - q1 * q1 - q3 * q3;
 R12 = q2 * q3 - q0 * q1;
 R20 = q1 * q3 - q0 * q2;
 R21 = q2 * q3 + q0 * q1;
 R22 = q0 * q0 + q3 * q3 -0.5f; // = 0.5f -q1 * q1 -q2 * q2;
 
 To rotate a vector V = (vx,vx,vz) in quaternion frame
 
 W = (wx,wy,wz) 
 wx = 2.0f * ( R00 * vx + R10 * vy + R20 * vz);
 wy = 2.0f * ( R01 * vx + R11 * vy + R21 * vz):
 wz = 2.0f * ( R02 * vx + R12 * vy + R22 * vz):
 For example gravity vector  g = (0,0,1)  --> G = (gx,gy,gz) = (R20, R21, R22)
 # Euler angles from rotation matrix:
 
 yaw   =  atan2f(R10 , R00);                    // heading
 pitch =  -  asinf(R20); =  -  asinf(gx);
 roll  =  atan2f(R21 , R22); = atan2f(gy , gz);
 
  
 # Euler angles from quaternions
 yaw   =  atan2f(Q1 * Q2 + Q0 * Q3, Q0 * Q0 + Q1 * Q1 - 0.5f);   // heading
 pitch =  -  asinf(2.0f * (Q1 * Q3 - Q0 * Q2));
 roll  =  atan2f(Q0 * Q1 + Q2 * Q3, Q0 * Q0 + Q3 * Q3 - 0.5f);
 
  
 yaw += PI * 0.5f;                                               // correction of the y axis direction if needed
 if(yaw < 0) yaw += 2.0f * PI;
 if(yaw > 2.0f *PI) yaw -= 2.0f * PI;  
  
 yaw   *= radtodeg;
 yaw    = 360.0f - yaw;                         // correction of the y axis direction if needed; apply location offset also here
 pitch *= radtodeg;
 roll  *= radtodeg;
 
//***********************************************************************************
TARE (to make qt = 1,0,0,0) from q = (q0,q1,q2,q3)
//store  konjugate komplex quat  (*-1 of i, j, k) at the moment of TARE
    t0 = q0;                                                                        
    t1 = -q1;
    t2 = -q2;
    t3 = -q3;
  // qt are then the tared coefficients
  qt0 = q0 * t0 - q1 * t1 - q2 * t2 - q3 * t3;      // multiplication matrix
  qt1 = q0 * t1 + q1 * t0 + q2 * t3 - q3 * t2;
  qt2 = q0 * t2 - q1 * t3 + q2 * t0 + q3 * t1;
  qt3 = q0 * t3 + q1 * t2 - q2 * t1 + q3 * t0;
alternative:
//store quats at the moment of TARE
    t0 = q0;                                                                        
    t1 = q1;
    t2 = q2;
    t3 = q3;
  // qt are the tared coefficients
  qt0 =  q0 * t0 + q1 * t1 + q2 * t2 + q3 * t3;    // konjugated multiplication matrix t1, t2, t3 negative sign
  qt1 = -q0 * t1 + q1 * t0 - q2 * t3 + q3 * t2; 
  qt2 = -q0 * t2 + q1 * t3 + q2 * t0 - q3 * t1;
  qt3 = -q0 * t3 - q1 * t2 + q2 * t1 + q3 * t0;
 // Quaternion Qt(1,0,0,0) leads to gravity G = (0, 0, 1) (see formula for gravity)
 // Hence the angle of the sensor  to gravity after tare (any orientation) is the dot product between the unity
 // vectors (0,0,1) and (gx,gy,gz) which is 
 
  PHI = acos(gz);       //  angle of the cone
  PHI *= 180.0f / PI;
  
//*************************************************************************************
//Angle between quaternions
float dotproduct = Q0 * q0 + Q1 * q1 + Q2 * q2 + Q3 * q3;
float angle = radtodeg * 2.0f * acos(dotproduct);
//*************************************************************************************Jizhong XiaoJizhong Xiao
     //**************** dphi calculation************
      float argument = (QT0 * Q0 + QT1 * Q1 + QT2 * Q2 + QT3 * Q3);  // calculate delta angle to previous sample
      if (abs(argument) > 1) argument = 1.0f;                        // check for N/A
     
      if(asin(argument) > 0) dphi = 2.0f * acos(argument);             
      else dphi = 2.0f * (PI - acos(argument));                      // in case rotation is >360 deg  ( quat flip signs)
     
      dphi *= radtodeg;                                              // conversion to deg
      if(dphi < 1.4f) dphi = 0.0f;                                   // Noise cancelling (empirical value)
      //*********************************************
*/
 
Last edited by a moderator:
First is anything printing from get_Quant? Only thing that I see is that if this criteria is never met:
Code:
if(cargo[9] == quat_report)
nothing will be sent via Can bus, i.e.:
Code:
      if(cargo[9] == quat_report)   <====== probably not met
        {
			....
			....
			....
			
        CAN_message_t msg;
        msg.flags.extended = 0;
        msg.flags.remote = 0;
        msg.len = 8;
        msg.id = 0x640;   //changed ID to 0x640
        msg.buf[0] = 0x3e;
        msg.buf[1] = 0xfe;
        msg.buf[2] = 0x7a;
        msg.buf[3] = 0x00;
        msg.buf[4] = 0x37;
        msg.buf[5] = 0x00;
        msg.buf[6] = 0x00;
        msg.buf[7] = 0x00;
        Can0.write(msg);
      }
 
First is anything printing from get_Quant? Only thing that I see is that if this criteria is never met:
Code:
if(cargo[9] == quat_report)
nothing will be sent via Can bus, i.e.:
Code:
      if(cargo[9] == quat_report)   <====== probably not met
        {
			....
			....
			....
			
        CAN_message_t msg;
        msg.flags.extended = 0;
        msg.flags.remote = 0;
        msg.len = 8;
        msg.id = 0x640;   //changed ID to 0x640
        msg.buf[0] = 0x3e;
        msg.buf[1] = 0xfe;
        msg.buf[2] = 0x7a;
        msg.buf[3] = 0x00;
        msg.buf[4] = 0x37;
        msg.buf[5] = 0x00;
        msg.buf[6] = 0x00;
        msg.buf[7] = 0x00;
        Can0.write(msg);
      }

Capture.jpg

i moved the CAN transmit block of code up into the area right under where it prints the values for the imu and this does not work either

as you can see it prints the values of the imu out correctly, but CAN values are not being transmitted, please note that the CAN message is being sent to another device and it is not expected to be seen in the serial monitor. the can device is out of the picture and not getting the values
 
It should work. Two things:
  1. Do you have to enable your transceiver? You never did say what transceiver you are using
  2. Are still wired correctly with CAN bus?
 
It should work. Two things:
  1. Do you have to enable your transceiver? You never did say what transceiver you are using
  2. Are still wired correctly with CAN bus?


its a waveshare sn65hvd230

i didnt think they had enable pins?

but it worked fine in before when it was just runing that very simply code of transmit only, it transmitted properly
 
That particular one doesn't have an enable pin - some of the transceivers do.

I would move the can transmit to the main loop for a test. If it still doesn't work then it has to be the wiring.
 
i have a scope on pins 3, 4 on the teeensy board, this pic shows what they are putting out, does it look normal?pin 5 6 .jpg the waveshare board was working earlier
 
there's a bigger problem here. is there anyone here that works for teensy?

when i upload one program it will work fine, but when i upload another program its like it wont initiliaze the CAN pins on the teensy board.

id like to know what pins 3 4 should look like on the scope so i can determine if its a problem with the wave share CAN transceiver, i have two of them and tried them both and now both do not work. They WERE working yesterday and transmitting can just fine. so i can't believe that both failed at this point when all i tried to do was import some CAN code into the IMU code and upload it

is it possible there is a conflict between pins 3 4 CAN0 and pins 19 18 SCL SDA? when trying to use the board to do can and i2c at the same time?
 
Last edited:
Well I don't work for Teensy but i can tell you that using CAN0 on pins 3 and 4 I have had a IMU (MPU-9350) attached to I2C pins 18,19 and a GPS attached to Serial1 (pins 0,1) without any issues.

As for the Waveshare Transceivers I used them all the time with a T3.2, T3.5, T3.6 and on a T4.x for the CAN2.0 pins, again without any problem.

the only time you would run into an issue with the transceivers is when you have hooked them up wrong, thats why I asked you to check your wiring again as you had that problem before and if you changed setups..:
Code:
Transceiver    Teensy Pin
[B]3.3v ----------- 3.3v pin[/B]
RX ------------- pin 4 on the T3.6
TX ------------- pin 3 on the T3.6
GND ------------ GND

And you connect CANH to CANH and CANL to CANL.

As for your CAN2.0 waveform if you do a search on web you will find a couple of things:
https://www.youtube.com/watch?v=fwPG0mxqK9M

canbus_waveform_1.png
 
Well I don't work for Teensy but i can tell you that using CAN0 on pins 3 and 4 I have had a IMU (MPU-9350) attached to I2C pins 18,19 and a GPS attached to Serial1 (pins 0,1) without any issues.

As for the Waveshare Transceivers I used them all the time with a T3.2, T3.5, T3.6 and on a T4.x for the CAN2.0 pins, again without any problem.

the only time you would run into an issue with the transceivers is when you have hooked them up wrong, thats why I asked you to check your wiring again as you had that problem before and if you changed setups..:
Code:
Transceiver    Teensy Pin
[B]3.3v ----------- 3.3v pin[/B]
RX ------------- pin 4 on the T3.6
TX ------------- pin 3 on the T3.6
GND ------------ GND

And you connect CANH to CANH and CANL to CANL.

As for your CAN2.0 waveform if you do a search on web you will find a couple of things:
https://www.youtube.com/watch?v=fwPG0mxqK9M

View attachment 20631

the wiring is good, it was working yesterday sending CAN messages just fine.

the issue arose when i tried to merge the two sets of code where the imu was connected the same time as the wave share

I know what a CAN bus should look like. the PINS i showed on my scope pic are BEFORE the wave share. there is nothing obviously coming out of the waveshare at this point. I want to know what the signal looks like coming out of pins 3 4 before it goes through the waveshare to understand if its correct.

i understand I had wiring issues earlier and that was a board problem and it has been replaced, the wiring connections are good and they are conected properly as I said I was getting CAN messages BEFORE and it worked great.

the scope pattern pic on pins 3 4 BEFORE the wave share should tell whats going on if a proper signal is making out of the board, if it is then somehow TWO waveshare boards failed at the same time which is doubtful.



side note: i dont believe a square wave would be transmitted from pins 3 4 before the waveshare, the waveshare board should be what converts it to a square wave so it will look like your scope pic AFTER the waveshare board (but this is what i want to confirm with someone who's scoped the pins before....)
 
the pins won't communicate if flexcan went into error passive state, it practically sits dormant till another node on the bus starts talking. this happens when it's trying to transmit and not getting a response (single node, or transceiver issue)
 
Thanks for jumping in @tonton81. I did put a scope on the pins a long time ago but unfortunately don't have them anymore and dont have access to my scope for a few days.

EDIT: Just a couple of questions until you get your wave form answer:
1. Does the CAN example sketch still run and are you seeing data on the receiving node?
2. When you are doing your testing do you have both nodes powered up as @tonton81 mentioned - think the answer is yes to this one from your comments.
3. Add back in the code for receiving messages that you took out - cansniff stuff. Just out of curiosity.
 
Last edited:
as an update for anyone who may read this in the future, as unlikely as it sounds, i had two bad waveshare CAN boards. ordering new ones solved the above issues. thanks to everyone who responded here. i was out of town for the last week so this was off my plate for awhile.
 
Status
Not open for further replies.
Back
Top