From 202fc17608f21d0b877af5e11cd992b1dba40045 Mon Sep 17 00:00:00 2001
From: dongie <timecop@gmail.com>
Date: Tue, 18 Feb 2014 15:23:02 +0900
Subject: [PATCH] added max_angle_inclination to CLI to configure (default) 50
 degree max inclination. configurable between 10 and 90 (100..900 in cli)

---
 src/cli.c    | 1 +
 src/config.c | 3 ++-
 src/mw.c     | 6 +++---
 src/mw.h     | 1 +
 4 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/src/cli.c b/src/cli.c
index 68596cb4d..3dd8d5265 100644
--- a/src/cli.c
+++ b/src/cli.c
@@ -145,6 +145,7 @@ const clivalue_t valueTable[] = {
     { "align_board_yaw", VAR_INT16, &mcfg.board_align_yaw, -180, 360 },
     { "yaw_control_direction", VAR_INT8, &mcfg.yaw_control_direction, -1, 1 },
     { "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 5 },
+    { "max_angle_inclination", VAR_UINT16, &mcfg.max_angle_inclination, 100, 900 },
     { "moron_threshold", VAR_UINT8, &mcfg.moron_threshold, 0, 128 },
     { "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 },
     { "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 },
diff --git a/src/config.c b/src/config.c
index 084e5437f..feba54f1c 100755
--- a/src/config.c
+++ b/src/config.c
@@ -13,7 +13,7 @@ master_t mcfg;  // master config struct with data independent from profiles
 config_t cfg;   // profile config struct
 const char rcChannelLetters[] = "AERT1234";
 
-static const uint8_t EEPROM_CONF_VERSION = 57;
+static const uint8_t EEPROM_CONF_VERSION = 58;
 static uint32_t enabledSensors = 0;
 static void resetConf(void);
 
@@ -184,6 +184,7 @@ static void resetConf(void)
     mcfg.board_align_pitch = 0;
     mcfg.board_align_yaw = 0;
     mcfg.acc_hardware = ACC_DEFAULT;     // default/autodetect
+    mcfg.max_angle_inclination = 500;    // 50 degrees
     mcfg.yaw_control_direction = 1;
     mcfg.moron_threshold = 32;
     mcfg.gyro_smoothing_factor = 0x00141403;     // default factors of 20, 20, 3 for R/P/Y
diff --git a/src/mw.c b/src/mw.c
index 05bd243f1..026ad7e31 100755
--- a/src/mw.c
+++ b/src/mw.c
@@ -302,7 +302,7 @@ static void pidMultiWii(void)
     for (axis = 0; axis < 3; axis++) {
         if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
             // 50 degrees max inclination
-            errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
+            errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)mcfg.max_angle_inclination), +mcfg.max_angle_inclination) - angle[axis] + cfg.angleTrim[axis];
             PTermACC = errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768   16 bits is ok for result
             PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
 
@@ -360,8 +360,8 @@ static void pidRewrite(void)
     for (axis = 0; axis < 3; axis++) {
         // -----Get the desired angle rate depending on flight mode
         if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2 ) { // MODE relying on ACC
-            // calculate error and limit the angle to 50 degrees max inclination
-            errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]; // 16 bits is ok here
+            // calculate error and limit the angle to max configured inclination
+            errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int)mcfg.max_angle_inclination), +mcfg.max_angle_inclination) - angle[axis] + cfg.angleTrim[axis]; // 16 bits is ok here
         }
         if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
             AngleRateTmp = (((int32_t)(cfg.yawRate + 27) * rcCommand[2]) >> 5);
diff --git a/src/mw.h b/src/mw.h
index e55ac18d1..7a0f981ca 100755
--- a/src/mw.h
+++ b/src/mw.h
@@ -249,6 +249,7 @@ typedef struct master_t {
     uint16_t gyro_cmpfm_factor;             // Set the Gyro Weight for Gyro/Magnetometer complementary filter. Increasing this value would reduce and delay Magnetometer influence on the output of the filter
     uint32_t gyro_smoothing_factor;         // How much to smoothen with per axis (32bit value with Roll, Pitch, Yaw in bits 24, 16, 8 respectively
     uint8_t moron_threshold;                // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
+    uint16_t max_angle_inclination;         // max inclination allowed in angle (level) mode. default 500 (50 degrees).
     int16_t accZero[3];
     int16_t magZero[3];