-
Notifications
You must be signed in to change notification settings - Fork 1.4k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Smith Compensator on PID measurement #6900
Changes from 1 commit
06fed7b
b50bf02
b3f42bc
c771162
4fd468b
b265369
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -83,5 +83,6 @@ typedef enum { | |
DEBUG_FW_D, | ||
DEBUG_IMU2, | ||
DEBUG_ALTITUDE, | ||
DEBUG_SMITH_COMPENSATOR, | ||
DEBUG_COUNT | ||
} debugType_e; |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,69 @@ | ||
/* | ||
* This file is part of INAV Project. | ||
* | ||
* This Source Code Form is subject to the terms of the Mozilla Public | ||
* License, v. 2.0. If a copy of the MPL was not distributed with this file, | ||
* You can obtain one at http://mozilla.org/MPL/2.0/. | ||
* | ||
* Alternatively, the contents of this file may be used under the terms | ||
* of the GNU General Public License Version 3, as described below: | ||
* | ||
* This file is free software: you may copy, redistribute and/or modify | ||
* it under the terms of the GNU General Public License as published by the | ||
* Free Software Foundation, either version 3 of the License, or (at your | ||
* option) any later version. | ||
* | ||
* This file is distributed in the hope that it will be useful, but | ||
* WITHOUT ANY WARRANTY; without even the implied warranty of | ||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General | ||
* Public License for more details. | ||
* | ||
* You should have received a copy of the GNU General Public License | ||
* along with this program. If not, see http://www.gnu.org/licenses/. | ||
* | ||
* This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight | ||
* | ||
*/ | ||
|
||
#include "platform.h" | ||
#ifdef USE_SMITH_PREDICTOR | ||
|
||
#include <stdbool.h> | ||
#include "common/axis.h" | ||
#include "flight/smith_predictor.h" | ||
#include "build/debug.h" | ||
|
||
FUNCTION_COMPILE_FOR_SPEED | ||
float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) { | ||
UNUSED(axis); | ||
if (predictor->enabled) { | ||
predictor->data[predictor->idx] = sample; | ||
|
||
predictor->idx++; | ||
if (predictor->idx > predictor->samples) { | ||
predictor->idx = 0; | ||
} | ||
|
||
// filter the delayedGyro to help reduce the overall noise this prediction adds | ||
float delayed = pt1FilterApply(&predictor->smithPredictorFilter, predictor->data[predictor->idx]); | ||
float delayCompensatedSample = predictor->smithPredictorStrength * (sample - delayed); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Small note here, in EmuFlight I recently changed this code. I moved the pt1Filter to be on the delayCompensatedSample instead of the delayed data. Seems to fly a bit better that way. There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. You've made other changes, but i'd still suggest this change. |
||
|
||
sample += delayCompensatedSample; | ||
} | ||
return sample; | ||
} | ||
|
||
FUNCTION_COMPILE_FOR_SIZE | ||
void smithPredictorInit(smithPredictor_t *predictor, uint8_t delay, uint8_t strength, uint16_t filterLpfHz, uint32_t looptime) { | ||
if (delay > 0) { | ||
predictor->enabled = true; | ||
predictor->samples = delay / (looptime / 100.0f); | ||
predictor->idx = 0; | ||
predictor->smithPredictorStrength = strength / 100.0f; | ||
pt1FilterInit(&predictor->smithPredictorFilter, filterLpfHz, looptime * 1e-6f); | ||
} else { | ||
predictor->enabled = false; | ||
} | ||
} | ||
|
||
#endif |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,45 @@ | ||
/* | ||
* This file is part of INAV Project. | ||
* | ||
* This Source Code Form is subject to the terms of the Mozilla Public | ||
* License, v. 2.0. If a copy of the MPL was not distributed with this file, | ||
* You can obtain one at http://mozilla.org/MPL/2.0/. | ||
* | ||
* Alternatively, the contents of this file may be used under the terms | ||
* of the GNU General Public License Version 3, as described below: | ||
* | ||
* This file is free software: you may copy, redistribute and/or modify | ||
* it under the terms of the GNU General Public License as published by the | ||
* Free Software Foundation, either version 3 of the License, or (at your | ||
* option) any later version. | ||
* | ||
* This file is distributed in the hope that it will be useful, but | ||
* WITHOUT ANY WARRANTY; without even the implied warranty of | ||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General | ||
* Public License for more details. | ||
* | ||
* You should have received a copy of the GNU General Public License | ||
* along with this program. If not, see http://www.gnu.org/licenses/. | ||
* | ||
* This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight | ||
* | ||
*/ | ||
|
||
#pragma once | ||
|
||
#include <stdint.h> | ||
#include "common/filter.h" | ||
|
||
#define MAX_SMITH_SAMPLES 64 | ||
|
||
typedef struct smithPredictor_s { | ||
bool enabled; | ||
uint8_t samples; | ||
uint8_t idx; | ||
float data[MAX_SMITH_SAMPLES + 1]; | ||
pt1Filter_t smithPredictorFilter; | ||
float smithPredictorStrength; | ||
} smithPredictor_t; | ||
|
||
float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample); | ||
void smithPredictorInit(smithPredictor_t *predictor, uint8_t delay, uint8_t strength, uint16_t filterLpfHz, uint32_t looptime); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
any reason you don't use a for loop here to get all three axis?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
At the end it's almost the same. Loop or 3 calls.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
do 3 calls use more memory than a loop? Thats what i've always thought and for init code we don't need to worry about performance. Maybe i'm wrong and the compiler will optimize for that anyway.