INS-VN-200/vn200/Converter/Src/can1.cpp

/**
 * 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);
}