Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 03:57:45 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Slight Changes to INS EXT POSE and INS EKF2 (#3233)

Browse Source
This commit is contained in:
Tomaso Maria Luigi De Ponti
2024-04-03 15:02:25 +02:00
committed by GitHub
parent 5b13467c71
commit 6667d5091d
6 changed files with 129 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/ins_ext_pose.xml
+1 -1
View File
@@ -22,7 +22,7 @@
<file name="ins_ext_pose.h"/> <file name="ins_ext_pose.h"/>
</header> </header>
<init fun="ins_ext_pose_init()"/> <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)"/> <datalink message="EXTERNAL_POSE" fun="ins_ext_pose_msg_update(buf)"/>
<makefile target="ap"> <makefile target="ap">
conf/telemetry/highspeed_rotorcraft.xml
+6 -2
View File
@@ -53,13 +53,15 @@
<message name="APPROACH_MOVING_TARGET" period="0.1"/> <message name="APPROACH_MOVING_TARGET" period="0.1"/>
<message name="AIRSPEED_RAW" period="0.1"/> <message name="AIRSPEED_RAW" period="0.1"/>
<message name="TARGET_POS_INFO" 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="ACTUATORS" period="0.02"/>
<message name="EFF_MAT_G" period="0.02"/> <message name="EFF_MAT_G" period="0.02"/>
<message name="GUIDANCE" period="0.02"/> <message name="GUIDANCE" period="0.02"/>
<message name="STAB_ATTITUDE" period="0.02"/> <message name="STAB_ATTITUDE" period="0.02"/>
<message name="ROTORCRAFT_CMD" period="0.2"/> <message name="ROTORCRAFT_CMD" period="0.2"/>
<message name="DEBUG_VECT" period="0.2"/> <message name="DEBUG_VECT" period="0.2"/>
<message name="EXTERNAL_POSE_DOWN" period="0.2"/>
</mode> </mode>
<mode name="calibration"> <mode name="calibration">
@@ -163,7 +165,7 @@
<mode name="vel_guidance" key_press="q"> <mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/> <message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/> <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_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/> <message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/> <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
@@ -220,6 +222,7 @@
<message name="ROTORCRAFT_STATUS" period="1.2"/> <message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.002"/> <message name="ROTORCRAFT_FP" period="0.002"/>
<message name="ROTORCRAFT_CMD" period=".002"/> <message name="ROTORCRAFT_CMD" period=".002"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.20"/>
<message name="COMMANDS" period=".02"/> <message name="COMMANDS" period=".02"/>
<message name="INS_REF" period="5.1"/> <message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/> <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
@@ -255,6 +258,7 @@
<message name="AIRSPEED_RAW" period="0.01"/> <message name="AIRSPEED_RAW" period="0.01"/>
<message name="EFF_MAT_G" period="0.002"/> <message name="EFF_MAT_G" period="0.002"/>
<message name="GUIDANCE" 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="ROTATING_WING_STATE" period="0.1"/>
<message name="DOUBLET" period="0.002"/> <message name="DOUBLET" period="0.002"/>
</mode> </mode>
sw/airborne/modules/ins/ins_ekf2.cpp
+49 -18
View File
@@ -301,7 +301,7 @@ static void ins_ekf2_publish_attitude(uint32_t stamp);
static Ekf ekf; ///< EKF class itself static Ekf ekf; ///< EKF class itself
static parameters *ekf_params; ///< The EKF parameters static parameters *ekf_params; ///< The EKF parameters
struct ekf2_t ekf2; ///< Local EKF2 status structure struct ekf2_t ekf2; ///< Local EKF2 status structure
static struct extVisionSample sample_ev = {0}; ///< External vision sample
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h" #include "modules/datalink/telemetry.h"
@@ -480,6 +480,37 @@ static void send_ahrs_quat(struct transport_tx *trans, struct link_device *dev)
&ahrs_id); &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 #endif
/* Initialize the EKF */ /* 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_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_BIAS, send_ahrs_bias);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_QUAT_INT, send_ahrs_quat); 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 #endif
/* /*
@@ -700,24 +732,23 @@ void ins_ekf2_remove_gps(int32_t mode)
void ins_ekf2_parse_EXTERNAL_POSE(uint8_t *buf) { void ins_ekf2_parse_EXTERNAL_POSE(uint8_t *buf) {
if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft
extVisionSample sample; sample_ev.time_us = get_sys_time_usec(); //FIXME
sample.time_us = get_sys_time_usec(); //FIXME sample_ev.pos(0) = DL_EXTERNAL_POSE_enu_y(buf);
sample.pos(0) = DL_EXTERNAL_POSE_enu_y(buf); sample_ev.pos(1) = DL_EXTERNAL_POSE_enu_x(buf);
sample.pos(1) = DL_EXTERNAL_POSE_enu_x(buf); sample_ev.pos(2) = -DL_EXTERNAL_POSE_enu_z(buf);
sample.pos(2) = -DL_EXTERNAL_POSE_enu_z(buf); sample_ev.vel(0) = DL_EXTERNAL_POSE_enu_yd(buf);
sample.vel(0) = DL_EXTERNAL_POSE_enu_yd(buf); sample_ev.vel(1) = DL_EXTERNAL_POSE_enu_xd(buf);
sample.vel(1) = DL_EXTERNAL_POSE_enu_xd(buf); sample_ev.vel(2) = -DL_EXTERNAL_POSE_enu_zd(buf);
sample.vel(2) = -DL_EXTERNAL_POSE_enu_zd(buf); sample_ev.quat(0) = DL_EXTERNAL_POSE_body_qi(buf);
sample.quat(0) = DL_EXTERNAL_POSE_body_qi(buf); sample_ev.quat(1) = DL_EXTERNAL_POSE_body_qy(buf);
sample.quat(1) = DL_EXTERNAL_POSE_body_qy(buf); sample_ev.quat(2) = DL_EXTERNAL_POSE_body_qx(buf);
sample.quat(2) = DL_EXTERNAL_POSE_body_qx(buf); sample_ev.quat(3) = -DL_EXTERNAL_POSE_body_qz(buf);
sample.quat(3) = -DL_EXTERNAL_POSE_body_qz(buf); sample_ev.posVar.setAll(INS_EKF2_EVP_NOISE);
sample.posVar.setAll(INS_EKF2_EVP_NOISE); sample_ev.velCov = matrix::eye<float, 3>() * INS_EKF2_EVV_NOISE;
sample.velCov = matrix::eye<float, 3>() * INS_EKF2_EVV_NOISE; sample_ev.angVar = INS_EKF2_EVA_NOISE;
sample.angVar = INS_EKF2_EVA_NOISE; sample_ev.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
sample.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
ekf.setExtVisionData(sample); ekf.setExtVisionData(sample_ev);
} }
void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t __attribute__((unused)) *buf) { void ins_ekf2_parse_EXTERNAL_POSE_SMALL(uint8_t __attribute__((unused)) *buf) {
sw/airborne/modules/ins/ins_ext_pose.c
+67 -25
View File
@@ -43,21 +43,21 @@
#define DEBUG_PRINT(...) {} #define DEBUG_PRINT(...) {}
#endif #endif
/** Data for telemetry and LTP origin. /** Data for telemetry and LTP origin.
*/ */
struct InsExtPose { struct InsExtPose {
/* Inputs */ /* Inputs */
struct FloatRates gyros_f; struct FloatRates gyros_f;
struct FloatVect3 accels_f; struct FloatVect3 accels_f;
bool has_new_gyro; bool has_new_gyro;
bool has_new_acc; bool has_new_acc;
struct FloatVect3 ev_pos; struct FloatVect3 ev_pos;
struct FloatVect3 ev_vel;
struct FloatEulers ev_att; struct FloatEulers ev_att;
bool has_new_ext_pose; struct FloatQuat ev_quat;
bool has_new_ext_pose;
float ev_time;
/* Origin */ /* Origin */
struct LtpDef_i ltp_def; struct LtpDef_i ltp_def;
@@ -67,7 +67,6 @@ struct InsExtPose {
struct NedCoor_i ltp_speed; struct NedCoor_i ltp_speed;
struct NedCoor_i ltp_accel; struct NedCoor_i ltp_accel;
}; };
struct InsExtPose ins_ext_pos; 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); ltp_def_from_ecef_i(&ins_ext_pos.ltp_def, &ecef_nav0);
ins_ext_pos.ltp_def.hmsl = NAV_ALT0; ins_ext_pos.ltp_def.hmsl = NAV_ALT0;
stateSetLocalOrigin_i(&ins_ext_pos.ltp_def); 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.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); &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 #endif
@@ -164,10 +195,12 @@ void ins_ext_pose_msg_update(uint8_t *buf)
{ {
if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft 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_x = DL_EXTERNAL_POSE_enu_x(buf);
float enu_y = DL_EXTERNAL_POSE_enu_y(buf); float enu_y = DL_EXTERNAL_POSE_enu_y(buf);
float enu_z = DL_EXTERNAL_POSE_enu_z(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_i = DL_EXTERNAL_POSE_body_qi(buf);
float quat_x = DL_EXTERNAL_POSE_body_qx(buf); float quat_x = DL_EXTERNAL_POSE_body_qx(buf);
float quat_y = DL_EXTERNAL_POSE_body_qy(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 FloatQuat orient;
struct FloatEulers orient_eulers; struct FloatEulers orient_eulers;
orient.qi = quat_i; // Transformation of External Pose. Optitrack motive 2.X Yup
orient.qx = quat_y; //north orient.qi = quat_i ;
orient.qy = -quat_x; //east orient.qx = quat_y ;
orient.qz = -quat_z; //down orient.qy = quat_x ;
orient.qz = -quat_z;
float_eulers_of_quat(&orient_eulers, &orient); 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_time = get_sys_time_usec();
ins_ext_pos.ev_pos.y = enu_x; ins_ext_pos.ev_pos.x = enu_y;
ins_ext_pos.ev_pos.z = -enu_z; ins_ext_pos.ev_pos.y = enu_x;
ins_ext_pos.ev_att.phi = orient_eulers.phi; ins_ext_pos.ev_pos.z = -enu_z;
ins_ext_pos.ev_att.theta = orient_eulers.theta; ins_ext_pos.ev_vel.x = enu_yd;
ins_ext_pos.ev_att.psi = orient_eulers.psi; 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; 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, send_ins);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z); 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_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 #endif
// Get IMU through ABI // Get IMU through ABI
AbiBindMsgIMU_ACCEL(INS_EXT_POSE_IMU_ID, &accel_ev, accel_cb); AbiBindMsgIMU_ACCEL(INS_EXT_POSE_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_GYRO(INS_EXT_POSE_IMU_ID, &gyro_ev, gyro_cb); AbiBindMsgIMU_GYRO(INS_EXT_POSE_IMU_ID, &gyro_ev, gyro_cb);
// Get External Pose through datalink message: setup in xml // Get External Pose through datalink message: setup in xml
// Initialize EKF // Initialize EKF
@@ -315,9 +357,9 @@ static inline void ekf_init(void)
float Z0[EKF_NUM_OUTPUTS] = {0, 0, 0, 0, 0, 0}; 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 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_P_, ekf_P, EKF_NUM_STATES);
MAKE_MATRIX_PTR(ekf_Q_, ekf_Q, EKF_NUM_INPUTS); MAKE_MATRIX_PTR(ekf_Q_, ekf_Q, EKF_NUM_INPUTS);
sw/airborne/modules/ins/ins_ext_pose.h
+2 -2
View File
@@ -37,7 +37,8 @@ Section 5.3: Non-additive noise formulation and equations
#define EKF_NUM_OUTPUTS 6 #define EKF_NUM_OUTPUTS 6
#include "std.h" #include "std.h"
#include "math/pprz_algebra_float.h"
#include "state.h"
#include <stdio.h> #include <stdio.h>
@@ -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_header(FILE *file);
extern void ins_ext_pos_log_data(FILE *file); extern void ins_ext_pos_log_data(FILE *file);
#endif #endif
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: 828de444d7...15a861f667