Skip to content

Commit

Permalink
Slight Changes to INS EXT POSE and INS EKF2 (#3233)
Browse files Browse the repository at this point in the history
  • Loading branch information
@tmldeponti
tmldeponti committed Apr 3, 2024
1 parent 5b13467 commit 6667d50
Show file tree
Hide file tree
Showing 6 changed files with 130 additions and 53 deletions.
2 changes: 1 addition & 1 deletion conf/modules/ins_ext_pose.xml
View file
Expand Up @@ -22,7 +22,7 @@
<file name="ins_ext_pose.h"/>
</header>
<init fun="ins_ext_pose_init()"/>
<periodic fun="ins_ext_pose_run()" freq="512" />
<periodic fun="ins_ext_pose_run()"/>
<datalink message="EXTERNAL_POSE" fun="ins_ext_pose_msg_update(buf)"/>

<makefile target="ap">
Expand Down
10 changes: 7 additions & 3 deletions conf/telemetry/highspeed_rotorcraft.xml
View file
Expand Up @@ -42,7 +42,7 @@
<message name="AHRS_BIAS" period="7.5"/>
<message name="HOVER_LOOP" period="0.3"/>
<message name="GUIDANCE_H_REF_INT" period="0.31"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.4"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.4"/>
<message name="HYBRID_GUIDANCE" period="0.4"/>
<message name="ESC" period="0.5"/>
<message name="AHRS_REF_QUAT" period="0.1"/>
Expand All @@ -53,13 +53,15 @@
<message name="APPROACH_MOVING_TARGET" period="0.1"/>
<message name="AIRSPEED_RAW" period="0.1"/>
<message name="TARGET_POS_INFO" period="0.1"/>
<message name="ROTATING_WING_STATE" period="0.2"/>
<message name="ROTATING_WING_STATE" period="0.2"/>
<message name="ACTUATORS" period="0.02"/>
<message name="EFF_MAT_G" period="0.02"/>
<message name="GUIDANCE" period="0.02"/>
<message name="STAB_ATTITUDE" period="0.02"/>
<message name="ROTORCRAFT_CMD" period="0.2"/>
<message name="DEBUG_VECT" period="0.2"/>
<message name="EXTERNAL_POSE_DOWN" period="0.2"/>

</mode>

<mode name="calibration">
Expand Down Expand Up @@ -163,7 +165,7 @@
<mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
Expand Down Expand Up @@ -220,6 +222,7 @@
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.002"/>
<message name="ROTORCRAFT_CMD" period=".002"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.20"/>
<message name="COMMANDS" period=".02"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
Expand Down Expand Up @@ -255,6 +258,7 @@
<message name="AIRSPEED_RAW" period="0.01"/>
<message name="EFF_MAT_G" period="0.002"/>
<message name="GUIDANCE" period="0.002"/>
<message name="EXTERNAL_POSE_DOWN" period="0.002"/>
<message name="ROTATING_WING_STATE" period="0.1"/>
<message name="DOUBLET" period="0.002"/>
</mode>
Expand Down
69 changes: 50 additions & 19 deletions sw/airborne/modules/ins/ins_ekf2.cpp
View file
Expand Up @@ -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"

Expand Down Expand Up @@ -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 */
Expand Down Expand Up @@ -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

/*
Expand Down Expand Up @@ -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<float, 3>() * 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<float, 3>() * 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) {
Expand Down
96 changes: 69 additions & 27 deletions sw/airborne/modules/ins/ins_ext_pose.c
View file
Expand Up @@ -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;
Expand All @@ -67,7 +67,6 @@ struct InsExtPose {
struct NedCoor_i ltp_speed;
struct NedCoor_i ltp_accel;
};

struct InsExtPose ins_ext_pos;


Expand All @@ -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();
}


Expand Down Expand Up @@ -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


Expand Down Expand Up @@ -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);
Expand All @@ -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;

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions sw/airborne/modules/ins/ins_ext_pose.h
View file
Expand Up @@ -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 <stdio.h>


Expand All @@ -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
2 changes: 1 addition & 1 deletion sw/ext/pprzlink
Submodule pprzlink updated 1 files
+16 −1 message_definitions/v1.0/messages.xml

0 comments on commit 6667d50

Please sign in to comment.