From 6b7d5010fb0e658b36329c6fc411f47d67e18dc0 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 7 May 2017 18:28:28 -0700 Subject: [PATCH] Fix parameter mapping --- src/AutoPilotPlugins/APM/APMCameraComponent.qml | 11 ++------ src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc | 33 ++++++++++++++++++++++ src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc | 33 ++++++++++++++++++++++ src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc | 33 ++++++++++++++++++++++ src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc | 33 ++++++++++++++++++++++ 5 files changed, 134 insertions(+), 9 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMCameraComponent.qml b/src/AutoPilotPlugins/APM/APMCameraComponent.qml index bd9308f..121cfdf 100644 --- a/src/AutoPilotPlugins/APM/APMCameraComponent.qml +++ b/src/AutoPilotPlugins/APM/APMCameraComponent.qml @@ -92,19 +92,12 @@ SetupPage { } function setGimbalSettingsServoInfo(loader, channel) { - var rcPrefix = "RC" + channel + "_" + var rcPrefix = "r.SERVO" + channel + "_" loader.gimbalOutIndex = channel - 4 loader.servoPWMMinFact = controller.getParameterFact(-1, rcPrefix + "MIN") loader.servoPWMMaxFact = controller.getParameterFact(-1, rcPrefix + "MAX") - if (controller.parameterExists(-1, "RC5_REVERSED")) { - // Newer firmware parameter - loader.servoReverseFact = controller.getParameterFact(-1, rcPrefix + "REVERSED") - } else { - // Older firmware parameter - loader.servoReverseFact = controller.getParameterFact(-1, rcPrefix + "REV") - } - + loader.servoReverseFact = controller.getParameterFact(-1, rcPrefix + "REVERSED") } /// Gimbal output channels are stored in SERVO#_FUNCTION parameters. We need to loop through those diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index dba4a5f..4d07f66 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -108,6 +108,39 @@ ArduCopterFirmwarePlugin::ArduCopterFirmwarePlugin(void) remapV3_5["SERVO13_FUNCTION"] = QStringLiteral("RC13_FUNCTION"); remapV3_5["SERVO14_FUNCTION"] = QStringLiteral("RC14_FUNCTION"); + remapV3_5["SERVO5_MIN"] = QStringLiteral("RC5_MIN"); + remapV3_5["SERVO6_MIN"] = QStringLiteral("RC6_MIN"); + remapV3_5["SERVO7_MIN"] = QStringLiteral("RC7_MIN"); + remapV3_5["SERVO8_MIN"] = QStringLiteral("RC8_MIN"); + remapV3_5["SERVO9_MIN"] = QStringLiteral("RC9_MIN"); + remapV3_5["SERVO10_MIN"] = QStringLiteral("RC10_MIN"); + remapV3_5["SERVO11_MIN"] = QStringLiteral("RC11_MIN"); + remapV3_5["SERVO12_MIN"] = QStringLiteral("RC12_MIN"); + remapV3_5["SERVO13_MIN"] = QStringLiteral("RC13_MIN"); + remapV3_5["SERVO14_MIN"] = QStringLiteral("RC14_MIN"); + + remapV3_5["SERVO5_MAX"] = QStringLiteral("RC5_MAX"); + remapV3_5["SERVO6_MAX"] = QStringLiteral("RC6_MAX"); + remapV3_5["SERVO7_MAX"] = QStringLiteral("RC7_MAX"); + remapV3_5["SERVO8_MAX"] = QStringLiteral("RC8_MAX"); + remapV3_5["SERVO9_MAX"] = QStringLiteral("RC9_MAX"); + remapV3_5["SERVO10_MAX"] = QStringLiteral("RC10_MAX"); + remapV3_5["SERVO11_MAX"] = QStringLiteral("RC11_MAX"); + remapV3_5["SERVO12_MAX"] = QStringLiteral("RC12_MAX"); + remapV3_5["SERVO13_MAX"] = QStringLiteral("RC13_MAX"); + remapV3_5["SERVO14_MAX"] = QStringLiteral("RC14_MAX"); + + remapV3_5["SERVO5_REVERSED"] = QStringLiteral("RC5_REVERSED"); + remapV3_5["SERVO6_REVERSED"] = QStringLiteral("RC6_REVERSED"); + remapV3_5["SERVO7_REVERSED"] = QStringLiteral("RC7_REVERSED"); + remapV3_5["SERVO8_REVERSED"] = QStringLiteral("RC8_REVERSED"); + remapV3_5["SERVO9_REVERSED"] = QStringLiteral("RC9_REVERSED"); + remapV3_5["SERVO10_REVERSED"] = QStringLiteral("RC10_REVERSED"); + remapV3_5["SERVO11_REVERSED"] = QStringLiteral("RC11_REVERSED"); + remapV3_5["SERVO12_REVERSED"] = QStringLiteral("RC12_REVERSED"); + remapV3_5["SERVO13_REVERSED"] = QStringLiteral("RC13_REVERSED"); + remapV3_5["SERVO14_REVERSED"] = QStringLiteral("RC14_REVERSED"); + _remapParamNameIntialized = true; } } diff --git a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc index 996551e..8b7953b 100644 --- a/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduPlaneFirmwarePlugin.cc @@ -77,6 +77,39 @@ ArduPlaneFirmwarePlugin::ArduPlaneFirmwarePlugin(void) remapV3_8["SERVO13_FUNCTION"] = QStringLiteral("RC13_FUNCTION"); remapV3_8["SERVO14_FUNCTION"] = QStringLiteral("RC14_FUNCTION"); + remapV3_8["SERVO5_MIN"] = QStringLiteral("RC5_MIN"); + remapV3_8["SERVO6_MIN"] = QStringLiteral("RC6_MIN"); + remapV3_8["SERVO7_MIN"] = QStringLiteral("RC7_MIN"); + remapV3_8["SERVO8_MIN"] = QStringLiteral("RC8_MIN"); + remapV3_8["SERVO9_MIN"] = QStringLiteral("RC9_MIN"); + remapV3_8["SERVO10_MIN"] = QStringLiteral("RC10_MIN"); + remapV3_8["SERVO11_MIN"] = QStringLiteral("RC11_MIN"); + remapV3_8["SERVO12_MIN"] = QStringLiteral("RC12_MIN"); + remapV3_8["SERVO13_MIN"] = QStringLiteral("RC13_MIN"); + remapV3_8["SERVO14_MIN"] = QStringLiteral("RC14_MIN"); + + remapV3_8["SERVO5_MAX"] = QStringLiteral("RC5_MAX"); + remapV3_8["SERVO6_MAX"] = QStringLiteral("RC6_MAX"); + remapV3_8["SERVO7_MAX"] = QStringLiteral("RC7_MAX"); + remapV3_8["SERVO8_MAX"] = QStringLiteral("RC8_MAX"); + remapV3_8["SERVO9_MAX"] = QStringLiteral("RC9_MAX"); + remapV3_8["SERVO10_MAX"] = QStringLiteral("RC10_MAX"); + remapV3_8["SERVO11_MAX"] = QStringLiteral("RC11_MAX"); + remapV3_8["SERVO12_MAX"] = QStringLiteral("RC12_MAX"); + remapV3_8["SERVO13_MAX"] = QStringLiteral("RC13_MAX"); + remapV3_8["SERVO14_MAX"] = QStringLiteral("RC14_MAX"); + + remapV3_8["SERVO5_REVERSED"] = QStringLiteral("RC5_REVERSED"); + remapV3_8["SERVO6_REVERSED"] = QStringLiteral("RC6_REVERSED"); + remapV3_8["SERVO7_REVERSED"] = QStringLiteral("RC7_REVERSED"); + remapV3_8["SERVO8_REVERSED"] = QStringLiteral("RC8_REVERSED"); + remapV3_8["SERVO9_REVERSED"] = QStringLiteral("RC9_REVERSED"); + remapV3_8["SERVO10_REVERSED"] = QStringLiteral("RC10_REVERSED"); + remapV3_8["SERVO11_REVERSED"] = QStringLiteral("RC11_REVERSED"); + remapV3_8["SERVO12_REVERSED"] = QStringLiteral("RC12_REVERSED"); + remapV3_8["SERVO13_REVERSED"] = QStringLiteral("RC13_REVERSED"); + remapV3_8["SERVO14_REVERSED"] = QStringLiteral("RC14_REVERSED"); + _remapParamNameIntialized = true; } } diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc index bbb7ed9..1e30340 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc @@ -55,6 +55,39 @@ ArduRoverFirmwarePlugin::ArduRoverFirmwarePlugin(void) remapV3_2["SERVO13_FUNCTION"] = QStringLiteral("RC13_FUNCTION"); remapV3_2["SERVO14_FUNCTION"] = QStringLiteral("RC14_FUNCTION"); + remapV3_2["SERVO5_MIN"] = QStringLiteral("RC5_MIN"); + remapV3_2["SERVO6_MIN"] = QStringLiteral("RC6_MIN"); + remapV3_2["SERVO7_MIN"] = QStringLiteral("RC7_MIN"); + remapV3_2["SERVO8_MIN"] = QStringLiteral("RC8_MIN"); + remapV3_2["SERVO9_MIN"] = QStringLiteral("RC9_MIN"); + remapV3_2["SERVO10_MIN"] = QStringLiteral("RC10_MIN"); + remapV3_2["SERVO11_MIN"] = QStringLiteral("RC11_MIN"); + remapV3_2["SERVO12_MIN"] = QStringLiteral("RC12_MIN"); + remapV3_2["SERVO13_MIN"] = QStringLiteral("RC13_MIN"); + remapV3_2["SERVO14_MIN"] = QStringLiteral("RC14_MIN"); + + remapV3_2["SERVO5_MAX"] = QStringLiteral("RC5_MAX"); + remapV3_2["SERVO6_MAX"] = QStringLiteral("RC6_MAX"); + remapV3_2["SERVO7_MAX"] = QStringLiteral("RC7_MAX"); + remapV3_2["SERVO8_MAX"] = QStringLiteral("RC8_MAX"); + remapV3_2["SERVO9_MAX"] = QStringLiteral("RC9_MAX"); + remapV3_2["SERVO10_MAX"] = QStringLiteral("RC10_MAX"); + remapV3_2["SERVO11_MAX"] = QStringLiteral("RC11_MAX"); + remapV3_2["SERVO12_MAX"] = QStringLiteral("RC12_MAX"); + remapV3_2["SERVO13_MAX"] = QStringLiteral("RC13_MAX"); + remapV3_2["SERVO14_MAX"] = QStringLiteral("RC14_MAX"); + + remapV3_2["SERVO5_REVERSED"] = QStringLiteral("RC5_REVERSED"); + remapV3_2["SERVO6_REVERSED"] = QStringLiteral("RC6_REVERSED"); + remapV3_2["SERVO7_REVERSED"] = QStringLiteral("RC7_REVERSED"); + remapV3_2["SERVO8_REVERSED"] = QStringLiteral("RC8_REVERSED"); + remapV3_2["SERVO9_REVERSED"] = QStringLiteral("RC9_REVERSED"); + remapV3_2["SERVO10_REVERSED"] = QStringLiteral("RC10_REVERSED"); + remapV3_2["SERVO11_REVERSED"] = QStringLiteral("RC11_REVERSED"); + remapV3_2["SERVO12_REVERSED"] = QStringLiteral("RC12_REVERSED"); + remapV3_2["SERVO13_REVERSED"] = QStringLiteral("RC13_REVERSED"); + remapV3_2["SERVO14_REVERSED"] = QStringLiteral("RC14_REVERSED"); + _remapParamNameIntialized = true; } } diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc index 78ca8d4..cc0ff97 100644 --- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc @@ -62,6 +62,39 @@ ArduSubFirmwarePlugin::ArduSubFirmwarePlugin(void) remapV3_5["SERVO13_FUNCTION"] = QStringLiteral("RC13_FUNCTION"); remapV3_5["SERVO14_FUNCTION"] = QStringLiteral("RC14_FUNCTION"); + remapV3_5["SERVO5_MIN"] = QStringLiteral("RC5_MIN"); + remapV3_5["SERVO6_MIN"] = QStringLiteral("RC6_MIN"); + remapV3_5["SERVO7_MIN"] = QStringLiteral("RC7_MIN"); + remapV3_5["SERVO8_MIN"] = QStringLiteral("RC8_MIN"); + remapV3_5["SERVO9_MIN"] = QStringLiteral("RC9_MIN"); + remapV3_5["SERVO10_MIN"] = QStringLiteral("RC10_MIN"); + remapV3_5["SERVO11_MIN"] = QStringLiteral("RC11_MIN"); + remapV3_5["SERVO12_MIN"] = QStringLiteral("RC12_MIN"); + remapV3_5["SERVO13_MIN"] = QStringLiteral("RC13_MIN"); + remapV3_5["SERVO14_MIN"] = QStringLiteral("RC14_MIN"); + + remapV3_5["SERVO5_MAX"] = QStringLiteral("RC5_MAX"); + remapV3_5["SERVO6_MAX"] = QStringLiteral("RC6_MAX"); + remapV3_5["SERVO7_MAX"] = QStringLiteral("RC7_MAX"); + remapV3_5["SERVO8_MAX"] = QStringLiteral("RC8_MAX"); + remapV3_5["SERVO9_MAX"] = QStringLiteral("RC9_MAX"); + remapV3_5["SERVO10_MAX"] = QStringLiteral("RC10_MAX"); + remapV3_5["SERVO11_MAX"] = QStringLiteral("RC11_MAX"); + remapV3_5["SERVO12_MAX"] = QStringLiteral("RC12_MAX"); + remapV3_5["SERVO13_MAX"] = QStringLiteral("RC13_MAX"); + remapV3_5["SERVO14_MAX"] = QStringLiteral("RC14_MAX"); + + remapV3_5["SERVO5_REVERSED"] = QStringLiteral("RC5_REVERSED"); + remapV3_5["SERVO6_REVERSED"] = QStringLiteral("RC6_REVERSED"); + remapV3_5["SERVO7_REVERSED"] = QStringLiteral("RC7_REVERSED"); + remapV3_5["SERVO8_REVERSED"] = QStringLiteral("RC8_REVERSED"); + remapV3_5["SERVO9_REVERSED"] = QStringLiteral("RC9_REVERSED"); + remapV3_5["SERVO10_REVERSED"] = QStringLiteral("RC10_REVERSED"); + remapV3_5["SERVO11_REVERSED"] = QStringLiteral("RC11_REVERSED"); + remapV3_5["SERVO12_REVERSED"] = QStringLiteral("RC12_REVERSED"); + remapV3_5["SERVO13_REVERSED"] = QStringLiteral("RC13_REVERSED"); + remapV3_5["SERVO14_REVERSED"] = QStringLiteral("RC14_REVERSED"); + remapV3_5["FENCE_ALT_MIN"] = QStringLiteral("FENCE_DEPTH_MAX"); _remapParamNameIntialized = true;