Skip to content

Commit e194025

Browse files
robertlong13meee1
authored andcommitted
CurrentState: add individual SRate variables
Allows the SRate for Roll/Pitch/Yaw to be displayed simultaneously, rather than needing to set GCS_PID_MASK to one axis at a time. This is useful since tuning one axis may trigger an oscillation in another.
1 parent 718162a commit e194025

File tree

1 file changed

+36
-2
lines changed

1 file changed

+36
-2
lines changed

ExtLibs/ArduPilot/CurrentState.cs

Lines changed: 36 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2074,6 +2074,18 @@ public float ter_alt
20742074

20752075
[GroupText("PID")] public float pidPDmod { get; set; }
20762076

2077+
[GroupText("PID")] public float pidSRateRoll { get; set; }
2078+
2079+
[GroupText("PID")] public float pidSRatePitch { get; set; }
2080+
2081+
[GroupText("PID")] public float pidSRateYaw { get; set; }
2082+
2083+
[GroupText("PID")] public float pidSRateAccZ { get; set; }
2084+
2085+
[GroupText("PID")] public float pidSRateSteer { get; set; }
2086+
2087+
[GroupText("PID")] public float pidSRateLanding { get; set; }
2088+
20772089
[GroupText("Vibe")] public uint vibeclip0 { get; set; }
20782090

20792091
[GroupText("Vibe")] public uint vibeclip1 { get; set; }
@@ -3686,8 +3698,7 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi
36863698
{
36873699
var pid = mavLinkMessage.ToStructure<MAVLink.mavlink_pid_tuning_t>();
36883700

3689-
//todo: currently only deals with single axis at once
3690-
3701+
// These variables currently only deal a with single axis at once, but SRate can be reported for multiple at once
36913702
pidff = pid.FF;
36923703
pidP = pid.P;
36933704
pidI = pid.I;
@@ -3697,6 +3708,29 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi
36973708
pidachieved = pid.achieved;
36983709
pidSRate = pid.SRate;
36993710
pidPDmod = pid.PDmod;
3711+
3712+
switch (pid.axis)
3713+
{
3714+
case (byte)MAVLink.PID_TUNING_AXIS.PID_TUNING_ROLL:
3715+
pidSRateRoll = pid.SRate;
3716+
break;
3717+
case (byte)MAVLink.PID_TUNING_AXIS.PID_TUNING_PITCH:
3718+
pidSRatePitch = pid.SRate;
3719+
break;
3720+
case (byte)MAVLink.PID_TUNING_AXIS.PID_TUNING_YAW:
3721+
pidSRateYaw = pid.SRate;
3722+
break;
3723+
case (byte)MAVLink.PID_TUNING_AXIS.PID_TUNING_ACCZ:
3724+
pidSRateAccZ = pid.SRate;
3725+
break;
3726+
case (byte)MAVLink.PID_TUNING_AXIS.PID_TUNING_STEER:
3727+
pidSRateSteer = pid.SRate;
3728+
break;
3729+
case (byte)MAVLink.PID_TUNING_AXIS.PID_TUNING_LANDING:
3730+
pidSRateLanding = pid.SRate;
3731+
break;
3732+
}
3733+
37003734
}
37013735

37023736
break;

0 commit comments

Comments
 (0)