162 lines
4.1 KiB
C++
162 lines
4.1 KiB
C++
#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_
|
|
|