From 6667d5091dbd7b0684cfcb72101529ee4bdae492 Mon Sep 17 00:00:00 2001
From: Tomaso Maria Luigi De Ponti
<48210579+tmldeponti@users.noreply.github.com>
Date: Wed, 3 Apr 2024 15:02:25 +0200
Subject: [PATCH] Slight Changes to INS EXT POSE and INS EKF2 (#3233)
---
conf/modules/ins_ext_pose.xml | 2 +-
conf/telemetry/highspeed_rotorcraft.xml | 10 ++-
sw/airborne/modules/ins/ins_ekf2.cpp | 69 +++++++++++++-----
sw/airborne/modules/ins/ins_ext_pose.c | 96 ++++++++++++++++++-------
sw/airborne/modules/ins/ins_ext_pose.h | 4 +-
sw/ext/pprzlink | 2 +-
6 files changed, 130 insertions(+), 53 deletions(-)
diff --git a/conf/modules/ins_ext_pose.xml b/conf/modules/ins_ext_pose.xml
index 482f9e10d20..b2df677b49c 100644
--- a/conf/modules/ins_ext_pose.xml
+++ b/conf/modules/ins_ext_pose.xml
@@ -22,7 +22,7 @@
-
+
diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml
index 2fa8cf28d8b..f7e7c7ca0c5 100644
--- a/conf/telemetry/highspeed_rotorcraft.xml
+++ b/conf/telemetry/highspeed_rotorcraft.xml
@@ -42,7 +42,7 @@
-
+
@@ -53,13 +53,15 @@
-
+
+
+
@@ -163,7 +165,7 @@
-
+
@@ -220,6 +222,7 @@
+
@@ -255,6 +258,7 @@
+
diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp
index 52a37c33dbd..aaaffebc0a9 100644
--- a/sw/airborne/modules/ins/ins_ekf2.cpp
+++ b/sw/airborne/modules/ins/ins_ekf2.cpp
@@ -301,7 +301,7 @@ static void ins_ekf2_publish_attitude(uint32_t stamp);
static Ekf ekf; ///< EKF class itself
static parameters *ekf_params; ///< The EKF parameters
struct ekf2_t ekf2; ///< Local EKF2 status structure
-
+static struct extVisionSample sample_ev = {0}; ///< External vision sample
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
@@ -480,6 +480,37 @@ static void send_ahrs_quat(struct transport_tx *trans, struct link_device *dev)
&ahrs_id);
}
+
+static void send_external_pose_down(struct transport_tx *trans, struct link_device *dev)
+{
+ if(sample_ev.time_us == 0){
+ return;
+ }
+ float sample_temp_ev[11];
+ sample_temp_ev[0] = (float) sample_ev.time_us;
+ sample_temp_ev[1] = sample_ev.pos(0) ;
+ sample_temp_ev[2] = sample_ev.pos(1) ;
+ sample_temp_ev[3] = sample_ev.pos(2) ;
+ sample_temp_ev[4] = sample_ev.vel(0) ;
+ sample_temp_ev[5] = sample_ev.vel(1) ;
+ sample_temp_ev[6] = sample_ev.vel(2) ;
+ sample_temp_ev[7] = sample_ev.quat(0);
+ sample_temp_ev[8] = sample_ev.quat(1);
+ sample_temp_ev[9] = sample_ev.quat(2);
+ sample_temp_ev[10] = sample_ev.quat(3);
+ pprz_msg_send_EXTERNAL_POSE_DOWN(trans, dev, AC_ID,
+ &sample_temp_ev[0],
+ &sample_temp_ev[1],
+ &sample_temp_ev[2],
+ &sample_temp_ev[3],
+ &sample_temp_ev[4],
+ &sample_temp_ev[5],
+ &sample_temp_ev[6],
+ &sample_temp_ev[7],
+ &sample_temp_ev[8],
+ &sample_temp_ev[9],
+ &sample_temp_ev[10] );
+}
#endif
/* Initialize the EKF */
@@ -576,6 +607,7 @@ void ins_ekf2_init(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WIND_INFO_RET, send_wind_info_ret);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_BIAS, send_ahrs_bias);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_QUAT_INT, send_ahrs_quat);
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EXTERNAL_POSE_DOWN, send_external_pose_down);
#endif
/*
@@ -700,24 +732,23 @@ void ins_ekf2_remove_gps(int32_t mode)
void ins_ekf2_parse_EXTERNAL_POSE(uint8_t *buf) {
if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft
- extVisionSample sample;
- sample.time_us = get_sys_time_usec(); //FIXME
- sample.pos(0) = DL_EXTERNAL_POSE_enu_y(buf);
- sample.pos(1) = DL_EXTERNAL_POSE_enu_x(buf);
- sample.pos(2) = -DL_EXTERNAL_POSE_enu_z(buf);
- sample.vel(0) = DL_EXTERNAL_POSE_enu_yd(buf);
- sample.vel(1) = DL_EXTERNAL_POSE_enu_xd(buf);
- sample.vel(2) = -DL_EXTERNAL_POSE_enu_zd(buf);
- sample.quat(0) = DL_EXTERNAL_POSE_body_qi(buf);
- sample.quat(1) = DL_EXTERNAL_POSE_body_qy(buf);
- sample.quat(2) = DL_EXTERNAL_POSE_body_qx(buf);
- sample.quat(3) = -DL_EXTERNAL_POSE_body_qz(buf);
- sample.posVar.setAll(INS_EKF2_EVP_NOISE);
- sample.velCov = matrix::eye() * INS_EKF2_EVV_NOISE;
- sample.angVar = INS_EKF2_EVA_NOISE;
- sample.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
-
- ekf.setExtVisionData(sample);
+ sample_ev.time_us = get_sys_time_usec(); //FIXME
+ sample_ev.pos(0) = DL_EXTERNAL_POSE_enu_y(buf);
+ sample_ev.pos(1) = DL_EXTERNAL_POSE_enu_x(buf);
+ sample_ev.pos(2) = -DL_EXTERNAL_POSE_enu_z(buf);
+ sample_ev.vel(0) = DL_EXTERNAL_POSE_enu_yd(buf);
+ sample_ev.vel(1) = DL_EXTERNAL_POSE_enu_xd(buf);
+ sample_ev.vel(2) = -DL_EXTERNAL_POSE_enu_zd(buf);
+ sample_ev.quat(0) = DL_EXTERNAL_POSE_body_qi(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);
+ sample_ev.posVar.setAll(INS_EKF2_EVP_NOISE);
+ sample_ev.velCov = matrix::eye() * INS_EKF2_EVV_NOISE;
+ sample_ev.angVar = INS_EKF2_EVA_NOISE;
+ sample_ev.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
+
+ ekf.setExtVisionData(sample_ev);
}
void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t __attribute__((unused)) *buf) {
diff --git a/sw/airborne/modules/ins/ins_ext_pose.c b/sw/airborne/modules/ins/ins_ext_pose.c
index 50d62a9de7e..dbc112e9edd 100644
--- a/sw/airborne/modules/ins/ins_ext_pose.c
+++ b/sw/airborne/modules/ins/ins_ext_pose.c
@@ -43,21 +43,21 @@
#define DEBUG_PRINT(...) {}
#endif
-
/** Data for telemetry and LTP origin.
*/
-
-
struct InsExtPose {
/* Inputs */
struct FloatRates gyros_f;
struct FloatVect3 accels_f;
- bool has_new_gyro;
- bool has_new_acc;
+ bool has_new_gyro;
+ bool has_new_acc;
struct FloatVect3 ev_pos;
+ struct FloatVect3 ev_vel;
struct FloatEulers ev_att;
- bool has_new_ext_pose;
+ struct FloatQuat ev_quat;
+ bool has_new_ext_pose;
+ float ev_time;
/* Origin */
struct LtpDef_i ltp_def;
@@ -67,7 +67,6 @@ struct InsExtPose {
struct NedCoor_i ltp_speed;
struct NedCoor_i ltp_accel;
};
-
struct InsExtPose ins_ext_pos;
@@ -86,6 +85,8 @@ static void ins_ext_pose_init_from_flightplan(void)
ltp_def_from_ecef_i(&ins_ext_pos.ltp_def, &ecef_nav0);
ins_ext_pos.ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(&ins_ext_pos.ltp_def);
+ /* update local ENU coordinates of global waypoints */
+ waypoints_localize_all();
}
@@ -119,6 +120,36 @@ static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
&ins_ext_pos.ltp_def.lla.lat, &ins_ext_pos.ltp_def.lla.lon, &ins_ext_pos.ltp_def.lla.alt,
&ins_ext_pos.ltp_def.hmsl, (float *)&fake_qfe);
}
+
+static void send_external_pose_down(struct transport_tx *trans, struct link_device *dev)
+{
+ pprz_msg_send_EXTERNAL_POSE_DOWN(trans, dev, AC_ID,
+ &ins_ext_pos.ev_time,
+ &ins_ext_pos.ev_pos.x,
+ &ins_ext_pos.ev_pos.y,
+ &ins_ext_pos.ev_pos.z,
+ &ins_ext_pos.ev_vel.x,
+ &ins_ext_pos.ev_vel.y,
+ &ins_ext_pos.ev_vel.z,
+ &ins_ext_pos.ev_quat.qi,
+ &ins_ext_pos.ev_quat.qx,
+ &ins_ext_pos.ev_quat.qy,
+ &ins_ext_pos.ev_quat.qz);
+}
+static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
+{
+ float dummy0 = 0.0;
+ pprz_msg_send_AHRS_BIAS(trans, dev, AC_ID,
+ &ekf_X[9],
+ &ekf_X[10],
+ &ekf_X[11],
+ &ekf_X[12],
+ &ekf_X[13],
+ &ekf_X[14],
+ &dummy0,
+ &dummy0,
+ &dummy0);
+}
#endif
@@ -163,11 +194,13 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
void ins_ext_pose_msg_update(uint8_t *buf)
{
if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft
-
- float enu_x = DL_EXTERNAL_POSE_enu_x(buf);
- float enu_y = DL_EXTERNAL_POSE_enu_y(buf);
- float enu_z = DL_EXTERNAL_POSE_enu_z(buf);
-
+
+ float enu_x = DL_EXTERNAL_POSE_enu_x(buf);
+ float enu_y = DL_EXTERNAL_POSE_enu_y(buf);
+ float enu_z = DL_EXTERNAL_POSE_enu_z(buf);
+ float enu_xd = DL_EXTERNAL_POSE_enu_xd(buf);
+ float enu_yd = DL_EXTERNAL_POSE_enu_yd(buf);
+ float enu_zd = DL_EXTERNAL_POSE_enu_zd(buf);
float quat_i = DL_EXTERNAL_POSE_body_qi(buf);
float quat_x = DL_EXTERNAL_POSE_body_qx(buf);
float quat_y = DL_EXTERNAL_POSE_body_qy(buf);
@@ -178,20 +211,28 @@ void ins_ext_pose_msg_update(uint8_t *buf)
struct FloatQuat orient;
struct FloatEulers orient_eulers;
- orient.qi = quat_i;
- orient.qx = quat_y; //north
- orient.qy = -quat_x; //east
- orient.qz = -quat_z; //down
+ // Transformation of External Pose. Optitrack motive 2.X Yup
+ orient.qi = quat_i ;
+ orient.qx = quat_y ;
+ orient.qy = quat_x ;
+ orient.qz = -quat_z;
float_eulers_of_quat(&orient_eulers, &orient);
- orient_eulers.theta = -orient_eulers.theta;
-
- ins_ext_pos.ev_pos.x = enu_y;
- ins_ext_pos.ev_pos.y = enu_x;
- ins_ext_pos.ev_pos.z = -enu_z;
- ins_ext_pos.ev_att.phi = orient_eulers.phi;
- ins_ext_pos.ev_att.theta = orient_eulers.theta;
- ins_ext_pos.ev_att.psi = orient_eulers.psi;
+
+ ins_ext_pos.ev_time = get_sys_time_usec();
+ ins_ext_pos.ev_pos.x = enu_y;
+ ins_ext_pos.ev_pos.y = enu_x;
+ ins_ext_pos.ev_pos.z = -enu_z;
+ ins_ext_pos.ev_vel.x = enu_yd;
+ ins_ext_pos.ev_vel.y = enu_xd;
+ ins_ext_pos.ev_vel.z = -enu_zd;
+ ins_ext_pos.ev_att.phi = orient_eulers.phi;
+ ins_ext_pos.ev_att.theta = orient_eulers.theta;
+ ins_ext_pos.ev_att.psi = orient_eulers.psi;
+ ins_ext_pos.ev_quat.qi = orient.qi;
+ ins_ext_pos.ev_quat.qx = orient.qx;
+ ins_ext_pos.ev_quat.qy = orient.qy;
+ ins_ext_pos.ev_quat.qz = orient.qz;
ins_ext_pos.has_new_ext_pose = true;
@@ -235,12 +276,13 @@ void ins_ext_pose_init(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS, send_ins);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref);
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EXTERNAL_POSE_DOWN, send_external_pose_down);
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_BIAS, send_ahrs_bias);
#endif
// Get IMU through ABI
AbiBindMsgIMU_ACCEL(INS_EXT_POSE_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_GYRO(INS_EXT_POSE_IMU_ID, &gyro_ev, gyro_cb);
-
// Get External Pose through datalink message: setup in xml
// Initialize EKF
@@ -315,9 +357,9 @@ static inline void ekf_init(void)
float Z0[EKF_NUM_OUTPUTS] = {0, 0, 0, 0, 0, 0};
float Pdiag[EKF_NUM_STATES] = {1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.};
- float Qdiag[EKF_NUM_INPUTS] = {0.5, 0.5, 0.5, 0.01, 0.01, 0.01};
+ float Qdiag[EKF_NUM_INPUTS] = {1.0, 1.0, 1.0, 0.0173, 4.878e-4, 3.547e-4};//{0.0325, 0.4494, 0.5087, 0.0173, 4.878e-4, 3.547e-4};
- float Rdiag[EKF_NUM_OUTPUTS] = {0.001, 0.001, 0.001, 0.1, 0.1, 0.1};
+ float Rdiag[EKF_NUM_OUTPUTS] = {8.372e-6, 3.832e-6, 4.761e-6, 2.830e-4, 8.684e-6, 7.013e-6};
MAKE_MATRIX_PTR(ekf_P_, ekf_P, EKF_NUM_STATES);
MAKE_MATRIX_PTR(ekf_Q_, ekf_Q, EKF_NUM_INPUTS);
diff --git a/sw/airborne/modules/ins/ins_ext_pose.h b/sw/airborne/modules/ins/ins_ext_pose.h
index 256af3b1a25..7e699bf5635 100644
--- a/sw/airborne/modules/ins/ins_ext_pose.h
+++ b/sw/airborne/modules/ins/ins_ext_pose.h
@@ -37,7 +37,8 @@ Section 5.3: Non-additive noise formulation and equations
#define EKF_NUM_OUTPUTS 6
#include "std.h"
-
+#include "math/pprz_algebra_float.h"
+#include "state.h"
#include
@@ -52,5 +53,4 @@ extern void ins_ext_pose_msg_update(uint8_t *buf);
extern void ins_ext_pos_log_header(FILE *file);
extern void ins_ext_pos_log_data(FILE *file);
-
#endif
diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink
index 828de444d7f..15a861f6673 160000
--- a/sw/ext/pprzlink
+++ b/sw/ext/pprzlink
@@ -1 +1 @@
-Subproject commit 828de444d7fe6f57d90ef96fa4ca1e62f67bd5a9
+Subproject commit 15a861f667372fee7e2996150906ed713d5e0a66