private void CheckFilteredIMU()
{
if (_filteredIMU == null)
{
_filteredIMU = new IMU();
}
}
private void UpdateFilteredIMUYaw(short yaw)
{
CheckFilteredIMU();
_filteredIMU = new IMU(_filteredIMU.Pitch, _filteredIMU.Roll, yaw);
}
private void UpdateFilteredIMURoll(short roll)
{
CheckFilteredIMU();
_filteredIMU = new IMU(_filteredIMU.Pitch, roll, _filteredIMU.Yaw);
}
private void UpdateFilteredIMUPitch(short pitch)
{
CheckFilteredIMU();
_filteredIMU = new IMU(pitch, _filteredIMU.Roll, _filteredIMU.Yaw);
}
private void CheckFilteredIMU()
{
if (_filteredIMU == null)
_filteredIMU = new IMU();
}
private void UpdateFilteredIMUYaw(short yaw)
{
CheckFilteredIMU();
_filteredIMU = new IMU(_filteredIMU.Pitch, _filteredIMU.Roll, yaw);
}
private void UpdateFilteredIMURoll(short roll)
{
CheckFilteredIMU();
_filteredIMU = new IMU(_filteredIMU.Pitch, roll, _filteredIMU.Yaw);
}
private void UpdateFilteredIMUPitch(short pitch)
{
CheckFilteredIMU();
_filteredIMU = new IMU(pitch, _filteredIMU.Roll, _filteredIMU.Yaw);
}