From bdb5c88d08bae5ed2298fe210c496bc428d76bbd Mon Sep 17 00:00:00 2001
From: Corvus Corax <corvuscorax@cybertrench.com>
Date: Fri, 3 Oct 2014 18:57:16 +0200
Subject: [PATCH] OP-1516 added bank configurable expo to stick to allow
 snappier rate response

---
 .../modules/ManualControl/stabilizedhandler.c | 22 +++++++++++++++++++
 .../uavobjectdefinition/stabilizationbank.xml |  3 ++-
 .../stabilizationsettingsbank1.xml            |  3 ++-
 .../stabilizationsettingsbank2.xml            |  3 ++-
 .../stabilizationsettingsbank3.xml            |  3 ++-
 5 files changed, 30 insertions(+), 4 deletions(-)

diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c
index e3c4e6816..e2696d3c9 100644
--- a/flight/modules/ManualControl/stabilizedhandler.c
+++ b/flight/modules/ManualControl/stabilizedhandler.c
@@ -39,6 +39,25 @@
 // Private types
 
 // Private functions
+static float applyExpo(float value, float expo);
+
+
+static float applyExpo(float value, float expo)
+{
+    float exp = powf(1.023293f, expo);
+
+    // magic number scales expo
+    // so that
+    // expo=100 yields value**10
+    // expo=0 yields value**1
+    // expo=-100 yields value**(1/10)
+    // (pow(10,1/100)~=1.023293)
+    if (value > 0.0f) {
+        return powf(value, exp);
+    } else {
+        return -powf(-value, exp);
+    }
+}
 
 
 /**
@@ -64,6 +83,9 @@ void stabilizedHandler(bool newinit)
     StabilizationBankData stabSettings;
     StabilizationBankGet(&stabSettings);
 
+    cmd.Roll  = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
+    cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
+    cmd.Yaw   = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
     uint8_t *stab_settings;
     FlightStatusData flightStatus;
     FlightStatusGet(&flightStatus);
diff --git a/shared/uavobjectdefinition/stabilizationbank.xml b/shared/uavobjectdefinition/stabilizationbank.xml
index f71e0b976..a1f5a710f 100644
--- a/shared/uavobjectdefinition/stabilizationbank.xml
+++ b/shared/uavobjectdefinition/stabilizationbank.xml
@@ -7,6 +7,7 @@
 	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="42" limits="%BE:0:180"/>
 	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="150,150,175" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,50" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
+	<field name="StickExpo" units="percent" type="int8" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:-100:100; %BE:-100:100; %BE:-100:100"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.003,0.003,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
@@ -15,7 +16,7 @@
 	<field name="PitchPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
 	<field name="YawPI" units="" type="float" elementnames="Kp,Ki,ILimit"/>
 
-	<field name="AcroInsanityFactor" units="percent" type="float" elements="1" defaultvalue="0.5"/>
+	<field name="AcroInsanityFactor" units="percent" type="float" elements="1" defaultvalue="0.5" limits="%BE:0.0:1.0; %BE:0.0:1.0; %BE:0.0:1.0"/>
 
 	<field name="EnablePiroComp" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
 
diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml
index 21790e132..90df0fac4 100644
--- a/shared/uavobjectdefinition/stabilizationsettingsbank1.xml
+++ b/shared/uavobjectdefinition/stabilizationsettingsbank1.xml
@@ -7,6 +7,7 @@
 	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="35" limits="%BE:0:180"/>
 	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="220,220,220" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
+	<field name="StickExpo" units="percent" type="int8" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:-100:100; %BE:-100:100; %BE:-100:100"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0025,0.004,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0025,0.004,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
@@ -15,7 +16,7 @@
 	<field name="PitchPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
 	<field name="YawPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
 
-	<field name="AcroInsanityFactor" units="percent" type="float" elements="1" defaultvalue="0.5"/>
+	<field name="AcroInsanityFactor" units="percent" type="float" elements="1" defaultvalue="0.5" limits="%BE:0.0:1.0; %BE:0.0:1.0; %BE:0.0:1.0"/>
 
 	<field name="EnablePiroComp" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
 
diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml
index cdb90017d..6c84eae04 100644
--- a/shared/uavobjectdefinition/stabilizationsettingsbank2.xml
+++ b/shared/uavobjectdefinition/stabilizationsettingsbank2.xml
@@ -7,6 +7,7 @@
 	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="35" limits="%BE:0:180"/>
 	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="220,220,220" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
+	<field name="StickExpo" units="percent" type="int8" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:-100:100; %BE:-100:100; %BE:-100:100"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0025,0.004,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0025,0.004,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
@@ -15,7 +16,7 @@
 	<field name="PitchPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
 	<field name="YawPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
 
-	<field name="AcroInsanityFactor" units="percent" type="float" elements="1" defaultvalue="0.5"/>
+	<field name="AcroInsanityFactor" units="percent" type="float" elements="1" defaultvalue="0.5" limits="%BE:0.0:1.0; %BE:0.0:1.0; %BE:0.0:1.0"/>
 
 	<field name="EnablePiroComp" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
 
diff --git a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml
index 02cbaec89..8a56ac361 100644
--- a/shared/uavobjectdefinition/stabilizationsettingsbank3.xml
+++ b/shared/uavobjectdefinition/stabilizationsettingsbank3.xml
@@ -7,6 +7,7 @@
 	<field name="YawMax" units="degrees" type="uint8" elements="1" defaultvalue="35" limits="%BE:0:180"/>
 	<field name="ManualRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="220,220,220" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
 	<field name="MaximumRate" units="degrees/sec" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="300,300,300" limits="%BE:0:500; %BE:0:500; %BE:0:500"/>
+	<field name="StickExpo" units="percent" type="int8" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0" limits="%BE:-100:100; %BE:-100:100; %BE:-100:100"/>
 
 	<field name="RollRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0025,0.004,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
 	<field name="PitchRatePID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.0025,0.004,0.00002,0.3" limits="%BE:0:0.01; %BE:0:0.01; ; "/>
@@ -15,7 +16,7 @@
 	<field name="PitchPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
 	<field name="YawPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="2.5,0,50" limits="%BE:0:10; %BE:0:10; "/>
 
-	<field name="AcroInsanityFactor" units="percent" type="float" elements="1" defaultvalue="0.5"/>
+	<field name="AcroInsanityFactor" units="percent" type="float" elements="1" defaultvalue="0.5" limits="%BE:0.0:1.0; %BE:0.0:1.0; %BE:0.0:1.0"/>
 
 	<field name="EnablePiroComp" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>