Use proper type for euler angles

Signed-off-by: TheJackiMonster <thejackimonster@gmail.com>
This commit is contained in:
TheJackiMonster 2023-11-30 02:14:41 +01:00
parent 43983153be
commit 45d6bc536e
No known key found for this signature in database
GPG key ID: D850A5F772E880F9
4 changed files with 20 additions and 13 deletions

View file

@ -30,7 +30,7 @@ void test3(uint64_t timestamp,
device3_event_type event, device3_event_type event,
const device3_ahrs_type* ahrs) { const device3_ahrs_type* ahrs) {
device3_quat_type orientation; device3_quat_type orientation;
device3_vec3_type euler; device3_euler_type euler;
switch (event) { switch (event) {
case DEVICE3_EVENT_INIT: case DEVICE3_EVENT_INIT:
@ -39,7 +39,7 @@ void test3(uint64_t timestamp,
case DEVICE3_EVENT_UPDATE: case DEVICE3_EVENT_UPDATE:
orientation = device3_get_orientation(ahrs); orientation = device3_get_orientation(ahrs);
euler = device3_get_euler(orientation); euler = device3_get_euler(orientation);
printf("Pitch: %.2f; Roll: %.2f; Yaw: %.2f\n", euler.x, euler.y, euler.z); printf("Roll: %.2f; Pitch: %.2f; Yaw: %.2f\n", euler.roll, euler.pitch, euler.yaw);
break; break;
default: default:
break; break;

View file

@ -110,6 +110,12 @@ struct device3_quat_t {
float w; float w;
}; };
struct device3_euler_t {
float roll;
float pitch;
float yaw;
};
typedef enum device3_error_t device3_error_type; typedef enum device3_error_t device3_error_type;
typedef struct device3_packet_t device3_packet_type; typedef struct device3_packet_t device3_packet_type;
typedef enum device3_event_t device3_event_type; typedef enum device3_event_t device3_event_type;
@ -119,6 +125,7 @@ typedef struct device3_calibration_t device3_calibration_type;
typedef struct device3_vec3_t device3_vec3_type; typedef struct device3_vec3_t device3_vec3_type;
typedef struct device3_quat_t device3_quat_type; typedef struct device3_quat_t device3_quat_type;
typedef struct device3_euler_t device3_euler_type;
typedef void (*device3_event_callback)( typedef void (*device3_event_callback)(
uint64_t timestamp, uint64_t timestamp,
@ -166,7 +173,7 @@ device3_vec3_type device3_get_linear_acceleration(const device3_ahrs_type* ahrs)
device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs); device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs);
device3_vec3_type device3_get_euler(device3_quat_type quat); device3_euler_type device3_get_euler(device3_quat_type quat);
device3_error_type device3_close(device3_type* device); device3_error_type device3_close(device3_type* device);

View file

@ -912,17 +912,17 @@ device3_quat_type device3_get_orientation(const device3_ahrs_type* ahrs) {
return q; return q;
} }
device3_vec3_type device3_get_euler(device3_quat_type quat) { device3_euler_type device3_get_euler(device3_quat_type quat) {
FusionQuaternion quaternion; FusionQuaternion quaternion;
quaternion.element.x = quat.x; quaternion.element.x = quat.x;
quaternion.element.y = quat.y; quaternion.element.y = quat.y;
quaternion.element.z = quat.z; quaternion.element.z = quat.z;
quaternion.element.w = quat.w; quaternion.element.w = quat.w;
FusionEuler euler = FusionQuaternionToEuler(quaternion); FusionEuler euler = FusionQuaternionToEuler(quaternion);
device3_vec3_type e; device3_euler_type e;
e.x = euler.angle.pitch; e.roll = euler.angle.roll;
e.y = euler.angle.roll; e.pitch = euler.angle.pitch;
e.z = euler.angle.yaw; e.yaw = euler.angle.yaw;
return e; return e;
} }

View file

@ -56,19 +56,19 @@ void test3(uint64_t timestamp,
dmax = (d > dmax? d : dmax); dmax = (d > dmax? d : dmax);
} }
device3_vec3_type e = device3_get_euler(q); device3_euler_type e = device3_get_euler(q);
if (d >= 0.00005f) { if (d >= 0.00005f) {
device3_vec3_type e0 = device3_get_euler(old); device3_euler_type e0 = device3_get_euler(old);
printf("Pitch: %f; Roll: %f; Yaw: %f\n", e0.x, e0.y, e0.z); printf("Roll: %f; Pitch: %f; Yaw: %f\n", e0.roll, e0.pitch, e0.yaw);
printf("Pitch: %f; Roll: %f; Yaw: %f\n", e.x, e.y, e.z); printf("Roll: %f; Pitch: %f; Yaw: %f\n", e.roll, e.pitch, e.yaw);
printf("D = %f; ( %f )\n", d, dmax); printf("D = %f; ( %f )\n", d, dmax);
printf("X: %f; Y: %f; Z: %f; W: %f;\n", old.x, old.y, old.z, old.w); printf("X: %f; Y: %f; Z: %f; W: %f;\n", old.x, old.y, old.z, old.w);
printf("X: %f; Y: %f; Z: %f; W: %f;\n", q.x, q.y, q.z, q.w); printf("X: %f; Y: %f; Z: %f; W: %f;\n", q.x, q.y, q.z, q.w);
} else { } else {
printf("Pitch: %.2f; Roll: %.2f; Yaw: %.2f\n", e.x, e.y, e.z); printf("Roll: %.2f; Pitch: %.2f; Yaw: %.2f\n", e.roll, e.pitch, e.yaw);
} }
old = q; old = q;