fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fake_imu: add fake ESC status for testing dynamic notch filters

Browse Source
This commit is contained in:
Daniel Agar 2021-05-25 12:17:22 -04:00
parent f25a70a674
commit 561cfca4f9
2 changed files with 62 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
src/examples/fake_imu/FakeImu.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -92,6 +92,10 @@ void FakeImu::Run()
const double timestamp_sample_s = static_cast<double>(gyro.timestamp_sample - _time_start_us) / 1e6;
float x_freq = 0;
float y_freq = 0;
float z_freq = 0;
for (int n = 0; n < gyro.samples; n++) {
// timestamp_sample corresponds to last sample
const double t = timestamp_sample_s - (gyro.samples - n - 1) * dt_s;
@ -106,15 +110,57 @@ void FakeImu::Run()
gyro.z[n] = roundf(A * sin(2 * M_PI * z_F * t));
if (n == 0) {
float x_freq = (x_f1 - x_f0) * (t / T) + x_f0;
float y_freq = (y_f1 - y_f0) * (t / T) + y_f0;
float z_freq = (z_f1 - z_f0) * (t / T) + z_f0;
x_freq = (x_f1 - x_f0) * (t / T) + x_f0;
y_freq = (y_f1 - y_f0) * (t / T) + y_f0;
z_freq = (z_f1 - z_f0) * (t / T) + z_f0;
_px4_accel.update(gyro.timestamp_sample, x_freq, y_freq, z_freq);
}
}
_px4_gyro.updateFIFO(gyro);
#if defined(FAKE_IMU_FAKE_ESC_STATUS)
// publish fake esc status at ~10 Hz
if (hrt_elapsed_time(&_esc_status_pub.get().timestamp) > 100_ms) {
auto &esc_status = _esc_status_pub.get();
esc_status.esc_count = 3;
// ESC 0 follow X axis RPM
if (!(timestamp_sample_s > 1.5 && timestamp_sample_s < 2.0)) {
// simulate drop out at 1.5 to 2 seconds
esc_status.esc[0].timestamp = hrt_absolute_time();
esc_status.esc[0].esc_rpm = x_freq * 60;
}
// ESC 1 follow Y axis RPM
if (!(timestamp_sample_s > 2.5 && timestamp_sample_s < 3.0)) {
// simulate drop out at 2.5 to 3 seconds
esc_status.esc[1].timestamp = hrt_absolute_time();
esc_status.esc[1].esc_rpm = y_freq * 60;
}
// ESC 2 follow Z axis RPM
if (!(timestamp_sample_s > 3.5 && timestamp_sample_s < 4.0)) {
// simulate drop out at 3.5 to 4 seconds
esc_status.esc[2].timestamp = hrt_absolute_time();
esc_status.esc[2].esc_rpm = z_freq * 60;
}
// simulate brief dropout of all data
if (timestamp_sample_s > 5.5 && timestamp_sample_s < 5.6) {
esc_status.esc[0].esc_rpm = 0;
esc_status.esc[1].esc_rpm = 0;
esc_status.esc[2].esc_rpm = 0;
}
esc_status.timestamp = hrt_absolute_time();
_esc_status_pub.update();
}
#endif // FAKE_IMU_FAKE_ESC_STATUS
}
int FakeImu::task_spawn(int argc, char *argv[])

13
src/examples/fake_imu/FakeImu.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -44,6 +44,13 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_gyro_fifo.h>
// fake ESC RPM for testing dynamic notch filtering
//#define FAKE_IMU_FAKE_ESC_STATUS
#if defined(FAKE_IMU_FAKE_ESC_STATUS)
# include <uORB/topics/esc_status.h>
#endif // FAKE_IMU_FAKE_ESC_STATUS
class FakeImu : public ModuleBase<FakeImu>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
@ -72,4 +79,8 @@ private:
hrt_abstime _time_start_us{0};
uint32_t _sensor_interval_us{1250};
#if defined(FAKE_IMU_FAKE_ESC_STATUS)
uORB::PublicationData<esc_status_s> _esc_status_pub {ORB_ID(esc_status)};
#endif // FAKE_IMU_FAKE_ESC_STATUS
};