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

verified measurements from spi read for imu

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.mxproject
View File

File diff suppressed because one or more lines are too long

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

1
Core/Inc/stm32f3xx_it.h
View File

@ -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 */

38
Core/Src/main.c
View File

@ -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 */
/**

3
Core/Src/stm32f3xx_hal_msp.c
View File

@ -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 */

14
Core/Src/stm32f3xx_it.c
View File

@ -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.
*/

116
Libs/vn-interface/include/vn-interface/frames.h
View File

@ -3,48 +3,88 @@
#include "registers.h"
extern "C"{
namespace vn{
typedef struct QuaternionMagneticAccelerationAndAngularRates{
static constexpr uint16_t ID = 8;
QuaternionMagneticAccelerationAndAngularRatesRegister Register;
QuaternionMagneticAccelerationAndAngularRates(){
memset(&Register, 0, sizeof(Register));
}
};
extern "C" {
namespace vn {
typedef struct QuaternionMagneticAccelerationAndAngularRates {
static constexpr uint16_t ID = 8;
QuaternionMagneticAccelerationAndAngularRatesRegister Register;
QuaternionMagneticAccelerationAndAngularRates() { memset(&Register, 0, sizeof(Register)); }
};
typedef struct InsSolutionLla{
static constexpr uint16_t ID = 63;
InsSolutionLlaRegister Register;
InsSolutionLla(){
memset(&Register, 0, sizeof(Register));
}
};
typedef struct InsSolutionLla {
static constexpr uint16_t ID = 63;
InsSolutionLlaRegister Register;
InsSolutionLla() { memset(&Register, 0, sizeof(Register)); }
};
typedef struct GpsSolutionLla{
static constexpr uint16_t ID = 58;
GpsSolutionLlaRegister Register;
GpsSolutionLla(){
memset(&Register, 0, sizeof(Register));
}
};
typedef struct GpsSolutionLla {
static constexpr uint16_t ID = 58;
GpsSolutionLlaRegister Register;
GpsSolutionLla() { memset(&Register, 0, sizeof(Register)); }
};
typedef struct YawPitchRollTrueBodyAccelerationAndAngularRates{
static constexpr uint16_t ID = 58;
YawPitchRollTrueBodyAccelerationAndAngularRatesRegister Register;
YawPitchRollTrueBodyAccelerationAndAngularRates(){
memset(&Register, 0, sizeof(Register));
}
};
typedef struct YawPitchRollTrueBodyAccelerationAndAngularRates {
static constexpr uint16_t ID = 239;
YawPitchRollTrueBodyAccelerationAndAngularRatesRegister Register;
YawPitchRollTrueBodyAccelerationAndAngularRates() { memset(&Register, 0, sizeof(Register)); }
};
typedef struct MagneticAccelerationAndAngularRates{
static constexpr uint16_t ID = 58;
MagneticAccelerationAndAngularRatesRegister Register;
MagneticAccelerationAndAngularRates(){
memset(&Register, 0, sizeof(Register));
}
};
}
typedef struct MagneticAccelerationAndAngularRates {
static constexpr uint16_t ID = 20;
MagneticAccelerationAndAngularRatesRegister Register;
MagneticAccelerationAndAngularRates() { 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_ */

4
vn200.ioc
View File

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