From 9508e1698f073c22fffa43208647e5aa2acb7c45 Mon Sep 17 00:00:00 2001 From: "k.n.mcguire@tudelft.nl" Date: Tue, 22 Dec 2015 14:31:33 +0100 Subject: [PATCH 1/7] [MODULES] stereocam Changed some conf file for stereocam2state module --- conf/TUDELFT/tudelft_KM_control_panel.xml | 4 ++-- .../TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/conf/TUDELFT/tudelft_KM_control_panel.xml b/conf/TUDELFT/tudelft_KM_control_panel.xml index 4eb19d920c..26e7a43fe5 100644 --- a/conf/TUDELFT/tudelft_KM_control_panel.xml +++ b/conf/TUDELFT/tudelft_KM_control_panel.xml @@ -68,8 +68,8 @@ - - + + diff --git a/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml b/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml index 0158d4dd58..8342728928 100644 --- a/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml +++ b/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml @@ -187,8 +187,8 @@
- - + + From ef016aa9d3a7a8c57234c4182c25cbac3c718964 Mon Sep 17 00:00:00 2001 From: "k.n.mcguire@tudelft.nl" Date: Tue, 22 Dec 2015 14:39:38 +0100 Subject: [PATCH 2/7] [INS] ins_int.c at vel_est_cb(), position is intergrated from velocity for guidance --- sw/airborne/subsystems/ins/ins_int.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 10f67dfc8e..97e59b7955 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -538,6 +538,8 @@ 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); #endif ins_ned_to_state(); From 5d3156eaf1949571918ee5ef70c9b54f7789afb4 Mon Sep 17 00:00:00 2001 From: "k.n.mcguire@tudelft.nl" Date: Tue, 22 Dec 2015 14:42:28 +0100 Subject: [PATCH 3/7] [INS] ins_int.c at gps_cb, USE_GPS is also used at the update function. --- sw/airborne/subsystems/ins/ins_int.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 97e59b7955..3444ef2c89 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -512,7 +512,9 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct GpsState *gps_s) { +#if use_GPS ins_int_update_gps(gps_s); +#endif } static void vel_est_cb(uint8_t sender_id __attribute__((unused)), From b0562b692e6a57ac07b159382afa6afb7a4baf87 Mon Sep 17 00:00:00 2001 From: "k.n.mcguire@tudelft.nl" Date: Tue, 22 Dec 2015 15:45:14 +0100 Subject: [PATCH 4/7] [INS] Added dT to velocity integrate to velocity --- sw/airborne/subsystems/ins/ins_int.c | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 3444ef2c89..ee0958e74e 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -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(); From 8956dcc286aca50e4526676840eeb76b6d2c332f Mon Sep 17 00:00:00 2001 From: "k.n.mcguire@tudelft.nl" Date: Tue, 22 Dec 2015 16:28:20 +0100 Subject: [PATCH 5/7] [INS] vel_est_cb() unused variable free with calculation of dt in hff --- sw/airborne/subsystems/ins/ins_int.c | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index ee0958e74e..80b1c5d9eb 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -525,6 +525,7 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 vel_body = {x, y, z}; static uint32_t last_stamp = 0; + float dt = 0; /* rotate velocity estimate to nav/ltp frame */ 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; 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 + (void)dt; //dt is unused variable in this define + struct FloatVect2 vel = {vel_ned.x, vel_ned.y}; 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.y = SPEED_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(); From 9fee89548c6aa5a47176d323b810c4075faac763 Mon Sep 17 00:00:00 2001 From: "k.n.mcguire@tudelft.nl" Date: Wed, 23 Dec 2015 13:14:21 +0100 Subject: [PATCH 6/7] [INS] created INS_INT_GPS_ID and removed USE_GPS at gps_cb() --- .../tudelft_ladylisa_bluegiga_stereoboard.xml | 12 +++++++----- sw/airborne/subsystems/ins/ins_int.c | 7 ++++--- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml b/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml index 8342728928..009456002a 100644 --- a/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml +++ b/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml @@ -181,10 +181,10 @@ - +
- - + +
@@ -223,7 +223,7 @@ - + @@ -252,6 +252,8 @@ - + + + diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 80b1c5d9eb..7081e20635 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -130,6 +130,9 @@ static void baro_cb(uint8_t sender_id, float pressure); #ifndef INS_INT_IMU_ID #define INS_INT_IMU_ID ABI_BROADCAST #endif +#ifndef INS_INT_GPS_ID +#define INS_INT_GPS_ID ABI_BROADCAST +#endif static abi_event accel_ev; static abi_event gps_ev; @@ -512,9 +515,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct GpsState *gps_s) { -#if use_GPS ins_int_update_gps(gps_s); -#endif } static void vel_est_cb(uint8_t sender_id __attribute__((unused)), @@ -570,6 +571,6 @@ void ins_int_register(void) * Subscribe to scaled IMU measurements and attach callbacks */ AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb); AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb); } From cb9f1cddeab7bc582e688df4cd5a1de708026f11 Mon Sep 17 00:00:00 2001 From: "k.n.mcguire@tudelft.nl" Date: Wed, 23 Dec 2015 17:24:56 +0100 Subject: [PATCH 7/7] [AIRFRAMES] For stereocam module after tests, some initial values for lisa s have been optimized --- .../TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml b/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml index 009456002a..110d41ccc0 100644 --- a/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml +++ b/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml @@ -187,7 +187,7 @@
- + @@ -223,7 +223,7 @@ - +