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