Started addressing comments

paparazzi · Feb 5, 2024 · 9559109 · 9559109
1 parent 5ec4241
commit 9559109
Show file tree

Hide file tree

Showing 5 changed files with 8 additions and 13 deletions.
diff --git a/conf/modules/ins_ext_pose.xml b/conf/modules/ins_ext_pose.xml
@@ -22,7 +22,7 @@
     <file name="ins_ext_pose.h"/>
   </header>
   <init fun="ins_ext_pose_init()"/>
-  <periodic fun="ins_ext_pose_run()" freq="500" />
+  <periodic fun="ins_ext_pose_run()"/>
   <datalink message="EXTERNAL_POSE" fun="ins_ext_pose_msg_update(buf)"/>
 
   <makefile target="ap">

diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml
@@ -250,14 +250,14 @@
       <message name="GUIDANCE_INDI_HYBRID"     period="0.002"/>
       <message name="HYBRID_GUIDANCE"          period="0.02"/>
       <message name="ESC"                      period="0.02"/>
-      <message name="STAB_ATTITUDE"            period="0.002"/>        
+      <message name="STAB_ATTITUDE"            period="0.002"/>
       <message name="PPM"                      period="0.05"/>
       <message name="ACTUATORS"                period="0.002"/>
       <message name="GPS_RTK"                  period="0.1"/>
       <message name="IMU_HEATER"               period="1.0"/>
       <message name="AIRSPEED_RAW"             period="0.01"/>
       <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="DOUBLET"                  period="0.002"/>

diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp
@@ -480,7 +480,7 @@ static void send_ahrs_quat(struct transport_tx *trans, struct link_device *dev)
                               &ahrs_id);
 }
 
-static void send_eternal_pose_optitrack(struct transport_tx *trans, struct link_device *dev)
+static void send_eternal_pose_down(struct transport_tx *trans, struct link_device *dev)
 {
 
   float sample_temp[11];
@@ -610,7 +610,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_eternal_pose_optitrack);
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EXTERNAL_POSE_DOWN, send_eternal_pose_down);
 #endif
 
   /*
@@ -740,7 +740,7 @@ void ins_ekf2_parse_EXTERNAL_POSE(uint8_t *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(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);

diff --git a/sw/airborne/modules/ins/ins_ext_pose.c b/sw/airborne/modules/ins/ins_ext_pose.c
@@ -43,10 +43,6 @@
 #define DEBUG_PRINT(...) {}
 #endif
 
-#ifndef INS_EXT_POSE
-#define INS_EXT_POSE TRUE
-#endif
-
 /** Data for telemetry and LTP origin.
  */
 
@@ -104,7 +100,7 @@ static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
                         &ins_ext_pos.ltp_def.hmsl, (float *)&fake_qfe);
 }
 
-static void send_eternal_pose_optitrack(struct transport_tx *trans, struct link_device *dev)
+static void send_eternal_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,
@@ -259,7 +255,7 @@ 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_eternal_pose_optitrack);
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EXTERNAL_POSE_DOWN, send_eternal_pose_down);
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_BIAS, send_ahrs_bias);
 #endif
 

diff --git a/sw/airborne/modules/ins/ins_ext_pose.h b/sw/airborne/modules/ins/ins_ext_pose.h
@@ -43,7 +43,6 @@ Section 5.3: Non-additive noise formulation and equations
 
 
 extern float ekf_X[EKF_NUM_STATES];
-extern float ekf_U[EKF_NUM_INPUTS];
 
 extern void ins_ext_pose_init(void);
 extern void ins_ext_pose_run(void);