Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: make systemid use new chirp math function

This commit is contained in:
Bill Geyer 2022-03-14 22:53:57 -04:00 committed by Randy Mackay
parent 74db41806b
commit 937f22d708
2 changed files with 7 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/mode.h
View File

@ -1,6 +1,7 @@
#pragma once
#include "Copter.h"
#include <AP_Math/chirp.h>
class Parameters;
class ParametersG2;
@ -1481,6 +1482,8 @@ public:
static const struct AP_Param::GroupInfo var_info[];
Chirp chirp_input;
protected:
const char *name() const override { return "SYSTEMID"; }
@ -1489,7 +1492,6 @@ protected:
private:
void log_data() const;
float waveform(float time);
enum class AxisType {
NONE = 0, // none

44
ArduCopter/mode_systemid.cpp
View File

@ -95,6 +95,8 @@ bool ModeSystemId::init(bool ignore_checks)
systemid_state = SystemIDModeState::SYSTEMID_STATE_TESTING;
log_subsample = 0;
chirp_input.init(time_record, frequency_start, frequency_stop, time_fade_in, time_fade_out, time_const_freq);
gcs().send_text(MAV_SEVERITY_INFO, "SystemID Starting: axis=%d", (unsigned)axis);
copter.Log_Write_SysID_Setup(axis, waveform_magnitude, frequency_start, frequency_stop, time_fade_in, time_const_freq, time_record, time_fade_out);
@ -172,7 +174,8 @@ void ModeSystemId::run()
}
waveform_time += G_Dt;
waveform_sample = waveform(waveform_time - SYSTEM_ID_DELAY);
waveform_sample = chirp_input.update(waveform_time - SYSTEM_ID_DELAY, waveform_magnitude);
waveform_freq_rads = chirp_input.get_frequency_rads();
switch (systemid_state) {
case SystemIDModeState::SYSTEMID_STATE_STOPPED:
@ -291,43 +294,4 @@ void ModeSystemId::log_data() const
copter.Log_Write_Attitude();
}
// init_test - initialises the test
float ModeSystemId::waveform(float time)
{
float wMin = 2 * M_PI * frequency_start;
float wMax = 2 * M_PI * frequency_stop;
float window;
float output;
float B = logf(wMax / wMin);
if (time <= 0.0f) {
window = 0.0f;
} else if (time <= time_fade_in) {
window = 0.5 - 0.5 * cosf(M_PI * time / time_fade_in);
} else if (time <= time_record - time_fade_out) {
window = 1.0;
} else if (time <= time_record) {
window = 0.5 - 0.5 * cosf(M_PI * (time - (time_record - time_fade_out)) / time_fade_out + M_PI);
} else {
window = 0.0;
}
if (time <= 0.0f) {
waveform_freq_rads = wMin;
output = 0.0f;
} else if (time <= time_const_freq) {
waveform_freq_rads = wMin;
output = window * waveform_magnitude * sinf(wMin * time - wMin * time_const_freq);
} else if (time <= time_record) {
waveform_freq_rads = wMin * expf(B * (time - time_const_freq) / (time_record - time_const_freq));
output = window * waveform_magnitude * sinf((wMin * (time_record - time_const_freq) / B) * (expf(B * (time - time_const_freq) / (time_record - time_const_freq)) - 1));
} else {
waveform_freq_rads = wMax;
output = 0.0f;
}
return output;
}
#endif