Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[INS] Added dT to velocity integrate to velocity

Browse Source
This commit is contained in:
k.n.mcguire@tudelft.nl
2015-12-22 15:45:14 +01:00
parent 5d3156eaf1
commit b0562b692e
1 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ins/ins_int.c
+8 -3
View File
@@ -518,12 +518,13 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
}
static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
uint32_t stamp,
float x, float y, float z,
float noise __attribute__((unused)))
{
struct FloatVect3 vel_body = {x, y, z};
static uint32_t last_stamp = 0;
/* rotate velocity estimate to nav/ltp frame */
struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f();
@@ -540,8 +541,12 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
#else
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_pos.x = ins_int.ltp_pos.x + POS_BFP_OF_REAL(vel_ned.x);
ins_int.ltp_pos.y = ins_int.ltp_pos.y + POS_BFP_OF_REAL(vel_ned.y);
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.y = ins_int.ltp_pos.y + POS_BFP_OF_REAL(dt * vel_ned.y);
}
last_stamp = stamp;
#endif
ins_ned_to_state();