private void InitRequestVehicleInfo()
{
_initState.OnNext(VehicleInitState.WaitConnection);
Link.DistinctUntilChanged().Where(_ => _ == LinkState.Disconnected).Subscribe(_ => _needToRequestAgain = true, DisposeCancel.Token);
Link.DistinctUntilChanged().Where(_ => _needToRequestAgain).Where(_ => _ == LinkState.Connected)
// only one time
.Subscribe(_ => Task.Factory.StartNew(TryToRequestData, TaskCreationOptions.LongRunning), DisposeCancel.Token);
}
protected void InternalOnError(Exception exception)
{
_portStateStream.OnNext(PortState.Error);
_portErrorStream.OnNext(exception);
Observable.Timer(ReconnectTimeout).Subscribe(_ => TryConnect(), _disposedCancel.Token);
Stop();
}
public virtual async Task ClearRoi(CancellationToken cancel)
{
Logger.Info($"=> ClearRoi()");
var res = await _mavlink.Commands.CommandLong(MavCmd.MavCmdDoSetRoiNone, (int)MavRoi.MavRoiLocation, 0, 0, 0, 0, 0, 0, 3, CancellationToken.None);
Logger.Info($"<= ClearRoi(): '{res.Result}'(porgress:{res.Progress};resultParam2:{res.ResultParam2})");
ValidateCommandResult(res);
_roi.OnNext(null);
}
private void OnHeartBeat(HeartbeatPacket heartbeatPacket)
{
_lastHearteat = DateTime.Now;
_link.Upgrade();
_heartBeat.OnNext(heartbeatPacket.Payload);
_armed.OnNext(heartbeatPacket.Payload.BaseMode.HasFlag(MavModeFlag.MavModeFlagSafetyArmed));
_disposeCancel.Token.Register(() => _armed.Dispose());
_disposeCancel.Token.Register(() => _heartBeat.Dispose());
}
private void HandleHome()
{
_inputPackets
.Where(_ => _.MessageId == HomePositionPacket.PacketMessageId)
.Cast <HomePositionPacket>()
.Select(_ => _.Payload)
.Subscribe(_ => _home.OnNext(_), _disposeCancel.Token);
_disposeCancel.Token.Register(() => _home.Dispose());
}
protected virtual void InitArmed()
{
var timer = Observable.Timer(TimeSpan.FromSeconds(1), TimeSpan.FromSeconds(1)).Where(_ => IsArmed.Value).Subscribe(_ =>
{
var lastBin = Interlocked.Read(ref _lastArmedTime);
if (lastBin == 0)
{
_armedTime.OnNext(TimeSpan.Zero);
return;
}
var last = DateTime.FromBinary(lastBin);
var now = DateTime.Now;
var delay = (now - last);
_armedTime.OnNext(delay);
});
_isArmed.DistinctUntilChanged().Where(_ => _isArmed.Value).Subscribe(_ => Interlocked.Exchange(ref _lastArmedTime, DateTime.Now.ToBinary()), DisposeCancel.Token);
DisposeCancel.Token.Register(() => timer.Dispose());
_mavlink.Heartbeat.RawHeartbeat.Select(_ => _.BaseMode.HasFlag(MavModeFlag.MavModeFlagSafetyArmed)).Subscribe(_isArmed, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _isArmed.Dispose());
}
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);
}
private void InternalEnable()
{
_portScheduler.VerifyAccess();
if (_disposedCancel.IsCancellationRequested)
{
return;
}
try
{
_portStateStream.OnNext(PortState.Connecting);
_logger.Trace("Try connect to the port {0}", _name);
InternalStart();
_portStateStream.OnNext(PortState.Connected);
_portErrorStream.OnNext(null);
}
catch (Exception e)
{
_logger.Trace(e, $"Error to connect to the port {_name}:{e.Message}. Reconnect after {ReconnectTimeout:g}");
_portErrorStream.OnNext(e);
_portStateStream.OnNext(PortState.Error);
Observable.Timer(ReconnectTimeout).Subscribe(_ => _taskFactory.StartNew(TryReconnect, _disposedCancel.Token));
}
}
public void Disable()
{
_enableStream.OnNext(false);
_portStateStream.OnNext(PortState.Disabled);
Stop();
}
public void Enable()
{
_enableStream.OnNext(true);
}
public void RemoteUpdate(T value)
{
_value.OnNext(value);
}
public void Enable()
{
_taskFactory.StartNew(() => _enableStream.OnNext(true), _disposedCancel.Token);
}
public void RemoteUpdate(string value)
{
_value.OnNext(value);
}
public void Disable()
{
_enableStream.OnNext(false);
_portStateStream.OnNext(PortState.Disabled);
Task.Factory.StartNew(Stop);
}
