verified measurements from spi read for imu

Converter/Inc/canhalal.h

@@ -5,3 +5,5 @@
 HAL_StatusTypeDef ftcan_init(CAN_HandleTypeDef *hcan);
 
 HAL_StatusTypeDef ftcan_transmit(CAN_HandleTypeDef *hcan, uint16_t id, const uint8_t *data, size_t datalen);
+
+HAL_StatusTypeDef ftcan_add_filter(CAN_HandleTypeDef *handle, uint16_t id, uint16_t mask);

Converter/Src/Converter.cpp

@@ -3,6 +3,41 @@
 HAL_StatusTypeDef spi2can(SPI_HandleTypeDef *hspi, CAN_HandleTypeDef *hcan) {
     // Standard at FaSTTUBe - Sadly. Please fix this with CAN FD for such sensors.
     size_t datalen = 8;
+    HAL_StatusTypeDef status = HAL_OK;
+    // ==================
+    // Hardware Version
+    // ==================
+    auto request_hw_version = vn::pkg_request_read_t(vn::HardwareVersion::ID);
+    auto response_hw_version = vn::pkg_response_t<vn::HardwareVersionRegister>();
+    spi_read(hspi, &request_hw_version, &response_hw_version);
+
+    // ==================
+    // Hardware Version --> 2001
+    // ==================
+    auto request_fw_version = vn::pkg_request_read_t(vn::FirmwareVersion::ID);
+    auto response_fw_version = vn::pkg_response_t<vn::FirmwareVersionRegister>();
+    spi_read(hspi, &request_fw_version, &response_fw_version);
+
+    // ==================
+    // ATT YawPitchRoll
+    // ==================
+    auto request_att_ypr = vn::pkg_request_read_t(vn::YawPitchRoll::ID);
+    auto response_att_ypr = vn::pkg_response_t<vn::YawPitchRollRegister>();
+    spi_read(hspi, &request_att_ypr, &response_att_ypr);
+
+    // ==================
+    // IMU Accel
+    // ==================
+    auto request_imu_accel = vn::pkg_request_read_t(vn::ImuAccelerometer::ID);
+    auto response_imu_accel = vn::pkg_response_t<vn::ImuAccelerometerRegister>();
+    spi_read(hspi, &request_imu_accel, &response_imu_accel);
+
+    // ==================
+    // IMU Gyro
+    // ==================
+    auto request_imu_gyro = vn::pkg_request_read_t(vn::ImuGyro::ID);
+    auto response_imu_gyro = vn::pkg_response_t<vn::ImuGyroRegister>();
+    spi_read(hspi, &request_imu_gyro, &response_imu_gyro);
 
     // ==================
     // INS
@@ -17,15 +52,15 @@ HAL_StatusTypeDef spi2can(SPI_HandleTypeDef *hspi, CAN_HandleTypeDef *hcan) {
     // SEND INS YPR
     auto data_ins_ypr = canlib::encode::can1::vn200_ins_ypr(canlib::frame::decoded::can1::vn200_ins_ypr_t(
         (double)payload.yawPitchRoll.x, (double)payload.yawPitchRoll.y, (double)payload.yawPitchRoll.z, (double)payload.attUncertainty));
-    ftcan_transmit(hcan, CAN1_VN200_INS_YPR_FRAME_ID, (uint8_t *)(&data_ins_ypr), datalen);
+    status = ftcan_transmit(hcan, CAN1_VN200_INS_YPR_FRAME_ID, (uint8_t *)(&data_ins_ypr), datalen);
     // SEND INS LLA
     auto data_ins_ll = canlib::encode::can1::vn200_ins_ll(canlib::frame::decoded::can1::vn200_ins_ll_t(
         (double)payload.position.x, (double)payload.position.y, (double)payload.posUncertainty, (double)payload.posUncertainty));
-    ftcan_transmit(hcan, CAN1_VN200_INS_LL_FRAME_ID, (uint8_t *)(&data_ins_ll), datalen);
+    status = ftcan_transmit(hcan, CAN1_VN200_INS_LL_FRAME_ID, (uint8_t *)(&data_ins_ll), datalen);
     // SEND INS VEL
     auto data_ins_vel = canlib::encode::can1::vn200_ins_vel(canlib::frame::decoded::can1::vn200_ins_vel_t(
         (double)payload.nedVel.x, (double)payload.nedVel.y, (double)payload.velUncertainty, (double)payload.velUncertainty));
-    ftcan_transmit(hcan, CAN1_VN200_INS_VEL_FRAME_ID, (uint8_t *)(&data_ins_vel), datalen);
+    status = ftcan_transmit(hcan, CAN1_VN200_INS_VEL_FRAME_ID, (uint8_t *)(&data_ins_vel), datalen);
 
     // ==================
     // IMU
@@ -40,11 +75,11 @@ HAL_StatusTypeDef spi2can(SPI_HandleTypeDef *hspi, CAN_HandleTypeDef *hcan) {
     // SEND ACC LIN
     auto data_imu_acc_lin = canlib::encode::can1::vn200_imu_acc_lin(
         canlib::frame::decoded::can1::vn200_imu_acc_lin_t((double)payload_imu.accel.x, (double)payload_imu.accel.y, (double)payload_imu.accel.z));
-    ftcan_transmit(hcan, CAN1_VN200_IMU_ACC_LIN_FRAME_ID, (uint8_t *)(&data_imu_acc_lin), datalen);
+    status = ftcan_transmit(hcan, CAN1_VN200_IMU_ACC_LIN_FRAME_ID, (uint8_t *)(&data_imu_acc_lin), datalen);
     // SEND ACC ANG
     auto data_imu_acc_ang = canlib::encode::can1::vn200_imu_acc_ang(
         canlib::frame::decoded::can1::vn200_imu_acc_ang_t((double)payload_imu.gyro.x, (double)payload_imu.gyro.y, (double)payload_imu.gyro.z));
-    ftcan_transmit(hcan, CAN1_VN200_IMU_ACC_ANG_FRAME_ID, (uint8_t *)(&data_imu_acc_ang), datalen);
+    status = ftcan_transmit(hcan, CAN1_VN200_IMU_ACC_ANG_FRAME_ID, (uint8_t *)(&data_imu_acc_ang), datalen);
 
     // ==================
     // GNSS
@@ -59,7 +94,7 @@ HAL_StatusTypeDef spi2can(SPI_HandleTypeDef *hspi, CAN_HandleTypeDef *hcan) {
     // SEND ACC LIN
     auto data_gnss = canlib::encode::can1::vn200_gnss_ll(canlib::frame::decoded::can1::vn200_gnss_ll_t(
         (double)payload_gnss.lla.x, (double)payload_gnss.lla.y, (double)payload_gnss.nedAcc.x, (double)payload_gnss.nedAcc.y));
-    ftcan_transmit(hcan, CAN1_VN200_GNSS_LL_FRAME_ID, (uint8_t *)(&data_gnss), datalen);
+    status = ftcan_transmit(hcan, CAN1_VN200_GNSS_LL_FRAME_ID, (uint8_t *)(&data_gnss), datalen);
 
     return HAL_OK;
 }

Converter/Src/canhalal.cpp

@@ -1,5 +1,5 @@
 #include "canhalal.h"
-
+#define FTCAN_NUM_FILTERS 2
 HAL_StatusTypeDef ftcan_init(CAN_HandleTypeDef *handle) {
     HAL_StatusTypeDef status = HAL_CAN_ActivateNotification(handle, CAN_IT_RX_FIFO0_MSG_PENDING);
     if (status != HAL_OK) {
@@ -18,3 +18,37 @@ HAL_StatusTypeDef ftcan_transmit(CAN_HandleTypeDef *handle, uint16_t id, const u
     uint32_t mailbox;
     return HAL_CAN_AddTxMessage(handle, &header, data, &mailbox);
 }
+
+HAL_StatusTypeDef ftcan_add_filter(CAN_HandleTypeDef *handle, uint16_t id, uint16_t mask) {
+    static uint32_t next_filter_no = 0;
+    static CAN_FilterTypeDef filter;
+    if (next_filter_no % 2 == 0) {
+        filter.FilterIdHigh = id << 5;
+        filter.FilterMaskIdHigh = mask << 5;
+        filter.FilterIdLow = id << 5;
+        filter.FilterMaskIdLow = mask << 5;
+    } else {
+        // Leave high filter untouched from the last configuration
+        filter.FilterIdLow = id << 5;
+        filter.FilterMaskIdLow = mask << 5;
+    }
+    filter.FilterFIFOAssignment = CAN_FILTER_FIFO0;
+    filter.FilterBank = next_filter_no / 2;
+    if (filter.FilterBank > FTCAN_NUM_FILTERS + 1) {
+        return HAL_ERROR;
+    }
+    filter.FilterMode = CAN_FILTERMODE_IDMASK;
+    filter.FilterScale = CAN_FILTERSCALE_16BIT;
+    filter.FilterActivation = CAN_FILTER_ENABLE;
+
+    // Disable slave filters
+    // TODO: Some STM32 have multiple CAN peripherals, and one uses the slave
+    // filter bank
+    filter.SlaveStartFilterBank = FTCAN_NUM_FILTERS;
+
+    HAL_StatusTypeDef status = HAL_CAN_ConfigFilter(handle, &filter);
+    if (status == HAL_OK) {
+        next_filter_no++;
+    }
+    return status;
+}

Core/Inc/stm32f3xx_it.h

@@ -55,6 +55,7 @@ void SVC_Handler(void);
 void DebugMon_Handler(void);
 void PendSV_Handler(void);
 void SysTick_Handler(void);
+void USB_LP_CAN_RX0_IRQHandler(void);
 void CAN_RX1_IRQHandler(void);
 /* USER CODE BEGIN EFP */
 

Core/Src/main.c

@@ -19,10 +19,10 @@
 /* Includes ------------------------------------------------------------------*/
 #include "main.h"
 
-#include <Converter.h>
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
-
+#include <canhalal.h>
+#include <Converter.h>
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -94,12 +94,30 @@ int main(void) {
     MX_SPI1_Init();
     /* USER CODE BEGIN 2 */
 
+    // CAN_FilterTypeDef canfilterconfig;
+    // canfilterconfig.FilterActivation = ENABLE;
+    // canfilterconfig.FilterBank = 1;  // which filter bank to use from the assigned ones
+    // canfilterconfig.FilterFIFOAssignment = CAN_FILTER_FIFO0;
+    // canfilterconfig.FilterIdHigh = 0x0000;
+    // canfilterconfig.FilterIdLow = 0x0000;
+    // canfilterconfig.FilterMaskIdHigh = 0x0000;
+    // canfilterconfig.FilterMaskIdLow = 0x0000;
+    // canfilterconfig.FilterMode = CAN_FILTERMODE_IDMASK;
+    // canfilterconfig.FilterScale = CAN_FILTERSCALE_32BIT;
+    // canfilterconfig.SlaveStartFilterBank = 20;  // how many filters to assign to the CAN1 (master can)
+    // HAL_CAN_ConfigFilter(&hcan, &canfilterconfig);
+    ftcan_init(&hcan);
+    ftcan_add_filter(&hcan, 0x10, 0x0000);
+    // if (HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) {
+    //     Error_Handler();
+    // }
+
     /* USER CODE END 2 */
 
     /* Infinite loop */
     /* USER CODE BEGIN WHILE */
     while (1) {
-        spi2can(&hspi1, &hcan);
+        // spi2can(&hspi1, &hcan);
         /* USER CODE END WHILE */
 
         /* USER CODE BEGIN 3 */
@@ -238,6 +256,20 @@ static void MX_GPIO_Init(void) {
 
 /* USER CODE BEGIN 4 */
 
+CAN_RxHeaderTypeDef RxHeader;
+
+uint8_t RxData[8];
+void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
+    if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK) {
+        Error_Handler();
+    }
+}
+
+void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) {
+    if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK) {
+        Error_Handler();
+    }
+}
 /* USER CODE END 4 */
 
 /**

Core/Src/stm32f3xx_hal_msp.c

@@ -107,6 +107,8 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* hcan)
     HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
     /* CAN interrupt Init */
+    HAL_NVIC_SetPriority(USB_LP_CAN_RX0_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(USB_LP_CAN_RX0_IRQn);
     HAL_NVIC_SetPriority(CAN_RX1_IRQn, 0, 0);
     HAL_NVIC_EnableIRQ(CAN_RX1_IRQn);
   /* USER CODE BEGIN CAN_MspInit 1 */
@@ -139,6 +141,7 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* hcan)
     HAL_GPIO_DeInit(GPIOA, GPIO_PIN_11|GPIO_PIN_12);
 
     /* CAN interrupt DeInit */
+    HAL_NVIC_DisableIRQ(USB_LP_CAN_RX0_IRQn);
     HAL_NVIC_DisableIRQ(CAN_RX1_IRQn);
   /* USER CODE BEGIN CAN_MspDeInit 1 */
 

Core/Src/stm32f3xx_it.c

@@ -198,6 +198,20 @@ void SysTick_Handler(void)
 /* please refer to the startup file (startup_stm32f3xx.s).                    */
 /******************************************************************************/
 
+/**
+  * @brief This function handles USB low priority or CAN_RX0 interrupts.
+  */
+void USB_LP_CAN_RX0_IRQHandler(void)
+{
+  /* USER CODE BEGIN USB_LP_CAN_RX0_IRQn 0 */
+
+  /* USER CODE END USB_LP_CAN_RX0_IRQn 0 */
+  HAL_CAN_IRQHandler(&hcan);
+  /* USER CODE BEGIN USB_LP_CAN_RX0_IRQn 1 */
+
+  /* USER CODE END USB_LP_CAN_RX0_IRQn 1 */
+}
+
 /**
   * @brief This function handles CAN RX1 interrupt.
   */

Libs/vn-interface/include/vn-interface/frames.h

@@ -3,48 +3,88 @@
 
 #include "registers.h"
 
-extern "C"{
+extern "C" {
-    namespace vn{
+namespace vn {
-        typedef struct QuaternionMagneticAccelerationAndAngularRates{
+typedef struct QuaternionMagneticAccelerationAndAngularRates {
     static constexpr uint16_t ID = 8;
     QuaternionMagneticAccelerationAndAngularRatesRegister Register;
-            QuaternionMagneticAccelerationAndAngularRates(){
+    QuaternionMagneticAccelerationAndAngularRates() { memset(&Register, 0, sizeof(Register)); }
-                memset(&Register, 0, sizeof(Register));
+};
-            }
-        };
 
-        typedef struct InsSolutionLla{
+typedef struct InsSolutionLla {
     static constexpr uint16_t ID = 63;
     InsSolutionLlaRegister Register;
-            InsSolutionLla(){
+    InsSolutionLla() { memset(&Register, 0, sizeof(Register)); }
-                memset(&Register, 0, sizeof(Register));
+};
-            }
-        };
 
-        typedef struct GpsSolutionLla{
+typedef struct GpsSolutionLla {
     static constexpr uint16_t ID = 58;
     GpsSolutionLlaRegister Register;
-            GpsSolutionLla(){
+    GpsSolutionLla() { memset(&Register, 0, sizeof(Register)); }
-                memset(&Register, 0, sizeof(Register));
+};
-            }
-        };
 
-        typedef struct YawPitchRollTrueBodyAccelerationAndAngularRates{
+typedef struct YawPitchRollTrueBodyAccelerationAndAngularRates {
-            static constexpr uint16_t ID = 58;
+    static constexpr uint16_t ID = 239;
     YawPitchRollTrueBodyAccelerationAndAngularRatesRegister Register;
-            YawPitchRollTrueBodyAccelerationAndAngularRates(){
+    YawPitchRollTrueBodyAccelerationAndAngularRates() { memset(&Register, 0, sizeof(Register)); }
-                memset(&Register, 0, sizeof(Register));
+};
-            }
-        };
 
-        typedef struct MagneticAccelerationAndAngularRates{
+typedef struct MagneticAccelerationAndAngularRates {
-            static constexpr uint16_t ID = 58;
+    static constexpr uint16_t ID = 20;
     MagneticAccelerationAndAngularRatesRegister Register;
-            MagneticAccelerationAndAngularRates(){
+    MagneticAccelerationAndAngularRates() { memset(&Register, 0, sizeof(Register)); }
-                memset(&Register, 0, sizeof(Register));
+};
-            }
+
-        };
+struct HardwareVersionRegister {
-    }
+    uint32_t version;
+};
+
+typedef struct HardwareVersion {
+    static constexpr uint16_t ID = 2;
+    struct HardwareVersionRegister Register;
+    HardwareVersion() { memset(&Register, 0, sizeof(Register)); }
+};
+
+struct FirmwareVersionRegister {
+    char version[20];
+};
+
+typedef struct FirmwareVersion {
+    static constexpr uint16_t ID = 4;
+    struct FirmwareVersionRegister Register;
+    FirmwareVersion() { memset(&Register, 0, 20); }
+};
+
+struct YawPitchRollRegister {
+    vec3f YawPitchRoll;
+};
+
+typedef struct YawPitchRoll {
+    static constexpr uint16_t ID = 8;
+    struct YawPitchRollRegister Register;
+    YawPitchRoll() { memset(&Register, 0, sizeof(Register)); }
+};
+
+struct ImuAccelerometerRegister {
+    vec3f Accel;
+};
+
+typedef struct ImuAccelerometer {
+    static constexpr uint16_t ID = 18;
+    struct ImuAccelerometerRegister Register;
+    ImuAccelerometer() { memset(&Register, 0, sizeof(Register)); }
+};
+
+struct ImuGyroRegister {
+    vec3f Gyro;
+};
+
+typedef struct ImuGyro {
+    static constexpr uint16_t ID = 19;
+    struct ImuGyroRegister Register;
+    ImuGyro() { memset(&Register, 0, sizeof(Register)); }
+};
+}  // namespace vn
 }
 
 #endif /* _FRAMES_H_  */

vn200.ioc

@@ -6,7 +6,8 @@ CAN.BS1=CAN_BS1_2TQ
 CAN.CalculateBaudRate=500000
 CAN.CalculateTimeBit=2000
 CAN.CalculateTimeQuantum=500.0
-CAN.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,Prescaler,BS1
+CAN.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,Prescaler,BS1,Mode
+CAN.Mode=CAN_MODE_NORMAL
 CAN.Prescaler=8
 File.Version=6
 GPIO.groupedBy=Group By Peripherals
@@ -50,6 +51,7 @@ NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4
 NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:false\:true\:false
+NVIC.USB_LP_CAN_RX0_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false
 PA10.GPIOParameters=GPIO_Speed,PinState,GPIO_PuPd,GPIO_Label
 PA10.GPIO_Label=NRST_VN