94 lines
3.2 KiB
C++
94 lines
3.2 KiB
C++
#include <stm32f302xc.h>
|
|
#include <stm32f3xx_hal.h>
|
|
#include <stm32f3xx_hal_conf.h>
|
|
#include <vn-interface/helper.h>
|
|
#include <vn-interface/registers.h>
|
|
#include "canhalal.h"
|
|
#include "can1.h"
|
|
#include "endec.hpp"
|
|
|
|
|
|
template <typename payload_t>
|
|
HAL_StatusTypeDef spi_read(SPI_HandleTypeDef *hspi, vn::pkg_request_read_t *pRequestMOSI, vn::pkg_response_t<payload_t> *pResponseMISO){
|
|
HAL_StatusTypeDef status = HAL_OK;
|
|
vn::header_t::response_t requestMISO;
|
|
|
|
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET);
|
|
status = HAL_SPI_TransmitReceive(hspi,
|
|
(uint8_t *)(pRequestMOSI),
|
|
(uint8_t *)(&requestMISO), // not relevant, but there to function
|
|
sizeof(*pRequestMOSI), 100);
|
|
|
|
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_SET);
|
|
|
|
HAL_Delay(1);
|
|
|
|
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET);
|
|
|
|
vn::pkg_response_t<payload_t> responseMOSI;
|
|
|
|
status = HAL_SPI_TransmitReceive(hspi,
|
|
(uint8_t *)(&responseMOSI), // just empty byte to allow the slave to transmit
|
|
(uint8_t *)(pResponseMISO),
|
|
sizeof(*pResponseMISO), 100);
|
|
|
|
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_SET);
|
|
return status;
|
|
}
|
|
|
|
template <typename payload_t>
|
|
HAL_StatusTypeDef spi_write(SPI_HandleTypeDef *hspi, vn::pkg_request_write_t<payload_t> *pRequestMOSI, vn::pkg_response_t<payload_t> *pResponseMISO){
|
|
HAL_StatusTypeDef status = HAL_OK;
|
|
vn::header_t::response_t requestMISO;
|
|
|
|
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET);
|
|
status = HAL_SPI_TransmitReceive(hspi,
|
|
(uint8_t *)(pRequestMOSI),
|
|
(uint8_t *)(&requestMISO), // not relevant, but there to function
|
|
sizeof(*pRequestMOSI), 100);
|
|
|
|
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_SET);
|
|
|
|
HAL_Delay(1);
|
|
|
|
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET);
|
|
|
|
vn::pkg_response_t<payload_t> responseMOSI;
|
|
|
|
status = HAL_SPI_TransmitReceive(hspi,
|
|
(uint8_t *)(&responseMOSI), // just empty byte to allow the slave to transmit
|
|
(uint8_t *)(pResponseMISO),
|
|
sizeof(*pResponseMISO), 100);
|
|
|
|
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_SET);
|
|
return status;
|
|
}
|
|
|
|
template <typename payload_t>
|
|
HAL_StatusTypeDef spi2can(SPI_HandleTypeDef *hspi, CAN_HandleTypeDef *hcan, const uint16_t& id){
|
|
auto request = vn::pkg_request_read_t(id);
|
|
auto response = vn::pkg_response_t<payload_t>();
|
|
size_t datalen = 8;
|
|
uint16_t can_id = 0;
|
|
|
|
spi_read(hspi, &request, &response);
|
|
|
|
switch (id) {
|
|
case vn::InsSolutionLlaRegisterID:
|
|
{
|
|
can_id = CAN1_VN200_INS_YPR_FRAME_ID;
|
|
vn::InsSolutionLlaRegister payload;
|
|
payload = response.payload;
|
|
auto data = 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, can_id, (uint8_t*)(&data), datalen);
|
|
}
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return HAL_OK;
|
|
}
|
|
|