public MavParam(MavParam param, long newValue) { Index = param.Index; Name = param.Name; Type = param.Type; RealValue = null; IntegerValue = newValue; }
public MavParam(MavParam param) { Index = param.Index; Name = param.Name; Type = param.Type; RealValue = param.RealValue; IntegerValue = param.IntegerValue; }
private void UpdateParam(ParamValuePacket p) { try { var name = GetParamName(p.Payload); float?floatVal; long? longVal; Converter.ConvertFromMavlinkUnionToParamValue(p.Payload.ParamValue, p.Payload.ParamType, out floatVal, out longVal); var mavParam = new MavParam(p.Payload.ParamIndex, name, p.Payload.ParamType, floatVal, longVal); _params.AddOrUpdate(name, mavParam, (s, param) => mavParam); _paramUpdated.OnNext(mavParam); _paramsCount.OnNext(p.Payload.ParamCount); _logger.Trace($"Recieve new param: {mavParam}"); } catch (Exception e) { _logger.Warn(e, $"Recieve MavLink param '{p.Name}' with error:{e.Message}"); } }
public async Task <MavParam> WriteParam(MavParam param, int attemptCount, CancellationToken cancel) { _logger.Info($"Write param '{param}': BEGIN"); var packet = new ParamSetPacket() { ComponenId = _identity.ComponentId, SystemId = _identity.SystemId, Payload = { TargetComponent = _identity.TargetComponentId, TargetSystem = _identity.TargetSystemId, ParamId = SetParamName(param.Name), ParamType = param.Type, ParamValue = Converter.ConvertToMavlinkUnionToParamValue(param) } }; byte currentAttempt = 0; MavParam result = null; while (currentAttempt < attemptCount) { ++currentAttempt; using (var timeoutCancel = new CancellationTokenSource(_config.ReadWriteTimeoutMs)) using (var linkedCancel = CancellationTokenSource.CreateLinkedTokenSource(cancel, timeoutCancel.Token)) { var eve = new AsyncAutoResetEvent(false); IDisposable subscribe = null; try { subscribe = OnParamUpdated.FirstAsync(_ => _.Name == param.Name).Subscribe(_ => { result = _; eve.Set(); }); await _connection.Send(packet, linkedCancel.Token).ConfigureAwait(false); await eve.WaitAsync(linkedCancel.Token); } catch (TaskCanceledException) { _logger.Warn($"Write param '{param}': TIMEOUT {currentAttempt} of {attemptCount}"); if (!timeoutCancel.IsCancellationRequested) { throw; } } finally { subscribe?.Dispose(); } return(result); } } if (result == null) { var ex = new TimeoutException(string.Format("Timeout to write param '{0}' with '{1}' attempts (timeout {1} times by {2:g} )", param.Name, currentAttempt, TimeSpan.FromMilliseconds(_config.ReadWriteTimeoutMs))); _logger.Error(ex, $"Write param '{param}': {ex.Message}"); throw ex; } return(result); }
public static Task <MavParam> WriteParam(this IMavlinkParameterClient src, MavParam param, CancellationToken cancel) { return(src.WriteParam(param, DefaultAttemptCount, cancel)); }
public float ConvertToMavlinkUnionToParamValue(MavParam param) { switch (param.Type) { case MavParamType.MavParamTypeUint8: if (!param.IntegerValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Integer_value_not_assigned_for_param, param.Name, param.Type)); } return((float)param.IntegerValue); case MavParamType.MavParamTypeInt8: if (!param.IntegerValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Integer_value_not_assigned_for_param, param.Name, param.Type)); } return((float)param.IntegerValue); case MavParamType.MavParamTypeUint16: if (!param.IntegerValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Integer_value_not_assigned_for_param, param.Name, param.Type)); } return((float)param.IntegerValue); case MavParamType.MavParamTypeInt16: if (!param.IntegerValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Integer_value_not_assigned_for_param, param.Name, param.Type)); } return((float)param.IntegerValue); case MavParamType.MavParamTypeUint32: if (!param.IntegerValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Integer_value_not_assigned_for_param, param.Name, param.Type)); } return((float)param.IntegerValue); case MavParamType.MavParamTypeInt32: if (!param.IntegerValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Integer_value_not_assigned_for_param, param.Name, param.Type)); } return((float)param.IntegerValue); case MavParamType.MavParamTypeUint64: throw new MavlinkException(RS.Vehicle_ConvertToMavlinkUnionToParamValue_NeedMoreByte); case MavParamType.MavParamTypeInt64: throw new MavlinkException(RS.Vehicle_ConvertToMavlinkUnionToParamValue_NeedMoreByte); case MavParamType.MavParamTypeReal32: if (!param.RealValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Real_value_not_assigned_for_param, param.Name, param.Type)); } return((float)param.RealValue); case MavParamType.MavParamTypeReal64: throw new MavlinkException(RS.Vehicle_ConvertToMavlinkUnionToParamValue_NeedMoreByte); default: throw new ArgumentOutOfRangeException(nameof(param.Type), param.Type, null); } }
public float ConvertToMavlinkUnionToParamValue(MavParam param) { byte[] arr; switch (param.Type) { case MavParamType.MavParamTypeUint8: if (!param.IntegerValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Integer_value_not_assigned_for_param, param.Name, param.Type)); } arr = BitConverter.GetBytes((byte)param.IntegerValue); break; case MavParamType.MavParamTypeInt8: if (!param.IntegerValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Integer_value_not_assigned_for_param, param.Name, param.Type)); } arr = BitConverter.GetBytes((sbyte)param.IntegerValue); break; case MavParamType.MavParamTypeUint16: if (!param.IntegerValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Integer_value_not_assigned_for_param, param.Name, param.Type)); } arr = BitConverter.GetBytes((ushort)param.IntegerValue); break; case MavParamType.MavParamTypeInt16: if (!param.IntegerValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Integer_value_not_assigned_for_param, param.Name, param.Type)); } arr = BitConverter.GetBytes((short)param.IntegerValue); break; case MavParamType.MavParamTypeUint32: if (!param.IntegerValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Integer_value_not_assigned_for_param, param.Name, param.Type)); } arr = BitConverter.GetBytes((UInt32)param.IntegerValue); break; case MavParamType.MavParamTypeInt32: if (!param.IntegerValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Integer_value_not_assigned_for_param, param.Name, param.Type)); } arr = BitConverter.GetBytes((Int32)param.IntegerValue); break; case MavParamType.MavParamTypeInt64: throw new MavlinkException(RS.Vehicle_ConvertToMavlinkUnionToParamValue_NeedMoreByte); case MavParamType.MavParamTypeReal32: if (!param.RealValue.HasValue) { throw new Exception(string.Format(RS.Vehicle_ConvertToMavlinkUnionToParamValue_Real_value_not_assigned_for_param, param.Name, param.Type)); } arr = BitConverter.GetBytes((float)param.RealValue); break; case MavParamType.MavParamTypeUint64: _logger.Warn($"Error param type {param}"); throw new MavlinkException(RS.Vehicle_ConvertToMavlinkUnionToParamValue_NeedMoreByte); case MavParamType.MavParamTypeReal64: _logger.Warn($"Error param type {param}"); throw new MavlinkException(RS.Vehicle_ConvertToMavlinkUnionToParamValue_NeedMoreByte); default: _logger.Warn($"Unknown param type {param}"); return(Single.NaN); } Array.Resize(ref arr, 4); return(BitConverter.ToSingle(arr, 0)); }