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

Merge pull request #1496 from knmcguire/pocketdrone_stereocameradown_edgeflow_test_151221

Browse Source 
Stereocam updates and ins_int.c changes

Add INS_INT_GPS_ID to configure/disable the GPS ABI callback
This commit is contained in:
Felix Ruess
2015-12-27 12:11:59 +01:00
parent 2d20d6b12b cb9f1cddea
commit a1a8941758
3 changed files with 29 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/TUDELFT/tudelft_KM_control_panel.xml
+2 -2
View File
@@ -68,8 +68,8 @@
<arg flag="-speech" />
</program>
<program name="NatNet">
<arg flag="-ac" constant="1"/>
<arg flag="12"/>
<arg flag="-ac" constant="7"/>
<arg flag="114"/>
<arg flag="-small"/>
<arg flag="-tf" constant="5"/>
</program>
conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml
+8 -6
View File
@@ -181,14 +181,14 @@
<define name="H_X" value="0.39049610"/>
<define name="H_Y" value="0.00278894"/>
<define name="H_Z" value="0.92060036"/>
<define name="USE_GPS_HEADING" value="1"/>
<define name="USE_GPS_HEADING" value="0"/>
</section>
<define name="USE_GPS" value="FALSE"/>
<define name="USE_GPS_HEADING" value="FALSE"/>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="40"/>
<define name="DGAIN" value="10"/>
<define name="PGAIN" value="120"/>
<define name="DGAIN" value="40"/>
<define name="IGAIN" value="0"/>
<define name="REF_MAX_SPEED" value="0.5"/>
<define name="USE_REF" value="FALSE"/>
@@ -252,6 +252,8 @@
<subsystem name="gps" type="datalink"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" />
<subsystem name="ins">
<define name="INS_INT_GPS_ID" value="ABI_DISABLE"/>
</subsystem>
</firmware>
</airframe>
sw/airborne/subsystems/ins/ins_int.c
+19 -2
View File
@@ -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;
@@ -516,12 +519,14 @@ 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;
float dt = 0;
/* rotate velocity estimate to nav/ltp frame */
struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f();
@@ -529,7 +534,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};
@@ -538,6 +551,10 @@ 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);
if (last_stamp > 0) {
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);
}
#endif
ins_ned_to_state();
@@ -554,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);
}