Skip to content

Commit

Permalink
Add ESC13-16 telemetry to CurrentState (#3378)
Browse files Browse the repository at this point in the history
  • Loading branch information
@EosBandi
EosBandi authored Jul 8, 2024
1 parent faff1e8 commit 64e721a
Showing 1 changed file with 56 additions and 6 deletions.
62 changes: 56 additions & 6 deletions ExtLibs/ArduPilot/CurrentState.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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
{
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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<MAVLink.mavlink_esc_telemetry_13_to_16_t>();
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:

{
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -3809,7 +3859,7 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi
var status = mavLinkMessage.ToStructure<MAVLink.mavlink_gimbal_device_attitude_status_t>();
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
}
Expand Down Expand Up @@ -4324,7 +4374,7 @@ public void UpdateCurrentSettings(Action<CurrentState> bs, bool updatenow,
public void dowindcalc()
{
//Wind Fixed gain Observer
//Ryan Beall
//Ryan Beall
//8FEB10

var Kw = 0.010; // 0.01 // 0.10
Expand Down

0 comments on commit 64e721a

Please sign in to comment.