Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Correct Heading of Rotating Wing Drone with Optitrack (#3331)

Browse Source 
* first push

* move quat rotation to scheduling module

* code cleanup

* Use rotation matrix for heading correction

* Small adjustments

* declaration in ins file
This commit is contained in:
Tomaso Maria Luigi De Ponti
2024-07-23 11:12:39 +02:00
committed by GitHub
parent 862f69267e
commit a608eb89a9
7 changed files with 48 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml
+2 -2
View File
@@ -95,8 +95,8 @@
<!-- IMU / INS -->
<module name="imu" type="cube"/>
<module name="ins" type="ekf2">
<define name="INS_EKF2_OPTITRACK" value="true"/>
<define name="INS_EKF2_RW" value="true"/>
<define name="INS_EKF2_OPTITRACK" value="TRUE"/>
<define name="INS_EXT_VISION_ROTATION" value="TRUE"/>
</module>
<!-- Actuators on dual CAN bus -->
conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml
+1 -1
View File
@@ -239,7 +239,7 @@
<define name="FWD_SIDESLIP_GAIN" value="0.25"/> <!-- cyfoam 0.32-->
<!-- <define name="THRESHOLD_GROUND_DETECT" value="40"/> -->
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EXT_POSE_RW" value="TRUE"/>
<define name="INS_EXT_VISION_ROTATION" value="TRUE"/>
</section>
<section name="GROUND_DETECT">
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
sw/airborne/modules/ctrl/eff_scheduling_rot_wing_V2.c
+6 -1
View File
@@ -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;
sw/airborne/modules/ins/ins_ekf2.cpp
+17 -2
View File
@@ -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<float, 3>() * INS_EKF2_EVV_NOISE;
sample_ev.angVar = INS_EKF2_EVA_NOISE;
sw/airborne/modules/ins/ins_ekf2.h
+4
View File
@@ -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
sw/airborne/modules/ins/ins_ext_pose.c
+14 -1
View File
@@ -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();
sw/airborne/modules/ins/ins_ext_pose.h
+4
View File
@@ -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);