66 lines
3.4 KiB
C++
66 lines
3.4 KiB
C++
#include "Converter.h"
|
|
|
|
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;
|
|
|
|
// ==================
|
|
// INS
|
|
// ==================
|
|
// EXTRACT INS
|
|
auto request_ins_solution_lla = vn::pkg_request_read_t(vn::InsSolutionLla::ID);
|
|
auto response_ins_solution_lla = vn::pkg_response_t<vn::InsSolutionLlaRegister>();
|
|
|
|
spi_read(hspi, &request_ins_solution_lla, &response_ins_solution_lla);
|
|
|
|
vn::InsSolutionLlaRegister payload = response_ins_solution_lla.payload;
|
|
// 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);
|
|
// 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);
|
|
// 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);
|
|
|
|
// ==================
|
|
// IMU
|
|
// ==================
|
|
// EXTRACT IMU
|
|
auto request_imu = vn::pkg_request_read_t(vn::MagneticAccelerationAndAngularRates::ID);
|
|
auto response_imu = vn::pkg_response_t<vn::MagneticAccelerationAndAngularRatesRegister>();
|
|
|
|
spi_read(hspi, &request_imu, &response_imu);
|
|
|
|
vn::MagneticAccelerationAndAngularRatesRegister payload_imu = response_imu.payload;
|
|
// 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);
|
|
// 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);
|
|
|
|
// ==================
|
|
// GNSS
|
|
// ==================
|
|
// EXTRACT GNSS LLA
|
|
auto request_gnss = vn::pkg_request_read_t(vn::GpsSolutionLla::ID);
|
|
auto response_gnss = vn::pkg_response_t<vn::GpsSolutionLlaRegister>();
|
|
|
|
spi_read(hspi, &request_gnss, &response_gnss);
|
|
|
|
vn::GpsSolutionLlaRegister payload_gnss = response_gnss.payload;
|
|
// 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);
|
|
|
|
return HAL_OK;
|
|
}
|