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"); } }