public void Start(ushort updatePeriodInMs)
{
if (_isRunning)
{
throw new InvalidOperationException("already running");
}
if (!_copter.Logger.IsLogVariableKnown("kalman.stateX"))
{
throw new InvalidOperationException("require kalman to be present in the firmware of the crazyflie");
}
_updatePeriodInMs = updatePeriodInMs;
_firstValueTracked = false;
InitVarianceHistory();
_navLogConfig = _copter.Logger.CreateEmptyLogConfigEntry("Kalman Variance", updatePeriodInMs);
// current estimated position
_navLogConfig.AddVariable("kalman.stateX", "float");
_navLogConfig.AddVariable("kalman.stateY", "float");
_navLogConfig.AddVariable("kalman.stateZ", "float");
// position variance
_navLogConfig.AddVariable("kalman.varPX", "float");
_navLogConfig.AddVariable("kalman.varPY", "float");
_navLogConfig.AddVariable("kalman.varPZ", "float");
_navLogConfig.LogDataReceived += Config_LogKalmanDataReceived;
_copter.Logger.AddConfig(_navLogConfig);
_copter.Logger.StartConfig(_navLogConfig);
_isRunning = true;
}
/// <summary>
/// See <see cref="ICrazyflieLogger.AddConfig(LogConfig)"/>.
/// </summary>
public void AddConfig(LogConfig config)
{
// If the log configuration contains variables that we added without
// type (i.e we want the stored as type for fetching as well) then
// resolve this now and add them to the block again.
foreach (var name in config.DefaultFetchAs)
{
LogTocElement tocVariable = EnsureVariableInToc(config, name);
// Now that we know what type this variable has, add it to the log
// config again with the correct type
config.AddVariable(name, tocVariable.CType);
}
// Now check that all the added variables are in the TOC and that
// the total size constraint of a data packet with logging data is
// not
var size = 0;
foreach (var variable in config.Variables)
{
size += LogTocElement.GetSizeFromId(variable.FetchAsId);
// Check that we are able to find the variable in the TOC so
// we can return error already now and not when the config is sent
if (variable.IsTocVariable)
{
EnsureVariableInToc(config, variable.Name);
}
}
if (size <= MAX_LOG_DATA_PACKET_SIZE)
{
config.Valid = true;
config.UseV2 = _useV2Protocol;
config.Identifier = _config_id_counter;
_config_id_counter = (byte)((_config_id_counter + 1) % 255);
_blocks.Add(config);
// TODO: event
//self.block_added_cb.call(logconf)
}
else
{
config.Valid = false;
throw new ArgumentException(
"The log configuration is too large");
}
}
