diff --git a/Converter/Src/Converter.cpp b/Converter/Src/Converter.cpp index c617913..0ccb02b 100644 --- a/Converter/Src/Converter.cpp +++ b/Converter/Src/Converter.cpp @@ -1,36 +1,39 @@ #include "Converter.h" +#define DELAY 2 // ms + 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(); - spi_read(hspi, &request_hw_version, &response_hw_version); + // auto request_hw_version = vn::pkg_request_read_t(vn::HardwareVersion::ID); + // auto response_hw_version = vn::pkg_response_t(); + // 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(); - spi_read(hspi, &request_fw_version, &response_fw_version); + // auto request_fw_version = vn::pkg_request_read_t(vn::FirmwareVersion::ID); + // auto response_fw_version = vn::pkg_response_t(); + // 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(); - spi_read(hspi, &request_att_ypr, &response_att_ypr); + // auto request_att_ypr = vn::pkg_request_read_t(vn::YawPitchRoll::ID); + // auto response_att_ypr = vn::pkg_response_t(); + // 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(); - spi_read(hspi, &request_imu_accel, &response_imu_accel); + // auto request_imu_accel = vn::pkg_request_read_t(vn::ImuAccelerometer::ID); + // auto response_imu_accel = vn::pkg_response_t(); + // spi_read(hspi, &request_imu_accel, &response_imu_accel); // ================== // IMU Gyro @@ -52,15 +55,29 @@ 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)); - status = ftcan_transmit(hcan, CAN1_VN200_INS_YPR_FRAME_ID, (uint8_t *)(&data_ins_ypr), datalen); + uint8_t packed_data_ins_ypr[datalen]; + can1_vn200_ins_ypr_pack(packed_data_ins_ypr, &data_ins_ypr, datalen); + status = ftcan_transmit(hcan, CAN1_VN200_INS_YPR_FRAME_ID, packed_data_ins_ypr, datalen); + + HAL_Delay(DELAY); + // 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)); - status = ftcan_transmit(hcan, CAN1_VN200_INS_LL_FRAME_ID, (uint8_t *)(&data_ins_ll), datalen); + uint8_t packed_data_ins_ll[datalen]; + can1_vn200_ins_ll_pack(packed_data_ins_ll, &data_ins_ll, datalen); + status = ftcan_transmit(hcan, CAN1_VN200_INS_LL_FRAME_ID, packed_data_ins_ll, datalen); + + HAL_Delay(DELAY); + // 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)); - status = ftcan_transmit(hcan, CAN1_VN200_INS_VEL_FRAME_ID, (uint8_t *)(&data_ins_vel), datalen); + uint8_t packed_data_ins_vel[datalen]; + can1_vn200_ins_vel_pack(packed_data_ins_vel, &data_ins_vel, datalen); + status = ftcan_transmit(hcan, CAN1_VN200_INS_VEL_FRAME_ID, packed_data_ins_vel, datalen); + + HAL_Delay(DELAY); // ================== // IMU @@ -73,13 +90,22 @@ HAL_StatusTypeDef spi2can(SPI_HandleTypeDef *hspi, CAN_HandleTypeDef *hcan) { vn::MagneticAccelerationAndAngularRatesRegister payload_imu = response_imu.payload; // SEND ACC LIN - auto data_imu_acc_lin = canlib::encode::can1::vn200_imu_acc_lin( + const 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)); - status = ftcan_transmit(hcan, CAN1_VN200_IMU_ACC_LIN_FRAME_ID, (uint8_t *)(&data_imu_acc_lin), datalen); + uint8_t packed_data_imu_acc_lin[datalen]; + can1_vn200_imu_acc_lin_pack(packed_data_imu_acc_lin, &data_imu_acc_lin, datalen); + status = ftcan_transmit(hcan, CAN1_VN200_IMU_ACC_LIN_FRAME_ID, packed_data_imu_acc_lin, datalen); + + HAL_Delay(DELAY); + // 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)); - status = ftcan_transmit(hcan, CAN1_VN200_IMU_ACC_ANG_FRAME_ID, (uint8_t *)(&data_imu_acc_ang), datalen); + uint8_t packed_data_imu_acc_ang[datalen]; + can1_vn200_imu_acc_ang_pack(packed_data_imu_acc_ang, &data_imu_acc_ang, datalen); + status = ftcan_transmit(hcan, CAN1_VN200_IMU_ACC_ANG_FRAME_ID, packed_data_imu_acc_ang, datalen); + + HAL_Delay(DELAY); // ================== // GNSS @@ -94,7 +120,11 @@ 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)); - status = ftcan_transmit(hcan, CAN1_VN200_GNSS_LL_FRAME_ID, (uint8_t *)(&data_gnss), datalen); + uint8_t packed_data_gnss[datalen]; + can1_vn200_gnss_ll_pack(packed_data_gnss, &data_gnss, datalen); + status = ftcan_transmit(hcan, CAN1_VN200_GNSS_LL_FRAME_ID, packed_data_gnss, datalen); + + HAL_Delay(DELAY); return HAL_OK; } diff --git a/Core/Src/main.c b/Core/Src/main.c index 0bb9521..4a69f66 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -94,30 +94,28 @@ 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); + 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(); - // } + // ftcan_add_filter(&hcan, 0x10, 0x0000); /* USER CODE END 2 */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ while (1) { - // spi2can(&hspi1, &hcan); + spi2can(&hspi1, &hcan); + HAL_Delay(100); /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ @@ -171,11 +169,11 @@ static void MX_CAN_Init(void) { /* USER CODE END CAN_Init 1 */ hcan.Instance = CAN; - hcan.Init.Prescaler = 8; + hcan.Init.Prescaler = 2; hcan.Init.Mode = CAN_MODE_NORMAL; hcan.Init.SyncJumpWidth = CAN_SJW_1TQ; - hcan.Init.TimeSeg1 = CAN_BS1_2TQ; - hcan.Init.TimeSeg2 = CAN_BS2_1TQ; + hcan.Init.TimeSeg1 = CAN_BS1_13TQ; + hcan.Init.TimeSeg2 = CAN_BS2_2TQ; hcan.Init.TimeTriggeredMode = DISABLE; hcan.Init.AutoBusOff = DISABLE; hcan.Init.AutoWakeUp = DISABLE; diff --git a/vn200.ioc b/vn200.ioc index c9db532..b1476d2 100644 --- a/vn200.ioc +++ b/vn200.ioc @@ -2,13 +2,14 @@ CAD.formats= CAD.pinconfig= CAD.provider= -CAN.BS1=CAN_BS1_2TQ +CAN.BS1=CAN_BS1_13TQ +CAN.BS2=CAN_BS2_2TQ CAN.CalculateBaudRate=500000 CAN.CalculateTimeBit=2000 -CAN.CalculateTimeQuantum=500.0 -CAN.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,Prescaler,BS1,Mode +CAN.CalculateTimeQuantum=125.0 +CAN.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,Prescaler,BS1,Mode,BS2 CAN.Mode=CAN_MODE_NORMAL -CAN.Prescaler=8 +CAN.Prescaler=2 File.Version=6 GPIO.groupedBy=Group By Peripherals KeepUserPlacement=false