From 5d50f2d2fd19a6020907b26f28bf9d4c5ac4cd8e Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Tue, 23 Dec 2014 10:32:15 -0800
Subject: [PATCH] Make sure trims are within min/max

---
 src/qgcunittest/PX4RCCalibrationTest.cc      | 27 ++++++++++----------
 src/ui/px4_configuration/PX4RCCalibration.cc |  7 ++++-
 2 files changed, 20 insertions(+), 14 deletions(-)

diff --git a/src/qgcunittest/PX4RCCalibrationTest.cc b/src/qgcunittest/PX4RCCalibrationTest.cc
index 05a926990..e59869373 100644
--- a/src/qgcunittest/PX4RCCalibrationTest.cc
+++ b/src/qgcunittest/PX4RCCalibrationTest.cc
@@ -109,17 +109,17 @@ const struct PX4RCCalibrationTest::ChannelSettings PX4RCCalibrationTest::_rgChan
 	{ PX4RCCalibration::rcCalFunctionMax,			PX4RCCalibration::_rcCalPWMDefaultMinValue,	PX4RCCalibration::_rcCalPWMDefaultMaxValue,	PX4RCCalibration::_rcCalPWMCenterPoint,         false },
 	
 	// Channels 1-11 are mapped to all available modes
-	{ PX4RCCalibration::rcCalFunctionRoll,			_testMinValue,		_testMaxValue,        _testCenterValue, true },
-    { PX4RCCalibration::rcCalFunctionPitch,			_testMinValue,		_testMaxValue,        _testCenterValue, false },
-    { PX4RCCalibration::rcCalFunctionYaw,			_testMinValue,		_testMaxValue,        _testCenterValue, true },
-    { PX4RCCalibration::rcCalFunctionThrottle,		_testMinValue,		_testMaxValue,        _testMinValue,    false },
-    { PX4RCCalibration::rcCalFunctionModeSwitch,	_testMinValue,		_testMaxValue,        _testCenterValue, false },
-    { PX4RCCalibration::rcCalFunctionPosCtlSwitch,	_testMinValue,		_testMaxValue,        _testCenterValue, false },
-    { PX4RCCalibration::rcCalFunctionLoiterSwitch,	_testMinValue,		_testMaxValue,        _testCenterValue, false },
-    { PX4RCCalibration::rcCalFunctionReturnSwitch,	_testMinValue,		_testMaxValue,        _testCenterValue, false },
-    { PX4RCCalibration::rcCalFunctionFlaps,			_testMinValue,		_testMaxValue,        _testCenterValue, false },
-    { PX4RCCalibration::rcCalFunctionAux1,			_testMinValue,		_testMaxValue,        _testCenterValue, false },
-    { PX4RCCalibration::rcCalFunctionAux2,			_testMinValue,		_testMaxValue,        _testCenterValue, false },
+	{ PX4RCCalibration::rcCalFunctionRoll,			_testMinValue,                              _testMaxValue,                              _testCenterValue,                               true },
+    { PX4RCCalibration::rcCalFunctionPitch,			_testMinValue,                              _testMaxValue,                              _testCenterValue,                               false },
+    { PX4RCCalibration::rcCalFunctionYaw,			_testMinValue,                              _testMaxValue,                              _testCenterValue,                               true },
+    { PX4RCCalibration::rcCalFunctionThrottle,		_testMinValue,                              _testMaxValue,                              _testMinValue,                                  false },
+    { PX4RCCalibration::rcCalFunctionModeSwitch,	_testMinValue,                              _testMaxValue,                              _testCenterValue,                               false },
+    { PX4RCCalibration::rcCalFunctionPosCtlSwitch,	_testMinValue,                              _testMaxValue,                              _testCenterValue,                               false },
+    { PX4RCCalibration::rcCalFunctionLoiterSwitch,	_testMinValue,                              _testMaxValue,                              _testCenterValue,                               false },
+    { PX4RCCalibration::rcCalFunctionReturnSwitch,	_testMinValue,                              _testMaxValue,                              _testCenterValue,                               false },
+    { PX4RCCalibration::rcCalFunctionFlaps,			_testMinValue,                              _testMaxValue,                              _testCenterValue,                               false },
+    { PX4RCCalibration::rcCalFunctionAux1,			_testMinValue,                              _testMaxValue,                              _testCenterValue,                               false },
+    { PX4RCCalibration::rcCalFunctionAux2,			_testMinValue,                              _testMaxValue,                              _testCenterValue,                               false },
 	
 	// Channels 12-17 are not mapped and should be set to defaults
 	{ PX4RCCalibration::rcCalFunctionMax,			PX4RCCalibration::_rcCalPWMDefaultMinValue,	PX4RCCalibration::_rcCalPWMDefaultMaxValue,	PX4RCCalibration::_rcCalPWMCenterPoint,         false },
@@ -435,8 +435,9 @@ void PX4RCCalibrationTest::_channelHomePosition(void)
         _mockUAS->emitRemoteControlChannelRawChanged(i, (float)PX4RCCalibration::_rcCalPWMCenterPoint);
     }
     
-    // Throttle to low position (throttle is not reversed)/
-    _mockUAS->emitRemoteControlChannelRawChanged(_rgFunctionChannelMap[PX4RCCalibration::rcCalFunctionThrottle], _testMinValue);
+    // Throttle to min - 1 (throttle is not reversed). We do this so that the trim value is below the min value. This should end up
+    // being validated and raised to min value. If not, something is wrong with RC Cal code.
+    _mockUAS->emitRemoteControlChannelRawChanged(_rgFunctionChannelMap[PX4RCCalibration::rcCalFunctionThrottle], _testMinValue - 1);
 }
 
 void PX4RCCalibrationTest::_validateParameters(void)
diff --git a/src/ui/px4_configuration/PX4RCCalibration.cc b/src/ui/px4_configuration/PX4RCCalibration.cc
index 6185849bd..85ea62e13 100644
--- a/src/ui/px4_configuration/PX4RCCalibration.cc
+++ b/src/ui/px4_configuration/PX4RCCalibration.cc
@@ -837,6 +837,12 @@ void PX4RCCalibration::_validateCalibration(void)
                     case rcCalFunctionYaw:
                     case rcCalFunctionRoll:
                     case rcCalFunctionPitch:
+                        // Make sure trim is within min/max
+                        if (info->rcTrim < info->rcMin) {
+                            info->rcTrim = info->rcMin;
+                        } else if (info->rcTrim > info->rcMax) {
+                            info->rcTrim = info->rcMax;
+                        }
                         break;
                     default:
                         // Non-attitude control channels have calculated trim
@@ -858,7 +864,6 @@ void PX4RCCalibration::_validateCalibration(void)
 
 
 /// @brief Saves the rc calibration values to the board parameters.
-///     @param trimsOnly true: write only trim values, false: write all calibration values
 void PX4RCCalibration::_writeCalibration(void)
 {
     if (!_mav) return;
-- 
2.22.0