FaSTTUBe/vn200
51
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

verified measurements from spi read for imu

Browse Source
This commit is contained in:
Richard Koeppe
2024-06-22 15:38:23 +00:00
parent a0f16e99ea
commit e2baacbc92
10 changed files with 232 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Converter/Inc/canhalal.h
View File

@ -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);

47
Converter/Src/Converter.cpp
View File

@ -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;
}

60
Converter/Src/canhalal.cpp
View File

@ -1,20 +1,54 @@
#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) {
return status;
}
HAL_StatusTypeDef status = HAL_CAN_ActivateNotification(handle, CAN_IT_RX_FIFO0_MSG_PENDING);
if (status != HAL_OK) {
return status;
}
return HAL_CAN_Start(handle);
return HAL_CAN_Start(handle);
}
HAL_StatusTypeDef ftcan_transmit(CAN_HandleTypeDef *handle, uint16_t id, const uint8_t *data, size_t datalen) {
static CAN_TxHeaderTypeDef header;
header.StdId = id;
header.IDE = CAN_ID_STD;
header.RTR = CAN_RTR_DATA;
header.DLC = datalen;
uint32_t mailbox;
return HAL_CAN_AddTxMessage(handle, &header, data, &mailbox);
static CAN_TxHeaderTypeDef header;
header.StdId = id;
header.IDE = CAN_ID_STD;
header.RTR = CAN_RTR_DATA;
header.DLC = datalen;
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;
}