Skip to content

Commit

Permalink
[imu] Add fast logging
Browse files Browse the repository at this point in the history
  • Loading branch information
@fvantienen
fvantienen committed Oct 17, 2023
1 parent 515cb91 commit 127409a
Show file tree
Hide file tree
Showing 3 changed files with 55 additions and 15 deletions.
21 changes: 12 additions & 9 deletions conf/airframes/tudelft/rot_wing_25kg.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,7 @@
<axis name="PITCH" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
<axis name="THRUST_X" failsafe_value="0"/>
<axis name="THRUST_X" failsafe_value="0"/>
</commands>


Expand Down Expand Up @@ -258,9 +258,12 @@
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={-96,-95,-292}, .scale={{33136,8709,8764},{59233,15343,15761}}}}"/> <!-- Calibrated 't Harde 17-08-2023 -->

<!-- Define axis in hover frame -->
<define name="BODY_TO_IMU_PHI" value="180." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PHI" value="180." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>

<!-- Log in high speed -->
<define name="LOG_HIGHSPEED" value="TRUE"/>
</section>

<section name="MAG_RM3100" prefix="RM3100_">
Expand Down Expand Up @@ -357,8 +360,8 @@

<!-- Other -->
<define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100}"/>
<define value="FALSE" name="USE_ADAPTIVE"/>
<define value="0.001" name="ADAPTIVE_MU"/>
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.001"/>
</section>

<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
Expand Down Expand Up @@ -399,9 +402,9 @@
<define name="PITCH_LIFT_EFF" value="0.0"/>
<define name="ZERO_AIRSPEED" value="TRUE"/>

<define name="QUADPLANE" value="TRUE"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="THRUST_X_EFF" value="0.00055"/>
<define name="QUADPLANE" value="TRUE"/>
<define name="THRUST_Z_EFF" value="-0.0023"/>
<define name="THRUST_X_EFF" value="0.00055"/>

</section>

Expand Down
2 changes: 2 additions & 0 deletions conf/modules/imu_common.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
<define name="GYRO_ABI_SEND_ID" value="ABI_BROADCAST" description="The gyro ABI ID which is send over telemetry/logging"/>
<define name="ACCEL_ABI_SEND_ID" value="ABI_BROADCAST" description="The accel ABI ID which is send over telemetry/logging"/>
<define name="MAG_ABI_SEND_ID" value="ABI_BROADCAST" description="The mag ABI ID which is send over telemetry/logging"/>
<define name="LOG_HIGHSPEED" value="FALSE" description="Log all the accel/gyro measurements at the IMU sampling rates in floats"/>
<define name="LOG_HIGHSPEED_DEVICE" value="flightrecorder_sdlog" description="The device to log all the highspeeds measurements"/>
</section>
</doc>
<settings>
Expand Down
47 changes: 41 additions & 6 deletions sw/airborne/modules/imu/imu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -173,6 +173,11 @@ PRINT_CONFIG_VAR(IMU_ACCEL_ABI_SEND_ID)
#endif
PRINT_CONFIG_VAR(IMU_MAG_ABI_SEND_ID)

/** By default log highspeed on the flightrecorder */
#ifndef IMU_LOG_HIGHSPEED_DEVICE
#define IMU_LOG_HIGHSPEED_DEVICE flightrecorder_sdlog
#endif


#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
Expand Down Expand Up @@ -590,9 +595,18 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates

// Add all the other samples
for(uint8_t i = 0; i < samples-1; i++) {
integrated_sensor.p += RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale[0].p / gyro->scale[1].p);
integrated_sensor.q += RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale[0].q / gyro->scale[1].q);
integrated_sensor.r += RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r);
struct FloatRates f_sample;
f_sample.p = RATE_FLOAT_OF_BFP((data[i].p - gyro->neutral.p) * gyro->scale[0].p / gyro->scale[1].p);
f_sample.q = RATE_FLOAT_OF_BFP((data[i].q - gyro->neutral.q) * gyro->scale[0].q / gyro->scale[1].q);
f_sample.r = RATE_FLOAT_OF_BFP((data[i].r - gyro->neutral.r) * gyro->scale[0].r / gyro->scale[1].r);

#if IMU_LOG_HIGHSPEED
pprz_msg_send_IMU_GYRO(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.p, &f_sample.q, &f_sample.r);
#endif

integrated_sensor.p += f_sample.p;
integrated_sensor.q += f_sample.q;
integrated_sensor.r += f_sample.r;
}

// Rotate to body frame
Expand All @@ -612,6 +626,12 @@ static void imu_gyro_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates
(void)rate; // Surpress compile warning not used
#endif

#if IMU_LOG_HIGHSPEED
struct FloatRates f_sample;
RATES_FLOAT_OF_BFP(f_sample, scaled);
pprz_msg_send_IMU_GYRO(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.p, &f_sample.q, &f_sample.r);
#endif

// Copy and send
gyro->temperature = temp;
RATES_COPY(gyro->scaled, scaled_rot);
Expand Down Expand Up @@ -658,9 +678,18 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect

// Add all the other samples
for(uint8_t i = 0; i < samples-1; i++) {
integrated_sensor.x += ACCEL_FLOAT_OF_BFP((data[i].x - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x);
integrated_sensor.y += ACCEL_FLOAT_OF_BFP((data[i].y - accel->neutral.y) * accel->scale[0].y / accel->scale[1].y);
integrated_sensor.z += ACCEL_FLOAT_OF_BFP((data[i].z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z);
struct FloatVect3 f_sample;
f_sample.x = ACCEL_FLOAT_OF_BFP((data[i].x - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x);
f_sample.y = ACCEL_FLOAT_OF_BFP((data[i].y - accel->neutral.y) * accel->scale[0].y / accel->scale[1].y);
f_sample.z = ACCEL_FLOAT_OF_BFP((data[i].z - accel->neutral.z) * accel->scale[0].z / accel->scale[1].z);

#if IMU_LOG_HIGHSPEED
pprz_msg_send_IMU_ACCEL(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.x, &f_sample.y, &f_sample.z);
#endif

integrated_sensor.x += f_sample.x;
integrated_sensor.y += f_sample.y;
integrated_sensor.z += f_sample.z;
}

// Rotate to body frame
Expand All @@ -680,6 +709,12 @@ static void imu_accel_raw_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect
(void)rate; // Surpress compile warning not used
#endif

#if IMU_LOG_HIGHSPEED
struct FloatVect3 f_sample;
ACCELS_FLOAT_OF_BFP(f_sample, scaled);
pprz_msg_send_IMU_ACCEL(&pprzlog_tp.trans_tx, &(IMU_LOG_HIGHSPEED_DEVICE).device, AC_ID, &sender_id, &f_sample.x, &f_sample.y, &f_sample.z);
#endif

// Copy and send
accel->temperature = temp;
VECT3_COPY(accel->scaled, scaled_rot);
Expand Down

0 comments on commit 127409a

Please sign in to comment.