Исправление в Mission Planner для улучшения качества линка телеметрии
Купил два комплекта модулей 3DR телеметрии на 433 МГц - от HK и himodel. Модули чуть разные, антенны тоже разные, даже прошивки разные (1.7 и 1.9). В Mission Planner при установлении связи на default настройках (MavLink, ECC, Opresend, 20db, Link quality был стабильно порядка 10%, иногда опускаясь или подымаясь на 5%, а счетчик потерянных пакетов бежал как угорелый. Я пробывал пропаивать модуль, разбирал антенны (они оказались нормальными, с пружинкой), даже сделал свой диполь - качество не улучшалось. Потом нашел лог принятых пакетов и из него понял, что часто происходят сбои синхронизации по причине того, что иногда приходят несколько сообщений с порядковым номером сильно отличающимся от того, который ожидался. Добавив немного отладки, я выяснил, что коды этих сообщений всегда 166 и 109, приходят они иногда парами, иногда по-одиночке, а их порядковые номера имеют свою собственную нумерацию, не похожую на другие сообщения. Число потерянных пакетов вычисляется как разница между порядковыми номерами (по модулю 256), отсюда и их большое число.
Интернет про эти пакеты знает мало, и пришлось залезть в исходники прошивки телеметрии (github.com/tridge/SiK/blob/master/…/mavlink.c). Видим, что сообщения 166 и 109 (MAVLINK_MSG_ID_RADIO и MAVLINK_MSG_ID_RADIO_STATUS) посылаются в MAVLink_report со своей собственной нумерацией из seqnum. Зачем нужно слать оба - не понятно.
Поняв в чем дело, я подправил MissionPlanner, чтобы нумерация именно этих сообщений игнорировалась. Теперь MissionPlanner показывает 100% линк, и если и появляется потерянный пакет, это реально так. Также я поправил подсчет потерянных пакетов, там были “ошибки десятого столба изгороди”.
Все изменения помечены комментом, начинающимся с // AP;
File: MAVLinkInterface.cs
public byte[] readPacket()
...
if ((buffer[0] == 'U' || buffer[0] == 254) && buffer.Length >= buffer[1])
{
// check if we lost pacakets based on seqno
byte packetSeqNo = buffer[2];
int expectedPacketSeqNo = ((MAVlist[sysid].recvpacketcount + 1) % 0x100);
{
// AP; START; Suppress sync lost error when RADIO and RADIO_STATUS packets coome with the same seqno;
if (buffer[5] == (byte)MAVLINK_MSG_ID.RADIO || buffer[5] == (byte)MAVLINK_MSG_ID.RADIO_STATUS) // It is RADIO or RADIO_STATUS packet;
{
log.InfoFormat("injected pkt seqno {0}, crc {1}, msg {2}", packetSeqNo, crc, buffer[5]);
}
else
// AP; STOP; Suppress sync lost error when RADIO and RADIO_STATUS packets coome with the same seqno;
{
if (packetSeqNo != expectedPacketSeqNo && !MAVlist[sysid].justStarted)
{
synclost++; // actualy sync loss's
int numLost = 0;
if (packetSeqNo < ((MAVlist[sysid].recvpacketcount + 1))) // recvpacketcount = 255 then 10 < 256 = true if was % 0x100 this would fail
{
numLost = 0x100 - MAVlist[sysid].recvpacketcount + packetSeqNo - 1; // AP; i.e. rec=255;exp=0;rec=1;256-255+1-1=1=lost;
}
else
{
numLost = packetSeqNo - MAVlist[sysid].recvpacketcount - 1; // AP; -1 added; i.e. MAV.recvpacketcount = 163; expected = 164;
}
packetslost += numLost;
WhenPacketLost.OnNext(numLost);
log.InfoFormat("lost pkts new seqno {0} expected seqno {1} pkts lost {2}; crc {3}, msg {4}",
packetSeqNo, expectedPacketSeqNo, numLost, crc, buffer[5]);
}
else
{
log.InfoFormat("good pkt seqno {0}; crc {1}, msg {2}", packetSeqNo, crc, buffer[5]);
}
// AP; START; Fixing counting packets, first packet recedived;
if (MAVlist[sysid].justStarted)
{
MAVlist[sysid].justStarted = false;
log.InfoFormat("started");
}
// AP; STOP; Fixing counting packets, first packet recedived;
packetsnotlost++;
MAVlist[sysid].recvpacketcount = packetSeqNo;
}
}
WhenPacketReceived.OnNext(1);
void SetupMavConnect(byte sysid, byte compid,byte mgsno, mavlink_heartbeat_t hb)
{
...
log.InfoFormat("recvpacketcount set to {0}", mgsno);
MAV.justStarted = true; // AP;
}
File: MAVState.cs
public class MAVState : MAVLink
{
...
internal int recvpacketcount = 0;
internal bool justStarted = true; // AP; not detecting synh loss just after connect;
}
Из мелких изменений: для ускорения коннекта и поиска портов поставил в начало зависающих функций проверку на специальный параметр skip_get_portName, который надо добавить в файл конфигурации в
подкаталоге bin того места, куда установлен MissionPlanner.
File: CommSerialPort.cs
public static string GetNiceName(string port)
{
if (System.Configuration.ConfigurationManager.AppSettings["skip_get_portName"] == "1") // AP; Skip quering for port name (hanging with Bluetooth ports in Win);
return "";
// make sure we are exclusive
...
internal bool ispx4(string port)
{
if (System.Configuration.ConfigurationManager.AppSettings["skip_get_portName"] == "1") // AP; Skip ispx4 detection (hanging with Bluetooth ports in Win);
return false;
try
{
...
Пока отлаживал свои изменения в прошивке MegaPirateNG при помощи текстовых сообщений в закладке Messages, обнаружил, что список сообщений автоматически не проматывается в конец при добавлении сообщения. Так что я поставил ему в дизайнере вертикальный ScrollBar и добавил промотку:
File: FlightData.cs
private void Messagetabtimer_Tick(object sender, EventArgs e)
{
...
txt_messagebox.Text = message.ToString();
// AP; START; Scroll to the end when messages added; Vertical bar is added in designer as well;
txt_messagebox.SelectionStart = txt_messagebox.Text.Length;
txt_messagebox.ScrollToCaret();
txt_messagebox.Refresh();
// AP; STOP; Scroll to the end;
messagecount = MainV2.comPort.MAV.cs.messages.Count;
Стало гораздо удобнее 😉
Вот так выглядит лог теперь (видны injected (вставленные) пакеты):
2014-10-24 01:00:44,141 INFO MissionPlanner.MAVLinkInterface - good pkt seqno 19; crc 16352, msg 163 (c:\Andrey\MegaPirateNG\MissionPlanner.2\MissionPlanner-master\Mavlink\MAVLinkInterface.cs:2511) [Main Serial reader]
2014-10-24 01:00:44,141 INFO MissionPlanner.MAVLinkInterface - good pkt seqno 20; crc 4247, msg 165 (c:\Andrey\MegaPirateNG\MissionPlanner.2\MissionPlanner-master\Mavlink\MAVLinkInterface.cs:2511) [Main Serial reader]
2014-10-24 01:00:44,142 INFO MissionPlanner.MAVLinkInterface - good pkt seqno 21; crc 11259, msg 2 (c:\Andrey\MegaPirateNG\MissionPlanner.2\MissionPlanner-master\Mavlink\MAVLinkInterface.cs:2511) [Main Serial reader]
2014-10-24 01:00:44,142 INFO MissionPlanner.MAVLinkInterface - good pkt seqno 22; crc 61601, msg 0 (c:\Andrey\MegaPirateNG\MissionPlanner.2\MissionPlanner-master\Mavlink\MAVLinkInterface.cs:2511) [Main Serial reader]
2014-10-24 01:00:44,142 INFO MissionPlanner.MAVLinkInterface - injected pkt seqno 165, crc 24721, msg 166 (c:\Andrey\MegaPirateNG\MissionPlanner.2\MissionPlanner-master\Mavlink\MAVLinkInterface.cs:2485) [Main Serial reader]
2014-10-24 01:00:44,143 INFO MissionPlanner.MAVLinkInterface - injected pkt seqno 165, crc 4848, msg 109 (c:\Andrey\MegaPirateNG\MissionPlanner.2\MissionPlanner-master\Mavlink\MAVLinkInterface.cs:2485) [Main Serial reader]
2014-10-24 01:00:44,202 INFO MissionPlanner.MAVLinkInterface - good pkt seqno 23; crc 28240, msg 30 (c:\Andrey\MegaPirateNG\MissionPlanner.2\MissionPlanner-master\Mavlink\MAVLinkInterface.cs:2511) [Main Serial reader]
2014-10-24 01:00:44,202 INFO MissionPlanner.MAVLinkInterface - good pkt seqno 24; crc 1174, msg 74 (c:\Andrey\MegaPirateNG\MissionPlanner.2\MissionPlanner-master\Mavlink\MAVLinkInterface.cs:2511) [Main Serial reader]
