Skip to content

Commit

Permalink
MAVLinkInterface: move to utcnow (reduce cpu)
Browse files Browse the repository at this point in the history
  • Loading branch information
@meee1
meee1 committed Jul 10, 2024
1 parent 64e721a commit 3588abc
Show file tree
Hide file tree
Showing 2 changed files with 29 additions and 29 deletions.
54 changes: 27 additions & 27 deletions ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2991,7 +2991,7 @@ public void requestDatastream(MAV_DATA_STREAM id, int hzrate, int sysid = -1, in
if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.SYS_STATUS))
{
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.SYS_STATUS] <
DateTime.Now.AddSeconds(-2))
DateTime.UtcNow.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.SYS_STATUS];
}
Expand All @@ -3007,7 +3007,7 @@ public void requestDatastream(MAV_DATA_STREAM id, int hzrate, int sysid = -1, in
if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.ATTITUDE))
{
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.ATTITUDE] <
DateTime.Now.AddSeconds(-2))
DateTime.UtcNow.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.ATTITUDE];
}
Expand All @@ -3023,7 +3023,7 @@ public void requestDatastream(MAV_DATA_STREAM id, int hzrate, int sysid = -1, in
if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.VFR_HUD))
{
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.VFR_HUD] <
DateTime.Now.AddSeconds(-2))
DateTime.UtcNow.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.VFR_HUD];
}
Expand All @@ -3039,7 +3039,7 @@ public void requestDatastream(MAV_DATA_STREAM id, int hzrate, int sysid = -1, in
if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.AHRS))
{
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.AHRS] <
DateTime.Now.AddSeconds(-2))
DateTime.UtcNow.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.AHRS];
}
Expand All @@ -3056,7 +3056,7 @@ public void requestDatastream(MAV_DATA_STREAM id, int hzrate, int sysid = -1, in
.packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.GLOBAL_POSITION_INT))
{
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.GLOBAL_POSITION_INT] <
DateTime.Now.AddSeconds(-2))
DateTime.UtcNow.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.GLOBAL_POSITION_INT];
}
Expand All @@ -3073,7 +3073,7 @@ public void requestDatastream(MAV_DATA_STREAM id, int hzrate, int sysid = -1, in
.packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.RC_CHANNELS_SCALED))
{
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.RC_CHANNELS_SCALED] <
DateTime.Now.AddSeconds(-2))
DateTime.UtcNow.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.RC_CHANNELS_SCALED];
}
Expand All @@ -3089,7 +3089,7 @@ public void requestDatastream(MAV_DATA_STREAM id, int hzrate, int sysid = -1, in
if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.RAW_IMU))
{
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.RAW_IMU] <
DateTime.Now.AddSeconds(-2))
DateTime.UtcNow.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.RAW_IMU];
}
Expand All @@ -3105,7 +3105,7 @@ public void requestDatastream(MAV_DATA_STREAM id, int hzrate, int sysid = -1, in
if (MAVlist[sysid, compid].packetspersecondbuild.ContainsKey((byte) MAVLINK_MSG_ID.RC_CHANNELS_RAW))
{
if (MAVlist[sysid, compid].packetspersecondbuild[(byte) MAVLINK_MSG_ID.RC_CHANNELS_RAW] <
DateTime.Now.AddSeconds(-2))
DateTime.UtcNow.AddSeconds(-2))
break;
pps = MAVlist[sysid, compid].packetspersecond[(byte) MAVLINK_MSG_ID.RC_CHANNELS_RAW];
}
Expand Down Expand Up @@ -4641,14 +4641,14 @@ public async Task<MAVLinkMessage> readPacketAsync()

if (!skipheader)
{
DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);
DateTime to = DateTime.UtcNow.AddMilliseconds(BaseStream.ReadTimeout);

if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR1a " + BaseStream?.BytesToRead);

while (BaseStream.IsOpen && BaseStream.BytesToRead <= 0)
{
if (DateTime.Now > to)
if (DateTime.UtcNow > to)
{
log.InfoFormat("MAVLINK: 1 wait time out btr {0} len {1}",
BaseStream?.BytesToRead,
Expand Down Expand Up @@ -4735,11 +4735,11 @@ public async Task<MAVLinkMessage> readPacketAsync()
// if we have the header, and no other chars, get the length and packet identifiers
if (count < headerlength && !logreadmode)
{
DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);
DateTime to = DateTime.UtcNow.AddMilliseconds(BaseStream.ReadTimeout);

while (BaseStream.IsOpen && BaseStream.BytesToRead < headerlength - count)
{
if (DateTime.Now > to)
if (DateTime.UtcNow > to)
{
log.InfoFormat("MAVLINK: 2 wait time out btr {0} len {1}", BaseStream.BytesToRead,
length);
Expand All @@ -4751,10 +4751,10 @@ public async Task<MAVLinkMessage> readPacketAsync()

if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR2a " + BaseStream?.BytesToRead);
var start1 = DateTime.Now;
//var start1 = DateTime.Now;
int read = BaseStream.Read(buffer, count + 1, headerlength - count);
var end = DateTime.Now - start1;
var lapse = end.TotalMilliseconds;
//var end = DateTime.Now - start1;
//var lapse = end.TotalMilliseconds;
//Console.WriteLine("read: " + lapse);
if (debug)
Console.WriteLine(DateTime.Now.Millisecond + " SR2b " + BaseStream?.BytesToRead);
Expand Down Expand Up @@ -4791,11 +4791,11 @@ public async Task<MAVLinkMessage> readPacketAsync()
}
else
{
DateTime to = DateTime.Now.AddMilliseconds(BaseStream.ReadTimeout);
DateTime to = DateTime.UtcNow.AddMilliseconds(BaseStream.ReadTimeout);

while (BaseStream.IsOpen && BaseStream.BytesToRead < (length - (headerlengthstx)))
{
if (DateTime.Now > to)
if (DateTime.UtcNow > to)
{
log.InfoFormat("MAVLINK: 3 wait time out btr {0} len {1}",
BaseStream.BytesToRead, length);
Expand All @@ -4807,10 +4807,10 @@ public async Task<MAVLinkMessage> readPacketAsync()

if (BaseStream.IsOpen)
{
var start1 = DateTime.Now;
//var start1 = DateTime.UtcNow;
int read = BaseStream.Read(buffer, headerlengthstx, length - (headerlengthstx));
var end = DateTime.Now - start1;
var lapse = end.TotalMilliseconds;
//var end = DateTime.UtcNow - start1;
//var lapse = end.TotalMilliseconds;
//Console.WriteLine("read: " + lapse);

if (read != (length - headerlengthstx))
Expand Down Expand Up @@ -4851,7 +4851,7 @@ public async Task<MAVLinkMessage> readPacketAsync()
_bytesReceivedSubj.OnNext(buffer.Length);

// update bps statistics
if (_bpstime.Second != DateTime.Now.Second)
if (_bpstime.Second != DateTime.UtcNow.Second)
{
long btr = 0;
if (BaseStream != null && BaseStream.IsOpen)
Expand All @@ -4873,7 +4873,7 @@ public async Task<MAVLinkMessage> readPacketAsync()
_mavlink2count, _mavlink2signed);
_bps2 = _bps1; // prev sec
_bps1 = 0; // current sec
_bpstime = DateTime.Now;
_bpstime = DateTime.UtcNow;
_mavlink1count = 0;
_mavlink2count = 0;
_mavlink2signed = 0;
Expand Down Expand Up @@ -5016,9 +5016,9 @@ public async Task<MAVLinkMessage> readPacketAsync()
}

// update packet loss statistics
if (!logreadmode && MAVlist[sysid, compid].packetlosttimer.AddSeconds(5) < DateTime.Now)
if (!logreadmode && MAVlist[sysid, compid].packetlosttimer.AddSeconds(5) < DateTime.UtcNow)
{
MAVlist[sysid, compid].packetlosttimer = DateTime.Now;
MAVlist[sysid, compid].packetlosttimer = DateTime.UtcNow;
MAVlist[sysid, compid].packetslost = (MAVlist[sysid, compid].packetslost * 0.8f);
MAVlist[sysid, compid].packetsnotlost = (MAVlist[sysid, compid].packetsnotlost * 0.8f);
}
Expand Down Expand Up @@ -5081,17 +5081,17 @@ public async Task<MAVLinkMessage> readPacketAsync()
double.IsInfinity(MAVlist[sysid, compid].packetspersecond[msgid]))
MAVlist[sysid, compid].packetspersecond[msgid] = 0;
if (!MAVlist[sysid, compid].packetspersecondbuild.ContainsKey(msgid))
MAVlist[sysid, compid].packetspersecondbuild[msgid] = DateTime.Now;
MAVlist[sysid, compid].packetspersecondbuild[msgid] = DateTime.UtcNow;

MAVlist[sysid, compid].packetspersecond[msgid] = (((1000 /
((DateTime.Now -
((DateTime.UtcNow -
MAVlist[sysid, compid]
.packetspersecondbuild[msgid])
.TotalMilliseconds) +
MAVlist[sysid, compid].packetspersecond[
msgid]) / 2));

MAVlist[sysid, compid].packetspersecondbuild[msgid] = DateTime.Now;
MAVlist[sysid, compid].packetspersecondbuild[msgid] = DateTime.UtcNow;
}
//Console.WriteLine("Packet {0}",temp[5]);
// store packet history
Expand Down
4 changes: 2 additions & 2 deletions MainV2.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3114,12 +3114,12 @@ private async void SerialReader()
break;
}

DateTime startread = DateTime.Now;
DateTime startread = DateTime.UtcNow;

// must be open, we have bytes, we are not yielding the port,
// the thread is meant to be running and we only spend 1 seconds max in this read loop
while (port.BaseStream.IsOpen && port.BaseStream.BytesToRead > minbytes &&
port.giveComport == false && serialThread && startread.AddSeconds(1) > DateTime.Now)
port.giveComport == false && serialThread && startread.AddSeconds(1) > DateTime.UtcNow)
{
try
{
Expand Down

0 comments on commit 3588abc

Please sign in to comment.