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

[imu] Add fast logging

Browse Source
This commit is contained in:
Freek van Tienen
2023-10-16 13:44:30 +02:00
parent 515cb9167f
commit 127409a14d
3 changed files with 55 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/rot_wing_25kg.xml
+12 -9
View File
@@ -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>
@@ -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_">
@@ -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_">
@@ -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>
conf/modules/imu_common.xml
+2
View File
@@ -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>
sw/airborne/modules/imu/imu.c
+41 -6
View File
@@ -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"
@@ -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
@@ -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);
@@ -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
@@ -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);