15 #ifndef VISIONTRANSFER_SENSORDATA_H 16 #define VISIONTRANSFER_SENSORDATA_H 18 #define _USE_MATH_DEFINES 22 #define M_PI 3.14159265358979323846 36 SensorRecord(
int timestampSec,
int timestampUSec,
unsigned char status): timestampSec(timestampSec), timestampUSec(timestampUSec), status(status) {}
40 void getTimestamp(
int& s,
int& us)
const { s = timestampSec; us = timestampUSec; }
53 double value()
const {
return valueIntl; }
54 TimestampedScalar(
int timestampSec,
int timestampUSec,
unsigned char status,
double value):
SensorRecord(timestampSec, timestampUSec, status), valueIntl(value) {}
66 double x()
const {
return xIntl; }
67 double y()
const {
return yIntl; }
68 double z()
const {
return zIntl; }
69 TimestampedVector(
int timestampSec,
int timestampUSec,
unsigned char status,
double x,
double y,
double z):
SensorRecord(timestampSec, timestampUSec, status), xIntl(x), yIntl(y), zIntl(z) {}
72 double xIntl, yIntl, zIntl;
82 double x()
const {
return xIntl; }
83 double y()
const {
return yIntl; }
84 double z()
const {
return zIntl; }
85 double w()
const {
return wIntl; }
91 double sinr_cosp = 2 * (wIntl * xIntl + -zIntl * yIntl);
92 double cosr_cosp = 1 - 2 * (xIntl * xIntl + -zIntl * -zIntl);
93 roll = std::atan2(sinr_cosp, cosr_cosp);
95 double sinp = 2 * (wIntl * -zIntl - yIntl * xIntl);
96 pitch = (std::abs(sinp) >= 1) ? ((sinp<0)?-(M_PI/2):(M_PI/2)) : std::asin(sinp);
98 double siny_cosp = 2 * (wIntl * yIntl + xIntl * -zIntl);
99 double cosy_cosp = 1 - 2 * (-zIntl * -zIntl + yIntl * yIntl);
100 yaw = std::atan2(siny_cosp, cosy_cosp);
106 TimestampedQuaternion(
int timestampSec,
int timestampUSec,
unsigned char status,
double x,
double y,
double z,
double w,
double accuracy):
SensorRecord(timestampSec, timestampUSec, status), xIntl(x), yIntl(y), zIntl(z), wIntl(w), accuracyIntl(accuracy) {}
109 double xIntl, yIntl, zIntl, wIntl;
Base class for sensor records with timestamp and status (reliability) fields.
void getRollPitchYaw(double &roll, double &pitch, double &yaw)
Encapsulate a 4D (quaternion) sensor report, containing X, Y, Z, W, as well as timestamp and status f...
Encapsulate a scalar sensor measurement, containing the value, as well as timestamp and status fields...
void getTimestamp(int &s, int &us) const
Encapsulate a 3D sensor report, containing X, Y, Z, as well as timestamp and status fields...
unsigned char getStatus() const