diff --git a/ExtLibs/Mavlink/Mavlink.cs b/ExtLibs/Mavlink/Mavlink.cs index fb2d326c49..344e4cc4ba 100644 --- a/ExtLibs/Mavlink/Mavlink.cs +++ b/ExtLibs/Mavlink/Mavlink.cs @@ -6012,12 +6012,21 @@ public enum MAG_CAL_STATUS: byte /// | [Description("")] MAG_CAL_FAILED=5, - /// | - [Description("")] - MAG_CAL_BAD_ORIENTATION=6, - /// | - [Description("")] - MAG_CAL_BAD_RADIUS=7, + /// Compass calibration failed: the vehicle orientation is outside the required tolerance. | + [Description("Compass calibration failed: the vehicle orientation is outside the required tolerance.")] + MAG_CAL_FAILED_ORIENTATION=6, + /// Compass calibration failed: the radius of the fitted sphere is unrealistically small or large. | + [Description("Compass calibration failed: the radius of the fitted sphere is unrealistically small or large.")] + MAG_CAL_FAILED_RADIUS=7, + /// Compass calibration failed: offset magnitude too large. | + [Description("Compass calibration failed: offset magnitude too large.")] + MAG_CAL_FAILED_OFFSETS=8, + /// Compass calibration failed: diagonal or off-diagonal scaling values out of valid range. | + [Description("Compass calibration failed: diagonal or off-diagonal scaling values out of valid range.")] + MAG_CAL_FAILED_DIAG_SCALING=9, + /// Compass calibration failed: fitness (RMS residual) exceeds tolerance. | + [Description("Compass calibration failed: fitness (RMS residual) exceeds tolerance.")] + MAG_CAL_FAILED_RESIDUALS_HIGH=10, }; diff --git a/ExtLibs/Mavlink/message_definitions/common.xml b/ExtLibs/Mavlink/message_definitions/common.xml index 856160b534..ef712ce3a1 100644 --- a/ExtLibs/Mavlink/message_definitions/common.xml +++ b/ExtLibs/Mavlink/message_definitions/common.xml @@ -4142,8 +4142,21 @@ - - + + Compass calibration failed: the vehicle orientation is outside the required tolerance. + + + Compass calibration failed: the radius of the fitted sphere is unrealistically small or large. + + + Compass calibration failed: offset magnitude too large. + + + Compass calibration failed: diagonal or off-diagonal scaling values out of valid range. + + + Compass calibration failed: fitness (RMS residual) exceeds tolerance. + diff --git a/GCSViews/ConfigurationView/ConfigHWCompass.cs b/GCSViews/ConfigurationView/ConfigHWCompass.cs index 1d2b6ed32d..3bb249a6a6 100644 --- a/GCSViews/ConfigurationView/ConfigHWCompass.cs +++ b/GCSViews/ConfigurationView/ConfigHWCompass.cs @@ -6,6 +6,7 @@ using System.ComponentModel; using System.Diagnostics; using System.Drawing; +using System.Linq; using System.Windows.Forms; namespace MissionPlanner.GCSViews.ConfigurationView @@ -419,6 +420,48 @@ private void linkLabel1_LinkClicked_1(object sender, LinkLabelLinkClickedEventAr private List mprog = new List(); private List mrep = new List(); + private Dictionary lastFailureStatus = new Dictionary(); + private HashSet _startedCompasses = new HashSet(); + private HashSet _succeededCompasses = new HashSet(); + private HashSet _autosavedCompasses = new HashSet(); + private byte _activeCalMask; + + // Firmware uses 0-based compass_id on the wire; users see "Mag 1/2/3" in the UI. + // Log strings show both so operators can correlate the visible row with logs and + // firmware messages. Keep this the sole formatter to avoid drift. + private static string CompassLabel(byte compassId) => "Mag " + (compassId + 1) + " (id: " + compassId + ")"; + + // Prefer the MAVLink [Description] if the dialect XML carries one for this code; + // otherwise fall back to the raw enum name. This keeps failure messages in sync + // with upstream firmware wording across mavlink bumps at zero maintenance cost. + // Note: mavgen emits its own MAVLink.Description attribute (see MavlinkParse.cs), + // NOT System.ComponentModel.DescriptionAttribute. + private static string StatusText(MAVLink.MAG_CAL_STATUS status) + { + var field = typeof(MAVLink.MAG_CAL_STATUS).GetField(status.ToString()); + var attr = field == null ? null + : (MAVLink.Description)Attribute.GetCustomAttribute(field, typeof(MAVLink.Description)); + return string.IsNullOrEmpty(attr?.Text) + ? status.ToString() + : status + " \u2014 " + attr.Text; + } + + private static int CountBits(byte mask) + { + int count = 0; + while (mask != 0) + { + count += mask & 1; + mask >>= 1; + } + + return count; + } + + private int ExpectedCompassCount() + { + return _activeCalMask == 0 ? _startedCompasses.Count : CountBits(_activeCalMask); + } private bool ReceviedPacket(MAVLink.MAVLinkMessage packet) { @@ -465,13 +508,29 @@ private void BUT_OBmagcalstart_Click(object sender, EventArgs e) mprog.Clear(); mrep.Clear(); + lastFailureStatus.Clear(); + _startedCompasses.Clear(); + _succeededCompasses.Clear(); + _autosavedCompasses.Clear(); + _activeCalMask = 0; horizontalProgressBar1.Value = 0; horizontalProgressBar2.Value = 0; horizontalProgressBar3.Value = 0; + // Reset the poll cadence — the tick handler bumps this to 1000 ms once + // autosave lands; without this reset the second cal per page visit polls + // once per second and the bars update visibly laggy. + timer1.Interval = 100; + + // Unsubscribe any prior subscriptions from an earlier Start click so we don't + // stack duplicates when the user restarts calibration without leaving the screen. + // Fields default to 0; UnSubscribeToPacketType(0) safely no-ops. + MainV2.comPort.UnSubscribeToPacketType(packetsub1); + MainV2.comPort.UnSubscribeToPacketType(packetsub2); packetsub1 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.MAG_CAL_PROGRESS, ReceviedPacket, (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent); packetsub2 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.MAG_CAL_REPORT, ReceviedPacket, (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent); + BUT_OBmagcalstart.Enabled = false; BUT_OBmagcalaccept.Enabled = true; BUT_OBmagcalcancel.Enabled = true; timer1.Start(); @@ -493,6 +552,7 @@ private void BUT_OBmagcalaccept_Click(object sender, EventArgs e) MainV2.comPort.UnSubscribeToPacketType(packetsub2); timer1.Stop(); + BUT_OBmagcalstart.Enabled = true; } private void BUT_OBmagcalcancel_Click(object sender, EventArgs e) @@ -510,17 +570,18 @@ private void BUT_OBmagcalcancel_Click(object sender, EventArgs e) MainV2.comPort.UnSubscribeToPacketType(packetsub2); timer1.Stop(); + BUT_OBmagcalstart.Enabled = true; } private void timer1_Tick(object sender, EventArgs e) { lbl_obmagresult.Clear(); int compasscount = 0; - int completecount = 0; lock (mprog) { - // somewhere to save our % - Dictionary status = new Dictionary(); + // Sorted by compass_id so the progress line always reads Mag 1, Mag 2, Mag 3 + // regardless of the order firmware's three calibrators first transmit. + SortedDictionary status = new SortedDictionary(); foreach (var item in mprog) { status[((MAVLink.mavlink_mag_cal_progress_t)item.data).compass_id] = item; @@ -531,22 +592,33 @@ private void timer1_Tick(object sender, EventArgs e) foreach (var item in status) { var obj = (MAVLink.mavlink_mag_cal_progress_t)item.Value.data; + _activeCalMask |= obj.cal_mask; try { - if (item.Key == 0) - horizontalProgressBar1.Value = obj.completion_pct; - if (item.Key == 1) - horizontalProgressBar2.Value = obj.completion_pct; - if (item.Key == 2) - horizontalProgressBar3.Value = obj.completion_pct; + // Don't let a stale progress packet overwrite the 100/green we set + // when the SUCCESS report arrived. + if (!_succeededCompasses.Contains(item.Key)) + { + if (item.Key == 0) + horizontalProgressBar1.Value = obj.completion_pct; + if (item.Key == 1) + horizontalProgressBar2.Value = obj.completion_pct; + if (item.Key == 2) + horizontalProgressBar3.Value = obj.completion_pct; + } } catch { } - message += "id:" + item.Key + " " + obj.completion_pct.ToString() + "% "; + // Firmware caps completion_pct at ~99 and signals 100 implicitly via the + // MAG_CAL_REPORT with MAG_CAL_SUCCESS. Match the bar (forced to 100 in the + // report handler) so text and visual agree — for SUCCESS, not just autosaved. + var pct = _succeededCompasses.Contains(item.Key) ? 100 : obj.completion_pct; + message += CompassLabel(item.Key) + " " + pct.ToString() + "% "; + _startedCompasses.Add(item.Key); compasscount++; } - lbl_obmagresult.AppendText(message + "\n"); + lbl_obmagresult.AppendText(message + Environment.NewLine); } lock (mrep) @@ -557,52 +629,134 @@ private void timer1_Tick(object sender, EventArgs e) { var obj = (MAVLink.mavlink_mag_cal_report_t)item.data; - if (obj.compass_id == 0 && obj.ofs_x == 0) + if (obj.ofs_x == 0 && obj.ofs_y == 0 && obj.ofs_z == 0 + && obj.cal_status == (byte)MAVLink.MAG_CAL_STATUS.MAG_CAL_NOT_STARTED) continue; status[obj.compass_id] = item; } // message for user + var consumedCompassIds = new List(); foreach (var item in status.Values) { var obj = (MAVLink.mavlink_mag_cal_report_t)item.data; + _activeCalMask |= obj.cal_mask; + + // Any report from a compass proves it was actually calibrated (even if + // it failed on the very first sample check with zero progress packets). + // The progress loop also adds to this set, but reports can arrive + // without any preceding progress under the new PR #32757 early-abort + // firmware — without this line the completion check below would fire + // prematurely, declaring cal "done" while a compass was still retrying. + _startedCompasses.Add(obj.compass_id); - lbl_obmagresult.AppendText("id:" + obj.compass_id + " x:" + obj.ofs_x.ToString("0.0") + " y:" + + lbl_obmagresult.AppendText(CompassLabel(obj.compass_id) + " x:" + obj.ofs_x.ToString("0.0") + " y:" + obj.ofs_y.ToString("0.0") + " z:" + obj.ofs_z.ToString("0.0") + " fit:" + obj.fitness.ToString("0.0") + " " + - (MAVLink.MAG_CAL_STATUS)obj.cal_status + "\n"); + StatusText((MAVLink.MAG_CAL_STATUS)obj.cal_status) + Environment.NewLine); try { - if (obj.compass_id == 0) - horizontalProgressBar1.Value = 100; - if (obj.compass_id == 1) - horizontalProgressBar2.Value = 100; - if (obj.compass_id == 2) - horizontalProgressBar3.Value = 100; + // Green + 100 on numeric SUCCESS (autosaved is a stricter + // downstream state used only to drive the reboot popup below). + if ((MAVLink.MAG_CAL_STATUS)obj.cal_status == MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS) + { + if (obj.compass_id == 0) + horizontalProgressBar1.Value = 100; + if (obj.compass_id == 1) + horizontalProgressBar2.Value = 100; + if (obj.compass_id == 2) + horizontalProgressBar3.Value = 100; + } } catch { } - if ((MAVLink.MAG_CAL_STATUS)obj.cal_status != MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS) + var calStatus = (MAVLink.MAG_CAL_STATUS)obj.cal_status; + // The "please reboot" completion check below is gated on autosaved==1 + // (the point at which firmware persisted the offsets), so it fires only + // when rebooting actually matters. + if (calStatus > MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS) { - //CustomMessageBox.Show(Strings.CommandFailed); + lastFailureStatus[obj.compass_id] = calStatus; + consumedCompassIds.Add(obj.compass_id); + _autosavedCompasses.Remove(obj.compass_id); + // if a prior SUCCESS was later reversed by firmware, drop the flag so + // the retry's progress packets can drive the bar again + _succeededCompasses.Remove(obj.compass_id); + // purge stale progress so the old 99% can't overwrite the reset + lock (mprog) + { + mprog.RemoveAll(m => ((MAVLink.mavlink_mag_cal_progress_t)m.data).compass_id == obj.compass_id); + } + // reset bar so the user sees the retry starting from 0 + try + { + if (obj.compass_id == 0) horizontalProgressBar1.Value = 0; + if (obj.compass_id == 1) horizontalProgressBar2.Value = 0; + if (obj.compass_id == 2) horizontalProgressBar3.Value = 0; + } + catch { } } + else if (calStatus == MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS) + { + lastFailureStatus.Remove(obj.compass_id); + _succeededCompasses.Add(obj.compass_id); + consumedCompassIds.Add(obj.compass_id); + } + // running/waiting states leave lastFailureStatus unchanged so the + // previous failure reason stays visible while calibration retries if (obj.autosaved == 1) { - completecount++; + _autosavedCompasses.Add(obj.compass_id); timer1.Interval = 1000; } } + + // consume terminal reports so we don't re-render the same "x:… y:… z:… fit:… SUCCESS" + // line every timer tick and so a failure report can't fight retry progress. Any + // later report from firmware (e.g. a second MAG_CAL_REPORT with autosaved==1) + // will re-enter mrep via ReceviedPacket and be handled afresh next tick. + // lastFailureStatus preserves the failure message for the sticky footer. + if (consumedCompassIds.Count > 0) + mrep.RemoveAll(m => consumedCompassIds.Contains(((MAVLink.mavlink_mag_cal_report_t)m.data).compass_id)); + } + + // show last known failure reason per compass (persists across firmware auto-restarts) + if (lastFailureStatus.Count > 0) + { + string failures = ""; + foreach (var kv in lastFailureStatus) + failures += CompassLabel(kv.Key) + ": " + StatusText(kv.Value) + Environment.NewLine; + lbl_obmagresult.AppendText(failures); + } + + // Mixed-result partial save: PR #32757 firmware autosaves successful compasses + // individually as soon as they hit SUCCESS, even when other compasses are still + // failing/retrying. The all-successful "Please reboot" popup below does NOT fire + // in this case, so without this explicit banner the user only learns about the + // partial persist via a cryptic PreArm "Compass calibrated requires reboot" on + // the next arm attempt. + bool partialSave = _autosavedCompasses.Count > 0 && lastFailureStatus.Count > 0; + if (partialSave) + { + var savedList = string.Join(", ", + _autosavedCompasses.OrderBy(id => id).Select(id => CompassLabel(id))); + lbl_obmagresult.AppendText( + "Partial save: " + savedList + + " persisted to params. Reboot required before those changes apply." + + " Failed compasses continue to retry." + Environment.NewLine); } - if (compasscount == completecount && compasscount != 0) + int expectedCompassCount = ExpectedCompassCount(); + if (expectedCompassCount > 0 && lastFailureStatus.Count == 0 && _autosavedCompasses.Count >= expectedCompassCount) { BUT_OBmagcalcancel.Enabled = false; BUT_OBmagcalaccept.Enabled = false; + BUT_OBmagcalstart.Enabled = true; timer1.Stop(); CustomMessageBox.Show("Please reboot the autopilot"); } diff --git a/GCSViews/ConfigurationView/ConfigHWCompass2.Designer.cs b/GCSViews/ConfigurationView/ConfigHWCompass2.Designer.cs index 3df2b15dd0..fd1e489357 100644 --- a/GCSViews/ConfigurationView/ConfigHWCompass2.Designer.cs +++ b/GCSViews/ConfigurationView/ConfigHWCompass2.Designer.cs @@ -46,7 +46,6 @@ private void InitializeComponent() this.horizontalProgressBar2 = new MissionPlanner.Controls.HorizontalProgressBar(); this.horizontalProgressBar1 = new MissionPlanner.Controls.HorizontalProgressBar(); this.lbl_obmagresult = new System.Windows.Forms.TextBox(); - this.BUT_OBmagcalaccept = new MissionPlanner.Controls.MyButton(); this.BUT_OBmagcalcancel = new MissionPlanner.Controls.MyButton(); this.BUT_OBmagcalstart = new MissionPlanner.Controls.MyButton(); this.timer1 = new System.Windows.Forms.Timer(this.components); @@ -55,6 +54,9 @@ private void InitializeComponent() this.label4 = new System.Windows.Forms.Label(); this.but_reboot = new MissionPlanner.Controls.MyButton(); this.label5 = new System.Windows.Forms.Label(); + this.compassDeviceInfoBindingSource = new System.Windows.Forms.BindingSource(this.components); + this.but_missing = new MissionPlanner.Controls.MyButton(); + this.toolTipReboot = new System.Windows.Forms.ToolTip(this.components); this.mavlinkCheckBoxUseCompass3 = new MissionPlanner.Controls.MavlinkCheckBox(); this.mavlinkCheckBoxUseCompass2 = new MissionPlanner.Controls.MavlinkCheckBox(); this.CHK_compass_learn = new MissionPlanner.Controls.MavlinkCheckBox(); @@ -71,14 +73,12 @@ private void InitializeComponent() this.Orientation = new System.Windows.Forms.DataGridViewComboBoxColumn(); this.Up = new System.Windows.Forms.DataGridViewImageColumn(); this.Down = new System.Windows.Forms.DataGridViewImageColumn(); - this.compassDeviceInfoBindingSource = new System.Windows.Forms.BindingSource(this.components); - this.but_missing = new MissionPlanner.Controls.MyButton(); this.groupBoxonboardcalib.SuspendLayout(); ((System.ComponentModel.ISupportInitialize)(this.pictureBox3)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.pictureBox2)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.pictureBox1)).BeginInit(); - ((System.ComponentModel.ISupportInitialize)(this.myDataGridView1)).BeginInit(); ((System.ComponentModel.ISupportInitialize)(this.compassDeviceInfoBindingSource)).BeginInit(); + ((System.ComponentModel.ISupportInitialize)(this.myDataGridView1)).BeginInit(); this.SuspendLayout(); // // label1 @@ -107,7 +107,7 @@ private void InitializeComponent() | System.Windows.Forms.AnchorStyles.Right))); this.groupBox5.Location = new System.Drawing.Point(-1, 18); this.groupBox5.Name = "groupBox5"; - this.groupBox5.Size = new System.Drawing.Size(1197, 10); + this.groupBox5.Size = new System.Drawing.Size(1237, 10); this.groupBox5.TabIndex = 79; this.groupBox5.TabStop = false; // @@ -126,19 +126,18 @@ private void InitializeComponent() this.groupBoxonboardcalib.Controls.Add(this.horizontalProgressBar2); this.groupBoxonboardcalib.Controls.Add(this.horizontalProgressBar1); this.groupBoxonboardcalib.Controls.Add(this.lbl_obmagresult); - this.groupBoxonboardcalib.Controls.Add(this.BUT_OBmagcalaccept); this.groupBoxonboardcalib.Controls.Add(this.BUT_OBmagcalcancel); this.groupBoxonboardcalib.Controls.Add(this.BUT_OBmagcalstart); this.groupBoxonboardcalib.Location = new System.Drawing.Point(3, 355); this.groupBoxonboardcalib.Name = "groupBoxonboardcalib"; - this.groupBoxonboardcalib.Size = new System.Drawing.Size(600, 162); + this.groupBoxonboardcalib.Size = new System.Drawing.Size(712, 162); this.groupBoxonboardcalib.TabIndex = 81; this.groupBoxonboardcalib.TabStop = false; this.groupBoxonboardcalib.Text = "Onboard Mag Calibration"; // // pictureBox3 // - this.pictureBox3.Location = new System.Drawing.Point(321, 107); + this.pictureBox3.Location = new System.Drawing.Point(203, 107); this.pictureBox3.Name = "pictureBox3"; this.pictureBox3.Size = new System.Drawing.Size(23, 23); this.pictureBox3.TabIndex = 20; @@ -146,7 +145,7 @@ private void InitializeComponent() // // pictureBox2 // - this.pictureBox2.Location = new System.Drawing.Point(321, 78); + this.pictureBox2.Location = new System.Drawing.Point(203, 78); this.pictureBox2.Name = "pictureBox2"; this.pictureBox2.Size = new System.Drawing.Size(23, 23); this.pictureBox2.TabIndex = 19; @@ -154,7 +153,7 @@ private void InitializeComponent() // // pictureBox1 // - this.pictureBox1.Location = new System.Drawing.Point(321, 49); + this.pictureBox1.Location = new System.Drawing.Point(203, 49); this.pictureBox1.Name = "pictureBox1"; this.pictureBox1.Size = new System.Drawing.Size(23, 23); this.pictureBox1.TabIndex = 18; @@ -198,7 +197,7 @@ private void InitializeComponent() // this.label9.AutoSize = true; this.label9.ImeMode = System.Windows.Forms.ImeMode.NoControl; - this.label9.Location = new System.Drawing.Point(6, 117); + this.label9.Location = new System.Drawing.Point(7, 112); this.label9.Name = "label9"; this.label9.Size = new System.Drawing.Size(37, 13); this.label9.TabIndex = 14; @@ -208,7 +207,7 @@ private void InitializeComponent() // this.label8.AutoSize = true; this.label8.ImeMode = System.Windows.Forms.ImeMode.NoControl; - this.label8.Location = new System.Drawing.Point(6, 88); + this.label8.Location = new System.Drawing.Point(7, 83); this.label8.Name = "label8"; this.label8.Size = new System.Drawing.Size(37, 13); this.label8.TabIndex = 13; @@ -218,7 +217,7 @@ private void InitializeComponent() // this.label7.AutoSize = true; this.label7.ImeMode = System.Windows.Forms.ImeMode.NoControl; - this.label7.Location = new System.Drawing.Point(6, 59); + this.label7.Location = new System.Drawing.Point(7, 54); this.label7.Name = "label7"; this.label7.Size = new System.Drawing.Size(37, 13); this.label7.TabIndex = 12; @@ -233,7 +232,7 @@ private void InitializeComponent() this.horizontalProgressBar3.maxline = 0; this.horizontalProgressBar3.minline = 0; this.horizontalProgressBar3.Name = "horizontalProgressBar3"; - this.horizontalProgressBar3.Size = new System.Drawing.Size(258, 23); + this.horizontalProgressBar3.Size = new System.Drawing.Size(140, 23); this.horizontalProgressBar3.TabIndex = 6; // // horizontalProgressBar2 @@ -245,7 +244,7 @@ private void InitializeComponent() this.horizontalProgressBar2.maxline = 0; this.horizontalProgressBar2.minline = 0; this.horizontalProgressBar2.Name = "horizontalProgressBar2"; - this.horizontalProgressBar2.Size = new System.Drawing.Size(258, 23); + this.horizontalProgressBar2.Size = new System.Drawing.Size(140, 23); this.horizontalProgressBar2.TabIndex = 5; // // horizontalProgressBar1 @@ -257,52 +256,42 @@ private void InitializeComponent() this.horizontalProgressBar1.maxline = 0; this.horizontalProgressBar1.minline = 0; this.horizontalProgressBar1.Name = "horizontalProgressBar1"; - this.horizontalProgressBar1.Size = new System.Drawing.Size(258, 23); + this.horizontalProgressBar1.Size = new System.Drawing.Size(140, 23); this.horizontalProgressBar1.TabIndex = 4; // // lbl_obmagresult // this.lbl_obmagresult.ImeMode = System.Windows.Forms.ImeMode.NoControl; - this.lbl_obmagresult.Location = new System.Drawing.Point(350, 20); + this.lbl_obmagresult.Location = new System.Drawing.Point(232, 20); this.lbl_obmagresult.Multiline = true; this.lbl_obmagresult.Name = "lbl_obmagresult"; this.lbl_obmagresult.ReadOnly = true; this.lbl_obmagresult.ScrollBars = System.Windows.Forms.ScrollBars.Vertical; - this.lbl_obmagresult.Size = new System.Drawing.Size(244, 110); + this.lbl_obmagresult.Size = new System.Drawing.Size(474, 110); this.lbl_obmagresult.TabIndex = 3; // - // BUT_OBmagcalaccept - // - this.BUT_OBmagcalaccept.Enabled = false; - this.BUT_OBmagcalaccept.ImeMode = System.Windows.Forms.ImeMode.NoControl; - this.BUT_OBmagcalaccept.Location = new System.Drawing.Point(125, 20); - this.BUT_OBmagcalaccept.Name = "BUT_OBmagcalaccept"; - this.BUT_OBmagcalaccept.Size = new System.Drawing.Size(75, 23); - this.BUT_OBmagcalaccept.TabIndex = 1; - this.BUT_OBmagcalaccept.Text = "Accept"; - this.BUT_OBmagcalaccept.UseVisualStyleBackColor = true; - this.BUT_OBmagcalaccept.Click += new System.EventHandler(this.BUT_OBmagcalaccept_Click); - // // BUT_OBmagcalcancel // this.BUT_OBmagcalcancel.Enabled = false; this.BUT_OBmagcalcancel.ImeMode = System.Windows.Forms.ImeMode.NoControl; - this.BUT_OBmagcalcancel.Location = new System.Drawing.Point(206, 20); + this.BUT_OBmagcalcancel.Location = new System.Drawing.Point(120, 20); this.BUT_OBmagcalcancel.Name = "BUT_OBmagcalcancel"; - this.BUT_OBmagcalcancel.Size = new System.Drawing.Size(75, 23); + this.BUT_OBmagcalcancel.Size = new System.Drawing.Size(106, 23); this.BUT_OBmagcalcancel.TabIndex = 2; this.BUT_OBmagcalcancel.Text = "Cancel"; + this.BUT_OBmagcalcancel.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_OBmagcalcancel.UseVisualStyleBackColor = true; this.BUT_OBmagcalcancel.Click += new System.EventHandler(this.BUT_OBmagcalcancel_Click); // // BUT_OBmagcalstart // this.BUT_OBmagcalstart.ImeMode = System.Windows.Forms.ImeMode.NoControl; - this.BUT_OBmagcalstart.Location = new System.Drawing.Point(44, 20); + this.BUT_OBmagcalstart.Location = new System.Drawing.Point(7, 20); this.BUT_OBmagcalstart.Name = "BUT_OBmagcalstart"; - this.BUT_OBmagcalstart.Size = new System.Drawing.Size(75, 23); + this.BUT_OBmagcalstart.Size = new System.Drawing.Size(106, 23); this.BUT_OBmagcalstart.TabIndex = 0; this.BUT_OBmagcalstart.Text = "Start"; + this.BUT_OBmagcalstart.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.BUT_OBmagcalstart.UseVisualStyleBackColor = true; this.BUT_OBmagcalstart.Click += new System.EventHandler(this.BUT_OBmagcalstart_Click); // @@ -324,9 +313,10 @@ private void InitializeComponent() this.but_largemagcal.ImeMode = System.Windows.Forms.ImeMode.NoControl; this.but_largemagcal.Location = new System.Drawing.Point(3, 523); this.but_largemagcal.Name = "but_largemagcal"; - this.but_largemagcal.Size = new System.Drawing.Size(75, 23); + this.but_largemagcal.Size = new System.Drawing.Size(132, 31); this.but_largemagcal.TabIndex = 21; this.but_largemagcal.Text = "Large Vehicle MagCal"; + this.but_largemagcal.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.but_largemagcal.UseVisualStyleBackColor = true; this.but_largemagcal.Click += new System.EventHandler(this.but_largemagcal_Click); // @@ -343,9 +333,10 @@ private void InitializeComponent() // this.but_reboot.Location = new System.Drawing.Point(3, 313); this.but_reboot.Name = "but_reboot"; - this.but_reboot.Size = new System.Drawing.Size(75, 23); + this.but_reboot.Size = new System.Drawing.Size(106, 23); this.but_reboot.TabIndex = 88; this.but_reboot.Text = "Reboot"; + this.but_reboot.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); this.but_reboot.UseVisualStyleBackColor = true; this.but_reboot.Click += new System.EventHandler(this.but_reboot_Click); // @@ -358,6 +349,22 @@ private void InitializeComponent() this.label5.TabIndex = 89; this.label5.Text = "A reboot is required to adjust the ordering.\r\n"; // + // compassDeviceInfoBindingSource + // + this.compassDeviceInfoBindingSource.DataSource = typeof(MissionPlanner.GCSViews.ConfigurationView.ConfigHWCompass2.CompassDeviceInfo); + this.compassDeviceInfoBindingSource.CurrentChanged += new System.EventHandler(this.compassDeviceInfoBindingSource_CurrentChanged); + // + // but_missing + // + this.but_missing.Location = new System.Drawing.Point(321, 264); + this.but_missing.Name = "but_missing"; + this.but_missing.Size = new System.Drawing.Size(75, 30); + this.but_missing.TabIndex = 90; + this.but_missing.Text = "Remove Missing"; + this.but_missing.TextColorNotEnabled = System.Drawing.Color.FromArgb(((int)(((byte)(64)))), ((int)(((byte)(87)))), ((int)(((byte)(4))))); + this.but_missing.UseVisualStyleBackColor = true; + this.but_missing.Click += new System.EventHandler(this.but_missing_ClickAsync); + // // mavlinkCheckBoxUseCompass3 // this.mavlinkCheckBoxUseCompass3.AutoSize = true; @@ -441,7 +448,7 @@ private void InitializeComponent() this.myDataGridView1.ReadOnly = true; this.myDataGridView1.RowHeadersWidth = 20; this.myDataGridView1.SelectionMode = System.Windows.Forms.DataGridViewSelectionMode.FullRowSelect; - this.myDataGridView1.Size = new System.Drawing.Size(672, 209); + this.myDataGridView1.Size = new System.Drawing.Size(712, 209); this.myDataGridView1.TabIndex = 0; this.myDataGridView1.CellContentClick += new System.Windows.Forms.DataGridViewCellEventHandler(this.myDataGridView1_CellContentClick); this.myDataGridView1.DataError += new System.Windows.Forms.DataGridViewDataErrorEventHandler(this.myDataGridView1_DataError); @@ -533,21 +540,6 @@ private void InitializeComponent() this.Down.ReadOnly = true; this.Down.Width = 40; // - // compassDeviceInfoBindingSource - // - this.compassDeviceInfoBindingSource.DataSource = typeof(MissionPlanner.GCSViews.ConfigurationView.ConfigHWCompass2.CompassDeviceInfo); - this.compassDeviceInfoBindingSource.CurrentChanged += new System.EventHandler(this.compassDeviceInfoBindingSource_CurrentChanged); - // - // but_missing - // - this.but_missing.Location = new System.Drawing.Point(321, 271); - this.but_missing.Name = "but_missing"; - this.but_missing.Size = new System.Drawing.Size(75, 23); - this.but_missing.TabIndex = 90; - this.but_missing.Text = "Remove Missing"; - this.but_missing.UseVisualStyleBackColor = true; - this.but_missing.Click += new System.EventHandler(this.but_missing_ClickAsync); - // // ConfigHWCompass2 // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); @@ -568,14 +560,14 @@ private void InitializeComponent() this.Controls.Add(this.label1); this.Controls.Add(this.myDataGridView1); this.Name = "ConfigHWCompass2"; - this.Size = new System.Drawing.Size(678, 582); + this.Size = new System.Drawing.Size(718, 561); this.groupBoxonboardcalib.ResumeLayout(false); this.groupBoxonboardcalib.PerformLayout(); ((System.ComponentModel.ISupportInitialize)(this.pictureBox3)).EndInit(); ((System.ComponentModel.ISupportInitialize)(this.pictureBox2)).EndInit(); ((System.ComponentModel.ISupportInitialize)(this.pictureBox1)).EndInit(); - ((System.ComponentModel.ISupportInitialize)(this.myDataGridView1)).EndInit(); ((System.ComponentModel.ISupportInitialize)(this.compassDeviceInfoBindingSource)).EndInit(); + ((System.ComponentModel.ISupportInitialize)(this.myDataGridView1)).EndInit(); this.ResumeLayout(false); this.PerformLayout(); @@ -598,7 +590,6 @@ private void InitializeComponent() private Controls.HorizontalProgressBar horizontalProgressBar2; private Controls.HorizontalProgressBar horizontalProgressBar1; private System.Windows.Forms.TextBox lbl_obmagresult; - private Controls.MyButton BUT_OBmagcalaccept; private Controls.MyButton BUT_OBmagcalcancel; private Controls.MyButton BUT_OBmagcalstart; private System.Windows.Forms.Timer timer1; @@ -627,5 +618,6 @@ private void InitializeComponent() private System.Windows.Forms.DataGridViewImageColumn Up; private System.Windows.Forms.DataGridViewImageColumn Down; private Controls.MyButton but_missing; + private System.Windows.Forms.ToolTip toolTipReboot; } } diff --git a/GCSViews/ConfigurationView/ConfigHWCompass2.cs b/GCSViews/ConfigurationView/ConfigHWCompass2.cs index e282a34405..aa617ef93d 100644 --- a/GCSViews/ConfigurationView/ConfigHWCompass2.cs +++ b/GCSViews/ConfigurationView/ConfigHWCompass2.cs @@ -4,6 +4,7 @@ using System.Collections.Generic; using System.Data; using System.Linq; +using System.Text; using System.Threading.Tasks; using System.Windows.Forms; using Color = System.Drawing.Color; @@ -14,10 +15,193 @@ public partial class ConfigHWCompass2 : MyUserControl, IActivate, IDeactivate { private List list; - private bool rebootrequired = false; + private bool _calChangesRequireReboot; + + // Number of physical compass slots the UI can display (progress bars + indicators). + private const int MaxCompassInstances = 3; + + // Firmware raw packet stream (MAG_CAL_PROGRESS + MAG_CAL_REPORT), filled on the comms + // thread and drained on the UI timer. A single queue keeps progress and report handling + // in one ordered pass instead of two. + private readonly List _calPackets = new List(); + + // Derived UI state, owned exclusively by the UI thread. + // _latestReports is the single source of truth for terminal per-compass results + // (success / failure / autosave); every line below the progress row is derived from + // it. _liveProgress is the transient 0-99% feed for the progress bars until a + // terminal report supersedes it. _attempt is the current firmware retry attempt per + // compass (from progress packets), shown on the progress row while a retry is running. + private readonly Dictionary _latestReports = + new Dictionary(); + private readonly Dictionary _liveProgress = new Dictionary(); + private readonly Dictionary _attempt = new Dictionary(); + private byte _activeCalMask; + // Show reboot modal on the next timer tick so the final SUCCESS lines are + // visible before the modal dialog steals focus from the control repaint. + private bool _rebootPromptPending; + private int _rebootPromptDelayTicks; + + // Firmware uses 0-based compass_id on the wire; users see "Mag 1/2/3" in the UI. + // Log strings show both so operators can correlate the visible row with logs and + // firmware messages. Keep this the sole formatter to avoid drift. + private static string CompassLabel(byte compassId) => "Mag " + (compassId + 1) + " (id: " + compassId + ")"; + + // Return MAVLink status text directly from the enum [Description] so wording stays in sync + // with upstream and new status messages are picked up without UI-side string mapping. + // Note: mavgen emits its own MAVLink.Description attribute (see MavlinkParse.cs), + // NOT System.ComponentModel.DescriptionAttribute. + // + // MAG_CAL_SUCCESS (4) and MAG_CAL_FAILED (5) both have empty [Description] upstream. + // When the description is empty we use a readable fallback rather than the raw enum + // name so the UI stays user-friendly while remaining forward-compatible: if upstream + // ever adds a description for these codes it will be used automatically. + private static readonly Dictionary _statusFallback = + new Dictionary + { + { MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS, "Success" }, + { MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED, "Calibration failed" }, + }; + + private static string StatusText(MAVLink.MAG_CAL_STATUS status) + { + var field = typeof(MAVLink.MAG_CAL_STATUS).GetField(status.ToString()); + var attr = field == null ? null + : (MAVLink.Description)Attribute.GetCustomAttribute(field, typeof(MAVLink.Description)); + + if (!string.IsNullOrWhiteSpace(attr?.Text)) + return attr.Text; + + // Upstream description is empty: use the readable fallback if one is registered, + // otherwise fall back to the raw enum name. + return _statusFallback.TryGetValue(status, out var fallback) ? fallback : status.ToString(); + } + + // Read a float param from the already-loaded param list; return fallback if absent. + private static float TryGetParam(string name, float fallback) + { + var p = MainV2.comPort.MAV.param; + return p.ContainsKey(name) ? (float)p[name].Value : fallback; + } + + // Compact detail string appended after the status text. + // Only includes values that are actionable for that specific failure code. + // Numbers use F1 (1 decimal place) to keep the text box readable. + // SUCCESS -> offsets + fitness (what was saved to flash) + // FAILED_OFFSETS -> offsets + limit (which axis overflowed and by how much) + // FAILED_RESIDUALS_HIGH -> fitness only (how bad the fit was) + // FAILED_ORIENTATION -> orientation_confidence + hardcoded min threshold + // all others -> empty string + private static string StatusDetail(MAVLink.mavlink_mag_cal_report_t r) + { + var s = (MAVLink.MAG_CAL_STATUS)r.cal_status; + switch (s) + { + case MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS: + return $" [x:{r.ofs_x:F1} y:{r.ofs_y:F1} z:{r.ofs_z:F1} fit:{r.fitness:F1}]"; + + case MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_OFFSETS: + { + float limit = TryGetParam("COMPASS_OFFS_MAX", 2000f); + return $" [x:{r.ofs_x:F1} y:{r.ofs_y:F1} z:{r.ofs_z:F1} limit:\u00b1{limit:F0}]"; + } + + case MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_RESIDUALS_HIGH: + return $" [fit:{r.fitness:F1}]"; + + case MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_ORIENTATION: + // 0.4 is hardcoded in ArduPilot firmware, not a configurable param. + return $" [orientation confidence:{r.orientation_confidence:F2} min:0.40]"; + + default: + return string.Empty; + } + } + + private static int CountBits(byte mask) + { + int count = 0; + while (mask != 0) + { + count += mask & 1; + mask >>= 1; + } + + return count; + } + + private bool IsSucceeded(byte compassId) + { + if (!_latestReports.TryGetValue(compassId, out var report)) + { + return false; + } + + return (MAVLink.MAG_CAL_STATUS)report.cal_status == MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS; + } + + private bool IsFailed(byte compassId) + { + return _latestReports.TryGetValue(compassId, out var report) + && (MAVLink.MAG_CAL_STATUS)report.cal_status > MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS; + } + + private bool AnyFailedCompass() + { + foreach (var report in _latestReports.Values) + { + if ((MAVLink.MAG_CAL_STATUS)report.cal_status > MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS) + return true; + } + return false; + } - private List mprog = new List(); - private List mrep = new List(); + private int SucceededCompassCount() + { + int count = 0; + foreach (var report in _latestReports.Values) + if ((MAVLink.MAG_CAL_STATUS)report.cal_status == MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS) + count++; + return count; + } + + private int ExpectedCompassCount() + { + return _activeCalMask == 0 ? _latestReports.Count : CountBits(_activeCalMask); + } + + // Maps a compass id to its progress bar, clamping to the control's valid range. + private void SetProgressBar(byte compassId, int percent) + { + percent = Math.Max(0, Math.Min(100, percent)); + switch (compassId) + { + case 0: horizontalProgressBar1.Value = percent; break; + case 1: horizontalProgressBar2.Value = percent; break; + case 2: horizontalProgressBar3.Value = percent; break; + } + } + + // Maps a compass id to its status indicator swatch. + private void SetIndicator(byte compassId, Color color) + { + switch (compassId) + { + case 0: pictureBox1.BackColor = color; break; + case 1: pictureBox2.BackColor = color; break; + case 2: pictureBox3.BackColor = color; break; + } + } + + // Compasses that both succeeded and were autosaved by firmware, ordered by id. + private List AutosavedSuccessCompasses() + { + return _latestReports + .Where(kv => kv.Value.autosaved == 1 + && (MAVLink.MAG_CAL_STATUS)kv.Value.cal_status == MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS) + .Select(kv => kv.Key) + .OrderBy(id => id) + .ToList(); + } private int packetsub1; private int packetsub2; @@ -146,7 +330,32 @@ public void Activate() public void Deactivate() { + // Cancel any in-progress calibration on page nav so firmware can't silently + // autosave a compass while the user is off this page — they can't see per- + // compass status anymore and would only learn about the partial save via + // PreArm "Compass calibrated requires reboot" on the next arm attempt. + // Only fire if calibration was actually running (timer1 stops on + // Cancel/completion). + if (timer1.Enabled) + { + try + { + MainV2.comPort.doCommand((byte)MainV2.comPort.sysidcurrent, + (byte)MainV2.comPort.compidcurrent, + MAVLink.MAV_CMD.DO_CANCEL_MAG_CAL, 0, 0, 1, 0, 0, 0, 0); + } + catch (Exception ex) { this.LogError(ex); } + } timer1.Stop(); + MainV2.comPort.UnSubscribeToPacketType(packetsub1); + MainV2.comPort.UnSubscribeToPacketType(packetsub2); + // Restore the button strip to its resting state so that on page re-entry + // the user can start a fresh cal. Without this, Start (which the Start + // handler disabled) stays greyed out because Activate() doesn't touch + // button state — the user would be locked out until they clicked Cancel + // (which is itself disabled here). + BUT_OBmagcalstart.Enabled = true; + BUT_OBmagcalcancel.Enabled = false; CheckReboot(); } @@ -156,18 +365,24 @@ private bool CheckReboot() if (!MainV2.comPort.BaseStream.IsOpen) return true; - if (rebootrequired) + if (_calChangesRequireReboot) { - if (CustomMessageBox.Show("Reboot required, reboot now?", "Reboot", + if (CustomMessageBox.Show("Compass changes have been saved to parameters but require a reboot to take effect. Reboot now?", "Reboot", CustomMessageBox.MessageBoxButtons.YesNo) == CustomMessageBox.DialogResult.Yes) { try { + // doReboot returns true on success; only clear the flag when the + // reboot actually went through so the ⚠ button stays visible if it fails. if (MainV2.comPort.doReboot()) { - CustomMessageBox.Show("Reboot failed. please manually reboot the hardware.", Strings.ERROR); + _calChangesRequireReboot = false; + UpdateRebootButtonState(); + } + else + { + CustomMessageBox.Show("Reboot failed. Please manually reboot the hardware.", Strings.ERROR); } - rebootrequired = false; } catch { @@ -181,6 +396,20 @@ private bool CheckReboot() return false; } + private void UpdateRebootButtonState() + { + if (_calChangesRequireReboot) + { + but_reboot.Text = "Reboot Now \u26A0"; + toolTipReboot.SetToolTip(but_reboot, "Compass calibration saved - reboot required to take effect"); + } + else + { + but_reboot.Text = "Reboot"; + toolTipReboot.SetToolTip(but_reboot, string.Empty); + } + } + private async void myDataGridView1_CellContentClick(object sender, DataGridViewCellEventArgs e) { if (e.ColumnIndex == Up.Index && e.RowIndex != 0) @@ -256,14 +485,15 @@ await MainV2.comPort.setParamAsync((byte)MainV2.comPort.sysidcurrent, 0); } - rebootrequired = true; + _calChangesRequireReboot = true; + UpdateRebootButtonState(); myDataGridView1.Invalidate(); } private void BUT_OBmagcalstart_Click(object sender, EventArgs e) { - if (rebootrequired && !CheckReboot()) + if (_calChangesRequireReboot && !CheckReboot()) { return; } @@ -279,16 +509,35 @@ private void BUT_OBmagcalstart_Click(object sender, EventArgs e) return; } - mprog.Clear(); - mrep.Clear(); + _calPackets.Clear(); + _latestReports.Clear(); + _liveProgress.Clear(); + _attempt.Clear(); + _activeCalMask = 0; + _rebootPromptPending = false; + _rebootPromptDelayTicks = 0; horizontalProgressBar1.Value = 0; horizontalProgressBar2.Value = 0; horizontalProgressBar3.Value = 0; + // Reset the per-compass status indicators so a fresh cal doesn't start + // with stale green/red from the previous attempt for the first ~30s. + pictureBox1.BackColor = Color.Transparent; + pictureBox2.BackColor = Color.Transparent; + pictureBox3.BackColor = Color.Transparent; + // Poll fast for the whole run so progress bars stay smooth, including a + // compass that is still retrying after another has already saved. + timer1.Interval = 100; + + // Unsubscribe any prior subscriptions from an earlier Start click so we don't + // stack duplicates when the user restarts calibration without leaving the screen. + // Fields default to 0; UnSubscribeToPacketType(0) safely no-ops. + MainV2.comPort.UnSubscribeToPacketType(packetsub1); + MainV2.comPort.UnSubscribeToPacketType(packetsub2); packetsub1 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.MAG_CAL_PROGRESS, ReceviedPacket, (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent); packetsub2 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.MAG_CAL_REPORT, ReceviedPacket, (byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent); - BUT_OBmagcalaccept.Enabled = true; + BUT_OBmagcalstart.Enabled = false; BUT_OBmagcalcancel.Enabled = true; timer1.Start(); } @@ -298,46 +547,20 @@ private bool ReceviedPacket(MAVLink.MAVLinkMessage packet) if (System.Diagnostics.Debugger.IsAttached) MainV2.comPort.DebugPacket(packet, true); - if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.MAG_CAL_PROGRESS) - { - lock (this.mprog) - { - this.mprog.Add(packet); - } - - return true; - } - else if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.MAG_CAL_REPORT) + // Queue progress and report packets together, preserving receive order, so the + // tick handler can attribute a failure to the attempt that produced it. + if (packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.MAG_CAL_PROGRESS || + packet.msgid == (byte)MAVLink.MAVLINK_MSG_ID.MAG_CAL_REPORT) { - lock (this.mrep) + lock (this._calPackets) { - this.mrep.Add(packet); + this._calPackets.Add(packet); } - - return true; } return true; } - private void BUT_OBmagcalaccept_Click(object sender, EventArgs e) - { - try - { - MainV2.comPort.doCommand((byte)MainV2.comPort.sysidcurrent, (byte)MainV2.comPort.compidcurrent, MAVLink.MAV_CMD.DO_ACCEPT_MAG_CAL, 0, 0, 1, 0, 0, 0, 0); - - } - catch (Exception ex) - { - CustomMessageBox.Show(ex.ToString(), Strings.ERROR, MessageBoxButtons.OK); - } - - MainV2.comPort.UnSubscribeToPacketType(packetsub1); - MainV2.comPort.UnSubscribeToPacketType(packetsub2); - - timer1.Stop(); - } - private void BUT_OBmagcalcancel_Click(object sender, EventArgs e) { try @@ -353,113 +576,175 @@ private void BUT_OBmagcalcancel_Click(object sender, EventArgs e) MainV2.comPort.UnSubscribeToPacketType(packetsub2); timer1.Stop(); + BUT_OBmagcalstart.Enabled = true; + BUT_OBmagcalcancel.Enabled = false; } private void timer1_Tick(object sender, EventArgs e) { - - lbl_obmagresult.Clear(); - int compasscount = 0; - int completecount = 0; - lock (mprog) + // Deferred reboot prompt: fire one tick after completion so the final + // SUCCESS lines paint before the modal dialog steals focus. + if (_rebootPromptPending) { - // somewhere to save our % - Dictionary status = new Dictionary(); - foreach (var item in mprog) + if (_rebootPromptDelayTicks > 0) { - status[((MAVLink.mavlink_mag_cal_progress_t)item.data).compass_id] = item; + _rebootPromptDelayTicks--; + return; } - // message for user - string message = ""; - foreach (var item in status) - { - var obj = (MAVLink.mavlink_mag_cal_progress_t)item.Value.data; - - try - { - if (item.Key == 0) - horizontalProgressBar1.Value = obj.completion_pct; - if (item.Key == 1) - horizontalProgressBar2.Value = obj.completion_pct; - if (item.Key == 2) - horizontalProgressBar3.Value = obj.completion_pct; - } - catch { } - - message += "id:" + item.Key + " " + obj.completion_pct.ToString() + "% "; - compasscount++; - } - lbl_obmagresult.AppendText(message + "\r\n"); + _rebootPromptPending = false; + timer1.Stop(); + // Stop listening: firmware keeps re-broadcasting terminal MAG_CAL_REPORTs + // until the calibrator is stopped, and with the timer stopped nothing + // would drain the queue any more. + MainV2.comPort.UnSubscribeToPacketType(packetsub1); + MainV2.comPort.UnSubscribeToPacketType(packetsub2); + // Autosave was requested on start, so all params are already written to flash. + // Offer an immediate reboot to activate them. + CheckReboot(); + return; } - lock (mrep) + IngestPackets(); + RenderCalibrationState(); + EvaluateCompletion(); + } + + // Drain the queued MAG_CAL packets. Progress packets feed the bar and the current + // attempt; report packets set the terminal per-compass result and the reboot flag. + private void IngestPackets() + { + lock (_calPackets) { - // somewhere to save our answer - Dictionary status = new Dictionary(); - foreach (var item in mrep) + foreach (var item in _calPackets) { - var obj = (MAVLink.mavlink_mag_cal_report_t)item.data; - - if (obj.compass_id == 0 && obj.ofs_x == 0) + if (item.msgid == (byte)MAVLink.MAVLINK_MSG_ID.MAG_CAL_PROGRESS) + { + var p = (MAVLink.mavlink_mag_cal_progress_t)item.data; + _activeCalMask |= p.cal_mask; + _attempt[p.compass_id] = p.attempt; + _liveProgress[p.compass_id] = p.completion_pct; continue; + } - status[obj.compass_id] = item; - } - - // message for user - foreach (var item in status.Values) - { var obj = (MAVLink.mavlink_mag_cal_report_t)item.data; - lbl_obmagresult.AppendText("id:" + obj.compass_id + " x:" + obj.ofs_x.ToString("0.0") + " y:" + - obj.ofs_y.ToString("0.0") + " z:" + - obj.ofs_z.ToString("0.0") + " fit:" + obj.fitness.ToString("0.0") + " " + - (MAVLink.MAG_CAL_STATUS)obj.cal_status + "\n"); - - try - { - if (obj.compass_id == 0) - { - horizontalProgressBar1.Value = 100; - pictureBox1.BackColor = Color.Green; - } - - if (obj.compass_id == 1) - { - horizontalProgressBar2.Value = 100; - pictureBox2.BackColor = Color.Green; - } - - if (obj.compass_id == 2) - { - horizontalProgressBar3.Value = 100; - pictureBox3.BackColor = Color.Green; - } - } - catch - { - } + // Skip the empty NOT_STARTED placeholder some firmware emits. + if (obj.ofs_x == 0 && obj.ofs_y == 0 && obj.ofs_z == 0 + && obj.cal_status == (byte)MAVLink.MAG_CAL_STATUS.MAG_CAL_NOT_STARTED) + continue; - if ((MAVLink.MAG_CAL_STATUS)obj.cal_status != MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS) - { - //CustomMessageBox.Show(Strings.CommandFailed); - } + _activeCalMask |= obj.cal_mask; + _latestReports[obj.compass_id] = obj; + // We deliberately do NOT touch _liveProgress here. The bar is a pure mirror + // of the firmware's completion_pct: while running it comes from + // MAG_CAL_PROGRESS, and on retry the firmware itself resets it to 0 + // (reset_state) and re-streams it. The failure *reason* is held in + // _latestReports until the compass passes, so it stays readable even though + // the firmware's FAILED status is only transient. if (obj.autosaved == 1) { - completecount++; - timer1.Interval = 1000; + // Firmware persisted this compass's offsets and raised + // _cal_requires_reboot; a reboot is now mandatory before arming. + // Route into the class-wide prompt so Deactivate/CheckReboot also + // fire on page navigation. + _calChangesRequireReboot = true; + UpdateRebootButtonState(); } } + + _calPackets.Clear(); + } + } + + // Repaint the result panel from state. The progress bars follow the firmware stream; + // the only extra thing the operator needs is the last failure reason so they can fix + // it and retry. This is the sole place that writes to the bars, indicators and text. + // All lines are assembled into one StringBuilder and written in a single Text assignment + // to avoid the \r\n / \n mismatch that multiple AppendText calls produce in a WinForms + // TextBox (which uses \n internally) and to prevent repeated scroll-to-end repaints. + private void RenderCalibrationState() + { + // Progress row spans every compass we have heard from, so a failed compass is + // still listed instead of silently vanishing from the row. + var ids = new SortedSet(_liveProgress.Keys); + foreach (var id in _latestReports.Keys) + ids.Add(id); + for (byte i = 0; i < MaxCompassInstances; i++) + if (((_activeCalMask >> i) & 1) != 0) + ids.Add(i); + + var sb = new StringBuilder(); + + // --- progress row --- + var progressRow = new StringBuilder(); + foreach (var id in ids) + { + // Just show what the stream reports; a SUCCESS report pins the bar to 100. + int pct = IsSucceeded(id) ? 100 : (_liveProgress.TryGetValue(id, out var live) ? live : 0); + SetProgressBar(id, pct); + + if (IsSucceeded(id)) + SetIndicator(id, Color.Green); + else if (IsFailed(id)) + SetIndicator(id, Color.Red); + + progressRow.Append(CompassLabel(id)).Append(' ').Append(pct).Append('%'); + // While a retry is under way (attempt >= 2) show it, so it's obvious the + // climbing bar is a fresh attempt that follows an earlier failed one. + if (!IsSucceeded(id) && _attempt.TryGetValue(id, out var att) && att >= 2) + progressRow.Append(" (attempt ").Append(att).Append(')'); + progressRow.Append(" "); + } + sb.AppendLine(progressRow.ToString().TrimEnd()); + + // --- one terminal status line per compass --- + // Ordered by id so the operator sees which compasses saved and which need fixing. + // The attempt count lives on the progress row above, so this line is the neutral + // result plus any actionable detail values. + foreach (var kv in _latestReports.OrderBy(kv => kv.Key)) + sb.AppendLine( + CompassLabel(kv.Key) + ": " + + StatusText((MAVLink.MAG_CAL_STATUS)kv.Value.cal_status) + + StatusDetail(kv.Value)); + + // --- partial-save guidance --- + // Firmware autosaves successful compasses individually — their params are already + // written. Reboot is required before those values take effect. + var autosaved = AutosavedSuccessCompasses(); + if (autosaved.Count > 0 && AnyFailedCompass()) + { + var savedList = string.Join(", ", autosaved.Select(id => CompassLabel(id))); + sb.AppendLine( + "Partial save: " + savedList + + " already persisted to params." + + " Reboot required before those changes take effect." + + " Failed compasses continue to retry."); } - if (compasscount == completecount && compasscount != 0) + // Single assignment: avoids repeated scroll-to-end repaints and the \r\n double- + // spacing that multiple AppendText calls produce in a WinForms TextBox. + lbl_obmagresult.Text = sb.ToString(); + } + + // Fire the completion path once every expected compass has succeeded and none is in a + // failed state. Uses succeeded count (not autosaved count) so firmware that reports + // MAG_CAL_SUCCESS without autosaved==1 still triggers the reboot prompt. + // The _rebootPromptPending guard prevents re-arming on subsequent ticks. + private void EvaluateCompletion() + { + if (_rebootPromptPending) + return; + + int expected = ExpectedCompassCount(); + + if (expected > 0 && !AnyFailedCompass() && SucceededCompassCount() >= expected) { BUT_OBmagcalcancel.Enabled = false; - BUT_OBmagcalaccept.Enabled = false; - timer1.Stop(); - CustomMessageBox.Show("Please reboot the autopilot"); + BUT_OBmagcalstart.Enabled = true; + _rebootPromptPending = true; + _rebootPromptDelayTicks = 1; } } @@ -499,10 +784,37 @@ private void but_largemagcal_Click(object sender, EventArgs e) private void but_reboot_Click(object sender, EventArgs e) { - if (CustomMessageBox.Show("Reboot?") == CustomMessageBox.DialogResult.OK) + if (CustomMessageBox.Show("Reboot the autopilot now?", "Reboot", + CustomMessageBox.MessageBoxButtons.YesNo) == CustomMessageBox.DialogResult.Yes) { - MainV2.comPort.doReboot(false, true); - rebootrequired = false; + // Cancel any in-flight cal before rebooting so firmware doesn't keep + // running the calibrator while the link tears down. Idempotent if no + // cal is running; keeps this button safe to click any time. + if (timer1.Enabled) + { + try + { + MainV2.comPort.doCommand((byte)MainV2.comPort.sysidcurrent, + (byte)MainV2.comPort.compidcurrent, + MAVLink.MAV_CMD.DO_CANCEL_MAG_CAL, 0, 0, 1, 0, 0, 0, 0); + } + catch (Exception ex) { this.LogError(ex); } + MainV2.comPort.UnSubscribeToPacketType(packetsub1); + MainV2.comPort.UnSubscribeToPacketType(packetsub2); + timer1.Stop(); + BUT_OBmagcalstart.Enabled = true; + BUT_OBmagcalcancel.Enabled = false; + } + + if (MainV2.comPort.doReboot(false, true)) + { + _calChangesRequireReboot = false; + UpdateRebootButtonState(); + } + else + { + CustomMessageBox.Show("Reboot failed. Please manually reboot the hardware.", Strings.ERROR); + } } } diff --git a/GCSViews/ConfigurationView/ConfigHWCompass2.resx b/GCSViews/ConfigurationView/ConfigHWCompass2.resx index 593921230e..7e5245196b 100644 --- a/GCSViews/ConfigurationView/ConfigHWCompass2.resx +++ b/GCSViews/ConfigurationView/ConfigHWCompass2.resx @@ -118,7 +118,13 @@ System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - 457, 17 + 146, 17 + + + 234, 17 + + + 17, 17 True @@ -138,9 +144,6 @@ True - - 544, 17 - 54 diff --git a/MissionPlannerTests/GCSViews/MagCalStatusTests.cs b/MissionPlannerTests/GCSViews/MagCalStatusTests.cs new file mode 100644 index 0000000000..1682af8f6a --- /dev/null +++ b/MissionPlannerTests/GCSViews/MagCalStatusTests.cs @@ -0,0 +1,179 @@ +using Microsoft.VisualStudio.TestTools.UnitTesting; +using System; +using System.Collections.Generic; + +namespace MissionPlanner.GCSViews.Tests +{ + /// + /// Tests for MAG_CAL_STATUS handling paired with ArduPilot/ardupilot#32757 + /// (AP_Compass: report specific failure reason when fit is rejected). + /// + /// Firmware sends cal_status in MAG_CAL_REPORT. Three failure codes are + /// exercised end-to-end here: + /// 8 = MAG_CAL_FAILED_OFFSETS – any offset component >= COMPASS_OFFS_MAX + /// 9 = MAG_CAL_FAILED_DIAG_SCALING – diagonal or off-diagonal scaling out of range + /// 10 = MAG_CAL_FAILED_RESIDUALS_HIGH – fitness (RMS residual) exceeds tolerance + /// + /// The named enum members now live in the generated Mavlink.cs (mavlink/mavlink#2478), + /// so the wire-value checks below are real regression guards: if upstream ever + /// renumbers a member or Mavlink.cs is regenerated against a diverged xml, + /// these fail loudly. + /// + [TestClass] + public class MagCalStatusTests + { + // ── 1. Wire values (regression guards) ──────────────────────────────── + // + // Pin the byte value of every failure code the UI branches on. A silent + // renumber upstream would otherwise break the >MAG_CAL_SUCCESS guard + // used in both ConfigHWCompass and ConfigHWCompass2. + + [TestMethod] + public void FailedOffsets_WireValue_Is8() + { + Assert.AreEqual((byte)8, (byte)MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_OFFSETS); + } + + [TestMethod] + public void FailedDiagScaling_WireValue_Is9() + { + Assert.AreEqual((byte)9, (byte)MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_DIAG_SCALING); + } + + [TestMethod] + public void FailedResidualsHigh_WireValue_Is10() + { + Assert.AreEqual((byte)10, (byte)MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_RESIDUALS_HIGH); + } + + [TestMethod] + public void KnownStatus_WireValues_ArePinned() + { + // Full snapshot of the enum contract the UI relies on. If any of + // these shift, the >MAG_CAL_SUCCESS guard partitions incorrectly. + Assert.AreEqual((byte)0, (byte)MAVLink.MAG_CAL_STATUS.MAG_CAL_NOT_STARTED); + Assert.AreEqual((byte)1, (byte)MAVLink.MAG_CAL_STATUS.MAG_CAL_WAITING_TO_START); + Assert.AreEqual((byte)2, (byte)MAVLink.MAG_CAL_STATUS.MAG_CAL_RUNNING_STEP_ONE); + Assert.AreEqual((byte)3, (byte)MAVLink.MAG_CAL_STATUS.MAG_CAL_RUNNING_STEP_TWO); + Assert.AreEqual((byte)4, (byte)MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS); + Assert.AreEqual((byte)5, (byte)MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED); + Assert.AreEqual((byte)6, (byte)MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_ORIENTATION); + Assert.AreEqual((byte)7, (byte)MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_RADIUS); + } + + // ── 2. Failure guard: calStatus > MAG_CAL_SUCCESS (value 4) ────────── + // + // The timer_Tick guard `if (calStatus > MAG_CAL_SUCCESS)` must capture + // every failure code, including the three added by PR#32757. + + [TestMethod] + public void FailedOffsets_IsGreaterThanSuccess() + { + Assert.IsTrue(MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_OFFSETS > MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS, + "MAG_CAL_FAILED_OFFSETS must trigger the failure guard"); + } + + [TestMethod] + public void FailedDiagScaling_IsGreaterThanSuccess() + { + Assert.IsTrue(MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_DIAG_SCALING > MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS, + "MAG_CAL_FAILED_DIAG_SCALING must trigger the failure guard"); + } + + [TestMethod] + public void FailedResidualsHigh_IsGreaterThanSuccess() + { + Assert.IsTrue(MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_RESIDUALS_HIGH > MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS, + "MAG_CAL_FAILED_RESIDUALS_HIGH must trigger the failure guard"); + } + + [TestMethod] + public void AllFailureCodes_AreGreaterThanSuccess() + { + var failures = new[] + { + MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED, + MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_ORIENTATION, + MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_RADIUS, + MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_OFFSETS, + MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_DIAG_SCALING, + MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_RESIDUALS_HIGH, + }; + foreach (var f in failures) + Assert.IsTrue(f > MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS, + $"{f} (raw={(byte)f}) should be > MAG_CAL_SUCCESS"); + } + + [TestMethod] + public void RunningAndWaiting_AreNotGreaterThanSuccess() + { + var nonFailed = new[] + { + MAVLink.MAG_CAL_STATUS.MAG_CAL_NOT_STARTED, + MAVLink.MAG_CAL_STATUS.MAG_CAL_WAITING_TO_START, + MAVLink.MAG_CAL_STATUS.MAG_CAL_RUNNING_STEP_ONE, + MAVLink.MAG_CAL_STATUS.MAG_CAL_RUNNING_STEP_TWO, + MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS, + }; + foreach (var s in nonFailed) + Assert.IsFalse(s > MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS, + $"{s} should NOT be > MAG_CAL_SUCCESS"); + } + + // ── 3. Named string representation ──────────────────────────────────── + // + // The UI prints `calStatus.ToString()` verbatim as a fallback when no + // [Description] is available. Lock the names so a renamed member is caught. + + [TestMethod] + public void Failed_ToStringIsNamed() + { + Assert.AreEqual("MAG_CAL_FAILED", MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED.ToString()); + } + + [TestMethod] + public void FailedRadius_ToStringIsNamed() + { + Assert.AreEqual("MAG_CAL_FAILED_RADIUS", MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_RADIUS.ToString()); + } + + // ── 4. MAVLink [Description] surfaces the failure reason ────────────── + // + // The compass config views print status text using the mavlink dialect's + // [Description] attribute when present. We don't pin exact upstream wording + // (mavlink is allowed to reword), but we do lock that each specific failure + // code carries *some* non-empty description whose key term matches the code. + // If this ever regresses to empty descriptions, the UI silently falls back + // to bare enum names and the user loses the "why". + + private static string DescriptionOf(MAVLink.MAG_CAL_STATUS status) + { + var field = typeof(MAVLink.MAG_CAL_STATUS).GetField(status.ToString()); + var attr = field == null ? null + : (MAVLink.Description)Attribute.GetCustomAttribute(field, typeof(MAVLink.Description)); + return attr?.Text ?? ""; + } + + [TestMethod] + public void SpecificFailures_CarryDescriptionsWithKeyTerm() + { + var expectedTerms = new Dictionary + { + { MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_ORIENTATION, "orientation" }, + { MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_RADIUS, "radius" }, + { MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_OFFSETS, "offset" }, + { MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_DIAG_SCALING, "scaling" }, + { MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED_RESIDUALS_HIGH, "fitness" }, + }; + + foreach (var kv in expectedTerms) + { + var desc = DescriptionOf(kv.Key); + Assert.IsFalse(string.IsNullOrEmpty(desc), + $"{kv.Key} has no [Description]; UI would fall back to bare enum name."); + Assert.IsTrue(desc.IndexOf(kv.Value, StringComparison.OrdinalIgnoreCase) >= 0, + $"{kv.Key} description \"{desc}\" no longer contains key term \"{kv.Value}\"."); + } + } + } +}