corrected and verified can bus output
This commit is contained in:
parent
e2baacbc92
commit
137aa9bcb3
|
|
@ -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<vn::HardwareVersionRegister>();
|
||||
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<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);
|
||||
// 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);
|
||||
// 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);
|
||||
// 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
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue