USNA-UMBC-Project Receiver - Add noise to CAN-bus received data and Implement Kalman Filter
Dependencies: ServoOut mcp2515 BNO055
Diff: Nucleo_L432KC_CAN3_BNO_Servo_EW485A.cpp
- Revision:
- 1:5794ff4efa9a
- Parent:
- 0:ee3eb98ec375
--- a/Nucleo_L432KC_CAN3_BNO_Servo_EW485A.cpp Thu Jul 22 23:05:18 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,113 +0,0 @@ -/* mbed Microcontroller Library - * Copyright (c) 2019 ARM Limited - * SPDX-License-Identifier: Apache-2.0 - */ -// Program to test the CAN bus using SPI on the L432KC Nucleo board -// to an MCP2551 CAN transceiver bus IC using https://os.mbed.com/users/tecnosys/code/mcp2515/ - -// J. Bradshaw 20210512 -#include "mbed.h" -#include "platform/mbed_thread.h" -#include "CAN3.h" -#include "BNO055.h" -#include "ServoOut.h" - -#define THIS_CAN_ID 0x05 //Address of this CAN device -#define DEST_CAN_ID 0 //Address of destination - -Serial pc(USBTX, USBRX); //pc serial (tx, rx) uses USB PA_9 and PA_10 on Nucleo D1 and D0 pins -BNO055 bno(D4, D5); -SPI spi(D11, D12, D13); // mosi, miso, sclk -CAN3 can3(spi, D10, D2); // spi bus, CS for MCP2515 controller -ServoOut servoOut1(PA_0); //A0); // PA_0 is the servo output pulse -AnalogIn ain3(A3); - -unsigned char can_txBufLen = 0; -unsigned char can_tx_buf[8] = {0, 0, 0, 0, 0, 0, 0, 0}; -CANMessage canTx_msg; - -unsigned char can_rx_bufLen = 8; -unsigned char can_rx_buf[8]; -unsigned short can_rxId = 0; -CANMessage canRx_msg; - -Timer t; - -void bno_init(void){ - if(bno.check()){ - pc.printf("BNO055 connected\r\n"); - bno.setmode(OPERATION_MODE_CONFIG); - bno.SetExternalCrystal(1); - //bno.set_orientation(1); - bno.setmode(OPERATION_MODE_NDOF); //Uses magnetometer - //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF); //no magnetometer - bno.set_angle_units(RADIANS); - } - else{ - pc.printf("BNO055 NOT connected\r\n Program Trap."); - while(1); - } -} - -int main() { - thread_sleep_for(500); - - t.start(); - pc.baud(115200); - bno_init(); - //can3.reset(); // reset the can bus interface - can3.frequency(500000); // set up for 500K baudrate - - servoOut1.pulse_us = 1500; - - pc.printf("CAN MCP2515 test: %s\r\n", __FILE__); - while(1) { - - servoOut1.pulse_us = 1000 + (1000.0*ain3); - - bno.get_angles(); - - pc.printf("%.2f %.2f %.2f\r\n",bno.euler.roll, bno.euler.pitch, bno.euler.yaw); - //sprintf(can_rx_buf, "b%.2f\r\n", bno.euler.yaw); //format output message string - can_tx_buf[0] = bno.euler.rawroll & 0x00ff; - can_tx_buf[1] = (bno.euler.rawroll >> 8) & 0x00ff; - can_tx_buf[2] = bno.euler.rawpitch & 0x00ff; - can_tx_buf[3] = (bno.euler.rawpitch >> 8) & 0x00ff; - can_tx_buf[4] = bno.euler.rawyaw & 0x00ff; - can_tx_buf[5] = (bno.euler.rawyaw >> 8) & 0x00ff; - can_tx_buf[6] = 0; - can_tx_buf[7] = 0; - - // CAN write message - for(int i=0;i<8;i++){ - canTx_msg.data[i] = can_tx_buf[i]; - } - canTx_msg.id = THIS_CAN_ID; //(rand() % 0xff); // Randomize transmit ID or THIS_CAN_ID; - - can3.write(&canTx_msg); - pc.printf("%.2f CAN TX id=%02X data: ", t.read(), canTx_msg.id); - for(int i=0;i<8;i++){ - pc.printf(" %2X", can_tx_buf[i]); - } - pc.printf("\r\n"); - //set up a random delay time of up to .79 seconds - //delayT = ((rand() % 7) * 31.0) + ((rand() % 9) * .1) + runTime.sec_total; - - // CAN receive message - if(can3.read(&canRx_msg) == CAN_OK){ //if message is available, read into msg - pc.printf("CAN RX id=0x%02X data: ", canRx_msg.id); - for (int i = 0; i < canRx_msg.len; i++) { - pc.printf(" %2X", canRx_msg.data[i]); - } - pc.printf("\r\n"); - } - - //if(canTxErrors > 50){ -// can.reset(); -// } -// if(canRxErrors > 50){ -// can.reset(); -// } - thread_sleep_for(50); - }//while(1) -}//main