update intermediate solution
This commit is contained in:
r.koeppe
@ -70,23 +70,24 @@ HAL_StatusTypeDef spi2can(SPI_HandleTypeDef *hspi, CAN_HandleTypeDef *hcan, cons
|
||||
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;
|
||||
case vn::InsSolutionLlaRegisterID:
|
||||
{
|
||||
can_id = CAN1_VN200_INS_YPR_FRAME_ID;
|
||||
vn::InsSolutionLlaRegister 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));
|
||||
}
|
||||
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;
|
||||
}
|
||||
ftcan_transmit(hcan, can_id, data, datalen);
|
||||
|
||||
return HAL_OK;
|
||||
}
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -9,71 +9,139 @@ namespace canlib {
|
||||
namespace frame {
|
||||
namespace decoded {
|
||||
namespace can1 {
|
||||
struct vn200_ins_t {
|
||||
double yaw;
|
||||
vn200_ins_t() {
|
||||
this->yaw = 0.0;
|
||||
struct vn200_gnss_ll_t {
|
||||
double latitude;
|
||||
double longitude;
|
||||
double uncertainty_n;
|
||||
double uncertainty_e;
|
||||
vn200_gnss_ll_t() {
|
||||
this->latitude = 0.0;
|
||||
this->longitude = 0.0;
|
||||
this->uncertainty_n = 0.0;
|
||||
this->uncertainty_e = 0.0;
|
||||
}
|
||||
|
||||
vn200_ins_t(
|
||||
double yaw) {
|
||||
this->yaw = yaw;
|
||||
vn200_gnss_ll_t(
|
||||
double latitude,
|
||||
double longitude,
|
||||
double uncertainty_n,
|
||||
double uncertainty_e) {
|
||||
this->latitude = latitude;
|
||||
this->longitude = longitude;
|
||||
this->uncertainty_n = uncertainty_n;
|
||||
this->uncertainty_e = uncertainty_e;
|
||||
}
|
||||
|
||||
};
|
||||
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 {
|
||||
struct vn200_ins_ypr_t {
|
||||
double yaw;
|
||||
double pitch;
|
||||
double roll;
|
||||
vn200_ypr_t() {
|
||||
double uncertainty;
|
||||
vn200_ins_ypr_t() {
|
||||
this->yaw = 0.0;
|
||||
this->pitch = 0.0;
|
||||
this->roll = 0.0;
|
||||
this->uncertainty = 0.0;
|
||||
}
|
||||
|
||||
vn200_ypr_t(
|
||||
vn200_ins_ypr_t(
|
||||
double yaw,
|
||||
double pitch,
|
||||
double roll) {
|
||||
double roll,
|
||||
double uncertainty) {
|
||||
this->yaw = yaw;
|
||||
this->pitch = pitch;
|
||||
this->roll = roll;
|
||||
this->uncertainty = uncertainty;
|
||||
}
|
||||
|
||||
};
|
||||
struct vn200_imu_acc_lin_t {
|
||||
double acc_lin_x;
|
||||
double acc_lin_y;
|
||||
double acc_lin_z;
|
||||
vn200_imu_acc_lin_t() {
|
||||
this->acc_lin_x = 0.0;
|
||||
this->acc_lin_y = 0.0;
|
||||
this->acc_lin_z = 0.0;
|
||||
}
|
||||
|
||||
vn200_imu_acc_lin_t(
|
||||
double acc_lin_x,
|
||||
double acc_lin_y,
|
||||
double acc_lin_z) {
|
||||
this->acc_lin_x = acc_lin_x;
|
||||
this->acc_lin_y = acc_lin_y;
|
||||
this->acc_lin_z = acc_lin_z;
|
||||
}
|
||||
|
||||
};
|
||||
struct vn200_imu_acc_ang_t {
|
||||
double acc_ang_x;
|
||||
double acc_ang_y;
|
||||
double acc_ang_z;
|
||||
vn200_imu_acc_ang_t() {
|
||||
this->acc_ang_x = 0.0;
|
||||
this->acc_ang_y = 0.0;
|
||||
this->acc_ang_z = 0.0;
|
||||
}
|
||||
|
||||
vn200_imu_acc_ang_t(
|
||||
double acc_ang_x,
|
||||
double acc_ang_y,
|
||||
double acc_ang_z) {
|
||||
this->acc_ang_x = acc_ang_x;
|
||||
this->acc_ang_y = acc_ang_y;
|
||||
this->acc_ang_z = acc_ang_z;
|
||||
}
|
||||
|
||||
};
|
||||
struct vn200_ins_ll_t {
|
||||
double latitude;
|
||||
double longitude;
|
||||
double uncertainty_n;
|
||||
double uncertainty_e;
|
||||
vn200_ins_ll_t() {
|
||||
this->latitude = 0.0;
|
||||
this->longitude = 0.0;
|
||||
this->uncertainty_n = 0.0;
|
||||
this->uncertainty_e = 0.0;
|
||||
}
|
||||
|
||||
vn200_ins_ll_t(
|
||||
double latitude,
|
||||
double longitude,
|
||||
double uncertainty_n,
|
||||
double uncertainty_e) {
|
||||
this->latitude = latitude;
|
||||
this->longitude = longitude;
|
||||
this->uncertainty_n = uncertainty_n;
|
||||
this->uncertainty_e = uncertainty_e;
|
||||
}
|
||||
|
||||
};
|
||||
struct vn200_ins_vel_t {
|
||||
double vel_lin_x;
|
||||
double vel_lin_y;
|
||||
double vel_lin_z;
|
||||
double uncertainty;
|
||||
vn200_ins_vel_t() {
|
||||
this->vel_lin_x = 0.0;
|
||||
this->vel_lin_y = 0.0;
|
||||
this->vel_lin_z = 0.0;
|
||||
this->uncertainty = 0.0;
|
||||
}
|
||||
|
||||
vn200_ins_vel_t(
|
||||
double vel_lin_x,
|
||||
double vel_lin_y,
|
||||
double vel_lin_z,
|
||||
double uncertainty) {
|
||||
this->vel_lin_x = vel_lin_x;
|
||||
this->vel_lin_y = vel_lin_y;
|
||||
this->vel_lin_z = vel_lin_z;
|
||||
this->uncertainty = uncertainty;
|
||||
}
|
||||
|
||||
};
|
||||
@ -85,29 +153,55 @@ vn200_ypr_t(
|
||||
// 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);
|
||||
inline can1_vn200_gnss_ll_t vn200_gnss_ll(const frame::decoded::can1::vn200_gnss_ll_t frame_decoded) {
|
||||
can1_vn200_gnss_ll_t frame_encoded;
|
||||
frame_encoded.latitude = can1_vn200_gnss_ll_latitude_encode(frame_decoded.latitude);
|
||||
frame_encoded.longitude = can1_vn200_gnss_ll_longitude_encode(frame_decoded.longitude);
|
||||
frame_encoded.uncertainty_n = can1_vn200_gnss_ll_uncertainty_n_encode(frame_decoded.uncertainty_n);
|
||||
frame_encoded.uncertainty_e = can1_vn200_gnss_ll_uncertainty_e_encode(frame_decoded.uncertainty_e);
|
||||
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);
|
||||
inline can1_vn200_ins_ypr_t vn200_ins_ypr(const frame::decoded::can1::vn200_ins_ypr_t frame_decoded) {
|
||||
can1_vn200_ins_ypr_t frame_encoded;
|
||||
frame_encoded.yaw = can1_vn200_ins_ypr_yaw_encode(frame_decoded.yaw);
|
||||
frame_encoded.pitch = can1_vn200_ins_ypr_pitch_encode(frame_decoded.pitch);
|
||||
frame_encoded.roll = can1_vn200_ins_ypr_roll_encode(frame_decoded.roll);
|
||||
frame_encoded.uncertainty = can1_vn200_ins_ypr_uncertainty_encode(frame_decoded.uncertainty);
|
||||
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);
|
||||
inline can1_vn200_imu_acc_lin_t vn200_imu_acc_lin(const frame::decoded::can1::vn200_imu_acc_lin_t frame_decoded) {
|
||||
can1_vn200_imu_acc_lin_t frame_encoded;
|
||||
frame_encoded.acc_lin_x = can1_vn200_imu_acc_lin_acc_lin_x_encode(frame_decoded.acc_lin_x);
|
||||
frame_encoded.acc_lin_y = can1_vn200_imu_acc_lin_acc_lin_y_encode(frame_decoded.acc_lin_y);
|
||||
frame_encoded.acc_lin_z = can1_vn200_imu_acc_lin_acc_lin_z_encode(frame_decoded.acc_lin_z);
|
||||
return frame_encoded;
|
||||
}
|
||||
|
||||
inline can1_vn200_imu_acc_ang_t vn200_imu_acc_ang(const frame::decoded::can1::vn200_imu_acc_ang_t frame_decoded) {
|
||||
can1_vn200_imu_acc_ang_t frame_encoded;
|
||||
frame_encoded.acc_ang_x = can1_vn200_imu_acc_ang_acc_ang_x_encode(frame_decoded.acc_ang_x);
|
||||
frame_encoded.acc_ang_y = can1_vn200_imu_acc_ang_acc_ang_y_encode(frame_decoded.acc_ang_y);
|
||||
frame_encoded.acc_ang_z = can1_vn200_imu_acc_ang_acc_ang_z_encode(frame_decoded.acc_ang_z);
|
||||
return frame_encoded;
|
||||
}
|
||||
|
||||
inline can1_vn200_ins_ll_t vn200_ins_ll(const frame::decoded::can1::vn200_ins_ll_t frame_decoded) {
|
||||
can1_vn200_ins_ll_t frame_encoded;
|
||||
frame_encoded.latitude = can1_vn200_ins_ll_latitude_encode(frame_decoded.latitude);
|
||||
frame_encoded.longitude = can1_vn200_ins_ll_longitude_encode(frame_decoded.longitude);
|
||||
frame_encoded.uncertainty_n = can1_vn200_ins_ll_uncertainty_n_encode(frame_decoded.uncertainty_n);
|
||||
frame_encoded.uncertainty_e = can1_vn200_ins_ll_uncertainty_e_encode(frame_decoded.uncertainty_e);
|
||||
return frame_encoded;
|
||||
}
|
||||
|
||||
inline can1_vn200_ins_vel_t vn200_ins_vel(const frame::decoded::can1::vn200_ins_vel_t frame_decoded) {
|
||||
can1_vn200_ins_vel_t frame_encoded;
|
||||
frame_encoded.vel_lin_x = can1_vn200_ins_vel_vel_lin_x_encode(frame_decoded.vel_lin_x);
|
||||
frame_encoded.vel_lin_y = can1_vn200_ins_vel_vel_lin_y_encode(frame_decoded.vel_lin_y);
|
||||
frame_encoded.vel_lin_z = can1_vn200_ins_vel_vel_lin_z_encode(frame_decoded.vel_lin_z);
|
||||
frame_encoded.uncertainty = can1_vn200_ins_vel_uncertainty_encode(frame_decoded.uncertainty);
|
||||
return frame_encoded;
|
||||
}
|
||||
|
||||
@ -117,29 +211,55 @@ inline can1_vn200_ypr_t vn200_ypr(const frame::decoded::can1::vn200_ypr_t frame_
|
||||
// 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);
|
||||
inline frame::decoded::can1::vn200_gnss_ll_t vn200_gnss_ll(const can1_vn200_gnss_ll_t frame_encoded) {
|
||||
frame::decoded::can1::vn200_gnss_ll_t frame_decoded;
|
||||
frame_decoded.latitude = can1_vn200_gnss_ll_latitude_decode(frame_encoded.latitude);
|
||||
frame_decoded.longitude = can1_vn200_gnss_ll_longitude_decode(frame_encoded.longitude);
|
||||
frame_decoded.uncertainty_n = can1_vn200_gnss_ll_uncertainty_n_decode(frame_encoded.uncertainty_n);
|
||||
frame_decoded.uncertainty_e = can1_vn200_gnss_ll_uncertainty_e_decode(frame_encoded.uncertainty_e);
|
||||
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);
|
||||
inline frame::decoded::can1::vn200_ins_ypr_t vn200_ins_ypr(const can1_vn200_ins_ypr_t frame_encoded) {
|
||||
frame::decoded::can1::vn200_ins_ypr_t frame_decoded;
|
||||
frame_decoded.yaw = can1_vn200_ins_ypr_yaw_decode(frame_encoded.yaw);
|
||||
frame_decoded.pitch = can1_vn200_ins_ypr_pitch_decode(frame_encoded.pitch);
|
||||
frame_decoded.roll = can1_vn200_ins_ypr_roll_decode(frame_encoded.roll);
|
||||
frame_decoded.uncertainty = can1_vn200_ins_ypr_uncertainty_decode(frame_encoded.uncertainty);
|
||||
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);
|
||||
inline frame::decoded::can1::vn200_imu_acc_lin_t vn200_imu_acc_lin(const can1_vn200_imu_acc_lin_t frame_encoded) {
|
||||
frame::decoded::can1::vn200_imu_acc_lin_t frame_decoded;
|
||||
frame_decoded.acc_lin_x = can1_vn200_imu_acc_lin_acc_lin_x_decode(frame_encoded.acc_lin_x);
|
||||
frame_decoded.acc_lin_y = can1_vn200_imu_acc_lin_acc_lin_y_decode(frame_encoded.acc_lin_y);
|
||||
frame_decoded.acc_lin_z = can1_vn200_imu_acc_lin_acc_lin_z_decode(frame_encoded.acc_lin_z);
|
||||
return frame_decoded;
|
||||
}
|
||||
|
||||
inline frame::decoded::can1::vn200_imu_acc_ang_t vn200_imu_acc_ang(const can1_vn200_imu_acc_ang_t frame_encoded) {
|
||||
frame::decoded::can1::vn200_imu_acc_ang_t frame_decoded;
|
||||
frame_decoded.acc_ang_x = can1_vn200_imu_acc_ang_acc_ang_x_decode(frame_encoded.acc_ang_x);
|
||||
frame_decoded.acc_ang_y = can1_vn200_imu_acc_ang_acc_ang_y_decode(frame_encoded.acc_ang_y);
|
||||
frame_decoded.acc_ang_z = can1_vn200_imu_acc_ang_acc_ang_z_decode(frame_encoded.acc_ang_z);
|
||||
return frame_decoded;
|
||||
}
|
||||
|
||||
inline frame::decoded::can1::vn200_ins_ll_t vn200_ins_ll(const can1_vn200_ins_ll_t frame_encoded) {
|
||||
frame::decoded::can1::vn200_ins_ll_t frame_decoded;
|
||||
frame_decoded.latitude = can1_vn200_ins_ll_latitude_decode(frame_encoded.latitude);
|
||||
frame_decoded.longitude = can1_vn200_ins_ll_longitude_decode(frame_encoded.longitude);
|
||||
frame_decoded.uncertainty_n = can1_vn200_ins_ll_uncertainty_n_decode(frame_encoded.uncertainty_n);
|
||||
frame_decoded.uncertainty_e = can1_vn200_ins_ll_uncertainty_e_decode(frame_encoded.uncertainty_e);
|
||||
return frame_decoded;
|
||||
}
|
||||
|
||||
inline frame::decoded::can1::vn200_ins_vel_t vn200_ins_vel(const can1_vn200_ins_vel_t frame_encoded) {
|
||||
frame::decoded::can1::vn200_ins_vel_t frame_decoded;
|
||||
frame_decoded.vel_lin_x = can1_vn200_ins_vel_vel_lin_x_decode(frame_encoded.vel_lin_x);
|
||||
frame_decoded.vel_lin_y = can1_vn200_ins_vel_vel_lin_y_decode(frame_encoded.vel_lin_y);
|
||||
frame_decoded.vel_lin_z = can1_vn200_ins_vel_vel_lin_z_decode(frame_encoded.vel_lin_z);
|
||||
frame_decoded.uncertainty = can1_vn200_ins_vel_uncertainty_decode(frame_encoded.uncertainty);
|
||||
return frame_decoded;
|
||||
}
|
||||
|
||||
@ -149,9 +269,12 @@ inline frame::decoded::can1::vn200_ypr_t vn200_ypr(const can1_vn200_ypr_t frame_
|
||||
// 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;
|
||||
inline std::function<void(can1_vn200_gnss_ll_t, frame::decoded::can1::vn200_gnss_ll_t)> vn200_gnss_ll = NULL;
|
||||
inline std::function<void(can1_vn200_ins_ypr_t, frame::decoded::can1::vn200_ins_ypr_t)> vn200_ins_ypr = NULL;
|
||||
inline std::function<void(can1_vn200_imu_acc_lin_t, frame::decoded::can1::vn200_imu_acc_lin_t)> vn200_imu_acc_lin = NULL;
|
||||
inline std::function<void(can1_vn200_imu_acc_ang_t, frame::decoded::can1::vn200_imu_acc_ang_t)> vn200_imu_acc_ang = NULL;
|
||||
inline std::function<void(can1_vn200_ins_ll_t, frame::decoded::can1::vn200_ins_ll_t)> vn200_ins_ll = NULL;
|
||||
inline std::function<void(can1_vn200_ins_vel_t, frame::decoded::can1::vn200_ins_vel_t)> vn200_ins_vel = NULL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user