From 17f2ad9effa7febd1ae7c1b6168b3321b38d1dd3 Mon Sep 17 00:00:00 2001 From: Andras Schaffer Date: Mon, 8 Jul 2024 20:42:00 +0200 Subject: [PATCH] Add ESC13-16 telemetry to CurrentState --- ExtLibs/ArduPilot/CurrentState.cs | 62 ++++++++++++++++++++++++++++--- 1 file changed, 56 insertions(+), 6 deletions(-) diff --git a/ExtLibs/ArduPilot/CurrentState.cs b/ExtLibs/ArduPilot/CurrentState.cs index 5f9ad1bc5e..aa15424b5d 100644 --- a/ExtLibs/ArduPilot/CurrentState.cs +++ b/ExtLibs/ArduPilot/CurrentState.cs @@ -321,7 +321,7 @@ public float alt get => (_alt - altoffsethome) * multiplieralt; set { - // check update rate, and ensure time hasnt gone backwards + // check update rate, and ensure time hasnt gone backwards _alt = value; if ((datetime - lastalt).TotalSeconds >= 0.2 && oldalt != alt || lastalt > datetime) @@ -903,6 +903,28 @@ public float groundspeed [GroupText("ESC")] public float esc12_rpm { get; set; } [GroupText("ESC")] public float esc12_temp { get; set; } + [GroupText("ESC")] public float esc13_volt { get; set; } + [GroupText("ESC")] public float esc13_curr { get; set; } + [GroupText("ESC")] public float esc13_rpm { get; set; } + [GroupText("ESC")] public float esc13_temp { get; set; } + + [GroupText("ESC")] public float esc14_volt { get; set; } + [GroupText("ESC")] public float esc14_curr { get; set; } + [GroupText("ESC")] public float esc14_rpm { get; set; } + [GroupText("ESC")] public float esc14_temp { get; set; } + + [GroupText("ESC")] public float esc15_volt { get; set; } + [GroupText("ESC")] public float esc15_curr { get; set; } + [GroupText("ESC")] public float esc15_rpm { get; set; } + [GroupText("ESC")] public float esc15_temp { get; set; } + + [GroupText("ESC")] public float esc16_volt { get; set; } + [GroupText("ESC")] public float esc16_curr { get; set; } + [GroupText("ESC")] public float esc16_rpm { get; set; } + [GroupText("ESC")] public float esc16_temp { get; set; } + + + [GroupText("RadioOut")] public float ch3percent { @@ -2197,9 +2219,9 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi || mavLinkMessage.msgid == (uint)MAVLink.MAVLINK_MSG_ID.RADIO // propagate the RADIO/RADIO_STATUS message across all devices on this link || mavLinkMessage.msgid == (uint)MAVLink.MAVLINK_MSG_ID.RADIO_STATUS || ( mavLinkMessage.sysid == parent.sysid // Propagate NAMED_VALUE_FLOAT messages across all components within the same device - && mavLinkMessage.msgid == (uint)MAVLink.MAVLINK_MSG_ID.NAMED_VALUE_FLOAT + && mavLinkMessage.msgid == (uint)MAVLink.MAVLINK_MSG_ID.NAMED_VALUE_FLOAT && Settings.Instance.GetBoolean("propagateNamedFloats", true)) ) - + { switch (mavLinkMessage.msgid) { @@ -3474,6 +3496,34 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi } break; + + case (uint)MAVLink.MAVLINK_MSG_ID.ESC_TELEMETRY_13_TO_16: + + { + var esc = mavLinkMessage.ToStructure(); + esc13_volt = esc.voltage[0] / 100.0f; + esc13_curr = esc.current[0] / 100.0f; + esc13_rpm = esc.rpm[0]; + esc13_temp = esc.temperature[0]; + + esc14_volt = esc.voltage[1] / 100.0f; + esc14_curr = esc.current[1] / 100.0f; + esc14_rpm = esc.rpm[1]; + esc14_temp = esc.temperature[1]; + + esc15_volt = esc.voltage[2] / 100.0f; + esc15_curr = esc.current[2] / 100.0f; + esc15_rpm = esc.rpm[2]; + esc15_temp = esc.temperature[2]; + + esc16_volt = esc.voltage[3] / 100.0f; + esc16_curr = esc.current[3] / 100.0f; + esc16_rpm = esc.rpm[3]; + esc16_temp = esc.temperature[3]; + } + + break; + case (uint)MAVLink.MAVLINK_MSG_ID.SERVO_OUTPUT_RAW: { @@ -3716,7 +3766,7 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi float value = named_float.value; var field = custom_field_names.FirstOrDefault(x => x.Value == name).Key; - //todo: if field is null then check if we have a free customfield and add the named_value + //todo: if field is null then check if we have a free customfield and add the named_value if (field == null) { short i; @@ -3809,7 +3859,7 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi var status = mavLinkMessage.ToStructure(); Quaternion q = new Quaternion(status.q[0], status.q[1], status.q[2], status.q[3]); campointa = (float)(q.get_euler_pitch() * (180.0 / Math.PI)); - campointb = (float)(q.get_euler_roll() * (180.0 / Math.PI)); + campointb = (float)(q.get_euler_roll() * (180.0 / Math.PI)); campointc = (float)(q.get_euler_yaw() * (180.0 / Math.PI)); if (campointc < 0) campointc += 360; //normalization } @@ -4324,7 +4374,7 @@ public void UpdateCurrentSettings(Action bs, bool updatenow, public void dowindcalc() { //Wind Fixed gain Observer - //Ryan Beall + //Ryan Beall //8FEB10 var Kw = 0.010; // 0.01 // 0.10