@@ -2074,6 +2074,18 @@ public float ter_alt
2074
2074
2075
2075
[ GroupText ( "PID" ) ] public float pidPDmod { get ; set ; }
2076
2076
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
+
2077
2089
[ GroupText ( "Vibe" ) ] public uint vibeclip0 { get ; set ; }
2078
2090
2079
2091
[ GroupText ( "Vibe" ) ] public uint vibeclip1 { get ; set ; }
@@ -3686,8 +3698,7 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi
3686
3698
{
3687
3699
var pid = mavLinkMessage . ToStructure < MAVLink . mavlink_pid_tuning_t > ( ) ;
3688
3700
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
3691
3702
pidff = pid . FF ;
3692
3703
pidP = pid . P ;
3693
3704
pidI = pid . I ;
@@ -3697,6 +3708,29 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi
3697
3708
pidachieved = pid . achieved ;
3698
3709
pidSRate = pid . SRate ;
3699
3710
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
+
3700
3734
}
3701
3735
3702
3736
break ;
0 commit comments