852 lines
21 KiB
C++
852 lines
21 KiB
C++
/**
|
|
* 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 11 14:48:42 2024.
|
|
*/
|
|
|
|
#include <string.h>
|
|
|
|
#include "can1.h"
|
|
|
|
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_left_shift_u32(
|
|
uint32_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 uint8_t pack_right_shift_u32(
|
|
uint32_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 uint32_t unpack_left_shift_u32(
|
|
uint8_t value,
|
|
uint8_t shift,
|
|
uint8_t mask)
|
|
{
|
|
return (uint32_t)((uint32_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);
|
|
}
|
|
|
|
static inline uint32_t unpack_right_shift_u32(
|
|
uint8_t value,
|
|
uint8_t shift,
|
|
uint8_t mask)
|
|
{
|
|
return (uint32_t)((uint32_t)(value & mask) >> shift);
|
|
}
|
|
|
|
int can1_vn200_gnss_ll_pack(
|
|
uint8_t *dst_p,
|
|
const struct can1_vn200_gnss_ll_t *src_p,
|
|
size_t size)
|
|
{
|
|
uint16_t latitude;
|
|
uint16_t longitude;
|
|
|
|
if (size < 8u) {
|
|
return (-EINVAL);
|
|
}
|
|
|
|
memset(&dst_p[0], 0, 8);
|
|
|
|
latitude = (uint16_t)src_p->latitude;
|
|
dst_p[0] |= pack_left_shift_u16(latitude, 0u, 0xffu);
|
|
dst_p[1] |= pack_right_shift_u16(latitude, 8u, 0xffu);
|
|
longitude = (uint16_t)src_p->longitude;
|
|
dst_p[2] |= pack_left_shift_u16(longitude, 0u, 0xffu);
|
|
dst_p[3] |= pack_right_shift_u16(longitude, 8u, 0xffu);
|
|
dst_p[4] |= pack_left_shift_u16(src_p->uncertainty_n, 0u, 0xffu);
|
|
dst_p[5] |= pack_right_shift_u16(src_p->uncertainty_n, 8u, 0xffu);
|
|
dst_p[6] |= pack_left_shift_u16(src_p->uncertainty_e, 0u, 0xffu);
|
|
dst_p[7] |= pack_right_shift_u16(src_p->uncertainty_e, 8u, 0xffu);
|
|
|
|
return (8);
|
|
}
|
|
|
|
int can1_vn200_gnss_ll_unpack(
|
|
struct can1_vn200_gnss_ll_t *dst_p,
|
|
const uint8_t *src_p,
|
|
size_t size)
|
|
{
|
|
uint16_t latitude;
|
|
uint16_t longitude;
|
|
|
|
if (size < 8u) {
|
|
return (-EINVAL);
|
|
}
|
|
|
|
latitude = unpack_right_shift_u16(src_p[0], 0u, 0xffu);
|
|
latitude |= unpack_left_shift_u16(src_p[1], 8u, 0xffu);
|
|
dst_p->latitude = (int16_t)latitude;
|
|
longitude = unpack_right_shift_u16(src_p[2], 0u, 0xffu);
|
|
longitude |= unpack_left_shift_u16(src_p[3], 8u, 0xffu);
|
|
dst_p->longitude = (int16_t)longitude;
|
|
dst_p->uncertainty_n = unpack_right_shift_u16(src_p[4], 0u, 0xffu);
|
|
dst_p->uncertainty_n |= unpack_left_shift_u16(src_p[5], 8u, 0xffu);
|
|
dst_p->uncertainty_e = unpack_right_shift_u16(src_p[6], 0u, 0xffu);
|
|
dst_p->uncertainty_e |= unpack_left_shift_u16(src_p[7], 8u, 0xffu);
|
|
|
|
return (0);
|
|
}
|
|
|
|
int can1_vn200_gnss_ll_init(struct can1_vn200_gnss_ll_t *msg_p)
|
|
{
|
|
if (msg_p == NULL) return -1;
|
|
|
|
memset(msg_p, 0, sizeof(struct can1_vn200_gnss_ll_t));
|
|
|
|
return 0;
|
|
}
|
|
|
|
int16_t can1_vn200_gnss_ll_latitude_encode(double value)
|
|
{
|
|
return (int16_t)(value / 0.005);
|
|
}
|
|
|
|
double can1_vn200_gnss_ll_latitude_decode(int16_t value)
|
|
{
|
|
return ((double)value * 0.005);
|
|
}
|
|
|
|
bool can1_vn200_gnss_ll_latitude_is_in_range(int16_t value)
|
|
{
|
|
return ((value >= -18000) && (value <= 18000));
|
|
}
|
|
|
|
int16_t can1_vn200_gnss_ll_longitude_encode(double value)
|
|
{
|
|
return (int16_t)(value / 0.05);
|
|
}
|
|
|
|
double can1_vn200_gnss_ll_longitude_decode(int16_t value)
|
|
{
|
|
return ((double)value * 0.05);
|
|
}
|
|
|
|
bool can1_vn200_gnss_ll_longitude_is_in_range(int16_t value)
|
|
{
|
|
return ((value >= -3600) && (value <= 3600));
|
|
}
|
|
|
|
uint16_t can1_vn200_gnss_ll_uncertainty_n_encode(double value)
|
|
{
|
|
return (uint16_t)(value / 0.001);
|
|
}
|
|
|
|
double can1_vn200_gnss_ll_uncertainty_n_decode(uint16_t value)
|
|
{
|
|
return ((double)value * 0.001);
|
|
}
|
|
|
|
bool can1_vn200_gnss_ll_uncertainty_n_is_in_range(uint16_t value)
|
|
{
|
|
return (value <= 65000u);
|
|
}
|
|
|
|
uint16_t can1_vn200_gnss_ll_uncertainty_e_encode(double value)
|
|
{
|
|
return (uint16_t)(value / 0.001);
|
|
}
|
|
|
|
double can1_vn200_gnss_ll_uncertainty_e_decode(uint16_t value)
|
|
{
|
|
return ((double)value * 0.001);
|
|
}
|
|
|
|
bool can1_vn200_gnss_ll_uncertainty_e_is_in_range(uint16_t value)
|
|
{
|
|
return (value <= 65000u);
|
|
}
|
|
|
|
int can1_vn200_ins_ypr_pack(
|
|
uint8_t *dst_p,
|
|
const struct can1_vn200_ins_ypr_t *src_p,
|
|
size_t size)
|
|
{
|
|
uint16_t pitch;
|
|
uint16_t roll;
|
|
uint16_t yaw;
|
|
|
|
if (size < 8u) {
|
|
return (-EINVAL);
|
|
}
|
|
|
|
memset(&dst_p[0], 0, 8);
|
|
|
|
yaw = (uint16_t)src_p->yaw;
|
|
dst_p[0] |= pack_left_shift_u16(yaw, 0u, 0xffu);
|
|
dst_p[1] |= pack_right_shift_u16(yaw, 8u, 0xffu);
|
|
pitch = (uint16_t)src_p->pitch;
|
|
dst_p[2] |= pack_left_shift_u16(pitch, 0u, 0xffu);
|
|
dst_p[3] |= pack_right_shift_u16(pitch, 8u, 0xffu);
|
|
roll = (uint16_t)src_p->roll;
|
|
dst_p[4] |= pack_left_shift_u16(roll, 0u, 0xffu);
|
|
dst_p[5] |= pack_right_shift_u16(roll, 8u, 0xffu);
|
|
dst_p[6] |= pack_left_shift_u16(src_p->uncertainty, 0u, 0xffu);
|
|
dst_p[7] |= pack_right_shift_u16(src_p->uncertainty, 8u, 0xffu);
|
|
|
|
return (8);
|
|
}
|
|
|
|
int can1_vn200_ins_ypr_unpack(
|
|
struct can1_vn200_ins_ypr_t *dst_p,
|
|
const uint8_t *src_p,
|
|
size_t size)
|
|
{
|
|
uint16_t pitch;
|
|
uint16_t roll;
|
|
uint16_t yaw;
|
|
|
|
if (size < 8u) {
|
|
return (-EINVAL);
|
|
}
|
|
|
|
yaw = unpack_right_shift_u16(src_p[0], 0u, 0xffu);
|
|
yaw |= unpack_left_shift_u16(src_p[1], 8u, 0xffu);
|
|
dst_p->yaw = (int16_t)yaw;
|
|
pitch = unpack_right_shift_u16(src_p[2], 0u, 0xffu);
|
|
pitch |= unpack_left_shift_u16(src_p[3], 8u, 0xffu);
|
|
dst_p->pitch = (int16_t)pitch;
|
|
roll = unpack_right_shift_u16(src_p[4], 0u, 0xffu);
|
|
roll |= unpack_left_shift_u16(src_p[5], 8u, 0xffu);
|
|
dst_p->roll = (int16_t)roll;
|
|
dst_p->uncertainty = unpack_right_shift_u16(src_p[6], 0u, 0xffu);
|
|
dst_p->uncertainty |= unpack_left_shift_u16(src_p[7], 8u, 0xffu);
|
|
|
|
return (0);
|
|
}
|
|
|
|
int can1_vn200_ins_ypr_init(struct can1_vn200_ins_ypr_t *msg_p)
|
|
{
|
|
if (msg_p == NULL) return -1;
|
|
|
|
memset(msg_p, 0, sizeof(struct can1_vn200_ins_ypr_t));
|
|
|
|
return 0;
|
|
}
|
|
|
|
int16_t can1_vn200_ins_ypr_yaw_encode(double value)
|
|
{
|
|
return (int16_t)(value / 0.001);
|
|
}
|
|
|
|
double can1_vn200_ins_ypr_yaw_decode(int16_t value)
|
|
{
|
|
return ((double)value * 0.001);
|
|
}
|
|
|
|
bool can1_vn200_ins_ypr_yaw_is_in_range(int16_t value)
|
|
{
|
|
(void)value;
|
|
|
|
return (true);
|
|
}
|
|
|
|
int16_t can1_vn200_ins_ypr_pitch_encode(double value)
|
|
{
|
|
return (int16_t)(value / 0.001);
|
|
}
|
|
|
|
double can1_vn200_ins_ypr_pitch_decode(int16_t value)
|
|
{
|
|
return ((double)value * 0.001);
|
|
}
|
|
|
|
bool can1_vn200_ins_ypr_pitch_is_in_range(int16_t value)
|
|
{
|
|
(void)value;
|
|
|
|
return (true);
|
|
}
|
|
|
|
int16_t can1_vn200_ins_ypr_roll_encode(double value)
|
|
{
|
|
return (int16_t)(value / 0.001);
|
|
}
|
|
|
|
double can1_vn200_ins_ypr_roll_decode(int16_t value)
|
|
{
|
|
return ((double)value * 0.001);
|
|
}
|
|
|
|
bool can1_vn200_ins_ypr_roll_is_in_range(int16_t value)
|
|
{
|
|
(void)value;
|
|
|
|
return (true);
|
|
}
|
|
|
|
uint16_t can1_vn200_ins_ypr_uncertainty_encode(double value)
|
|
{
|
|
return (uint16_t)(value / 0.0001);
|
|
}
|
|
|
|
double can1_vn200_ins_ypr_uncertainty_decode(uint16_t value)
|
|
{
|
|
return ((double)value * 0.0001);
|
|
}
|
|
|
|
bool can1_vn200_ins_ypr_uncertainty_is_in_range(uint16_t value)
|
|
{
|
|
return (value <= 65000u);
|
|
}
|
|
|
|
int can1_vn200_imu_acc_lin_pack(
|
|
uint8_t *dst_p,
|
|
const struct can1_vn200_imu_acc_lin_t *src_p,
|
|
size_t size)
|
|
{
|
|
uint32_t acc_lin_x;
|
|
uint32_t acc_lin_y;
|
|
uint32_t acc_lin_z;
|
|
|
|
if (size < 8u) {
|
|
return (-EINVAL);
|
|
}
|
|
|
|
memset(&dst_p[0], 0, 8);
|
|
|
|
acc_lin_x = (uint32_t)src_p->acc_lin_x;
|
|
dst_p[0] |= pack_left_shift_u32(acc_lin_x, 0u, 0xffu);
|
|
dst_p[1] |= pack_right_shift_u32(acc_lin_x, 8u, 0xffu);
|
|
dst_p[2] |= pack_right_shift_u32(acc_lin_x, 16u, 0x0fu);
|
|
acc_lin_y = (uint32_t)src_p->acc_lin_y;
|
|
dst_p[2] |= pack_left_shift_u32(acc_lin_y, 4u, 0xf0u);
|
|
dst_p[3] |= pack_right_shift_u32(acc_lin_y, 4u, 0xffu);
|
|
dst_p[4] |= pack_right_shift_u32(acc_lin_y, 12u, 0xffu);
|
|
acc_lin_z = (uint32_t)src_p->acc_lin_z;
|
|
dst_p[5] |= pack_left_shift_u32(acc_lin_z, 0u, 0xffu);
|
|
dst_p[6] |= pack_right_shift_u32(acc_lin_z, 8u, 0xffu);
|
|
dst_p[7] |= pack_right_shift_u32(acc_lin_z, 16u, 0x0fu);
|
|
|
|
return (8);
|
|
}
|
|
|
|
int can1_vn200_imu_acc_lin_unpack(
|
|
struct can1_vn200_imu_acc_lin_t *dst_p,
|
|
const uint8_t *src_p,
|
|
size_t size)
|
|
{
|
|
uint32_t acc_lin_x;
|
|
uint32_t acc_lin_y;
|
|
uint32_t acc_lin_z;
|
|
|
|
if (size < 8u) {
|
|
return (-EINVAL);
|
|
}
|
|
|
|
acc_lin_x = unpack_right_shift_u32(src_p[0], 0u, 0xffu);
|
|
acc_lin_x |= unpack_left_shift_u32(src_p[1], 8u, 0xffu);
|
|
acc_lin_x |= unpack_left_shift_u32(src_p[2], 16u, 0x0fu);
|
|
|
|
if ((acc_lin_x & (1u << 19)) != 0u) {
|
|
acc_lin_x |= 0xfff00000u;
|
|
}
|
|
|
|
dst_p->acc_lin_x = (int32_t)acc_lin_x;
|
|
acc_lin_y = unpack_right_shift_u32(src_p[2], 4u, 0xf0u);
|
|
acc_lin_y |= unpack_left_shift_u32(src_p[3], 4u, 0xffu);
|
|
acc_lin_y |= unpack_left_shift_u32(src_p[4], 12u, 0xffu);
|
|
|
|
if ((acc_lin_y & (1u << 19)) != 0u) {
|
|
acc_lin_y |= 0xfff00000u;
|
|
}
|
|
|
|
dst_p->acc_lin_y = (int32_t)acc_lin_y;
|
|
acc_lin_z = unpack_right_shift_u32(src_p[5], 0u, 0xffu);
|
|
acc_lin_z |= unpack_left_shift_u32(src_p[6], 8u, 0xffu);
|
|
acc_lin_z |= unpack_left_shift_u32(src_p[7], 16u, 0x0fu);
|
|
|
|
if ((acc_lin_z & (1u << 19)) != 0u) {
|
|
acc_lin_z |= 0xfff00000u;
|
|
}
|
|
|
|
dst_p->acc_lin_z = (int32_t)acc_lin_z;
|
|
|
|
return (0);
|
|
}
|
|
|
|
int can1_vn200_imu_acc_lin_init(struct can1_vn200_imu_acc_lin_t *msg_p)
|
|
{
|
|
if (msg_p == NULL) return -1;
|
|
|
|
memset(msg_p, 0, sizeof(struct can1_vn200_imu_acc_lin_t));
|
|
|
|
return 0;
|
|
}
|
|
|
|
int32_t can1_vn200_imu_acc_lin_acc_lin_x_encode(double value)
|
|
{
|
|
return (int32_t)(value / 0.0001);
|
|
}
|
|
|
|
double can1_vn200_imu_acc_lin_acc_lin_x_decode(int32_t value)
|
|
{
|
|
return ((double)value * 0.0001);
|
|
}
|
|
|
|
bool can1_vn200_imu_acc_lin_acc_lin_x_is_in_range(int32_t value)
|
|
{
|
|
return ((value >= -540000) && (value <= 540000));
|
|
}
|
|
|
|
int32_t can1_vn200_imu_acc_lin_acc_lin_y_encode(double value)
|
|
{
|
|
return (int32_t)(value / 0.0001);
|
|
}
|
|
|
|
double can1_vn200_imu_acc_lin_acc_lin_y_decode(int32_t value)
|
|
{
|
|
return ((double)value * 0.0001);
|
|
}
|
|
|
|
bool can1_vn200_imu_acc_lin_acc_lin_y_is_in_range(int32_t value)
|
|
{
|
|
return ((value >= -540000) && (value <= 540000));
|
|
}
|
|
|
|
int32_t can1_vn200_imu_acc_lin_acc_lin_z_encode(double value)
|
|
{
|
|
return (int32_t)(value / 0.0001);
|
|
}
|
|
|
|
double can1_vn200_imu_acc_lin_acc_lin_z_decode(int32_t value)
|
|
{
|
|
return ((double)value * 0.0001);
|
|
}
|
|
|
|
bool can1_vn200_imu_acc_lin_acc_lin_z_is_in_range(int32_t value)
|
|
{
|
|
return ((value >= -540000) && (value <= 540000));
|
|
}
|
|
|
|
int can1_vn200_imu_acc_ang_pack(
|
|
uint8_t *dst_p,
|
|
const struct can1_vn200_imu_acc_ang_t *src_p,
|
|
size_t size)
|
|
{
|
|
uint32_t acc_ang_x;
|
|
uint32_t acc_ang_y;
|
|
uint32_t acc_ang_z;
|
|
|
|
if (size < 8u) {
|
|
return (-EINVAL);
|
|
}
|
|
|
|
memset(&dst_p[0], 0, 8);
|
|
|
|
acc_ang_x = (uint32_t)src_p->acc_ang_x;
|
|
dst_p[0] |= pack_left_shift_u32(acc_ang_x, 0u, 0xffu);
|
|
dst_p[1] |= pack_right_shift_u32(acc_ang_x, 8u, 0xffu);
|
|
dst_p[2] |= pack_right_shift_u32(acc_ang_x, 16u, 0x0fu);
|
|
acc_ang_y = (uint32_t)src_p->acc_ang_y;
|
|
dst_p[2] |= pack_left_shift_u32(acc_ang_y, 4u, 0xf0u);
|
|
dst_p[3] |= pack_right_shift_u32(acc_ang_y, 4u, 0xffu);
|
|
dst_p[4] |= pack_right_shift_u32(acc_ang_y, 12u, 0xffu);
|
|
acc_ang_z = (uint32_t)src_p->acc_ang_z;
|
|
dst_p[5] |= pack_left_shift_u32(acc_ang_z, 0u, 0xffu);
|
|
dst_p[6] |= pack_right_shift_u32(acc_ang_z, 8u, 0xffu);
|
|
dst_p[7] |= pack_right_shift_u32(acc_ang_z, 16u, 0x0fu);
|
|
|
|
return (8);
|
|
}
|
|
|
|
int can1_vn200_imu_acc_ang_unpack(
|
|
struct can1_vn200_imu_acc_ang_t *dst_p,
|
|
const uint8_t *src_p,
|
|
size_t size)
|
|
{
|
|
uint32_t acc_ang_x;
|
|
uint32_t acc_ang_y;
|
|
uint32_t acc_ang_z;
|
|
|
|
if (size < 8u) {
|
|
return (-EINVAL);
|
|
}
|
|
|
|
acc_ang_x = unpack_right_shift_u32(src_p[0], 0u, 0xffu);
|
|
acc_ang_x |= unpack_left_shift_u32(src_p[1], 8u, 0xffu);
|
|
acc_ang_x |= unpack_left_shift_u32(src_p[2], 16u, 0x0fu);
|
|
|
|
if ((acc_ang_x & (1u << 19)) != 0u) {
|
|
acc_ang_x |= 0xfff00000u;
|
|
}
|
|
|
|
dst_p->acc_ang_x = (int32_t)acc_ang_x;
|
|
acc_ang_y = unpack_right_shift_u32(src_p[2], 4u, 0xf0u);
|
|
acc_ang_y |= unpack_left_shift_u32(src_p[3], 4u, 0xffu);
|
|
acc_ang_y |= unpack_left_shift_u32(src_p[4], 12u, 0xffu);
|
|
|
|
if ((acc_ang_y & (1u << 19)) != 0u) {
|
|
acc_ang_y |= 0xfff00000u;
|
|
}
|
|
|
|
dst_p->acc_ang_y = (int32_t)acc_ang_y;
|
|
acc_ang_z = unpack_right_shift_u32(src_p[5], 0u, 0xffu);
|
|
acc_ang_z |= unpack_left_shift_u32(src_p[6], 8u, 0xffu);
|
|
acc_ang_z |= unpack_left_shift_u32(src_p[7], 16u, 0x0fu);
|
|
|
|
if ((acc_ang_z & (1u << 19)) != 0u) {
|
|
acc_ang_z |= 0xfff00000u;
|
|
}
|
|
|
|
dst_p->acc_ang_z = (int32_t)acc_ang_z;
|
|
|
|
return (0);
|
|
}
|
|
|
|
int can1_vn200_imu_acc_ang_init(struct can1_vn200_imu_acc_ang_t *msg_p)
|
|
{
|
|
if (msg_p == NULL) return -1;
|
|
|
|
memset(msg_p, 0, sizeof(struct can1_vn200_imu_acc_ang_t));
|
|
|
|
return 0;
|
|
}
|
|
|
|
int32_t can1_vn200_imu_acc_ang_acc_ang_x_encode(double value)
|
|
{
|
|
return (int32_t)(value / 0.0001);
|
|
}
|
|
|
|
double can1_vn200_imu_acc_ang_acc_ang_x_decode(int32_t value)
|
|
{
|
|
return ((double)value * 0.0001);
|
|
}
|
|
|
|
bool can1_vn200_imu_acc_ang_acc_ang_x_is_in_range(int32_t value)
|
|
{
|
|
return ((value >= -540000) && (value <= 540000));
|
|
}
|
|
|
|
int32_t can1_vn200_imu_acc_ang_acc_ang_y_encode(double value)
|
|
{
|
|
return (int32_t)(value / 0.0001);
|
|
}
|
|
|
|
double can1_vn200_imu_acc_ang_acc_ang_y_decode(int32_t value)
|
|
{
|
|
return ((double)value * 0.0001);
|
|
}
|
|
|
|
bool can1_vn200_imu_acc_ang_acc_ang_y_is_in_range(int32_t value)
|
|
{
|
|
return ((value >= -540000) && (value <= 540000));
|
|
}
|
|
|
|
int32_t can1_vn200_imu_acc_ang_acc_ang_z_encode(double value)
|
|
{
|
|
return (int32_t)(value / 0.0001);
|
|
}
|
|
|
|
double can1_vn200_imu_acc_ang_acc_ang_z_decode(int32_t value)
|
|
{
|
|
return ((double)value * 0.0001);
|
|
}
|
|
|
|
bool can1_vn200_imu_acc_ang_acc_ang_z_is_in_range(int32_t value)
|
|
{
|
|
return ((value >= -540000) && (value <= 540000));
|
|
}
|
|
|
|
int can1_vn200_ins_ll_pack(
|
|
uint8_t *dst_p,
|
|
const struct can1_vn200_ins_ll_t *src_p,
|
|
size_t size)
|
|
{
|
|
uint16_t latitude;
|
|
uint16_t longitude;
|
|
|
|
if (size < 8u) {
|
|
return (-EINVAL);
|
|
}
|
|
|
|
memset(&dst_p[0], 0, 8);
|
|
|
|
latitude = (uint16_t)src_p->latitude;
|
|
dst_p[0] |= pack_left_shift_u16(latitude, 0u, 0xffu);
|
|
dst_p[1] |= pack_right_shift_u16(latitude, 8u, 0xffu);
|
|
longitude = (uint16_t)src_p->longitude;
|
|
dst_p[2] |= pack_left_shift_u16(longitude, 0u, 0xffu);
|
|
dst_p[3] |= pack_right_shift_u16(longitude, 8u, 0xffu);
|
|
dst_p[4] |= pack_left_shift_u16(src_p->uncertainty_n, 0u, 0xffu);
|
|
dst_p[5] |= pack_right_shift_u16(src_p->uncertainty_n, 8u, 0xffu);
|
|
dst_p[6] |= pack_left_shift_u16(src_p->uncertainty_e, 0u, 0xffu);
|
|
dst_p[7] |= pack_right_shift_u16(src_p->uncertainty_e, 8u, 0xffu);
|
|
|
|
return (8);
|
|
}
|
|
|
|
int can1_vn200_ins_ll_unpack(
|
|
struct can1_vn200_ins_ll_t *dst_p,
|
|
const uint8_t *src_p,
|
|
size_t size)
|
|
{
|
|
uint16_t latitude;
|
|
uint16_t longitude;
|
|
|
|
if (size < 8u) {
|
|
return (-EINVAL);
|
|
}
|
|
|
|
latitude = unpack_right_shift_u16(src_p[0], 0u, 0xffu);
|
|
latitude |= unpack_left_shift_u16(src_p[1], 8u, 0xffu);
|
|
dst_p->latitude = (int16_t)latitude;
|
|
longitude = unpack_right_shift_u16(src_p[2], 0u, 0xffu);
|
|
longitude |= unpack_left_shift_u16(src_p[3], 8u, 0xffu);
|
|
dst_p->longitude = (int16_t)longitude;
|
|
dst_p->uncertainty_n = unpack_right_shift_u16(src_p[4], 0u, 0xffu);
|
|
dst_p->uncertainty_n |= unpack_left_shift_u16(src_p[5], 8u, 0xffu);
|
|
dst_p->uncertainty_e = unpack_right_shift_u16(src_p[6], 0u, 0xffu);
|
|
dst_p->uncertainty_e |= unpack_left_shift_u16(src_p[7], 8u, 0xffu);
|
|
|
|
return (0);
|
|
}
|
|
|
|
int can1_vn200_ins_ll_init(struct can1_vn200_ins_ll_t *msg_p)
|
|
{
|
|
if (msg_p == NULL) return -1;
|
|
|
|
memset(msg_p, 0, sizeof(struct can1_vn200_ins_ll_t));
|
|
|
|
return 0;
|
|
}
|
|
|
|
int16_t can1_vn200_ins_ll_latitude_encode(double value)
|
|
{
|
|
return (int16_t)(value / 0.001);
|
|
}
|
|
|
|
double can1_vn200_ins_ll_latitude_decode(int16_t value)
|
|
{
|
|
return ((double)value * 0.001);
|
|
}
|
|
|
|
bool can1_vn200_ins_ll_latitude_is_in_range(int16_t value)
|
|
{
|
|
(void)value;
|
|
|
|
return (true);
|
|
}
|
|
|
|
int16_t can1_vn200_ins_ll_longitude_encode(double value)
|
|
{
|
|
return (int16_t)(value / 0.001);
|
|
}
|
|
|
|
double can1_vn200_ins_ll_longitude_decode(int16_t value)
|
|
{
|
|
return ((double)value * 0.001);
|
|
}
|
|
|
|
bool can1_vn200_ins_ll_longitude_is_in_range(int16_t value)
|
|
{
|
|
(void)value;
|
|
|
|
return (true);
|
|
}
|
|
|
|
uint16_t can1_vn200_ins_ll_uncertainty_n_encode(double value)
|
|
{
|
|
return (uint16_t)(value / 0.0001);
|
|
}
|
|
|
|
double can1_vn200_ins_ll_uncertainty_n_decode(uint16_t value)
|
|
{
|
|
return ((double)value * 0.0001);
|
|
}
|
|
|
|
bool can1_vn200_ins_ll_uncertainty_n_is_in_range(uint16_t value)
|
|
{
|
|
return (value <= 65000u);
|
|
}
|
|
|
|
uint16_t can1_vn200_ins_ll_uncertainty_e_encode(double value)
|
|
{
|
|
return (uint16_t)(value / 0.0001);
|
|
}
|
|
|
|
double can1_vn200_ins_ll_uncertainty_e_decode(uint16_t value)
|
|
{
|
|
return ((double)value * 0.0001);
|
|
}
|
|
|
|
bool can1_vn200_ins_ll_uncertainty_e_is_in_range(uint16_t value)
|
|
{
|
|
return (value <= 65000u);
|
|
}
|
|
|
|
int can1_vn200_ins_vel_pack(
|
|
uint8_t *dst_p,
|
|
const struct can1_vn200_ins_vel_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->vel_lin_x, 0u, 0xffu);
|
|
dst_p[1] |= pack_right_shift_u16(src_p->vel_lin_x, 8u, 0xffu);
|
|
dst_p[2] |= pack_left_shift_u16(src_p->vel_lin_y, 0u, 0xffu);
|
|
dst_p[3] |= pack_right_shift_u16(src_p->vel_lin_y, 8u, 0xffu);
|
|
dst_p[4] |= pack_left_shift_u16(src_p->vel_lin_z, 0u, 0xffu);
|
|
dst_p[5] |= pack_right_shift_u16(src_p->vel_lin_z, 8u, 0xffu);
|
|
dst_p[6] |= pack_left_shift_u16(src_p->uncertainty, 0u, 0xffu);
|
|
dst_p[7] |= pack_right_shift_u16(src_p->uncertainty, 8u, 0xffu);
|
|
|
|
return (8);
|
|
}
|
|
|
|
int can1_vn200_ins_vel_unpack(
|
|
struct can1_vn200_ins_vel_t *dst_p,
|
|
const uint8_t *src_p,
|
|
size_t size)
|
|
{
|
|
if (size < 8u) {
|
|
return (-EINVAL);
|
|
}
|
|
|
|
dst_p->vel_lin_x = unpack_right_shift_u16(src_p[0], 0u, 0xffu);
|
|
dst_p->vel_lin_x |= unpack_left_shift_u16(src_p[1], 8u, 0xffu);
|
|
dst_p->vel_lin_y = unpack_right_shift_u16(src_p[2], 0u, 0xffu);
|
|
dst_p->vel_lin_y |= unpack_left_shift_u16(src_p[3], 8u, 0xffu);
|
|
dst_p->vel_lin_z = unpack_right_shift_u16(src_p[4], 0u, 0xffu);
|
|
dst_p->vel_lin_z |= unpack_left_shift_u16(src_p[5], 8u, 0xffu);
|
|
dst_p->uncertainty = unpack_right_shift_u16(src_p[6], 0u, 0xffu);
|
|
dst_p->uncertainty |= unpack_left_shift_u16(src_p[7], 8u, 0xffu);
|
|
|
|
return (0);
|
|
}
|
|
|
|
int can1_vn200_ins_vel_init(struct can1_vn200_ins_vel_t *msg_p)
|
|
{
|
|
if (msg_p == NULL) return -1;
|
|
|
|
memset(msg_p, 0, sizeof(struct can1_vn200_ins_vel_t));
|
|
|
|
return 0;
|
|
}
|
|
|
|
uint16_t can1_vn200_ins_vel_vel_lin_x_encode(double value)
|
|
{
|
|
return (uint16_t)(value / 0.001);
|
|
}
|
|
|
|
double can1_vn200_ins_vel_vel_lin_x_decode(uint16_t value)
|
|
{
|
|
return ((double)value * 0.001);
|
|
}
|
|
|
|
bool can1_vn200_ins_vel_vel_lin_x_is_in_range(uint16_t value)
|
|
{
|
|
return (value <= 65000u);
|
|
}
|
|
|
|
uint16_t can1_vn200_ins_vel_vel_lin_y_encode(double value)
|
|
{
|
|
return (uint16_t)(value / 0.001);
|
|
}
|
|
|
|
double can1_vn200_ins_vel_vel_lin_y_decode(uint16_t value)
|
|
{
|
|
return ((double)value * 0.001);
|
|
}
|
|
|
|
bool can1_vn200_ins_vel_vel_lin_y_is_in_range(uint16_t value)
|
|
{
|
|
return (value <= 65000u);
|
|
}
|
|
|
|
uint16_t can1_vn200_ins_vel_vel_lin_z_encode(double value)
|
|
{
|
|
return (uint16_t)(value / 0.001);
|
|
}
|
|
|
|
double can1_vn200_ins_vel_vel_lin_z_decode(uint16_t value)
|
|
{
|
|
return ((double)value * 0.001);
|
|
}
|
|
|
|
bool can1_vn200_ins_vel_vel_lin_z_is_in_range(uint16_t value)
|
|
{
|
|
return (value <= 65000u);
|
|
}
|
|
|
|
uint16_t can1_vn200_ins_vel_uncertainty_encode(double value)
|
|
{
|
|
return (uint16_t)(value / 0.0001);
|
|
}
|
|
|
|
double can1_vn200_ins_vel_uncertainty_decode(uint16_t value)
|
|
{
|
|
return ((double)value * 0.0001);
|
|
}
|
|
|
|
bool can1_vn200_ins_vel_uncertainty_is_in_range(uint16_t value)
|
|
{
|
|
return (value <= 65000u);
|
|
}
|