Control view heading/pitch with (mini) joystick

[buganini]

Finally I put together some code to control view heading/pitch with mini joystick on my CH throttle

The raw value from HW is kind of flickering, so I have to use a filter and quantizer to stablize it, but sometimes it's still not stable enough near zero point, next step would be a non-linear scaling.


Patch

Code: Select all

diff --git a/Nasal/view.nas b/Nasal/view.nas
index 6b4aae7..de9feca 100644
--- a/Nasal/view.nas
+++ b/Nasal/view.nas
@@ -579,7 +580,101 @@ var ViewAxis = {
        },
 };
 
+var kalman = {
+       new: func(value) {
+               var m = { parents: [kalman] };
+               m.value = value;
+               m.p = 10;
+               m.q = 0.01;
+               m.r = 0.05;
+               m.gain = 0;
+               return m;
+       },
+       # filter(raw_value)    -> push new value, returns filtered value
+       filter: func(v) {
+               var p = me.p + me.q;
+               me.gain = p / (p + me.r);
+               v = me.value + (me.gain * (v - me.value));
+               me.p = (1 - me.gain) * p;
+               me.value = v;
+               return v;
+       },
+       # get()                -> returns filtered value
+       get: func {
+               me.value;
+       },
+       # set()                -> sets new value and returns it
+       set: func(v) {
+               me.value = v;
+       },
+};
+
+var quantizer = {
+       new: func(n, min, max) {
+               var m = { parents: [quantizer] };
+               m.n = n;
+               m.min = min;
+               m.max = max;
+               m.range = max - min;
+               return m;
+       },
+       quantize: func(v) {
+               var i = (v - me.min)/me.range;
+               i = math.round(i * me.n) / me.n;
+               return (i * me.range) + me.min;
+       },
+};
+
+var smoothHandler = {
+       new : func(prop) {
+               var m = { parents: [smoothHandler] };
+               m.prop = prop;
+               m.axis = ViewAxis.new(prop);
+               m.filter = kalman.new(0);
+               m.quantizer = quantizer.new(32, -180, 180);
+               m.blend = 0;
+               m.loop_id = 0;
+               m.value = 0;
+               m.dtN = props.globals.getNode("/sim/time/delta-realtime-sec", 1);
+               return m;
+       },
+       target : func(val, time = 0.25) {
+               me.value = val;
+               me.blend = -1;   # range -1 .. 1
+               me._loop_(me.loop_id += 1, time);
+       },
+       _loop_ : func(id, time) {
+               me.loop_id == id or return;
+               var val = me.filter.filter(me.value);
+               val = me.quantizer.quantize(val);
+               #print("target ", me.prop, " to ", val);
+               me.axis.reset();
+               me.axis.target(val);
+
+               me.blend += me.dtN.getValue() / time;
+               if (me.blend > 1)
+                       me.blend = 1;
+
+               var b = (math.sin(me.blend * math.pi / 2) + 1) / 2; # range 0 .. 1
+               me.axis.move(b);
+
+               if (me.blend < 1)
+                       settimer(func { me._loop_(id, time) }, 0);
+       },
+};
+
+var axisHandler = func(axis) {
+       var h = smoothHandler.new(axis);
+    func(invert = 1) {
+        var val = cmdarg().getNode("setting").getValue();
+        val *= 180;
+        if(invert) val = -val;
+        h.target(val);
+    }
+}
 
+var horizontalAxis = axisHandler("/sim/current-view/goal-heading-offset-deg");
+var verticalAxis = axisHandler("/sim/current-view/goal-pitch-offset-deg");


And in CH-PRODUCTS-CH-PRO-THROTTLE-USB-.xml

Code: Select all

  <axis>
    <desc type="string">View Horizontal Axis</desc>
    <binding>
      <command type="string">nasal</command>
      <script type="string">view.horizontalAxis();</script>
    </binding>
  </axis>
  <axis n="1">
    <desc type="string">View Vertical Axis</desc>
    <binding>
      <command type="string">nasal</command>
      <script type="string">view.verticalAxis();</script>
    </binding>
  </axis>


[buganini]

x^3 works good, however the experience is still better with quantizer, but this time I can use smaller step for quantizer.
my raw value doesn't cover -1..1 so I give it a gain of 1.3 .

Code: Select all

diff --git a/Nasal/view.nas b/Nasal/view.nas
index 6b4aae7..458ecfa 100644
--- a/Nasal/view.nas
+++ b/Nasal/view.nas
@@ -579,7 +579,102 @@ var ViewAxis = {
        },
 };
 
+var kalman = {
+       new: func(value) {
+               var m = { parents: [kalman] };
+               m.value = value;
+               m.p = 10;
+               m.q = 0.01;
+               m.r = 0.05;
+               m.gain = 0;
+               return m;
+       },
+       # filter(raw_value)    -> push new value, returns filtered value
+       filter: func(v) {
+               var p = me.p + me.q;
+               me.gain = p / (p + me.r);
+               v = me.value + (me.gain * (v - me.value));
+               me.p = (1 - me.gain) * p;
+               me.value = v;
+               return v;
+       },
+       # get()                -> returns filtered value
+       get: func {
+               me.value;
+       },
+       # set()                -> sets new value and returns it
+       set: func(v) {
+               me.value = v;
+       },
+};
+
+var quantizer = {
+       new: func(n, min, max) {
+               var m = { parents: [quantizer] };
+               m.n = n;
+               m.min = min;
+               m.max = max;
+               m.range = max - min;
+               return m;
+       },
+       quantize: func(v) {
+               var i = (v - me.min)/me.range;
+               i = math.round(i * me.n) / me.n;
+               return (i * me.range) + me.min;
+       },
+};
+
+var smoothHandler = {
+       new : func(prop) {
+               var m = { parents: [smoothHandler] };
+               m.prop = prop;
+               m.axis = ViewAxis.new(prop);
+               m.filter = kalman.new(0);
+               m.quantizer = quantizer.new(64, -180, 180);
+               m.blend = 0;
+               m.loop_id = 0;
+               m.value = 0;
+               m.dtN = props.globals.getNode("/sim/time/delta-realtime-sec", 1);
+               return m;
+       },
+       target : func(val, time = 0.25) {
+               me.value = val;
+               me.blend = -1;   # range -1 .. 1
+               me._loop_(me.loop_id += 1, time);
+       },
+       _loop_ : func(id, time) {
+               me.loop_id == id or return;
+               var val = me.filter.filter(me.value);
+               val = me.quantizer.quantize(val);
+               #print("target ", me.prop, " to ", val);
+               me.axis.reset();
+               me.axis.target(val);
+
+               me.blend += me.dtN.getValue() / time;
+               if (me.blend > 1)
+                       me.blend = 1;
+
+               var b = (math.sin(me.blend * math.pi / 2) + 1) / 2; # range 0 .. 1
+               me.axis.move(b);
+
+               if (me.blend < 1)
+                       settimer(func { me._loop_(id, time) }, 0);
+       },
+};
+
+var axisHandler = func(axis) {
+       var h = smoothHandler.new(axis);
+    func(invert = 1) {
+        var val = cmdarg().getNode("setting").getValue();
+               val = math.pow(val*1.3, 3);
+               val *= 180;
+        if(invert) val = -val;
+               h.target(val);
+    }
+}
 
+var horizontalAxis = axisHandler("/sim/current-view/goal-heading-offset-deg");
+var verticalAxis = axisHandler("/sim/current-view/goal-pitch-offset-deg");


[buganini]

Applying cubic scaling after kalman filter almost eliminates the need of quantizer.

Code: Select all

diff --git a/Nasal/view.nas b/Nasal/view.nas
index 6b4aae7..b522eb6 100644
--- a/Nasal/view.nas
+++ b/Nasal/view.nas
@@ -579,7 +579,102 @@ var ViewAxis = {
        },
 };
 
+var kalman = {
+       new: func(value) {
+               var m = { parents: [kalman] };
+               m.value = value;
+               m.p = 10;
+               m.q = 0.01;
+               m.r = 0.05;
+               m.gain = 0;
+               return m;
+       },
+       # filter(raw_value)    -> push new value, returns filtered value
+       filter: func(v) {
+               var p = me.p + me.q;
+               me.gain = p / (p + me.r);
+               v = me.value + (me.gain * (v - me.value));
+               me.p = (1 - me.gain) * p;
+               me.value = v;
+               return v;
+       },
+       # get()                -> returns filtered value
+       get: func {
+               me.value;
+       },
+       # set()                -> sets new value and returns it
+       set: func(v) {
+               me.value = v;
+       },
+};
+
+var quantizer = {
+       new: func(n, min, max) {
+               var m = { parents: [quantizer] };
+               m.n = n;
+               m.min = min;
+               m.max = max;
+               m.range = max - min;
+               return m;
+       },
+       quantize: func(v) {
+               var i = (v - me.min)/me.range;
+               i = math.round(i * me.n) / me.n;
+               return (i * me.range) + me.min;
+       },
+};
+
+var smoothHandler = {
+       new : func(prop) {
+               var m = { parents: [smoothHandler] };
+               m.prop = prop;
+               m.axis = ViewAxis.new(prop);
+               m.filter = kalman.new(0);
+               m.quantizer = quantizer.new(64, -180, 180);
+               m.blend = 0;
+               m.loop_id = 0;
+               m.value = 0;
+               m.dtN = props.globals.getNode("/sim/time/delta-realtime-sec", 1);
+               return m;
+       },
+       target : func(val, time = 0.25) {
+               me.value = val;
+               me.blend = -1;   # range -1 .. 1
+               me._loop_(me.loop_id += 1, time);
+       },
+       _loop_ : func(id, time) {
+               me.loop_id == id or return;
+               var val = me.filter.filter(me.value);
+               val = math.pow(val*1.3, 3);
+               val *= 180;
+               val = me.quantizer.quantize(val);
+               #print("target ", me.prop, " to ", val);
+               me.axis.reset();
+               me.axis.target(val);
+
+               me.blend += me.dtN.getValue() / time;
+               if (me.blend > 1)
+                       me.blend = 1;
+
+               var b = (math.sin(me.blend * math.pi / 2) + 1) / 2; # range 0 .. 1
+               me.axis.move(b);
+
+               if (me.blend < 1)
+                       settimer(func { me._loop_(id, time) }, 0);
+       },
+};
+
+var axisHandler = func(axis) {
+    var h = smoothHandler.new(axis);
+    func(invert = 1) {
+        var val = cmdarg().getNode("setting").getValue();
+        if(invert) val = -val;
+        h.target(val);
+    }
+}
 
+var horizontalAxis = axisHandler("/sim/current-view/goal-heading-offset-deg");
+var verticalAxis = axisHandler("/sim/current-view/goal-pitch-offset-deg");


[buganini]

And actually the kalman filter can be removed, it was implemented at the beginning, before I figured out how to use view.ViewAxis and loop, which also effect a filter.