Skip to content

Commit

Permalink
gyro config refactor
Browse files Browse the repository at this point in the history
  • Loading branch information
@rtlopez
rtlopez committed Oct 27, 2024
1 parent efdf952 commit fada2d2
Show file tree
Hide file tree
Showing 10 changed files with 113 additions and 107 deletions.
4 changes: 2 additions & 2 deletions lib/Espfc/src/Actuator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -221,8 +221,8 @@ class Actuator
{
return; // temporary disable
int scale = Math::clamp((int)_model.state.inputUs[AXIS_THRUST], 1000, 2000);
if(_model.config.gyroDynLpfFilter.cutoff > 0) {
int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyroDynLpfFilter.cutoff, _model.config.gyroDynLpfFilter.freq);
if(_model.config.gyro.dynLpfFilter.cutoff > 0) {
int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq);
for(size_t i = 0; i <= AXIS_YAW; i++) {
_model.state.gyroFilter[i].reconfigure(gyroFreq);
}
Expand Down
22 changes: 11 additions & 11 deletions lib/Espfc/src/Blackbox/Blackbox.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -110,18 +110,18 @@ int Blackbox::begin()
rcControlsConfigMutable()->deadband = _model.config.input.deadband;
rcControlsConfigMutable()->yaw_deadband = _model.config.input.deadband;

gyroConfigMutable()->gyro_hardware_lpf = _model.config.gyroDlpf;
gyroConfigMutable()->gyro_lpf1_type = _model.config.gyroFilter.type;
gyroConfigMutable()->gyro_lpf1_static_hz = _model.config.gyroFilter.freq;
gyroConfigMutable()->gyro_lpf1_dyn_min_hz = _model.config.gyroDynLpfFilter.cutoff;
gyroConfigMutable()->gyro_lpf1_dyn_max_hz = _model.config.gyroDynLpfFilter.freq;
gyroConfigMutable()->gyro_hardware_lpf = _model.config.gyro.dlpf;
gyroConfigMutable()->gyro_lpf1_type = _model.config.gyro.filter.type;
gyroConfigMutable()->gyro_lpf1_static_hz = _model.config.gyro.filter.freq;
gyroConfigMutable()->gyro_lpf1_dyn_min_hz = _model.config.gyro.dynLpfFilter.cutoff;
gyroConfigMutable()->gyro_lpf1_dyn_max_hz = _model.config.gyro.dynLpfFilter.freq;
gyroConfigMutable()->gyro_lpf1_dyn_expo = 5;
gyroConfigMutable()->gyro_lpf2_type = _model.config.gyroFilter2.type;
gyroConfigMutable()->gyro_lpf2_static_hz = _model.config.gyroFilter2.freq;
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = _model.config.gyroNotch1Filter.cutoff;
gyroConfigMutable()->gyro_soft_notch_hz_1 = _model.config.gyroNotch1Filter.freq;
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = _model.config.gyroNotch2Filter.cutoff;
gyroConfigMutable()->gyro_soft_notch_hz_2 = _model.config.gyroNotch2Filter.freq;
gyroConfigMutable()->gyro_lpf2_type = _model.config.gyro.filter2.type;
gyroConfigMutable()->gyro_lpf2_static_hz = _model.config.gyro.filter2.freq;
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = _model.config.gyro.notch1Filter.cutoff;
gyroConfigMutable()->gyro_soft_notch_hz_1 = _model.config.gyro.notch1Filter.freq;
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = _model.config.gyro.notch2Filter.cutoff;
gyroConfigMutable()->gyro_soft_notch_hz_2 = _model.config.gyro.notch2Filter.freq;
gyroConfigMutable()->gyro_sync_denom = 1;

dynNotchConfigMutable()->dyn_notch_count = _model.config.dynamicFilter.width;
Expand Down
44 changes: 22 additions & 22 deletions lib/Espfc/src/Cli.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -432,22 +432,22 @@ class Cli
Param(PSTR("debug_mode"), &c.debugMode, debugModeChoices),
Param(PSTR("debug_axis"), &c.debugAxis),

Param(PSTR("gyro_bus"), &c.gyroBus, busDevChoices),
Param(PSTR("gyro_dev"), &c.gyroDev, gyroDevChoices),
Param(PSTR("gyro_dlpf"), &c.gyroDlpf, gyroDlpfChoices),
Param(PSTR("gyro_align"), &c.gyroAlign, alignChoices),
Param(PSTR("gyro_lpf_type"), &c.gyroFilter.type, filterTypeChoices),
Param(PSTR("gyro_lpf_freq"), &c.gyroFilter.freq),
Param(PSTR("gyro_lpf2_type"), &c.gyroFilter2.type, filterTypeChoices),
Param(PSTR("gyro_lpf2_freq"), &c.gyroFilter2.freq),
Param(PSTR("gyro_lpf3_type"), &c.gyroFilter3.type, filterTypeChoices),
Param(PSTR("gyro_lpf3_freq"), &c.gyroFilter3.freq),
Param(PSTR("gyro_notch1_freq"), &c.gyroNotch1Filter.freq),
Param(PSTR("gyro_notch1_cutoff"), &c.gyroNotch1Filter.cutoff),
Param(PSTR("gyro_notch2_freq"), &c.gyroNotch2Filter.freq),
Param(PSTR("gyro_notch2_cutoff"), &c.gyroNotch2Filter.cutoff),
Param(PSTR("gyro_dyn_lpf_min"), &c.gyroDynLpfFilter.cutoff),
Param(PSTR("gyro_dyn_lpf_max"), &c.gyroDynLpfFilter.freq),
Param(PSTR("gyro_bus"), &c.gyro.bus, busDevChoices),
Param(PSTR("gyro_dev"), &c.gyro.dev, gyroDevChoices),
Param(PSTR("gyro_dlpf"), &c.gyro.dlpf, gyroDlpfChoices),
Param(PSTR("gyro_align"), &c.gyro.align, alignChoices),
Param(PSTR("gyro_lpf_type"), &c.gyro.filter.type, filterTypeChoices),
Param(PSTR("gyro_lpf_freq"), &c.gyro.filter.freq),
Param(PSTR("gyro_lpf2_type"), &c.gyro.filter2.type, filterTypeChoices),
Param(PSTR("gyro_lpf2_freq"), &c.gyro.filter2.freq),
Param(PSTR("gyro_lpf3_type"), &c.gyro.filter3.type, filterTypeChoices),
Param(PSTR("gyro_lpf3_freq"), &c.gyro.filter3.freq),
Param(PSTR("gyro_notch1_freq"), &c.gyro.notch1Filter.freq),
Param(PSTR("gyro_notch1_cutoff"), &c.gyro.notch1Filter.cutoff),
Param(PSTR("gyro_notch2_freq"), &c.gyro.notch2Filter.freq),
Param(PSTR("gyro_notch2_cutoff"), &c.gyro.notch2Filter.cutoff),
Param(PSTR("gyro_dyn_lpf_min"), &c.gyro.dynLpfFilter.cutoff),
Param(PSTR("gyro_dyn_lpf_max"), &c.gyro.dynLpfFilter.freq),
Param(PSTR("gyro_dyn_notch_q"), &c.dynamicFilter.q),
Param(PSTR("gyro_dyn_notch_count"), &c.dynamicFilter.width),
Param(PSTR("gyro_dyn_notch_min"), &c.dynamicFilter.min_freq),
Expand All @@ -460,9 +460,9 @@ class Cli
Param(PSTR("gyro_rpm_weight_2"), &c.rpmFilter.weights[1]),
Param(PSTR("gyro_rpm_weight_3"), &c.rpmFilter.weights[2]),
Param(PSTR("gyro_rpm_tlm_lpf_freq"), &c.rpmFilter.freqLpf),
Param(PSTR("gyro_offset_x"), &c.gyroBias[0]),
Param(PSTR("gyro_offset_y"), &c.gyroBias[1]),
Param(PSTR("gyro_offset_z"), &c.gyroBias[2]),
Param(PSTR("gyro_offset_x"), &c.gyro.bias[0]),
Param(PSTR("gyro_offset_y"), &c.gyro.bias[1]),
Param(PSTR("gyro_offset_z"), &c.gyro.bias[2]),

Param(PSTR("accel_bus"), &c.accelBus, busDevChoices),
Param(PSTR("accel_dev"), &c.accelDev, gyroDevChoices),
Expand Down Expand Up @@ -1037,9 +1037,9 @@ class Cli
if(!cmd.args[1])
{
s.print(F(" gyro offset: "));
s.print(_model.config.gyroBias[0]); s.print(' ');
s.print(_model.config.gyroBias[1]); s.print(' ');
s.print(_model.config.gyroBias[2]); s.print(F(" ["));
s.print(_model.config.gyro.bias[0]); s.print(' ');
s.print(_model.config.gyro.bias[1]); s.print(' ');
s.print(_model.config.gyro.bias[2]); s.print(F(" ["));
s.print(Math::toDeg(_model.state.gyroBias[0])); s.print(' ');
s.print(Math::toDeg(_model.state.gyroBias[1])); s.print(' ');
s.print(Math::toDeg(_model.state.gyroBias[2])); s.println(F("]"));
Expand Down
4 changes: 2 additions & 2 deletions lib/Espfc/src/Hardware.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,7 @@ class Hardware

void detectGyro()
{
if(_model.config.gyroDev == GYRO_NONE) return;
if(_model.config.gyro.dev == GYRO_NONE) return;

Device::GyroDevice * detectedGyro = nullptr;
#if defined(ESPFC_SPI_0)
Expand Down Expand Up @@ -126,7 +126,7 @@ class Hardware
#endif
if(!detectedGyro) return;

detectedGyro->setDLPFMode(_model.config.gyroDlpf);
detectedGyro->setDLPFMode(_model.config.gyro.dlpf);
_model.state.gyroDev = detectedGyro;
_model.state.gyroPresent = (bool)detectedGyro;
_model.state.accelPresent = _model.state.gyroPresent && _model.config.accelDev != GYRO_NONE;
Expand Down
20 changes: 10 additions & 10 deletions lib/Espfc/src/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ class Model

bool gyroActive() const /* IRAM_ATTR */
{
return state.gyroPresent && config.gyroDev != GYRO_NONE;
return state.gyroPresent && config.gyro.dev != GYRO_NONE;
}

bool accelActive() const
Expand Down Expand Up @@ -461,18 +461,18 @@ class Model
state.gyroDynNotchFilter[p][i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 380), gyroFilterRate);
}
}
state.gyroNotch1Filter[i].begin(config.gyroNotch1Filter, gyroFilterRate);
state.gyroNotch2Filter[i].begin(config.gyroNotch2Filter, gyroFilterRate);
if(config.gyroDynLpfFilter.cutoff > 0)
state.gyroNotch1Filter[i].begin(config.gyro.notch1Filter, gyroFilterRate);
state.gyroNotch2Filter[i].begin(config.gyro.notch2Filter, gyroFilterRate);
if(config.gyro.dynLpfFilter.cutoff > 0)
{
state.gyroFilter[i].begin(FilterConfig((FilterType)config.gyroFilter.type, config.gyroDynLpfFilter.cutoff), gyroFilterRate);
state.gyroFilter[i].begin(FilterConfig((FilterType)config.gyro.filter.type, config.gyro.dynLpfFilter.cutoff), gyroFilterRate);
}
else
{
state.gyroFilter[i].begin(config.gyroFilter, gyroFilterRate);
state.gyroFilter[i].begin(config.gyro.filter, gyroFilterRate);
}
state.gyroFilter2[i].begin(config.gyroFilter2, gyroFilterRate);
state.gyroFilter3[i].begin(config.gyroFilter3, gyroPreFilterRate);
state.gyroFilter2[i].begin(config.gyro.filter2, gyroFilterRate);
state.gyroFilter3[i].begin(config.gyro.filter3, gyroPreFilterRate);
state.accelFilter[i].begin(config.accelFilter, gyroFilterRate);
state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 3), gyroFilterRate);
for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++)
Expand Down Expand Up @@ -574,7 +574,7 @@ class Model
// load current sensor calibration
for(size_t i = 0; i <= AXIS_YAW; i++)
{
state.gyroBias.set(i, config.gyroBias[i] / 1000.0f);
state.gyroBias.set(i, config.gyro.bias[i] / 1000.0f);
state.accelBias.set(i, config.accelBias[i] / 1000.0f);
state.magCalibrationOffset.set(i, config.magCalibrationOffset[i] / 10.0f);
state.magCalibrationScale.set(i, config.magCalibrationScale[i] / 1000.0f);
Expand All @@ -586,7 +586,7 @@ class Model
// store current sensor calibration
for(size_t i = 0; i < 3; i++)
{
config.gyroBias[i] = lrintf(state.gyroBias[i] * 1000.0f);
config.gyro.bias[i] = lrintf(state.gyroBias[i] * 1000.0f);
config.accelBias[i] = lrintf(state.accelBias[i] * 1000.0f);
config.magCalibrationOffset[i] = lrintf(state.magCalibrationOffset[i] * 10.0f);
config.magCalibrationScale[i] = lrintf(state.magCalibrationScale[i] * 1000.0f);
Expand Down
30 changes: 18 additions & 12 deletions lib/Espfc/src/ModelConfig.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -558,21 +558,28 @@ struct IBatConfig
int16_t offset = 0;
};

struct GyroConfig
{
int8_t bus = BUS_AUTO;
int8_t dev = GYRO_AUTO;
int8_t dlpf = GYRO_DLPF_256;
int8_t align = ALIGN_DEFAULT;

FilterConfig filter{FILTER_PT1, 100};
FilterConfig filter2{FILTER_PT1, 213};
FilterConfig filter3{FILTER_FO, 150};
FilterConfig notch1Filter{FILTER_NOTCH, 0, 0};
FilterConfig notch2Filter{FILTER_NOTCH, 0, 0};
FilterConfig dynLpfFilter{FILTER_PT1, 425, 170};

int16_t bias[3] = { 0, 0, 0 };
};

// persistent data
class ModelConfig
{
public:
int8_t gyroBus = BUS_AUTO;
int8_t gyroDev = GYRO_AUTO;
int8_t gyroDlpf = GYRO_DLPF_256;
int8_t gyroAlign = ALIGN_DEFAULT;

FilterConfig gyroFilter{FILTER_PT1, 100};
FilterConfig gyroFilter2{FILTER_PT1, 213};
FilterConfig gyroFilter3{FILTER_FO, 150};
FilterConfig gyroNotch1Filter{FILTER_NOTCH, 0, 0};
FilterConfig gyroNotch2Filter{FILTER_NOTCH, 0, 0};
FilterConfig gyroDynLpfFilter{FILTER_PT1, 425, 170};
GyroConfig gyro;

int8_t accelBus = BUS_AUTO;
int8_t accelDev = GYRO_AUTO;
Expand Down Expand Up @@ -646,7 +653,6 @@ class ModelConfig

FusionConfig fusion;

int16_t gyroBias[3] = { 0, 0, 0 };
int16_t accelBias[3] = { 0, 0, 0 };
int16_t magCalibrationScale[3] = { 0, 0, 0 };
int16_t magCalibrationOffset[3] = { 1000, 1000, 1000 };
Expand Down
74 changes: 37 additions & 37 deletions lib/Espfc/src/Msp/MspProcessor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -532,13 +532,13 @@ class MspProcessor
break;

case MSP_SENSOR_ALIGNMENT:
r.writeU8(_model.config.gyroAlign); // gyro align
r.writeU8(_model.config.gyroAlign); // acc align, Starting with 4.0 gyro and acc alignment are the same
r.writeU8(_model.config.gyro.align); // gyro align
r.writeU8(_model.config.gyro.align); // acc align, Starting with 4.0 gyro and acc alignment are the same
r.writeU8(_model.config.magAlign); // mag align
//1.41+
r.writeU8(_model.state.gyroPresent ? 1 : 0); // gyro detection mask GYRO_1_MASK
r.writeU8(0); // gyro_to_use
r.writeU8(_model.config.gyroAlign); // gyro 1
r.writeU8(_model.config.gyro.align); // gyro 1
r.writeU8(0); // gyro 2
break;

Expand All @@ -554,7 +554,7 @@ class MspProcessor
gyroAlign = m.readU8(); // gyro 1 align
m.readU8(); // gyro 2 align
}
_model.config.gyroAlign = gyroAlign;
_model.config.gyro.align = gyroAlign;
}
break;

Expand Down Expand Up @@ -1060,29 +1060,29 @@ class MspProcessor
// break;

case MSP_FILTER_CONFIG:
r.writeU8(_model.config.gyroFilter.freq); // gyro lpf
r.writeU16(_model.config.dtermFilter.freq); // dterm lpf
r.writeU16(_model.config.yawFilter.freq); // yaw lpf
r.writeU16(_model.config.gyroNotch1Filter.freq); // gyro notch 1 hz
r.writeU16(_model.config.gyroNotch1Filter.cutoff); // gyro notch 1 cutoff
r.writeU16(_model.config.dtermNotchFilter.freq); // dterm notch hz
r.writeU16(_model.config.dtermNotchFilter.cutoff); // dterm notch cutoff
r.writeU16(_model.config.gyroNotch2Filter.freq); // gyro notch 2 hz
r.writeU16(_model.config.gyroNotch2Filter.cutoff); // gyro notch 2 cutoff
r.writeU8(_model.config.dtermFilter.type); // dterm type
r.writeU8(fromGyroDlpf(_model.config.gyroDlpf));
r.writeU8(0); // dlfp 32khz type
r.writeU16(_model.config.gyroFilter.freq); // lowpass1 freq
r.writeU16(_model.config.gyroFilter2.freq); // lowpass2 freq
r.writeU8(_model.config.gyroFilter.type); // lowpass1 type
r.writeU8(_model.config.gyroFilter2.type); // lowpass2 type
r.writeU16(_model.config.dtermFilter2.freq); // dterm lopwass2 freq
r.writeU8(_model.config.gyro.filter.freq); // gyro lpf
r.writeU16(_model.config.dtermFilter.freq); // dterm lpf
r.writeU16(_model.config.yawFilter.freq); // yaw lpf
r.writeU16(_model.config.gyro.notch1Filter.freq); // gyro notch 1 hz
r.writeU16(_model.config.gyro.notch1Filter.cutoff); // gyro notch 1 cutoff
r.writeU16(_model.config.dtermNotchFilter.freq); // dterm notch hz
r.writeU16(_model.config.dtermNotchFilter.cutoff); // dterm notch cutoff
r.writeU16(_model.config.gyro.notch2Filter.freq); // gyro notch 2 hz
r.writeU16(_model.config.gyro.notch2Filter.cutoff); // gyro notch 2 cutoff
r.writeU8(_model.config.dtermFilter.type); // dterm type
r.writeU8(fromGyroDlpf(_model.config.gyro.dlpf));
r.writeU8(0); // dlfp 32khz type
r.writeU16(_model.config.gyro.filter.freq); // lowpass1 freq
r.writeU16(_model.config.gyro.filter2.freq); // lowpass2 freq
r.writeU8(_model.config.gyro.filter.type); // lowpass1 type
r.writeU8(_model.config.gyro.filter2.type); // lowpass2 type
r.writeU16(_model.config.dtermFilter2.freq); // dterm lopwass2 freq
// 1.41+
r.writeU8(_model.config.dtermFilter2.type); // dterm lopwass2 type
r.writeU16(_model.config.gyroDynLpfFilter.cutoff); // dyn lpf gyro min
r.writeU16(_model.config.gyroDynLpfFilter.freq); // dyn lpf gyro max
r.writeU16(_model.config.dtermDynLpfFilter.cutoff); // dyn lpf dterm min
r.writeU16(_model.config.dtermDynLpfFilter.freq); // dyn lpf dterm max
r.writeU8(_model.config.dtermFilter2.type); // dterm lopwass2 type
r.writeU16(_model.config.gyro.dynLpfFilter.cutoff); // dyn lpf gyro min
r.writeU16(_model.config.gyro.dynLpfFilter.freq); // dyn lpf gyro max
r.writeU16(_model.config.dtermDynLpfFilter.cutoff); // dyn lpf dterm min
r.writeU16(_model.config.dtermDynLpfFilter.freq); // dyn lpf dterm max
// gyro analyse
r.writeU8(3); // deprecated dyn notch range
r.writeU8(_model.config.dynamicFilter.width); // dyn_notch_width_percent
Expand All @@ -1096,36 +1096,36 @@ class MspProcessor
break;

case MSP_SET_FILTER_CONFIG:
_model.config.gyroFilter.freq = m.readU8();
_model.config.gyro.filter.freq = m.readU8();
_model.config.dtermFilter.freq = m.readU16();
_model.config.yawFilter.freq = m.readU16();
if (m.remain() >= 8) {
_model.config.gyroNotch1Filter.freq = m.readU16();
_model.config.gyroNotch1Filter.cutoff = m.readU16();
_model.config.gyro.notch1Filter.freq = m.readU16();
_model.config.gyro.notch1Filter.cutoff = m.readU16();
_model.config.dtermNotchFilter.freq = m.readU16();
_model.config.dtermNotchFilter.cutoff = m.readU16();
}
if (m.remain() >= 4) {
_model.config.gyroNotch2Filter.freq = m.readU16();
_model.config.gyroNotch2Filter.cutoff = m.readU16();
_model.config.gyro.notch2Filter.freq = m.readU16();
_model.config.gyro.notch2Filter.cutoff = m.readU16();
}
if (m.remain() >= 1) {
_model.config.dtermFilter.type = (FilterType)m.readU8();
}
if (m.remain() >= 10) {
m.readU8(); // dlfp type
m.readU8(); // 32k dlfp type
_model.config.gyroFilter.freq = m.readU16();
_model.config.gyroFilter2.freq = m.readU16();
_model.config.gyroFilter.type = m.readU8();
_model.config.gyroFilter2.type = m.readU8();
_model.config.gyro.filter.freq = m.readU16();
_model.config.gyro.filter2.freq = m.readU16();
_model.config.gyro.filter.type = m.readU8();
_model.config.gyro.filter2.type = m.readU8();
_model.config.dtermFilter2.freq = m.readU16();
}
// 1.41+
if (m.remain() >= 9) {
_model.config.dtermFilter2.type = m.readU8();
_model.config.gyroDynLpfFilter.cutoff = m.readU16(); // dyn gyro lpf min
_model.config.gyroDynLpfFilter.freq = m.readU16(); // dyn gyro lpf max
_model.config.gyro.dynLpfFilter.cutoff = m.readU16(); // dyn gyro lpf min
_model.config.gyro.dynLpfFilter.freq = m.readU16(); // dyn gyro lpf max
_model.config.dtermDynLpfFilter.cutoff = m.readU16(); // dyn dterm lpf min
_model.config.dtermDynLpfFilter.freq = m.readU16(); // dyn dterm lpf min
}
Expand Down
2 changes: 1 addition & 1 deletion lib/Espfc/src/Sensor/AccelSensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ class AccelSensor: public BaseSensor

_model.state.accel = (VectorFloat)_model.state.accelRaw * _model.state.accelScale;

align(_model.state.accel, _model.config.gyroAlign);
align(_model.state.accel, _model.config.gyro.align);
_model.state.accel = _model.state.boardAlignment.apply(_model.state.accel);

for(size_t i = 0; i < 3; i++)
Expand Down
Loading

0 comments on commit fada2d2

Please sign in to comment.