aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Vehicle: add function to log dynamic notch frequencies

Browse Source 
move harmonic notch update here
This commit is contained in:
Andy Piper 2020-06-09 21:14:25 +01:00 committed by Andrew Tridgell
parent 5bedf44734
commit cb524b7d3d
2 changed files with 23 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -1,5 +1,6 @@
#include "AP_Vehicle.h"
#include <AP_BLHeli/AP_BLHeli.h>
#include <AP_Common/AP_FWVersion.h>
#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(AP_Vehicle, &vehicle, func, rate_hz, max_time_micros)
@ -136,6 +137,7 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update, 400, 50),
SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50),
#endif
SCHED_TASK(update_dynamic_notch, 200, 200),
SCHED_TASK(send_watchdog_reset_statustext, 0.1, 20),
};
@ -205,6 +207,22 @@ void AP_Vehicle::send_watchdog_reset_statustext()
);
}
// @LoggerMessage: FTN
// @Description: Filter Tuning Messages
// @Field: TimeUS: microseconds since system startup
// @Field: NDn: number of active dynamic harmonic notches
// @Field: DnF1: dynamic harmonic notch centre frequency for motor 1
// @Field: DnF2: dynamic harmonic notch centre frequency for motor 2
// @Field: DnF3: dynamic harmonic notch centre frequency for motor 3
// @Field: DnF4: dynamic harmonic notch centre frequency for motor 4
void AP_Vehicle::write_notch_log_messages() const
{
const float* notches = ins.get_gyro_dynamic_notch_center_frequencies_hz();
AP::logger().Write(
"FTN", "TimeUS,NDn,DnF1,DnF2,DnF3,DnF4", "s-zzzz", "F-----", "QBffff", AP_HAL::micros64(), ins.get_num_gyro_dynamic_notch_center_frequencies(),
notches[0], notches[1], notches[2], notches[3]);
}
AP_Vehicle *AP_Vehicle::_singleton = nullptr;
AP_Vehicle *AP_Vehicle::get_singleton()

5
libraries/AP_Vehicle/AP_Vehicle.h
View File

@ -176,6 +176,11 @@ public:
// get target location (for use by scripting)
virtual bool get_target_location(Location& target_loc) { return false; }
// write out harmonic notch log messages
void write_notch_log_messages() const;
// update the harmonic notch
virtual void update_dynamic_notch() {};
protected:
virtual void init_ardupilot() = 0;