diff --git a/GCSViews/ConfigurationView/ConfigHWCompass.cs b/GCSViews/ConfigurationView/ConfigHWCompass.cs index 1d2b6ed32d..5dd7d493bb 100644 --- a/GCSViews/ConfigurationView/ConfigHWCompass.cs +++ b/GCSViews/ConfigurationView/ConfigHWCompass.cs @@ -419,6 +419,9 @@ 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 _autosavedCompasses = new HashSet(); private bool ReceviedPacket(MAVLink.MAVLinkMessage packet) { @@ -465,6 +468,9 @@ private void BUT_OBmagcalstart_Click(object sender, EventArgs e) mprog.Clear(); mrep.Clear(); + lastFailureStatus.Clear(); + _startedCompasses.Clear(); + _autosavedCompasses.Clear(); horizontalProgressBar1.Value = 0; horizontalProgressBar2.Value = 0; horizontalProgressBar3.Value = 0; @@ -534,19 +540,23 @@ private void timer1_Tick(object sender, EventArgs e) 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; + if (!_autosavedCompasses.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() + "% "; + _startedCompasses.Add(item.Key); compasscount++; } - lbl_obmagresult.AppendText(message + "\n"); + lbl_obmagresult.AppendText(message + Environment.NewLine); } lock (mrep) @@ -557,13 +567,15 @@ 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.compass_id == 0 && 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 failedCompassIds = new List(); foreach (var item in status.Values) { var obj = (MAVLink.mavlink_mag_cal_report_t)item.data; @@ -571,35 +583,76 @@ private void timer1_Tick(object sender, EventArgs e) 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"); + (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; + if (obj.autosaved == 1) + { + 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; + // Assumption: autosaved==1 is only ever set by firmware on SUCCESS. + // The calStatus>SUCCESS branch below always runs after the autosaved + // block, so if that assumption ever breaks the bar would be reset to + // 0/red while completecount still increments — guard here if needed. + if (calStatus > MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS) { - //CustomMessageBox.Show(Strings.CommandFailed); + lastFailureStatus[obj.compass_id] = calStatus; + failedCompassIds.Add(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); + // running/waiting states leave lastFailureStatus unchanged so the + // previous failure reason stays visible while calibration retries if (obj.autosaved == 1) { + _autosavedCompasses.Add(obj.compass_id); completecount++; timer1.Interval = 1000; } } + + // consume failure reports so they don't re-fire next tick and kill retry progress + // (lastFailureStatus preserves the message for display) + if (failedCompassIds.Count > 0) + mrep.RemoveAll(m => failedCompassIds.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 += "Mag " + kv.Key + ": " + kv.Value.ToString() + Environment.NewLine; + lbl_obmagresult.AppendText(failures); } - if (compasscount == completecount && compasscount != 0) + if (_startedCompasses.Count > 0 && completecount == _startedCompasses.Count) { BUT_OBmagcalcancel.Enabled = false; BUT_OBmagcalaccept.Enabled = false; diff --git a/GCSViews/ConfigurationView/ConfigHWCompass2.cs b/GCSViews/ConfigurationView/ConfigHWCompass2.cs index e282a34405..30d53426e9 100644 --- a/GCSViews/ConfigurationView/ConfigHWCompass2.cs +++ b/GCSViews/ConfigurationView/ConfigHWCompass2.cs @@ -18,6 +18,9 @@ public partial class ConfigHWCompass2 : MyUserControl, IActivate, IDeactivate private List mprog = new List(); private List mrep = new List(); + private Dictionary lastFailureStatus = new Dictionary(); + private HashSet _startedCompasses = new HashSet(); + private HashSet _autosavedCompasses = new HashSet(); private int packetsub1; private int packetsub2; @@ -281,6 +284,9 @@ private void BUT_OBmagcalstart_Click(object sender, EventArgs e) mprog.Clear(); mrep.Clear(); + lastFailureStatus.Clear(); + _startedCompasses.Clear(); + _autosavedCompasses.Clear(); horizontalProgressBar1.Value = 0; horizontalProgressBar2.Value = 0; horizontalProgressBar3.Value = 0; @@ -378,19 +384,23 @@ private void timer1_Tick(object sender, EventArgs e) 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; + if (!_autosavedCompasses.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() + "% "; + _startedCompasses.Add(item.Key); compasscount++; } - lbl_obmagresult.AppendText(message + "\r\n"); + lbl_obmagresult.AppendText(message + Environment.NewLine); } lock (mrep) @@ -401,13 +411,15 @@ 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.compass_id == 0 && 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 failedCompassIds = new List(); foreach (var item in status.Values) { var obj = (MAVLink.mavlink_mag_cal_report_t)item.data; @@ -415,46 +427,87 @@ private void timer1_Tick(object sender, EventArgs e) 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"); + (MAVLink.MAG_CAL_STATUS)obj.cal_status + Environment.NewLine); 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) + if (obj.autosaved == 1) { - horizontalProgressBar3.Value = 100; - pictureBox3.BackColor = Color.Green; + 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 { } - if ((MAVLink.MAG_CAL_STATUS)obj.cal_status != MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS) + var calStatus = (MAVLink.MAG_CAL_STATUS)obj.cal_status; + // Assumption: autosaved==1 is only ever set by firmware on SUCCESS. + // The calStatus>SUCCESS branch below always runs after the autosaved + // block, so if that assumption ever breaks the bar would be reset to + // 0/red while completecount still increments — guard here if needed. + if (calStatus > MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS) { - //CustomMessageBox.Show(Strings.CommandFailed); + lastFailureStatus[obj.compass_id] = calStatus; + failedCompassIds.Add(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; pictureBox1.BackColor = Color.Red; } + if (obj.compass_id == 1) { horizontalProgressBar2.Value = 0; pictureBox2.BackColor = Color.Red; } + if (obj.compass_id == 2) { horizontalProgressBar3.Value = 0; pictureBox3.BackColor = Color.Red; } + } + catch { } } + else if (calStatus == MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS) + lastFailureStatus.Remove(obj.compass_id); + // running/waiting states leave lastFailureStatus unchanged so the + // previous failure reason stays visible while calibration retries if (obj.autosaved == 1) { + _autosavedCompasses.Add(obj.compass_id); completecount++; timer1.Interval = 1000; } } + + // consume failure reports so they don't re-fire next tick and kill retry progress + // (lastFailureStatus preserves the message for display) + if (failedCompassIds.Count > 0) + mrep.RemoveAll(m => failedCompassIds.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 += "Mag " + kv.Key + ": " + kv.Value.ToString() + Environment.NewLine; + lbl_obmagresult.AppendText(failures); } - if (compasscount == completecount && compasscount != 0) + if (_startedCompasses.Count > 0 && completecount == _startedCompasses.Count) { BUT_OBmagcalcancel.Enabled = false; BUT_OBmagcalaccept.Enabled = false; diff --git a/MissionPlannerTests/GCSViews/MagCalStatusTests.cs b/MissionPlannerTests/GCSViews/MagCalStatusTests.cs new file mode 100644 index 0000000000..bf951d99e9 --- /dev/null +++ b/MissionPlannerTests/GCSViews/MagCalStatusTests.cs @@ -0,0 +1,204 @@ +using Microsoft.VisualStudio.TestTools.UnitTesting; +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 new values are added: + /// 8 = BAD_OFFSETS – any offset component >= COMPASS_OFFS_MAX + /// 9 = BAD_DIAG_SCALING – diagonal or off-diagonal scaling out of range + /// 10 = BAD_FITNESS – fitness (RMS residual) exceeds tolerance + /// + /// These values are tested via raw byte cast because the named enum members + /// (MAG_CAL_BAD_OFFSETS/DIAG_SCALING/FITNESS) are not yet in the generated Mavlink.cs. + /// They will be added when mavlink/mavlink#2478 merges and Mavlink.cs is + /// regenerated. A follow-up to switch from raw casts to named members and + /// add ToString assertions is tracked in that upstream PR. + /// + [TestClass] + public class MagCalStatusTests + { + // Wire values for the three new failure codes from ArduPilot/ardupilot#32757. + // Named enum members arrive with mavlink/mavlink#2478 + Mavlink.cs regen. + private const byte RAW_BAD_OFFSETS = 8; + private const byte RAW_BAD_DIAG_SCALING = 9; + private const byte RAW_BAD_FITNESS = 10; + + // ── 1. Wire values ──────────────────────────────────────────────────── + // + // These are intentional documentation tests — the constants are defined + // as literal integers above, so the assertions always pass today. Once + // mavlink/mavlink#2478 merges and Mavlink.cs is regenerated the plan is + // to replace the raw constants with the named enum members and assert + // their numeric values here, turning these into real regression guards. + + [TestMethod] + public void BadOffsets_RawValue_Is8() + { + Assert.AreEqual(8, RAW_BAD_OFFSETS); + } + + [TestMethod] + public void BadDiagScaling_RawValue_Is9() + { + Assert.AreEqual(9, RAW_BAD_DIAG_SCALING); + } + + [TestMethod] + public void BadFitness_RawValue_Is10() + { + Assert.AreEqual(10, RAW_BAD_FITNESS); + } + + // ── 2. Cast from raw byte (as received from firmware) ──────────────── + + [TestMethod] + public void CastByte8_IsDistinctFromKnownValues() + { + var status = (MAVLink.MAG_CAL_STATUS)RAW_BAD_OFFSETS; + Assert.AreNotEqual(MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS, status); + Assert.AreNotEqual(MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED, status); + Assert.AreNotEqual(MAVLink.MAG_CAL_STATUS.MAG_CAL_BAD_RADIUS, status); + Assert.AreEqual((byte)8, (byte)status); + } + + [TestMethod] + public void CastByte9_IsDistinctFromKnownValues() + { + var status = (MAVLink.MAG_CAL_STATUS)RAW_BAD_DIAG_SCALING; + Assert.AreNotEqual(MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS, status); + Assert.AreNotEqual(MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED, status); + Assert.AreNotEqual(MAVLink.MAG_CAL_STATUS.MAG_CAL_BAD_RADIUS, status); + Assert.AreEqual((byte)9, (byte)status); + } + + [TestMethod] + public void CastByte10_IsDistinctFromKnownValues() + { + var status = (MAVLink.MAG_CAL_STATUS)RAW_BAD_FITNESS; + Assert.AreNotEqual(MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS, status); + Assert.AreNotEqual(MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED, status); + Assert.AreNotEqual(MAVLink.MAG_CAL_STATUS.MAG_CAL_BAD_RADIUS, status); + Assert.AreEqual((byte)10, (byte)status); + } + + // ── 3. Failure guard: calStatus > MAG_CAL_SUCCESS (value 4) ────────── + // + // The timer_Tick guard `if (calStatus > MAG_CAL_SUCCESS)` must capture + // all failure codes, including the three new ones sent as raw 8/9/10. + + [TestMethod] + public void RawByte8_IsGreaterThanSuccess() + { + var status = (MAVLink.MAG_CAL_STATUS)RAW_BAD_OFFSETS; + Assert.IsTrue(status > MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS, + "cal_status=8 (BAD_OFFSETS) must trigger the failure guard"); + } + + [TestMethod] + public void RawByte9_IsGreaterThanSuccess() + { + var status = (MAVLink.MAG_CAL_STATUS)RAW_BAD_DIAG_SCALING; + Assert.IsTrue(status > MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS, + "cal_status=9 (BAD_DIAG_SCALING) must trigger the failure guard"); + } + + [TestMethod] + public void RawByte10_IsGreaterThanSuccess() + { + var status = (MAVLink.MAG_CAL_STATUS)RAW_BAD_FITNESS; + Assert.IsTrue(status > MAVLink.MAG_CAL_STATUS.MAG_CAL_SUCCESS, + "cal_status=10 (BAD_FITNESS) 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_BAD_ORIENTATION, + MAVLink.MAG_CAL_STATUS.MAG_CAL_BAD_RADIUS, + (MAVLink.MAG_CAL_STATUS)RAW_BAD_OFFSETS, + (MAVLink.MAG_CAL_STATUS)RAW_BAD_DIAG_SCALING, + (MAVLink.MAG_CAL_STATUS)RAW_BAD_FITNESS, + }; + 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"); + } + + // ── 4. Named string representation ──────────────────────────────────── + + [TestMethod] + public void Failed_ToStringIsNamed() + { + Assert.AreEqual("MAG_CAL_FAILED", MAVLink.MAG_CAL_STATUS.MAG_CAL_FAILED.ToString()); + } + + [TestMethod] + public void BadRadius_ToStringIsNamed() + { + Assert.AreEqual("MAG_CAL_BAD_RADIUS", MAVLink.MAG_CAL_STATUS.MAG_CAL_BAD_RADIUS.ToString()); + } + + // ── 5. lastFailureStatus dictionary semantics ───────────────────────── + // + // Simulates the per-compass failure tracking in timer1_Tick: + // - failure status is stored per compass ID + // - a later success removes the entry + // - a later failure replaces (not accumulates) the entry + + [TestMethod] + public void LastFailureStatus_StoresFailurePerCompass() + { + var dict = new Dictionary(); + dict[0] = (MAVLink.MAG_CAL_STATUS)RAW_BAD_OFFSETS; + dict[1] = (MAVLink.MAG_CAL_STATUS)RAW_BAD_FITNESS; + + Assert.AreEqual((MAVLink.MAG_CAL_STATUS)RAW_BAD_OFFSETS, dict[0]); + Assert.AreEqual((MAVLink.MAG_CAL_STATUS)RAW_BAD_FITNESS, dict[1]); + } + + [TestMethod] + public void LastFailureStatus_SuccessRemovesEntry() + { + var dict = new Dictionary(); + dict[0] = (MAVLink.MAG_CAL_STATUS)RAW_BAD_FITNESS; + + dict.Remove(0); // success received + + Assert.IsFalse(dict.ContainsKey(0)); + } + + [TestMethod] + public void LastFailureStatus_LaterFailureReplaces() + { + var dict = new Dictionary(); + dict[0] = (MAVLink.MAG_CAL_STATUS)RAW_BAD_FITNESS; + dict[0] = (MAVLink.MAG_CAL_STATUS)RAW_BAD_OFFSETS; // second attempt fails differently + + Assert.AreEqual((MAVLink.MAG_CAL_STATUS)RAW_BAD_OFFSETS, dict[0]); + Assert.AreEqual(1, dict.Count); // no accumulation + } + } +}