public async Task <MavParam> WriteParam(MavParam param, int attemptCount, CancellationToken cancel)
{
var packet = new ParamSetPacket()
{
ComponenId = _config.ComponentId,
SystemId = _config.SystemId,
Payload =
{
TargetComponent = _config.TargetComponenId,
TargetSystem = _config.TargetSystemId,
ParamId = SetParamName(param.Name),
ParamType = param.Type,
ParamValue = 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)
{
if (!timeoutCancel.IsCancellationRequested)
{
throw;
}
}
finally
{
subscribe?.Dispose();
}
return(result);
}
}
if (result == null)
{
throw 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)));
}
return(result);
}
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)
{
var name = GetParamName(p.Payload);
float?floatVal;
long? longVal;
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);
}
public static Task <MavParam> WriteParam(this IVehicleParameterProtocol src, MavParam param, CancellationToken cancel)
{
return(src.WriteParam(param, DefaultAttemptCount, cancel));
}
private float ConvertToMavlinkUnionToParamValue(MavParam param)
{
var arr = BitConverter.GetBytes(0.0F);
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[0] = (byte)(param.IntegerValue & 0xFF);
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[0] = (byte)(param.IntegerValue & 0xFF);
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[0] = (byte)(param.IntegerValue & 0xFF);
arr[1] = (byte)((param.IntegerValue >> 8) & 0xFF);
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[0] = (byte)(param.IntegerValue & 0xFF);
arr[1] = (byte)((param.IntegerValue >> 8) & 0xFF);
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[0] = (byte)(param.IntegerValue & 0xFF);
arr[1] = (byte)((param.IntegerValue >> 8) & 0xFF);
arr[2] = (byte)((param.IntegerValue >> 16) & 0xFF);
arr[3] = (byte)((param.IntegerValue >> 24) & 0xFF);
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[0] = (byte)(param.IntegerValue & 0xFF);
arr[1] = (byte)((param.IntegerValue >> 8) & 0xFF);
arr[2] = (byte)((param.IntegerValue >> 16) & 0xFF);
arr[3] = (byte)((param.IntegerValue >> 24) & 0xFF);
break;
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(param.RealValue.Value);
case MavParamType.MavParamTypeReal64:
throw new MavlinkException(RS.Vehicle_ConvertToMavlinkUnionToParamValue_NeedMoreByte);
default:
throw new ArgumentOutOfRangeException(nameof(param.Type), param.Type, null);
}
return(BitConverter.ToSingle(arr, 0));
}