public void Dispose()
{
_paramUpdated?.Dispose();
_paramsCount?.Dispose();
_params.Clear();
_paramsSubscribe?.Dispose();
}
public void Dispose()
{
Disable();
_portErrorStream.Dispose();
_portStateStream.Dispose();
_enableStream.Dispose();
_outputData.Dispose();
}
private void HandleStatistic()
{
_inputPackets
.Select(_ => 1)
.Buffer(TimeSpan.FromSeconds(1))
.Select(_ => _.Sum()).Subscribe(_packetRate, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _packetRate.Dispose());
}
private void HandleHighresImu()
{
_inputPackets
.Where(_ => _.MessageId == HighresImuPacket.PacketMessageId)
.Cast <HighresImuPacket>()
.Select(_ => _.Payload)
.Subscribe(_highresImu, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _highresImu.Dispose());
}
private void HandleVfrHud()
{
_inputPackets
.Where(_ => _.MessageId == VfrHudPacket.PacketMessageId)
.Cast <VfrHudPacket>()
.Select(_ => _.Payload)
.Subscribe(_vfrHud, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _vfrHud.Dispose());
}
private void HandleAttitude()
{
_inputPackets
.Where(_ => _.MessageId == AttitudePacket.PacketMessageId)
.Cast <AttitudePacket>()
.Select(_ => _.Payload)
.Subscribe(_attitude, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _attitude.Dispose());
}
private void HandleBatteryStatus()
{
_inputPackets
.Where(_ => _.MessageId == BatteryStatusPacket.PacketMessageId)
.Cast <BatteryStatusPacket>()
.Select(_ => _.Payload)
.Subscribe(_batteryStatus, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _batteryStatus.Dispose());
}
private void HandleExtendedSysState()
{
_inputPackets
.Where(_ => _.MessageId == ExtendedSysStatePacket.PacketMessageId)
.Cast <ExtendedSysStatePacket>()
.Select(_ => _.Payload)
.Subscribe(_extendedSysState, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _extendedSysState.Dispose());
}
private void HandleGlobalPositionInt()
{
_inputPackets
.Where(_ => _.MessageId == GlobalPositionIntPacket.PacketMessageId)
.Cast <GlobalPositionIntPacket>()
.Select(_ => _.Payload)
.Subscribe(_globalPositionInt, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _radioStatus.Dispose());
}
private void HandleRadioStatus()
{
_connection
.Where(_ => _.MessageId == RadioStatusPacket.PacketMessageId)
.Cast <RadioStatusPacket>()
.Select(_ => _.Payload)
.Subscribe(_radioStatus, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _globalPositionInt.Dispose());
}
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());
}
public void Dispose()
{
if (Interlocked.CompareExchange(ref _isDisposed, 1, 0) != 0)
{
return;
}
_disposeCancel?.Cancel(false);
_disposeCancel?.Dispose();
_linkQualitySubject.Dispose();
_logMessage.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 InitBattery()
{
/// TODO: add _mavlink.Rtt.RawBatteryStatus
_mavlink.Rtt.RawSysStatus.Select(_ => _.BatteryRemaining < 0 ? default(double?) : (_.BatteryRemaining / 100.0d)).Subscribe(_batteryCharge, DisposeCancel.Token);
_mavlink.Rtt.RawSysStatus.Select(_ => _.CurrentBattery < 0 ? default(double?) : (_.CurrentBattery / 100.0d)).Subscribe(_currentBattery, DisposeCancel.Token);
_mavlink.Rtt.RawSysStatus.Select(_ => _.VoltageBattery / 1000.0d).Subscribe(_voltageBattery, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _batteryCharge.Dispose());
DisposeCancel.Token.Register(() => _currentBattery.Dispose());
DisposeCancel.Token.Register(() => _voltageBattery.Dispose());
}
private void HandleHome()
{
_inputPackets
.Where(_ => _.MessageId == HomePositionPacket.PacketMessageId)
.Cast <HomePositionPacket>()
.Select(_ => _.Payload)
.Subscribe(_home, _disposeCancel.Token);
_home
.Select(_ => new GeoPoint(_.Latitude / 10000000D, _.Longitude / 10000000D, _.Altitude / 1000D))
.Subscribe(_homePos, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _home.Dispose());
}
private void InitStatus()
{
_mavlink.Rtt.RawStatusText.Select(_ => new VehicleStatusMessage {
Text = new string(_.Text), Type = _.Severity
}).Subscribe(_textStatus, DisposeCancel.Token);
_mavlink.Rtt.RawSysStatus.Select(_ => _.Load / 1000D).Subscribe(_cpuLoad, DisposeCancel.Token);
_mavlink.Rtt.RawSysStatus.Select(_ => _.DropRateComm / 1000D).Subscribe(_dropRateComm, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _textStatus.Dispose());
DisposeCancel.Token.Register(() => _cpuLoad.Dispose());
DisposeCancel.Token.Register(() => _dropRateComm.Dispose());
}
private void HandleGps()
{
_inputPackets
.Where(_ => _.MessageId == GpsRawIntPacket.PacketMessageId)
.Cast <GpsRawIntPacket>()
.Select(_ => _.Payload).Subscribe(_gpsRawInt, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _gpsRawInt.Dispose());
_inputPackets
.Where(_ => _.MessageId == Gps2RawPacket.PacketMessageId)
.Cast <Gps2RawPacket>()
.Select(_ => _.Payload).Subscribe(_gps2Raw, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _gps2Raw.Dispose());
}
public void Dispose()
{
if (Interlocked.CompareExchange(ref _isDisposed, 1, 0) != 0)
{
return;
}
Disable();
_portErrorStream.Dispose();
_portStateStream.Dispose();
_enableStream.Dispose();
_outputData.OnCompleted();
_outputData.Dispose();
InternalDisposeOnce();
}
protected virtual void InitHome()
{
_mavlink.Rtt.RawHome
.Select(_ => (GeoPoint?)new GeoPoint(_.Latitude / 10000000D, _.Longitude / 10000000D, _.Altitude / 1000D))
.Subscribe(_home, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _homeDistance.Dispose());
this.GlobalPosition
.Where(_ => _home.Value.HasValue)
// ReSharper disable once PossibleInvalidOperationException
.Select(_ => (double?)GeoMath.Distance(_home.Value.Value, _))
.Subscribe(_homeDistance, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _home.Dispose());
}
private void HandleGps()
{
var s = _inputPackets
.Where(_ => _.MessageId == GpsRawIntPacket.PacketMessageId)
.Cast <GpsRawIntPacket>()
.Select(_ => _.Payload);
s.Subscribe(_gpsRawInt, _disposeCancel.Token);
s.Select(_ => new GeoPoint(_.Lat / 10000000D, _.Lon / 10000000D, (_.Alt / 1000D) - Home.Value.Altitude ?? 0)).Subscribe(_relGps, _disposeCancel.Token);
s.Select(_ => new GeoPoint(_.Lat / 10000000D, _.Lon / 10000000D, _.Alt / 1000D)).Subscribe(_globGps, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _gpsRawInt.Dispose());
_disposeCancel.Token.Register(() => _relGps.Dispose());
_disposeCancel.Token.Register(() => _globGps.Dispose());
}
private void HandleSystemStatus()
{
_inputPackets
.Where(_ => _.MessageId == SysStatusPacket.PacketMessageId)
.Cast <SysStatusPacket>()
.Select(_ => _.Payload)
.Subscribe(_sysStatus, _disposeCancel.Token);
_inputPackets
.Where(_ => _.MessageId == StatustextPacket.PacketMessageId)
.Cast <StatustextPacket>()
.Select(_ => _.Payload)
.Subscribe(_statusText, _disposeCancel.Token);
_disposeCancel.Token.Register(() => _sysStatus.Dispose());
_disposeCancel.Token.Register(() => _statusText.Dispose());
}
public PortBaseV2(string name)
{
_name = name;
_portScheduler = new SingleThreadTaskScheduler(name);
_taskFactory = new TaskFactory(_portScheduler);
_enableStream.Subscribe(_ => TryReconnect(), _disposedCancel.Token);
_disposedCancel.Token.Register(() => _portScheduler.Dispose());
_disposedCancel.Token.Register(() => _portErrorStream.Dispose());
_disposedCancel.Token.Register(() => _portStateStream.Dispose());
_disposedCancel.Token.Register(() => _enableStream.Dispose());
_disposedCancel.Token.Register(() =>
{
_outputData.OnCompleted();
_outputData.Dispose();
});
}
protected virtual void InitAttitude()
{
_mavlink.Rtt.RawAttitude.Select(_ => (double)GeoMath.RadiansToDegrees(_.Pitch)).Subscribe(_pitch, DisposeCancel.Token);
_mavlink.Rtt.RawAttitude.Select(_ => (double)GeoMath.RadiansToDegrees(_.Roll)).Subscribe(_roll, DisposeCancel.Token);
_mavlink.Rtt.RawAttitude.Select(_ => (double)GeoMath.RadiansToDegrees(_.Yaw)).Subscribe(_yaw, DisposeCancel.Token);
_mavlink.Rtt.RawAttitude.Select(_ => (double)_.Pitchspeed).Subscribe(_pitchSpeed, DisposeCancel.Token);
_mavlink.Rtt.RawAttitude.Select(_ => (double)_.Rollspeed).Subscribe(_rollSpeed, DisposeCancel.Token);
_mavlink.Rtt.RawAttitude.Select(_ => (double)_.Yawspeed).Subscribe(_yawSpeed, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _pitch.Dispose());
DisposeCancel.Token.Register(() => _roll.Dispose());
DisposeCancel.Token.Register(() => _yaw.Dispose());
DisposeCancel.Token.Register(() => _pitchSpeed.Dispose());
DisposeCancel.Token.Register(() => _rollSpeed.Dispose());
DisposeCancel.Token.Register(() => _yawSpeed.Dispose());
}
protected virtual void InitGps()
{
_mavlink.Rtt.RawGlobalPositionInt.Select(_ => new GeoPoint(_.Lat / 10000000D, _.Lon / 10000000D, _.Alt / 1000D)).Subscribe(_globalPosition, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _globalPosition.Dispose());
_mavlink.Rtt.RawGpsRawInt.IgnoreObserverExceptions().Select(_ => new GpsInfo(_)).Subscribe(_gpsInfo, DisposeCancel.Token);
_mavlink.Rtt.RawGpsRawInt.Select(_ => _.Vel / 100D).Subscribe(_gVelocity, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _gVelocity.Dispose());
DisposeCancel.Token.Register(() => _gpsInfo.Dispose());
_mavlink.Rtt.RawGps2Raw.IgnoreObserverExceptions().Select(_ => new GpsInfo(_)).Subscribe(_gps2Info, DisposeCancel.Token);
_mavlink.Rtt.RawGps2Raw.Select(_ => _.Vel / 100D).Subscribe(_g2Velocity, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _g2Velocity.Dispose());
DisposeCancel.Token.Register(() => _gps2Info.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 InitVehicleClass()
{
DisposeCancel.Token.Register(() => _vehicleClass.Dispose());
_mavlink.Heartbeat.RawHeartbeat.Select(InterpretVehicleClass).Subscribe(_vehicleClass, DisposeCancel.Token);
}
private void InitRadioStatus()
{
_mavlink.Rtt.RawRadioStatus.Select(_ => new RadioLinkStatus(_)).Subscribe(_radioStatus, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _radioStatus.Dispose());
}
private void InitGoTo()
{
DisposeCancel.Token.Register(() => _goToTarget.Dispose());
}
public void Dispose()
{
_value.Dispose();
}
private void InitAltitude()
{
_mavlink.Rtt.RawGlobalPositionInt.Select(_ => _.RelativeAlt / 1000D).Subscribe(_altitudeRelative, DisposeCancel.Token);
DisposeCancel.Token.Register(() => _altitudeRelative.Dispose());
}