Skip to content

Commit 8f373d4

Browse files
meee1meee1
committed
Multiple: refactoring mavlink
1 parent d6a1433 commit 8f373d4

File tree

12 files changed

+162
-170
lines changed

12 files changed

+162
-170
lines changed

‎CurrentState.cs

Lines changed: 98 additions & 97 deletions
Large diffs are not rendered by default.

‎ExtLibs/Mavlink/MavlinkUtil.cs

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ public static TMavlinkPacket ByteArrayToStructureBigEndian<TMavlinkPacket>(this
2929
ByteArrayToStructureEndian(bytearray, ref newPacket, startoffset);
3030
return (TMavlinkPacket)newPacket;
3131
}
32-
32+
3333
public static void ByteArrayToStructure(byte[] bytearray, ref object obj, int startoffset)
3434
{
3535
int len = Marshal.SizeOf(obj);

‎GCSViews/ConfigurationView/ConfigCompassMot.cs

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -125,13 +125,13 @@ private void timer1_Tick(object sender, EventArgs e)
125125

126126
txt_status.Text = message.ToString();
127127
txt_status.SelectionStart = txt_status.Text.Length;
128-
txt_status.ScrollToCaret();
129-
130-
var bytearray = MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.COMPASSMOT_STATUS];
128+
txt_status.ScrollToCaret();
129+
130+
var bytearray = MainV2.comPort.MAV.getPacket((uint)MAVLink.MAVLINK_MSG_ID.COMPASSMOT_STATUS);
131131

132132
if (bytearray != null)
133133
{
134-
var status = bytearray.ByteArrayToStructure<MAVLink.mavlink_compassmot_status_t>(6);
134+
var status = bytearray.ByteArrayToStructure<MAVLink.mavlink_compassmot_status_t>();
135135

136136
lbl_status.Text = "Current: " + status.current.ToString("0.00") + "\nx,y,z " +
137137
status.CompensationX.ToString("0.00") + "," + status.CompensationY.ToString("0.00") +

‎GeoRef/georefimage.cs

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -184,8 +184,6 @@ private Dictionary<long, VehicleLocation> readGPSMsgInLog(string fn)
184184
new BinaryReader(File.Open(fn, FileMode.Open, FileAccess.Read, FileShare.Read));
185185
mine.logreadmode = true;
186186

187-
mine.MAV.packets.Initialize(); // clear
188-
189187
CurrentState cs = new CurrentState();
190188

191189
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)

‎Log/LogIndex.cs

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -143,8 +143,6 @@ void processbg(string file)
143143
}
144144
mine.logreadmode = true;
145145

146-
mine.MAV.packets.Initialize(); // clear
147-
148146
mine.getHeartBeat();
149147

150148
loginfo.Date = mine.lastlogread;

‎Log/MatLab.cs

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -271,8 +271,6 @@ public static void tlog(string logfile)
271271
}
272272
mine.logreadmode = true;
273273

274-
mine.MAV.packets.Initialize(); // clear
275-
276274
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
277275
{
278276
MAVLink.MAVLinkMessage packet = mine.readPacket();

‎Log/MavlinkLog.cs

Lines changed: 9 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -429,9 +429,7 @@ private void BUT_redokml_Click(object sender, EventArgs e)
429429
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
430430
return;
431431
}
432-
mine.logreadmode = true;
433-
434-
mine.MAV.packets.Initialize(); // clear
432+
mine.logreadmode = true;
435433

436434
double oldlatlngsum = 0;
437435

@@ -656,9 +654,7 @@ private void BUT_humanreadable_Click(object sender, EventArgs e)
656654
return;
657655
}
658656

659-
mine.logreadmode = true;
660-
661-
mine.MAV.packets.Initialize(); // clear
657+
mine.logreadmode = true;
662658

663659
StreamWriter sw =
664660
new StreamWriter(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar +
@@ -790,9 +786,7 @@ private List<string> GetLogFileValidFields(string logfile)
790786
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
791787
return options;
792788
}
793-
MavlinkInterface.logreadmode = true;
794-
795-
MavlinkInterface.MAV.packets.Initialize(); // clear
789+
MavlinkInterface.logreadmode = true;
796790

797791
CurrentState cs = new CurrentState();
798792

@@ -1444,9 +1438,7 @@ private void BUT_convertcsv_Click(object sender, EventArgs e)
14441438
CustomMessageBox.Show("Log Can not be opened. Are you still connected?");
14451439
return;
14461440
}
1447-
mine.logreadmode = true;
1448-
1449-
mine.MAV.packets.Initialize(); // clear
1441+
mine.logreadmode = true;
14501442

14511443
StreamWriter sw =
14521444
new StreamWriter(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar +
@@ -1521,9 +1513,7 @@ private void BUT_paramsfromlog_Click(object sender, EventArgs e)
15211513
return;
15221514
}
15231515

1524-
mine.logreadmode = true;
1525-
1526-
mine.MAV.packets.Initialize(); // clear
1516+
mine.logreadmode = true;
15271517

15281518
StreamWriter sw =
15291519
new StreamWriter(Path.GetDirectoryName(logfile) + Path.DirectorySeparatorChar +
@@ -1601,9 +1591,7 @@ private void BUT_getwpsfromlog_Click(object sender, EventArgs e)
16011591
return;
16021592
}
16031593

1604-
mine.logreadmode = true;
1605-
1606-
mine.MAV.packets.Initialize(); // clear
1594+
mine.logreadmode = true;
16071595

16081596
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
16091597
{
@@ -1641,10 +1629,10 @@ private void BUT_getwpsfromlog_Click(object sender, EventArgs e)
16411629
try
16421630
{
16431631
//get mission count info
1644-
var item =
1645-
mine.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.MISSION_COUNT]
1632+
var item =
1633+
mine.MAV.getPacket((uint)MAVLink.MAVLINK_MSG_ID.MISSION_COUNT)
16461634
.ByteArrayToStructure<MAVLink.mavlink_mission_count_t>();
1647-
mine.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.MISSION_COUNT] = null;
1635+
mine.MAV.clearPacket((uint)MAVLink.MAVLINK_MSG_ID.MISSION_COUNT);
16481636
sw.WriteLine("# count packet sent to comp " + item.target_component + " sys " +
16491637
item.target_system);
16501638
}

‎MagCalib.cs

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -833,8 +833,6 @@ public static double[] getOffsets(string fn, int throttleThreshold = 0)
833833

834834
mine.logreadmode = true;
835835

836-
mine.MAV.packets.Initialize(); // clear
837-
838836
// gather data
839837
while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
840838
{

‎Mavlink/MAVLinkInterface.cs

Lines changed: 15 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -2018,14 +2018,17 @@ public object DebugPacket(MAVLinkMessage datin, ref string text)
20182018
/// <returns>struct of data</returns>
20192019
public object DebugPacket(MAVLinkMessage datin, ref string text, bool PrintToConsole, string delimeter = " ")
20202020
{
2021-
string textoutput;
2021+
string textoutput = "";
20222022
try
20232023
{
20242024
if (datin.Length > 5)
20252025
{
2026-
2027-
textoutput = string.Format("{0,2:X}{6}{1,2:X}{6}{2,2:X}{6}{3,2:X}{6}{4,2:X}{6}{5,2:X}{6}", datin.header,
2028-
datin.payloadlength, datin.seq, datin.sysid, datin.compid, datin.msgid, delimeter);
2026+
textoutput =
2027+
string.Format(
2028+
"{0,2:X}{8}{1,2:X}{8}{2,2:X}{8}{3,2:X}{8}{4,2:X}{8}{5,2:X}{8}{6,2:X}{8}{7,6:X}{8}",
2029+
datin.header,
2030+
datin.payloadlength, datin.incompat_flags, datin.compat_flags, datin.seq, datin.sysid,
2031+
datin.compid, datin.msgid, delimeter);
20292032

20302033
object data = datin.data;
20312034

@@ -2953,7 +2956,7 @@ public MAVLinkMessage readPacket()
29532956

29542957
try
29552958
{
2956-
if ((message.header == 'U' || message.header == 0xfe || message.header == 0xfd) && buffer.Length >= buffer[1])
2959+
if ((message.header == 'U' || message.header == 0xfe || message.header == 0xfd) && buffer.Length >= message.payloadlength)
29572960
{
29582961
// check if we lost pacakets based on seqno
29592962
int expectedPacketSeqNo = ((MAVlist[sysid, compid].recvpacketcount + 1)%0x100);
@@ -3008,21 +3011,19 @@ public MAVLinkMessage readPacket()
30083011
// store packet history
30093012
lock (objlock)
30103013
{
3011-
MAVlist[sysid, compid].packets[msgid] = buffer;
3012-
MAVlist[sysid, compid].packetseencount[msgid]++;
3014+
MAVlist[sysid, compid].packets[msgid] = message;
30133015

30143016
// 3dr radio status packet are injected into the current mav
30153017
if (msgid == (byte)MAVLink.MAVLINK_MSG_ID.RADIO_STATUS ||
30163018
msgid == (byte)MAVLink.MAVLINK_MSG_ID.RADIO)
30173019
{
3018-
MAVlist[sysidcurrent, compidcurrent].packets[msgid] = buffer;
3019-
MAVlist[sysidcurrent, compidcurrent].packetseencount[msgid]++;
3020+
MAVlist[sysidcurrent, compidcurrent].packets[msgid] = message;
30203021
}
30213022

30223023
// adsb packets are forwarded and can be from any sysid/compid
30233024
if (msgid == (byte)MAVLink.MAVLINK_MSG_ID.ADSB_VEHICLE)
30243025
{
3025-
var adsb = buffer.ByteArrayToStructure<MAVLink.mavlink_adsb_vehicle_t>(6);
3026+
var adsb = message.ByteArrayToStructure<MAVLink.mavlink_adsb_vehicle_t>();
30263027

30273028
MainV2.instance.adsbPlanes[adsb.ICAO_address.ToString("X5")] = new MissionPlanner.Utilities.adsb.PointLatLngAltHdg(adsb.lat / 1e7, adsb.lon / 1e7, adsb.altitude / 1000, adsb.heading * 0.01f, adsb.ICAO_address.ToString("X5"));
30283029
MainV2.instance.adsbPlaneAge[adsb.ICAO_address.ToString("X5")] = DateTime.Now;
@@ -3032,7 +3033,7 @@ public MAVLinkMessage readPacket()
30323033
// set seens sysid's based on hb packet - this will hide 3dr radio packets
30333034
if (msgid == (byte)MAVLink.MAVLINK_MSG_ID.HEARTBEAT)
30343035
{
3035-
mavlink_heartbeat_t hb = buffer.ByteArrayToStructure<mavlink_heartbeat_t>(6);
3036+
mavlink_heartbeat_t hb = message.ByteArrayToStructure<mavlink_heartbeat_t>();
30363037

30373038
// not a gcs
30383039
if (hb.type != (byte) MAV_TYPE.GCS)
@@ -3065,7 +3066,7 @@ public MAVLinkMessage readPacket()
30653066

30663067
if (msgid == (byte)MAVLink.MAVLINK_MSG_ID.STATUSTEXT) // status text
30673068
{
3068-
var msg = buffer.ByteArrayToStructure<MAVLink.mavlink_statustext_t>();
3069+
var msg = message.ByteArrayToStructure<MAVLink.mavlink_statustext_t>();
30693070

30703071
byte sev = msg.severity;
30713072

@@ -3174,8 +3175,9 @@ public MAVLinkMessage readPacket()
31743175
}
31753176
}
31763177
}
3177-
catch
3178+
catch (Exception ex)
31783179
{
3180+
log.Error(ex);
31793181
}
31803182

31813183
// update last valid packet receive time

‎Mavlink/MAVState.cs

Lines changed: 19 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -18,8 +18,7 @@ public MAVState()
1818
this.sysid = 0;
1919
this.compid = 0;
2020
this.param = new MAVLinkParamList();
21-
this.packets = new byte[0x100][];
22-
this.packetseencount = new int[0x100];
21+
this.packets = new Dictionary<uint, MAVLinkMessage>();
2322
this.aptype = 0;
2423
this.apname = 0;
2524
this.recvpacketcount = 0;
@@ -80,9 +79,25 @@ public MAVState()
8079
/// <summary>
8180
/// storage of a previous packet recevied of a specific type
8281
/// </summary>
83-
public byte[][] packets { get; set; }
82+
public Dictionary<uint, MAVLinkMessage> packets { get; set; }
8483

85-
public int[] packetseencount { get; set; }
84+
public MAVLinkMessage getPacket(uint mavlinkid)
85+
{
86+
if (packets.ContainsKey(mavlinkid))
87+
{
88+
return packets[mavlinkid];
89+
}
90+
91+
return null;
92+
}
93+
94+
public void clearPacket(uint mavlinkid)
95+
{
96+
if (packets.ContainsKey(mavlinkid))
97+
{
98+
packets[mavlinkid] = null;
99+
}
100+
}
86101

87102
/// <summary>
88103
/// time seen of last mavlink packet

‎OSDVideo.cs

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -320,8 +320,6 @@ void dolog()
320320
}
321321
mine.logreadmode = true;
322322

323-
mine.MAV.packets.Initialize(); // clear
324-
325323
mine.readPacket();
326324

327325
startlogtime = mine.lastlogread;

‎Utilities/httpserver.cs

Lines changed: 16 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -688,73 +688,69 @@ public void DoAcceptTcpClientCallback(IAsyncResult ar)
688688

689689
Messagejson message = new Messagejson();
690690

691-
692691
if (MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.ATTITUDE] != null)
693692
message.ATTITUDE = new Message2()
694693
{
695-
index = MainV2.comPort.MAV.packetseencount[(byte) MAVLink.MAVLINK_MSG_ID.ATTITUDE],
694+
index = 1,
696695
msg =
697696
MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.ATTITUDE]
698-
.ByteArrayToStructure<MAVLink.mavlink_attitude_t>(6)
697+
.ByteArrayToStructure<MAVLink.mavlink_attitude_t>()
699698
};
700699
if (MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.VFR_HUD] != null)
701700
message.VFR_HUD = new Message2()
702701
{
703-
index = MainV2.comPort.MAV.packetseencount[(byte) MAVLink.MAVLINK_MSG_ID.VFR_HUD],
702+
index = 1,
704703
msg =
705704
MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.VFR_HUD]
706-
.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>(6)
705+
.ByteArrayToStructure<MAVLink.mavlink_vfr_hud_t>()
707706
};
708707
if (MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT] != null)
709708
message.NAV_CONTROLLER_OUTPUT = new Message2()
710709
{
711-
index =
712-
MainV2.comPort.MAV.packetseencount[
713-
(byte) MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT],
710+
index = 1,
714711
msg =
715712
MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.NAV_CONTROLLER_OUTPUT]
716-
.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>(6)
713+
.ByteArrayToStructure<MAVLink.mavlink_nav_controller_output_t>()
717714
};
718715
if (MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT] != null)
719716
message.GPS_RAW_INT = new Message2()
720717
{
721-
index =
722-
MainV2.comPort.MAV.packetseencount[(byte) MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT],
718+
index = 1,
723719
msg =
724720
MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.GPS_RAW_INT]
725-
.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>(6)
721+
.ByteArrayToStructure<MAVLink.mavlink_gps_raw_int_t>()
726722
};
727723
if (MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.HEARTBEAT] != null)
728724
message.HEARTBEAT = new Message2()
729725
{
730-
index = MainV2.comPort.MAV.packetseencount[(byte) MAVLink.MAVLINK_MSG_ID.HEARTBEAT],
726+
index = 1,
731727
msg =
732728
MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.HEARTBEAT]
733-
.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>(6)
729+
.ByteArrayToStructure<MAVLink.mavlink_heartbeat_t>()
734730
};
735731
if (MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.GPS_STATUS] != null)
736732
message.GPS_STATUS = new Message2()
737733
{
738-
index = MainV2.comPort.MAV.packetseencount[(byte) MAVLink.MAVLINK_MSG_ID.GPS_STATUS],
734+
index = 1,
739735
msg =
740736
MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.GPS_STATUS]
741-
.ByteArrayToStructure<MAVLink.mavlink_gps_status_t>(6)
737+
.ByteArrayToStructure<MAVLink.mavlink_gps_status_t>()
742738
};
743739
if (MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.STATUSTEXT] != null)
744740
message.STATUSTEXT = new Message2()
745741
{
746-
index = MainV2.comPort.MAV.packetseencount[(byte) MAVLink.MAVLINK_MSG_ID.STATUSTEXT],
742+
index = 1,
747743
msg =
748744
MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.STATUSTEXT]
749-
.ByteArrayToStructure<MAVLink.mavlink_statustext_t>(6)
745+
.ByteArrayToStructure<MAVLink.mavlink_statustext_t>()
750746
};
751747
if (MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SYS_STATUS] != null)
752748
message.SYS_STATUS = new Message2()
753749
{
754-
index = MainV2.comPort.MAV.packetseencount[(byte) MAVLink.MAVLINK_MSG_ID.SYS_STATUS],
750+
index = 1,
755751
msg =
756752
MainV2.comPort.MAV.packets[(byte) MAVLink.MAVLINK_MSG_ID.SYS_STATUS]
757-
.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>(6)
753+
.ByteArrayToStructure<MAVLink.mavlink_sys_status_t>()
758754
};
759755

760756
message.META_LINKQUALITY =

0 commit comments

Comments
 (0)