From 875e9ae1c89ef9490738ef31d7a555ee6b449a2e Mon Sep 17 00:00:00 2001 From: Bob Long Date: Sat, 14 Oct 2023 16:16:55 -0400 Subject: [PATCH] CurrentState: add more rangefinders ArduPilot presently supports 10 --- ExtLibs/ArduPilot/CurrentState.cs | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/ExtLibs/ArduPilot/CurrentState.cs b/ExtLibs/ArduPilot/CurrentState.cs index 875641f478..93fb665efa 100644 --- a/ExtLibs/ArduPilot/CurrentState.cs +++ b/ExtLibs/ArduPilot/CurrentState.cs @@ -1653,10 +1653,17 @@ public float sonarrange [DisplayText("Sonar Voltage (Volt)")][GroupText("Sensor")] public float sonarvoltage { get; set; } [DisplayText("RangeFinder1 (cm)")][GroupText("Sensor")] public uint rangefinder1 { get; set; } - [DisplayText("RangeFinder2 (cm)")][GroupText("Sensor")] public uint rangefinder2 { get; set; } - [DisplayText("RangeFinder3 (cm)")][GroupText("Sensor")] public uint rangefinder3 { get; set; } + [DisplayText("RangeFinder4 (cm)")][GroupText("Sensor")] public uint rangefinder4 { get; set; } + [DisplayText("RangeFinder5 (cm)")][GroupText("Sensor")] public uint rangefinder5 { get; set; } + [DisplayText("RangeFinder6 (cm)")][GroupText("Sensor")] public uint rangefinder6 { get; set; } + [DisplayText("RangeFinder7 (cm)")][GroupText("Sensor")] public uint rangefinder7 { get; set; } + [DisplayText("RangeFinder8 (cm)")][GroupText("Sensor")] public uint rangefinder8 { get; set; } + [DisplayText("RangeFinder9 (cm)")][GroupText("Sensor")] public uint rangefinder9 { get; set; } + [DisplayText("RangeFinder10 (cm)")][GroupText("Sensor")] public uint rangefinder10 { get; set; } + + [GroupText("Software")] public float freemem { get; set; } [GroupText("Software")] public float load { get; set; } @@ -2476,11 +2483,16 @@ private void Parent_OnPacketReceived(object sender, MAVLink.MAVLinkMessage mavLi { var sonar = mavLinkMessage.ToStructure(); - if (sonar.id == 0) - rangefinder1 = sonar.current_distance; - else if (sonar.id == 1) - rangefinder2 = sonar.current_distance; + if (sonar.id == 0) rangefinder1 = sonar.current_distance; + else if (sonar.id == 1) rangefinder2 = sonar.current_distance; else if (sonar.id == 2) rangefinder3 = sonar.current_distance; + else if (sonar.id == 3) rangefinder4 = sonar.current_distance; + else if (sonar.id == 4) rangefinder5 = sonar.current_distance; + else if (sonar.id == 5) rangefinder6 = sonar.current_distance; + else if (sonar.id == 6) rangefinder7 = sonar.current_distance; + else if (sonar.id == 7) rangefinder8 = sonar.current_distance; + else if (sonar.id == 8) rangefinder9 = sonar.current_distance; + else if (sonar.id == 9) rangefinder10 = sonar.current_distance; } break;