From a608eb89a9996728f827bc1fd7b14796e315daf6 Mon Sep 17 00:00:00 2001
From: Tomaso Maria Luigi De Ponti
<48210579+tmldeponti@users.noreply.github.com>
Date: Tue, 23 Jul 2024 11:12:39 +0200
Subject: [PATCH] Correct Heading of Rotating Wing Drone with Optitrack (#3331)
* first push
* move quat rotation to scheduling module
* code cleanup
* Use rotation matrix for heading correction
* Small adjustments
* declaration in ins file
---
.../rot_wing_v3c_oneloop_optitrack.xml | 4 ++--
...ot_wing_v3c_oneloop_optitrack_ext_pose.xml | 2 +-
.../modules/ctrl/eff_scheduling_rot_wing_V2.c | 7 ++++++-
sw/airborne/modules/ins/ins_ekf2.cpp | 19 +++++++++++++++++--
sw/airborne/modules/ins/ins_ekf2.h | 4 ++++
sw/airborne/modules/ins/ins_ext_pose.c | 15 ++++++++++++++-
sw/airborne/modules/ins/ins_ext_pose.h | 4 ++++
7 files changed, 48 insertions(+), 7 deletions(-)
diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml
index d72c7d910b..f09a6b8a96 100644
--- a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml
+++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml
@@ -95,8 +95,8 @@
-
-
+
+
diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml
index d11d7fb55f..898e9f9129 100644
--- a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml
+++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml
@@ -239,7 +239,7 @@
-
+
diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing_V2.c b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing_V2.c
index 6e85fe7bbd..687e42fd3f 100644
--- a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing_V2.c
+++ b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing_V2.c
@@ -84,7 +84,7 @@ void ele_pref_sched(void);
void update_attitude(void);
void sum_EFF_MAT_RW(void);
void init_RW_Model(void);
-void calc_G1_G2_RW(void);
+void calc_G1_G2_RW(void);
/** ABI binding wing position data.
*/
@@ -371,6 +371,11 @@ void eff_scheduling_rot_wing_update_wing_angle(void)
RW.skew.sinr2 = RW.skew.sinr * RW.skew.sinr;
RW.skew.sinr3 = RW.skew.sinr2 * RW.skew.sinr;
RW.skew.cosr3 = RW.skew.cosr2 * RW.skew.cosr;
+#ifdef INS_EXT_VISION_ROTATION
+ // Define an INS external pose quaternion rotation from the wing rotation angle
+ struct FloatEulers rot_e = {0, 0, RW.skew.rad};
+ float_quat_of_eulers(&ins_ext_vision_rot, &rot_e);
+#endif
}
float time = 0.0;
diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp
index 6fa736c477..f002e45f9c 100644
--- a/sw/airborne/modules/ins/ins_ekf2.cpp
+++ b/sw/airborne/modules/ins/ins_ekf2.cpp
@@ -278,6 +278,10 @@ PRINT_CONFIG_VAR(INS_EKF2_GPS_P_NOISE)
#endif
PRINT_CONFIG_VAR(INS_EKF2_BARO_NOISE)
+#ifdef INS_EXT_VISION_ROTATION
+struct FloatQuat ins_ext_vision_rot;
+#endif
+
/* All registered ABI events */
static abi_event baro_ev;
static abi_event temperature_ev;
@@ -296,8 +300,7 @@ static void gyro_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatRates *de
static void accel_int_cb(uint8_t sender_id, uint32_t stamp, struct FloatVect3 *delta_accel, uint16_t dt);
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag);
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
-static void optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x,
- int32_t flow_der_y, float quality, float size_divergence);
+static void optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
/* Static local functions */
static void ins_ekf2_publish_attitude(uint32_t stamp);
@@ -751,6 +754,18 @@ void ins_ekf2_parse_EXTERNAL_POSE(uint8_t *buf) {
sample_ev.quat(1) = DL_EXTERNAL_POSE_body_qy(buf);
sample_ev.quat(2) = DL_EXTERNAL_POSE_body_qx(buf);
sample_ev.quat(3) = -DL_EXTERNAL_POSE_body_qz(buf);
+
+#ifdef INS_EXT_VISION_ROTATION
+ // Rotate the quaternion
+ struct FloatQuat body_q = {sample_ev.quat(0), sample_ev.quat(1), sample_ev.quat(2), sample_ev.quat(3)};
+ struct FloatQuat rot_q;
+ float_quat_comp(&rot_q, &body_q, &ins_ext_vision_rot);
+ sample_ev.quat(0) = rot_q.qi;
+ sample_ev.quat(1) = rot_q.qx;
+ sample_ev.quat(2) = rot_q.qy;
+ sample_ev.quat(3) = rot_q.qz;
+#endif
+
sample_ev.posVar.setAll(INS_EKF2_EVP_NOISE);
sample_ev.velCov = matrix::eye() * INS_EKF2_EVV_NOISE;
sample_ev.angVar = INS_EKF2_EVA_NOISE;
diff --git a/sw/airborne/modules/ins/ins_ekf2.h b/sw/airborne/modules/ins/ins_ekf2.h
index 4a817b6ec0..6f894e2012 100644
--- a/sw/airborne/modules/ins/ins_ekf2.h
+++ b/sw/airborne/modules/ins/ins_ekf2.h
@@ -65,6 +65,10 @@ extern void ins_ekf2_parse_EXTERNAL_POSE(uint8_t *buf);
extern void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t *buf);
extern struct ekf2_t ekf2;
+#ifdef INS_EXT_VISION_ROTATION
+extern struct FloatQuat ins_ext_vision_rot;
+#endif
+
#ifdef __cplusplus
}
#endif
diff --git a/sw/airborne/modules/ins/ins_ext_pose.c b/sw/airborne/modules/ins/ins_ext_pose.c
index dbc112e9ed..a4490affa4 100644
--- a/sw/airborne/modules/ins/ins_ext_pose.c
+++ b/sw/airborne/modules/ins/ins_ext_pose.c
@@ -33,7 +33,6 @@
#include "modules/imu/imu.h"
#include "modules/ins/ins.h"
#include "generated/flight_plan.h"
-
#include "modules/core/abi.h"
#if 0
@@ -43,6 +42,10 @@
#define DEBUG_PRINT(...) {}
#endif
+#ifdef INS_EXT_VISION_ROTATION
+struct FloatQuat ins_ext_vision_rot;
+#endif
+
/** Data for telemetry and LTP origin.
*/
struct InsExtPose {
@@ -217,6 +220,16 @@ void ins_ext_pose_msg_update(uint8_t *buf)
orient.qy = quat_x ;
orient.qz = -quat_z;
+#ifdef INS_EXT_VISION_ROTATION
+ // Rotate the quaternion
+ struct FloatQuat rot_q;
+ float_quat_comp(&rot_q, &orient, &ins_ext_vision_rot);
+ orient.qi = rot_q.qi;
+ orient.qx = rot_q.qx;
+ orient.qy = rot_q.qy;
+ orient.qz = rot_q.qz;
+#endif
+
float_eulers_of_quat(&orient_eulers, &orient);
ins_ext_pos.ev_time = get_sys_time_usec();
diff --git a/sw/airborne/modules/ins/ins_ext_pose.h b/sw/airborne/modules/ins/ins_ext_pose.h
index 7e699bf563..31e493f61f 100644
--- a/sw/airborne/modules/ins/ins_ext_pose.h
+++ b/sw/airborne/modules/ins/ins_ext_pose.h
@@ -49,6 +49,10 @@ extern void ins_ext_pose_run(void);
extern void ins_ext_pose_msg_update(uint8_t *buf);
+#ifdef INS_EXT_VISION_ROTATION
+extern struct FloatQuat ins_ext_vision_rot;
+#endif
+
// Logging
extern void ins_ext_pos_log_header(FILE *file);
extern void ins_ext_pos_log_data(FILE *file);