FaSTTUBe/INS-VN-200
54
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

vn200 stm project

Browse Source
This commit is contained in:
r.koeppe
2024-06-06 11:36:00 +02:00
parent 62818b5ea7
commit c5f5e6fb98
159 changed files with 128314 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

92
vn200/Converter/Inc/Converter.h Normal file
View File

@ -0,0 +1,92 @@
#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;
uint8_t *data = nullptr;
spi_read(hspi, &request, &response);
switch (id) {
case vn::YawPitchRollTrueBodyAccelerationAndAngularRatesRegisterID:
{
can_id = CAN1_VN200_YPR_FRAME_ID;
vn::YawPitchRollTrueBodyAccelerationAndAngularRatesRegister payload;
payload = response.payload;
//canlib::encode::can1::vn200_ypr(canlib::frame::decoded::can1::vn200_ypr_t((double) payload.yawPitchRoll.x, (double) payload.yawPitchRoll.y, (double) payload.yawPitchRoll.z));
}
break;
default:
break;
}
ftcan_transmit(hcan, can_id, data, datalen);
return HAL_OK;
}

600
vn200/Converter/Inc/can1.h Normal file
View File

@ -0,0 +1,600 @@
/**
* The MIT License (MIT)
*
* Copyright (c) 2018-2019 Erik Moqvist
*
* Permission is hereby granted, free of charge, to any person
* obtaining a copy of this software and associated documentation
* files (the "Software"), to deal in the Software without
* restriction, including without limitation the rights to use, copy,
* modify, merge, publish, distribute, sublicense, and/or sell copies
* of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
/**
* This file was generated by cantools version 0.1.dev1740+ge714fab Tue Jun 4 14:20:13 2024.
*/
#ifndef CAN1_H
#define CAN1_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#ifndef EINVAL
# define EINVAL 22
#endif
/* Frame ids. */
#define CAN1_VN200_INS_FRAME_ID (0x01u)
#define CAN1_VN200_IMU_FRAME_ID (0x02u)
#define CAN1_VN200_YPR_FRAME_ID (0x05u)
/* Frame lengths in bytes. */
#define CAN1_VN200_INS_LENGTH (8u)
#define CAN1_VN200_IMU_LENGTH (8u)
#define CAN1_VN200_YPR_LENGTH (8u)
/* Extended or standard frame types. */
#define CAN1_VN200_INS_IS_EXTENDED (0)
#define CAN1_VN200_IMU_IS_EXTENDED (0)
#define CAN1_VN200_YPR_IS_EXTENDED (0)
/* Frame cycle times in milliseconds. */
/* Signal choices. */
/* Frame Names. */
#define CAN1_VN200_INS_NAME "VN200_INS"
#define CAN1_VN200_IMU_NAME "VN200_IMU"
#define CAN1_VN200_YPR_NAME "VN200_YPR"
/* Signal Names. */
#define CAN1_VN200_INS_YAW_NAME "Yaw"
#define CAN1_VN200_IMU_MULTIPLEXER_NAME "multiplexer"
#define CAN1_VN200_IMU_ACCX_NAME "accx"
#define CAN1_VN200_IMU_ACCY_NAME "accy"
#define CAN1_VN200_IMU_ACCZ_NAME "accz"
#define CAN1_VN200_IMU_ANGX_NAME "angx"
#define CAN1_VN200_IMU_ANGY_NAME "angy"
#define CAN1_VN200_IMU_ANGZ_NAME "angz"
#define CAN1_VN200_YPR_YAW_NAME "yaw"
#define CAN1_VN200_YPR_PITCH_NAME "pitch"
#define CAN1_VN200_YPR_ROLL_NAME "roll"
/**
* Signals in message VN200_INS.
*
* VN200 INS - Measurements with sensor fusion
*
* All signal values are as on the CAN bus.
*/
struct can1_vn200_ins_t {
/**
* Range: 0..64255 (0..8031.875 rpm)
* Scale: 0.125
* Offset: 0
*/
uint16_t yaw;
};
/**
* Signals in message VN200_IMU.
*
* VN200 IMU - Measurements only IMU without algorithms
*
* All signal values are as on the CAN bus.
*/
struct can1_vn200_imu_t {
/**
* Range: -
* Scale: 0
* Offset: 0
*/
uint8_t multiplexer;
/**
* Range: -
* Scale: 0.06
* Offset: 0
*/
int16_t accx;
/**
* Range: -
* Scale: 0.06
* Offset: 0
*/
int16_t accy;
/**
* Range: -
* Scale: 0.06
* Offset: 0
*/
int16_t accz;
/**
* Range: -
* Scale: 0.002
* Offset: 0
*/
int16_t angx;
/**
* Range: -
* Scale: 0.002
* Offset: 0
*/
int16_t angy;
/**
* Range: -
* Scale: 0.002
* Offset: 0
*/
int16_t angz;
};
/**
* Signals in message VN200_YPR.
*
* All signal values are as on the CAN bus.
*/
struct can1_vn200_ypr_t {
/**
* Range: -
* Scale: 0.006
* Offset: 0
*/
uint16_t yaw;
/**
* Range: -
* Scale: 0.006
* Offset: 0
*/
uint16_t pitch;
/**
* Range: -
* Scale: 0.006
* Offset: 0
*/
uint16_t roll;
};
/**
* Pack message VN200_INS.
*
* @param[out] dst_p Buffer to pack the message into.
* @param[in] src_p Data to pack.
* @param[in] size Size of dst_p.
*
* @return Size of packed data, or negative error code.
*/
int can1_vn200_ins_pack(
uint8_t *dst_p,
const struct can1_vn200_ins_t *src_p,
size_t size);
/**
* Unpack message VN200_INS.
*
* @param[out] dst_p Object to unpack the message into.
* @param[in] src_p Message to unpack.
* @param[in] size Size of src_p.
*
* @return zero(0) or negative error code.
*/
int can1_vn200_ins_unpack(
struct can1_vn200_ins_t *dst_p,
const uint8_t *src_p,
size_t size);
/**
* Init message fields to default values from VN200_INS.
*
* @param[in] msg_p Message to init.
*
* @return zero(0) on success or (-1) in case of nullptr argument.
*/
int can1_vn200_ins_init(struct can1_vn200_ins_t *msg_p);
/**
* Encode given signal by applying scaling and offset.
*
* @param[in] value Signal to encode.
*
* @return Encoded signal.
*/
uint16_t can1_vn200_ins_yaw_encode(double value);
/**
* Decode given signal by applying scaling and offset.
*
* @param[in] value Signal to decode.
*
* @return Decoded signal.
*/
double can1_vn200_ins_yaw_decode(uint16_t value);
/**
* Check that given signal is in allowed range.
*
* @param[in] value Signal to check.
*
* @return true if in range, false otherwise.
*/
bool can1_vn200_ins_yaw_is_in_range(uint16_t value);
/**
* Pack message VN200_IMU.
*
* @param[out] dst_p Buffer to pack the message into.
* @param[in] src_p Data to pack.
* @param[in] size Size of dst_p.
*
* @return Size of packed data, or negative error code.
*/
int can1_vn200_imu_pack(
uint8_t *dst_p,
const struct can1_vn200_imu_t *src_p,
size_t size);
/**
* Unpack message VN200_IMU.
*
* @param[out] dst_p Object to unpack the message into.
* @param[in] src_p Message to unpack.
* @param[in] size Size of src_p.
*
* @return zero(0) or negative error code.
*/
int can1_vn200_imu_unpack(
struct can1_vn200_imu_t *dst_p,
const uint8_t *src_p,
size_t size);
/**
* Init message fields to default values from VN200_IMU.
*
* @param[in] msg_p Message to init.
*
* @return zero(0) on success or (-1) in case of nullptr argument.
*/
int can1_vn200_imu_init(struct can1_vn200_imu_t *msg_p);
/**
* Encode given signal by applying scaling and offset.
*
* @param[in] value Signal to encode.
*
* @return Encoded signal.
*/
uint8_t can1_vn200_imu_multiplexer_encode(double value);
/**
* Decode given signal by applying scaling and offset.
*
* @param[in] value Signal to decode.
*
* @return Decoded signal.
*/
double can1_vn200_imu_multiplexer_decode(uint8_t value);
/**
* Check that given signal is in allowed range.
*
* @param[in] value Signal to check.
*
* @return true if in range, false otherwise.
*/
bool can1_vn200_imu_multiplexer_is_in_range(uint8_t value);
/**
* Encode given signal by applying scaling and offset.
*
* @param[in] value Signal to encode.
*
* @return Encoded signal.
*/
int16_t can1_vn200_imu_accx_encode(double value);
/**
* Decode given signal by applying scaling and offset.
*
* @param[in] value Signal to decode.
*
* @return Decoded signal.
*/
double can1_vn200_imu_accx_decode(int16_t value);
/**
* Check that given signal is in allowed range.
*
* @param[in] value Signal to check.
*
* @return true if in range, false otherwise.
*/
bool can1_vn200_imu_accx_is_in_range(int16_t value);
/**
* Encode given signal by applying scaling and offset.
*
* @param[in] value Signal to encode.
*
* @return Encoded signal.
*/
int16_t can1_vn200_imu_accy_encode(double value);
/**
* Decode given signal by applying scaling and offset.
*
* @param[in] value Signal to decode.
*
* @return Decoded signal.
*/
double can1_vn200_imu_accy_decode(int16_t value);
/**
* Check that given signal is in allowed range.
*
* @param[in] value Signal to check.
*
* @return true if in range, false otherwise.
*/
bool can1_vn200_imu_accy_is_in_range(int16_t value);
/**
* Encode given signal by applying scaling and offset.
*
* @param[in] value Signal to encode.
*
* @return Encoded signal.
*/
int16_t can1_vn200_imu_accz_encode(double value);
/**
* Decode given signal by applying scaling and offset.
*
* @param[in] value Signal to decode.
*
* @return Decoded signal.
*/
double can1_vn200_imu_accz_decode(int16_t value);
/**
* Check that given signal is in allowed range.
*
* @param[in] value Signal to check.
*
* @return true if in range, false otherwise.
*/
bool can1_vn200_imu_accz_is_in_range(int16_t value);
/**
* Encode given signal by applying scaling and offset.
*
* @param[in] value Signal to encode.
*
* @return Encoded signal.
*/
int16_t can1_vn200_imu_angx_encode(double value);
/**
* Decode given signal by applying scaling and offset.
*
* @param[in] value Signal to decode.
*
* @return Decoded signal.
*/
double can1_vn200_imu_angx_decode(int16_t value);
/**
* Check that given signal is in allowed range.
*
* @param[in] value Signal to check.
*
* @return true if in range, false otherwise.
*/
bool can1_vn200_imu_angx_is_in_range(int16_t value);
/**
* Encode given signal by applying scaling and offset.
*
* @param[in] value Signal to encode.
*
* @return Encoded signal.
*/
int16_t can1_vn200_imu_angy_encode(double value);
/**
* Decode given signal by applying scaling and offset.
*
* @param[in] value Signal to decode.
*
* @return Decoded signal.
*/
double can1_vn200_imu_angy_decode(int16_t value);
/**
* Check that given signal is in allowed range.
*
* @param[in] value Signal to check.
*
* @return true if in range, false otherwise.
*/
bool can1_vn200_imu_angy_is_in_range(int16_t value);
/**
* Encode given signal by applying scaling and offset.
*
* @param[in] value Signal to encode.
*
* @return Encoded signal.
*/
int16_t can1_vn200_imu_angz_encode(double value);
/**
* Decode given signal by applying scaling and offset.
*
* @param[in] value Signal to decode.
*
* @return Decoded signal.
*/
double can1_vn200_imu_angz_decode(int16_t value);
/**
* Check that given signal is in allowed range.
*
* @param[in] value Signal to check.
*
* @return true if in range, false otherwise.
*/
bool can1_vn200_imu_angz_is_in_range(int16_t value);
/**
* Pack message VN200_YPR.
*
* @param[out] dst_p Buffer to pack the message into.
* @param[in] src_p Data to pack.
* @param[in] size Size of dst_p.
*
* @return Size of packed data, or negative error code.
*/
int can1_vn200_ypr_pack(
uint8_t *dst_p,
const struct can1_vn200_ypr_t *src_p,
size_t size);
/**
* Unpack message VN200_YPR.
*
* @param[out] dst_p Object to unpack the message into.
* @param[in] src_p Message to unpack.
* @param[in] size Size of src_p.
*
* @return zero(0) or negative error code.
*/
int can1_vn200_ypr_unpack(
struct can1_vn200_ypr_t *dst_p,
const uint8_t *src_p,
size_t size);
/**
* Init message fields to default values from VN200_YPR.
*
* @param[in] msg_p Message to init.
*
* @return zero(0) on success or (-1) in case of nullptr argument.
*/
int can1_vn200_ypr_init(struct can1_vn200_ypr_t *msg_p);
/**
* Encode given signal by applying scaling and offset.
*
* @param[in] value Signal to encode.
*
* @return Encoded signal.
*/
uint16_t can1_vn200_ypr_yaw_encode(double value);
/**
* Decode given signal by applying scaling and offset.
*
* @param[in] value Signal to decode.
*
* @return Decoded signal.
*/
double can1_vn200_ypr_yaw_decode(uint16_t value);
/**
* Check that given signal is in allowed range.
*
* @param[in] value Signal to check.
*
* @return true if in range, false otherwise.
*/
bool can1_vn200_ypr_yaw_is_in_range(uint16_t value);
/**
* Encode given signal by applying scaling and offset.
*
* @param[in] value Signal to encode.
*
* @return Encoded signal.
*/
uint16_t can1_vn200_ypr_pitch_encode(double value);
/**
* Decode given signal by applying scaling and offset.
*
* @param[in] value Signal to decode.
*
* @return Decoded signal.
*/
double can1_vn200_ypr_pitch_decode(uint16_t value);
/**
* Check that given signal is in allowed range.
*
* @param[in] value Signal to check.
*
* @return true if in range, false otherwise.
*/
bool can1_vn200_ypr_pitch_is_in_range(uint16_t value);
/**
* Encode given signal by applying scaling and offset.
*
* @param[in] value Signal to encode.
*
* @return Encoded signal.
*/
uint16_t can1_vn200_ypr_roll_encode(double value);
/**
* Decode given signal by applying scaling and offset.
*
* @param[in] value Signal to decode.
*
* @return Decoded signal.
*/
double can1_vn200_ypr_roll_decode(uint16_t value);
/**
* Check that given signal is in allowed range.
*
* @param[in] value Signal to check.
*
* @return true if in range, false otherwise.
*/
bool can1_vn200_ypr_roll_is_in_range(uint16_t value);
#ifdef __cplusplus
}
#endif
#endif

7
vn200/Converter/Inc/canhalal.h Normal file
View File

@ -0,0 +1,7 @@
#include <stm32f302xc.h>
#include <stm32f3xx_hal.h>
#include <stm32f3xx_hal_conf.h>
HAL_StatusTypeDef ftcan_init(CAN_HandleTypeDef *hcan);
HAL_StatusTypeDef ftcan_transmit(CAN_HandleTypeDef *hcan, uint16_t id, const uint8_t *data, size_t datalen);

161
vn200/Converter/Inc/endec.hpp Normal file
View File

@ -0,0 +1,161 @@
#ifndef _ENDEC_HPP_
#define _ENDEC_HPP_
#include "can1.h"
#include <functional>
namespace canlib {
// frame structures
namespace frame {
namespace decoded {
namespace can1 {
struct vn200_ins_t {
double yaw;
vn200_ins_t() {
this->yaw = 0.0;
}
vn200_ins_t(
double yaw) {
this->yaw = yaw;
}
};
struct vn200_imu_t {
double multiplexer;
double accx;
double accy;
double accz;
double angx;
double angy;
double angz;
vn200_imu_t() {
this->multiplexer = 0.0;
this->accx = 0.0;
this->accy = 0.0;
this->accz = 0.0;
this->angx = 0.0;
this->angy = 0.0;
this->angz = 0.0;
}
vn200_imu_t(
double multiplexer,
double accx,
double accy,
double accz,
double angx,
double angy,
double angz) {
this->multiplexer = multiplexer;
this->accx = accx;
this->accy = accy;
this->accz = accz;
this->angx = angx;
this->angy = angy;
this->angz = angz;
}
};
struct vn200_ypr_t {
double yaw;
double pitch;
double roll;
vn200_ypr_t() {
this->yaw = 0.0;
this->pitch = 0.0;
this->roll = 0.0;
}
vn200_ypr_t(
double yaw,
double pitch,
double roll) {
this->yaw = yaw;
this->pitch = pitch;
this->roll = roll;
}
};
}
}
}
// encode functions
namespace encode {
namespace can1 {
inline can1_vn200_ins_t vn200_ins(const frame::decoded::can1::vn200_ins_t frame_decoded) {
can1_vn200_ins_t frame_encoded;
frame_encoded.yaw = can1_vn200_ins_yaw_encode(frame_decoded.yaw);
return frame_encoded;
}
inline can1_vn200_imu_t vn200_imu(const frame::decoded::can1::vn200_imu_t frame_decoded) {
can1_vn200_imu_t frame_encoded;
frame_encoded.multiplexer = can1_vn200_imu_multiplexer_encode(frame_decoded.multiplexer);
frame_encoded.accx = can1_vn200_imu_accx_encode(frame_decoded.accx);
frame_encoded.accy = can1_vn200_imu_accy_encode(frame_decoded.accy);
frame_encoded.accz = can1_vn200_imu_accz_encode(frame_decoded.accz);
frame_encoded.angx = can1_vn200_imu_angx_encode(frame_decoded.angx);
frame_encoded.angy = can1_vn200_imu_angy_encode(frame_decoded.angy);
frame_encoded.angz = can1_vn200_imu_angz_encode(frame_decoded.angz);
return frame_encoded;
}
inline can1_vn200_ypr_t vn200_ypr(const frame::decoded::can1::vn200_ypr_t frame_decoded) {
can1_vn200_ypr_t frame_encoded;
frame_encoded.yaw = can1_vn200_ypr_yaw_encode(frame_decoded.yaw);
frame_encoded.pitch = can1_vn200_ypr_pitch_encode(frame_decoded.pitch);
frame_encoded.roll = can1_vn200_ypr_roll_encode(frame_decoded.roll);
return frame_encoded;
}
}
}
// decode functions
namespace decode {
namespace can1 {
inline frame::decoded::can1::vn200_ins_t vn200_ins(const can1_vn200_ins_t frame_encoded) {
frame::decoded::can1::vn200_ins_t frame_decoded;
frame_decoded.yaw = can1_vn200_ins_yaw_decode(frame_encoded.yaw);
return frame_decoded;
}
inline frame::decoded::can1::vn200_imu_t vn200_imu(const can1_vn200_imu_t frame_encoded) {
frame::decoded::can1::vn200_imu_t frame_decoded;
frame_decoded.multiplexer = can1_vn200_imu_multiplexer_decode(frame_encoded.multiplexer);
frame_decoded.accx = can1_vn200_imu_accx_decode(frame_encoded.accx);
frame_decoded.accy = can1_vn200_imu_accy_decode(frame_encoded.accy);
frame_decoded.accz = can1_vn200_imu_accz_decode(frame_encoded.accz);
frame_decoded.angx = can1_vn200_imu_angx_decode(frame_encoded.angx);
frame_decoded.angy = can1_vn200_imu_angy_decode(frame_encoded.angy);
frame_decoded.angz = can1_vn200_imu_angz_decode(frame_encoded.angz);
return frame_decoded;
}
inline frame::decoded::can1::vn200_ypr_t vn200_ypr(const can1_vn200_ypr_t frame_encoded) {
frame::decoded::can1::vn200_ypr_t frame_decoded;
frame_decoded.yaw = can1_vn200_ypr_yaw_decode(frame_encoded.yaw);
frame_decoded.pitch = can1_vn200_ypr_pitch_decode(frame_encoded.pitch);
frame_decoded.roll = can1_vn200_ypr_roll_decode(frame_encoded.roll);
return frame_decoded;
}
}
}
// callback structure
namespace callback {
namespace can1 {
inline std::function<void(can1_vn200_ins_t, frame::decoded::can1::vn200_ins_t)> vn200_ins = NULL;
inline std::function<void(can1_vn200_imu_t, frame::decoded::can1::vn200_imu_t)> vn200_imu = NULL;
inline std::function<void(can1_vn200_ypr_t, frame::decoded::can1::vn200_ypr_t)> vn200_ypr = NULL;
}
}
}
#endif // _ENDEC_HPP_

2
vn200/Converter/Src/Converter.cpp Normal file
View File

@ -0,0 +1,2 @@
#include "Converter.h"

461
vn200/Converter/Src/can1.cpp Normal file
View File

@ -0,0 +1,461 @@
/**
* The MIT License (MIT)
*
* Copyright (c) 2018-2019 Erik Moqvist
*
* Permission is hereby granted, free of charge, to any person
* obtaining a copy of this software and associated documentation
* files (the "Software"), to deal in the Software without
* restriction, including without limitation the rights to use, copy,
* modify, merge, publish, distribute, sublicense, and/or sell copies
* of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be
* included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
/**
* This file was generated by cantools version 0.1.dev1740+ge714fab Tue Jun 4 14:20:13 2024.
*/
#include <string.h>
#include "can1.h"
static inline uint8_t pack_left_shift_u8(
uint8_t value,
uint8_t shift,
uint8_t mask)
{
return (uint8_t)((uint8_t)(value << shift) & mask);
}
static inline uint8_t pack_left_shift_u16(
uint16_t value,
uint8_t shift,
uint8_t mask)
{
return (uint8_t)((uint8_t)(value << shift) & mask);
}
static inline uint8_t pack_right_shift_u16(
uint16_t value,
uint8_t shift,
uint8_t mask)
{
return (uint8_t)((uint8_t)(value >> shift) & mask);
}
static inline uint16_t unpack_left_shift_u16(
uint8_t value,
uint8_t shift,
uint8_t mask)
{
return (uint16_t)((uint16_t)(value & mask) << shift);
}
static inline uint8_t unpack_right_shift_u8(
uint8_t value,
uint8_t shift,
uint8_t mask)
{
return (uint8_t)((uint8_t)(value & mask) >> shift);
}
static inline uint16_t unpack_right_shift_u16(
uint8_t value,
uint8_t shift,
uint8_t mask)
{
return (uint16_t)((uint16_t)(value & mask) >> shift);
}
int can1_vn200_ins_pack(
uint8_t *dst_p,
const struct can1_vn200_ins_t *src_p,
size_t size)
{
if (size < 8u) {
return (-EINVAL);
}
memset(&dst_p[0], 0, 8);
dst_p[3] |= pack_left_shift_u16(src_p->yaw, 0u, 0xffu);
dst_p[4] |= pack_right_shift_u16(src_p->yaw, 8u, 0xffu);
return (8);
}
int can1_vn200_ins_unpack(
struct can1_vn200_ins_t *dst_p,
const uint8_t *src_p,
size_t size)
{
if (size < 8u) {
return (-EINVAL);
}
dst_p->yaw = unpack_right_shift_u16(src_p[3], 0u, 0xffu);
dst_p->yaw |= unpack_left_shift_u16(src_p[4], 8u, 0xffu);
return (0);
}
int can1_vn200_ins_init(struct can1_vn200_ins_t *msg_p)
{
if (msg_p == NULL) return -1;
memset(msg_p, 0, sizeof(struct can1_vn200_ins_t));
return 0;
}
uint16_t can1_vn200_ins_yaw_encode(double value)
{
return (uint16_t)(value / 0.125);
}
double can1_vn200_ins_yaw_decode(uint16_t value)
{
return ((double)value * 0.125);
}
bool can1_vn200_ins_yaw_is_in_range(uint16_t value)
{
return (value <= 64255u);
}
int can1_vn200_imu_pack(
uint8_t *dst_p,
const struct can1_vn200_imu_t *src_p,
size_t size)
{
uint16_t accx;
uint16_t accy;
uint16_t accz;
uint16_t angx;
uint16_t angy;
uint16_t angz;
if (size < 8u) {
return (-EINVAL);
}
memset(&dst_p[0], 0, 8);
dst_p[0] |= pack_left_shift_u8(src_p->multiplexer, 0u, 0x0fu);
accx = (uint16_t)src_p->accx;
dst_p[0] |= pack_left_shift_u16(accx, 4u, 0xf0u);
dst_p[1] |= pack_right_shift_u16(accx, 4u, 0x3fu);
accy = (uint16_t)src_p->accy;
dst_p[1] |= pack_left_shift_u16(accy, 6u, 0xc0u);
dst_p[2] |= pack_right_shift_u16(accy, 2u, 0xffu);
accz = (uint16_t)src_p->accz;
dst_p[3] |= pack_left_shift_u16(accz, 0u, 0xffu);
dst_p[4] |= pack_right_shift_u16(accz, 8u, 0x03u);
angx = (uint16_t)src_p->angx;
dst_p[4] |= pack_left_shift_u16(angx, 2u, 0xfcu);
dst_p[5] |= pack_right_shift_u16(angx, 6u, 0x0fu);
angy = (uint16_t)src_p->angy;
dst_p[5] |= pack_left_shift_u16(angy, 4u, 0xf0u);
dst_p[6] |= pack_right_shift_u16(angy, 4u, 0x3fu);
angz = (uint16_t)src_p->angz;
dst_p[6] |= pack_left_shift_u16(angz, 6u, 0xc0u);
dst_p[7] |= pack_right_shift_u16(angz, 2u, 0xffu);
return (8);
}
int can1_vn200_imu_unpack(
struct can1_vn200_imu_t *dst_p,
const uint8_t *src_p,
size_t size)
{
uint16_t accx;
uint16_t accy;
uint16_t accz;
uint16_t angx;
uint16_t angy;
uint16_t angz;
if (size < 8u) {
return (-EINVAL);
}
dst_p->multiplexer = unpack_right_shift_u8(src_p[0], 0u, 0x0fu);
accx = unpack_right_shift_u16(src_p[0], 4u, 0xf0u);
accx |= unpack_left_shift_u16(src_p[1], 4u, 0x3fu);
if ((accx & (1u << 9)) != 0u) {
accx |= 0xfc00u;
}
dst_p->accx = (int16_t)accx;
accy = unpack_right_shift_u16(src_p[1], 6u, 0xc0u);
accy |= unpack_left_shift_u16(src_p[2], 2u, 0xffu);
if ((accy & (1u << 9)) != 0u) {
accy |= 0xfc00u;
}
dst_p->accy = (int16_t)accy;
accz = unpack_right_shift_u16(src_p[3], 0u, 0xffu);
accz |= unpack_left_shift_u16(src_p[4], 8u, 0x03u);
if ((accz & (1u << 9)) != 0u) {
accz |= 0xfc00u;
}
dst_p->accz = (int16_t)accz;
angx = unpack_right_shift_u16(src_p[4], 2u, 0xfcu);
angx |= unpack_left_shift_u16(src_p[5], 6u, 0x0fu);
if ((angx & (1u << 9)) != 0u) {
angx |= 0xfc00u;
}
dst_p->angx = (int16_t)angx;
angy = unpack_right_shift_u16(src_p[5], 4u, 0xf0u);
angy |= unpack_left_shift_u16(src_p[6], 4u, 0x3fu);
if ((angy & (1u << 9)) != 0u) {
angy |= 0xfc00u;
}
dst_p->angy = (int16_t)angy;
angz = unpack_right_shift_u16(src_p[6], 6u, 0xc0u);
angz |= unpack_left_shift_u16(src_p[7], 2u, 0xffu);
if ((angz & (1u << 9)) != 0u) {
angz |= 0xfc00u;
}
dst_p->angz = (int16_t)angz;
return (0);
}
int can1_vn200_imu_init(struct can1_vn200_imu_t *msg_p)
{
if (msg_p == NULL) return -1;
memset(msg_p, 0, sizeof(struct can1_vn200_imu_t));
return 0;
}
uint8_t can1_vn200_imu_multiplexer_encode(double value)
{
return (uint8_t)(value / 0.0);
}
double can1_vn200_imu_multiplexer_decode(uint8_t value)
{
return ((double)value * 0.0);
}
bool can1_vn200_imu_multiplexer_is_in_range(uint8_t value)
{
return (value <= 15u);
}
int16_t can1_vn200_imu_accx_encode(double value)
{
return (int16_t)(value / 0.06);
}
double can1_vn200_imu_accx_decode(int16_t value)
{
return ((double)value * 0.06);
}
bool can1_vn200_imu_accx_is_in_range(int16_t value)
{
return ((value >= -512) && (value <= 511));
}
int16_t can1_vn200_imu_accy_encode(double value)
{
return (int16_t)(value / 0.06);
}
double can1_vn200_imu_accy_decode(int16_t value)
{
return ((double)value * 0.06);
}
bool can1_vn200_imu_accy_is_in_range(int16_t value)
{
return ((value >= -512) && (value <= 511));
}
int16_t can1_vn200_imu_accz_encode(double value)
{
return (int16_t)(value / 0.06);
}
double can1_vn200_imu_accz_decode(int16_t value)
{
return ((double)value * 0.06);
}
bool can1_vn200_imu_accz_is_in_range(int16_t value)
{
return ((value >= -512) && (value <= 511));
}
int16_t can1_vn200_imu_angx_encode(double value)
{
return (int16_t)(value / 0.002);
}
double can1_vn200_imu_angx_decode(int16_t value)
{
return ((double)value * 0.002);
}
bool can1_vn200_imu_angx_is_in_range(int16_t value)
{
return ((value >= -512) && (value <= 511));
}
int16_t can1_vn200_imu_angy_encode(double value)
{
return (int16_t)(value / 0.002);
}
double can1_vn200_imu_angy_decode(int16_t value)
{
return ((double)value * 0.002);
}
bool can1_vn200_imu_angy_is_in_range(int16_t value)
{
return ((value >= -512) && (value <= 511));
}
int16_t can1_vn200_imu_angz_encode(double value)
{
return (int16_t)(value / 0.002);
}
double can1_vn200_imu_angz_decode(int16_t value)
{
return ((double)value * 0.002);
}
bool can1_vn200_imu_angz_is_in_range(int16_t value)
{
return ((value >= -512) && (value <= 511));
}
int can1_vn200_ypr_pack(
uint8_t *dst_p,
const struct can1_vn200_ypr_t *src_p,
size_t size)
{
if (size < 8u) {
return (-EINVAL);
}
memset(&dst_p[0], 0, 8);
dst_p[0] |= pack_left_shift_u16(src_p->yaw, 0u, 0xffu);
dst_p[1] |= pack_right_shift_u16(src_p->yaw, 8u, 0xffu);
dst_p[2] |= pack_left_shift_u16(src_p->pitch, 0u, 0xffu);
dst_p[3] |= pack_right_shift_u16(src_p->pitch, 8u, 0xffu);
dst_p[4] |= pack_left_shift_u16(src_p->roll, 0u, 0xffu);
dst_p[5] |= pack_right_shift_u16(src_p->roll, 8u, 0xffu);
return (8);
}
int can1_vn200_ypr_unpack(
struct can1_vn200_ypr_t *dst_p,
const uint8_t *src_p,
size_t size)
{
if (size < 8u) {
return (-EINVAL);
}
dst_p->yaw = unpack_right_shift_u16(src_p[0], 0u, 0xffu);
dst_p->yaw |= unpack_left_shift_u16(src_p[1], 8u, 0xffu);
dst_p->pitch = unpack_right_shift_u16(src_p[2], 0u, 0xffu);
dst_p->pitch |= unpack_left_shift_u16(src_p[3], 8u, 0xffu);
dst_p->roll = unpack_right_shift_u16(src_p[4], 0u, 0xffu);
dst_p->roll |= unpack_left_shift_u16(src_p[5], 8u, 0xffu);
return (0);
}
int can1_vn200_ypr_init(struct can1_vn200_ypr_t *msg_p)
{
if (msg_p == NULL) return -1;
memset(msg_p, 0, sizeof(struct can1_vn200_ypr_t));
return 0;
}
uint16_t can1_vn200_ypr_yaw_encode(double value)
{
return (uint16_t)(value / 0.006);
}
double can1_vn200_ypr_yaw_decode(uint16_t value)
{
return ((double)value * 0.006);
}
bool can1_vn200_ypr_yaw_is_in_range(uint16_t value)
{
(void)value;
return (true);
}
uint16_t can1_vn200_ypr_pitch_encode(double value)
{
return (uint16_t)(value / 0.006);
}
double can1_vn200_ypr_pitch_decode(uint16_t value)
{
return ((double)value * 0.006);
}
bool can1_vn200_ypr_pitch_is_in_range(uint16_t value)
{
(void)value;
return (true);
}
uint16_t can1_vn200_ypr_roll_encode(double value)
{
return (uint16_t)(value / 0.006);
}
double can1_vn200_ypr_roll_decode(uint16_t value)
{
return ((double)value * 0.006);
}
bool can1_vn200_ypr_roll_is_in_range(uint16_t value)
{
(void)value;
return (true);
}

20
vn200/Converter/Src/canhalal.cpp Normal file
View File

@ -0,0 +1,20 @@
#include "canhalal.h"
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;
}
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);
}