Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

CurrentState: Add ESC13-16 telemetry #3378

Merged
merged 1 commit into from
Jul 8, 2024
Merged

CurrentState: Add ESC13-16 telemetry #3378

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
62 changes: 56 additions & 6 deletions ExtLibs/ArduPilot/CurrentState.cs
View file
Open in desktop
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
Loading