Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

ThirdOrderCompFilter: remove last_time_constant_xy and _z static variables to save 8 bytes.

Browse Source
This commit is contained in:
rmackay9 2012-12-06 10:24:58 +09:00
parent 12e1ce50e2
commit a83f6e54b5
1 changed files with 6 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/Filter/ThirdOrderCompFilter.cpp
View File

@ -13,31 +13,22 @@
// update_gains - update gains from time constant (given in seconds)
void ThirdOrderCompFilter::update_gains(float time_constant_seconds_xy, float time_constant_seconds_z)
{
static float last_time_constant_xy = 0;
static float last_time_constant_z = 0;
// X & Y axis time constant
if( time_constant_seconds_xy == 0 ) {
_k1_xy = _k2_xy = _k3_xy = 0;
}else{
if( time_constant_seconds_xy != last_time_constant_xy ) {
_k1_xy = 3 / time_constant_seconds_xy;
_k2_xy = 3 / (time_constant_seconds_xy*time_constant_seconds_xy);
_k3_xy = 1 / (time_constant_seconds_xy*time_constant_seconds_xy*time_constant_seconds_xy);
last_time_constant_xy = time_constant_seconds_xy;
}
_k1_xy = 3 / time_constant_seconds_xy;
_k2_xy = 3 / (time_constant_seconds_xy*time_constant_seconds_xy);
_k3_xy = 1 / (time_constant_seconds_xy*time_constant_seconds_xy*time_constant_seconds_xy);
}
// Z axis time constant
if( time_constant_seconds_z == 0 ) {
_k1_z = _k2_z = _k3_z = 0;
}else{
if( time_constant_seconds_z != last_time_constant_z ) {
_k1_z = 3 / time_constant_seconds_z;
_k2_z = 3 / (time_constant_seconds_z*time_constant_seconds_z);
_k3_z = 1 / (time_constant_seconds_z*time_constant_seconds_z*time_constant_seconds_z);
last_time_constant_z = time_constant_seconds_z;
}
_k1_z = 3 / time_constant_seconds_z;
_k2_z = 3 / (time_constant_seconds_z*time_constant_seconds_z);
_k3_z = 1 / (time_constant_seconds_z*time_constant_seconds_z*time_constant_seconds_z);
}
}