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

[INS] vel_est_cb() unused variable free with calculation of dt in hff

Browse Source
This commit is contained in:
k.n.mcguire@tudelft.nl
2015-12-22 16:28:20 +01:00
parent b0562b692e
commit 8956dcc286
1 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ins/ins_int.c
+9 -2
View File
@@ -525,6 +525,7 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
struct FloatVect3 vel_body = {x, y, z}; struct FloatVect3 vel_body = {x, y, z};
static uint32_t last_stamp = 0; static uint32_t last_stamp = 0;
float dt = 0;
/* rotate velocity estimate to nav/ltp frame */ /* rotate velocity estimate to nav/ltp frame */
struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f(); struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f();
@@ -532,7 +533,15 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
struct FloatVect3 vel_ned; struct FloatVect3 vel_ned;
float_quat_vmult(&vel_ned, &q_b2n, &vel_body); float_quat_vmult(&vel_ned, &q_b2n, &vel_body);
if (last_stamp > 0) {
dt = (float)(stamp - last_stamp) * 1e-6;
}
last_stamp = stamp;
#if USE_HFF #if USE_HFF
(void)dt; //dt is unused variable in this define
struct FloatVect2 vel = {vel_ned.x, vel_ned.y}; struct FloatVect2 vel = {vel_ned.x, vel_ned.y};
struct FloatVect2 Rvel = {noise, noise}; struct FloatVect2 Rvel = {noise, noise};
@@ -542,11 +551,9 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(vel_ned.x); ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(vel_ned.x);
ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(vel_ned.y); ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(vel_ned.y);
if (last_stamp > 0) { if (last_stamp > 0) {
float dt = (float)(stamp - last_stamp) * 1e-6;
ins_int.ltp_pos.x = ins_int.ltp_pos.x + POS_BFP_OF_REAL(dt * vel_ned.x); ins_int.ltp_pos.x = ins_int.ltp_pos.x + POS_BFP_OF_REAL(dt * vel_ned.x);
ins_int.ltp_pos.y = ins_int.ltp_pos.y + POS_BFP_OF_REAL(dt * vel_ned.y); ins_int.ltp_pos.y = ins_int.ltp_pos.y + POS_BFP_OF_REAL(dt * vel_ned.y);
} }
last_stamp = stamp;
#endif #endif
ins_ned_to_state(); ins_ned_to_state();