Listeners/providers architecture in Tcl (IncrTcl)

This is very popular model for Dataflow (and DSP) applications and no needing to introduce it. So, only listing:

package provide mylsn 1.0
package require Itcl

# ListenEvent
# =====================================================================
itcl::class ListenEvent {
    public variable name
    public variable src
    public variable data
    constructor {aname asrc {adata ""}} {
        set name aname
        set src asrc
        set data adata
    }
}

# DataListener 
# =====================================================================
itcl::class DataListener {
    # list of understandable providers' datatypes. Empty - all
    public variable datatypes ""

    protected variable _event {};  # ListenEvent

    # all listeners
    public common all {}

    constructor {} { lappend all $this }

    destructor {
        set i [lsearch -exact $all $this]
        if {$i >= 0} { set all [lreplace $all $i $i] }
    }

    # FSM state: LISTEN|SESSION|OFF. Means:
    #   LISTEN - ready for data from data provider
    #   SESSION - already reading some data (first data packet was received)
    #   OFF - reading of data is disabled
    protected variable _lstate LISTEN

    # values is list of values
    protected method ondata {values} {}

    # on run polling (listening session). columns is the list of columns names,
    # units is the list of unit names
    protected method onrun {columns units} {}

    # on stop listening session
    protected method onstop {} {}

    # on add to data provider
    protected method onadd {provider} {}

    # on delete from data provider
    protected method ondel {provider} {}

    method event {ev src args} {
        # generate event (call callback) for this listener.
        # ev is ListenEvent object, src is the source of event.
        # ev is one of run|stop|data|on|off|add|del:
        #   run - before first packet sent
        #   stop - after last packet sent
        #   data - packet is received
        #   on - enable listening
        #   off - disable listening
        #   add - connect with some data provider
        #   del - disconnect from some data provider


        set _event [ListenEvent #auto $ev $src $args]
        switch -- $_lstate {
            LISTEN {
                switch -- $ev {
                    run  { set _lstate SESSION
                           catch { $this onrun {*}$args } }
                    off  { set _lstate OFF }
                    add  { catch { $this onadd {*}$args } }
                    del  { catch { $this ondel {*}$args } }
                }
            }

            SESSION {
                switch -- $ev {
                    stop { set _lstate LISTEN
                           catch { $this onstop } }
                    data { catch { $this ondata {*}$args } }
                }
            }

            OFF {
                switch -- $ev {
                    on   { set _lstate LISTEN }
                    add  { catch { $this onadd {*}$args } }
                    del  { catch { $this ondel {*}$args } }
                }
            }
        }
    }

    # listen -on|-off
    method listen {what} {
        # listen -on -- turn-on listening
        # listen -off -- turn-off listening


        # event without src ("" - user call is event source)
        switch -- $what {
            -on     { $this event on "" }
            -off    { $this event off "" }
            default { error  "listen can be -on or -off only" }
        }
        return
    }

    method listened {} {
        # Is listening now?


        return [expr {$_lstate ne "OFF"}]
    }

    # join columns and units with delimiter into new list
    method join_columns_units {columns units {delim ","}} {
        set res {}
        foreach c $columns u $units {
            lappend res "$c$delim$u"
        }
        return $res
    }
}

# DataProvider
# =====================================================================
itcl::class DataProvider {
    # static list of all providers
    public common all {}
    public variable datatype ""

    protected variable _listeners

    constructor {} { lappend all $this }

    destructor {
        set i [lsearch -exact $all $this]
        if {$i >= 0} { set all [lreplace $all $i $i] }
    }

    # returns list (pair) of columns (list) and units - if they are
    # fixed all the time (for any session)
    method fixed_columns_units {} {}

    # normalize name, need bcz user can use quilified name as
    # ::a, not a.
    # FIXME namespace which -variable does not work in Itcl, so
    # i cut all :, but is possible to add/del only listeners on
    # top-level namespace
    protected method _normname {name} {
        return [regsub -all ":" $name ""]
    }

    method get_listeners {} {
        # Returns names of all listeners


        return [array names _listeners]
    }

    method add_listener {listener} {
        # Add some listener


        set lsndts [$listener cget -datatypes]; # datatypes expected by listener
        if {[llength $lsndts] && !($datatype in $lsndts)} {
            # if listener datatypes not empty (expect some) and my datatype
            # is not in it's datatypes, so I can't add this listener
            error "Listener $listener does not understand $this provider"
        }

        set name [_normname $listener]
        if {[itcl::is object -class DataListener $listener]} {
            if {[array get _listeners $name] ne ""} {
                error "Listener $name already exists"
            }
            set _listeners($name) $listener
            $listener event add $this $this
        } else {
            error "listener should be DataListener object"
        }
    }

    method del_listener {listener {stop 1}} {
        # Deletes listener, sends before stop event if needed


        set name [_normname $listener]
        set listener [lindex [array get _listeners $name] 1]
        if {$listener ne ""} {
            if {$stop} { $listener event stop $this }
            array unset _listeners $name
            $listener event del $this $this
        }
        return $listener
    }

    # XXX not effective
    method del_all_listeners {{stop 1}} {
        # Deletes all listeners, send stop event before, if needed


        foreach name [array names _listeners] {
            del_listener $name $stop
        }
    }

    method notify_all {ev args} {
        # Notify all listeners with event ev and some args


        foreach {name listener} [array get _listeners] {
            $listener event $ev $this {*}$args
        }
    }

    method number {} {
        # Returns number of listeners


        return [array size _listeners]
    }

}

# ProxyListener - general object for process events without creating
# a special class
# =====================================================================
itcl::class ProxyListener {
    inherit DataListener DataProvider

    public variable onrunproc ""
    public variable onstopproc ""
    public variable ondataproc ""
    public variable onaddproc ""
    public variable ondelproc ""

    protected method onrun {columns units} {
        if {$onrunproc eq ""} {
            notify_all run $columns $units
        } else {
            $onrunproc $columns $units
        }
    }

    protected method onstop {} {
        if {$onstopproc eq ""} {
            notify_all stop
        } else {
            $onstopproc
        }
    }

    protected method ondata {values} {
        if {$ondataproc eq ""} {
            notify_all data $values
        } else {
            $ondataproc $values
        }
    }

    protected method onadd {provider} {
        if {$onaddproc eq ""} {
            notify_all add $provider
        } else {
            $onaddproc $provider
        }
    }

    protected method ondel {provider} {
        if {$ondelproc eq ""} {
            notify_all del $provider
        } else {
            $ondelproc $provider
        }
    }
}

# DebugListener - repeater
# =====================================================================
itcl::class DebugListener {
    inherit ProxyListener

    public variable fixed_cu ""

    constructor {} {
        $this configure -datatype phys -datatypes {v s flt raw phys} \
            -onrunproc [itcl::code $this onrunproc] \
            -onstopproc [itcl::code $this onstopproc] \
            -ondataproc [itcl::code $this ondataproc] \
    }

    method fixed_columns_units {} {
        return $fixed_cu
    }

    method onrunproc {columns units} {
        notify_all run $columns $units
        puts "*DEBUG_${this}* onrun: $columns, $units"
    }

    method onstopproc {} {
        notify_all stop
        puts "*DEBUG_${this}* onstop"
    }

    method ondataproc {values} {
        notify_all data $values
        puts "*DEBUG_${this}* ondata: $values"
    }
}

# procedures
# =====================================================================

proc debug_listen {d between0 between1} {
    #DebugListener $this.#auto
    $d configure -fixed_cu [$between0 fixed_columns_units]
    listen $between0:$d
    listen $d:$between1
}

proc listen args {
    # Set who listen who:
    #   listen provider...: listener...
    # or
    #   listen prov...: all
    # or
    #   listen -- return list of lists {provider {listeners}}
    # or
    #   listen -txt -- return formatted string (for user)
    # or
    #   listen -p -- returns formatted string with providers and it's datatypes
    # or
    #   listen -l -- returns formatted string with listeners and it's datatypes


    if {[lsearch -exact $args "-txt"] != -1} {
        # if there is "-txt" option, return formatted string
        set res {}
        foreach prov $DataProvider::all {
            set nprov [regsub -all ":" $prov ""]
            lappend res "$nprov: [join [$prov get_listeners] ,\ ]"
        }
        return [join $res "\n"]

    } elseif {[lsearch -exact $args "-p"] != -1} {
        set res {}
        foreach prov $DataProvider::all {
            set nprov [regsub -all ":" $prov ""]
            set dt [$prov cget -datatype]
            lappend res "$nprov: provides '$dt'"
        }
        return [join $res "\n"]

    } elseif {[lsearch -exact $args "-l"] != -1} {
        set res {}
        foreach lsn $DataListener::all {
            set nlsn [regsub -all ":" $lsn ""]
            set dt [join [$lsn cget -datatypes] "','"]
            lappend res "$nlsn: listens '$dt'"
        }
        return [join $res "\n"]

    } elseif {$args eq ""} {
        # if no args, returns listening table (all links)
        set res {}
        foreach prov $DataProvider::all {
            lappend res [list $prov [$prov get_listeners]]
        }
        return $res

    } else {
        # normalize args (make ':' like arg, not part of another arg)
        lassign [split [join $args " "] :] providers listeners

        # if there is 'all' in listeners then it'll be all of them :)
        if {[lsearch -exact $listeners all] != -1} {
            set listeners $DataListener::all
        }

        # delete each listener from ALL known providers then attach
        # to selected
        foreach lsn $listeners {
            foreach prov $DataProvider::all { $prov del_listener $lsn 1 }
            foreach prov $providers { $prov add_listener $lsn }
        }
    }
}

There are classes:

• ListenEvent
• DataListener (should be inherited)
• DataProvider (should be inherited)
• ProxyListener
• DebugListener

First 3 are clear. ProxyListener avoids creation of class - options-callbacks are used instead. Also, it's like bridge: listens and provides. DebugListener is proxy too, but for debug purpose (transparent bridge with console logging). Here is a scheme:

Procedure listen is used to link providers and listeners or to obtain all of providers/listeners in system, or to obtain current link scheme (with native or pretty text representation). debug_listen procedure is used to insert DebugListener between two nodes.

Real time (stream) integration with Simpson's rule

Turn on MathML plugin

About this method of numerical integration you can read here. To do it in streaming manner we should do little formal mathematics:

Composite Simpson's rule:

$\underset{a}{\overset{b}{\int }}f\left(x\right)dx\approx \frac{h}{3}\sum _{k=1,2}^{N-1}\left(f\left(x_{k-1}\right)+4f\left(x_k\right)+f\left(x_{k+1}\right)\right)$

where k=1,2 means from 1 step by 2, N is even intervals number.

The sum will be: $\frac{h}{3}\sum _{k=1,2}^{N-1}{S}_k$

Simpsone's method requires 3 points least so we can integrate not every point (sample) but through one, so neighboring samples are I_N-2 and I_N. Different between them is:

$I_N-I_{N-2}=\frac{h}{3}\left(\sum _{k=1,2}^{N-1}{S}_k-\sum _{k=1,2}^{N-3}{S}_k\right)=\frac{h}{3}\left(S_{N-1}+\sum _{k=1,2}^{N-3}{S}_k-\sum _{k=1,2}^{N-3}{S}_k\right)$

no S_N-2 member bcz of step k=2.

So difference is: $\frac{h}{3}{S}_{N-1}$

For example, $I_4-I_2=\frac{h}{3}\left(y_2+4y_3+y_4\right)$, where I₄ is 4^th interval but 5^th sample.

And resulting formula of I_i=

$0,i\le 1$
$\frac{h}{3}\left(y_{i-2}+4y_{i-1}+y_i\right)+I_{i-1},imod2=0$
$I_{i-1},imod2\ne 0$

where i is a number of input sample. We can not get values of I on each sample, but through one (skipping odds and 0-th, 1-th). On odds samples, value will repeat previous one.

Example in SWIG:
some.h file:

// Simpsone's rule integrator
typedef struct CSIntr {
        double I; // last value of integral
        double y_1; // y[-1]
        double y_2; // y[-2]
        double h; // step of subinterval
        double h_d; // step of subinterval/3
        long long i; // iteration
} CSIntr;

some.i file:

%include "some.h"
%extend CSIntr {
    // FIXME how is more right to do this?
    void reset() {
        $self->I = 0.;
        $self->y_1 = $self->y_2 = 0.;
        $self->i = 0;
    }

    void setup(double h) {
        $self->h = h;
        $self->h_d = h/3.;
    }

    CSIntr(double h) {
        CSIntr *obj;

        if (NULL == (obj=(CSIntr*) malloc(sizeof (CSIntr))))
            return (NULL);
        CSIntr_reset(obj);
        CSIntr_setup(obj, h);
        return (obj);
    }

    ~CSIntr() {
        if ($self) free($self);
    }

    double calculate(double y) {
        if ($self->i <= 1)
            $self->I = 0;
        else if (0 == $self->i % 2)
            $self->I += $self->h_d * ($self->y_2 + 4. * $self->y_1 + y);

        $self->y_2 = $self->y_1;
        $self->y_1 = y;
        $self->i++;
        return ($self->I);
    }
};

So, if we use Tcl, then we can build and test our class:
Makefile's fragment:

some_wrap.c: some.i some.h some.c
 swig -tcl -pkgversion 1.0 some.i

some.dll: some_wrap.c
 gcc -Ic:/tcl/include -I./ -DUSE_TCL_STUBS -c some_wrap.c -o some_wrap.o
 gcc -shared -o some.dll some/some_wrap.o -L c:/tcl/lib -ltclstub85 -lm

test on linear function with y_i = {1, 2, 3, 4, 5}:

load some
% CSIntr cs 1
_f85f7601_p_CSIntr
% cs calculate 1
0.0
% cs calculate 2
0.0
% cs calculate 3
4.0
% cs calculate 4
4.0
% cs calculate 5
12.0

I₄ is really 12.0

Gravity compensation of accelerometer measurements

This solution is only for normal mount of sensor package (see Fig 1)

When we read accelerometer values, we get measurements with Earth gravity. How to remove gravity amount?

Gravity is the vector in direction to center of Earth. Accelerometer values (for Ox, Oy, Oz axis) contains gravity vector's projections (projection, because of tilt of sensor: sensor can have pitch and roll angles not equals to 0!). We need to remove gravity projections to get clear values of acceleration.

Fig 1

 Fig 1 shows normal mount of the sensor and it's axis. First let's remove gravity projection from accelerometer value for Ox axis. When no tilt and roll, pitch angles are 0 - gravity projections on Ox and Oy are 0. But if we have some tilt, then we have also "gravity component" in accelerometer measurement value: Gx. See Fig 2 for Gx definition:

Fig 2
(Oy is "out from eyes")

Projection g_x = g * cos α. Angle p is the pitch angle. α = 180° - 90° - p = 90° - p.

So,




g_x = g * cos (90° - p) = g * sin p.

Ok, let's define gravity component on Oy axis. See Fig 3:

Fig 3
(Ox is "out from eyes")

Projection g_y = g * cos β. Angle r is the roll angle. β = 180° - 90° - r = 90° - r.

So,




g_y = g * cos (90° - r) = g * sin r

Ok, let's define gravity component on vertical, Oz axis.

Projection g_z has some unknown angle γ between Oz and vector of gravity g. And how to define this angle γ ? It's easy: we know the theorem about orths:




cos²p + cos²r + cos²γ = 1


And g_z = g * cos γ. We need to find cos γ. From this theorem: cos²γ = 1 - (cos²p + cos²r).

We know that cos²x = 0.5 * (1 + cos2x) so, cos²γ = 1 - (0.5*(1 + cos2p) + 0.5*(1 + cos2r)) = -0.5 * (cos2r + cos2p).

And now:




cos γ = ± SQRT(-0.5 * (cos2p + cos2r)) = ± 0.70710678118654752440 * SQRT(-(cos2p + cos2r))


So, we will use



g_z = g * 0.70710678118654752440 * SQRT(cos2p + cos2r)
with correction of sign (direction of g_z) by pitch/roll amounts.

These formulas are in g units, not in m/sec²! We suppose |g| = 1. So, you need to multiply by g constant (9.8). But more correct may be to use |g| = sqrt(Acc_x² + Acc_y² + Acc_z²) instead of 1

Here is the example in C:

#define DEG2RAD(D) ((D)*0.01745329251994329576)

typedef struct accv_t {
        double v[3]; // values of 3 axis of accelerometer
} accv_t;

accv_t corr_gravity(double ax, double ay, double az, double pitch, double roll) {
    /* XXX: for normal mount!!! Other not implemented */
    double gx, gy, gz, tmp;

    gx = sin(DEG2RAD(pitch));
    gy = sin(DEG2RAD(roll));
    tmp = cos(DEG2RAD(2*pitch)) + cos(DEG2RAD(2*roll));
    gz = 0.70710678118654752440 * sqrt(fabs(tmp));

    ax += gx;
    ay += gy;
    if (fabs(pitch) > 90.0 || fabs(roll) > 90.0) {
        az -= gz;
    }
    else {
        az += gz;
    }
    return ((accv_t){ .v = {ax, ay, az}});

}

NOTE: condition abs(pitch) > 90° || abs(roll) > 90° => invert g_z.

On real accelerometer values I got:

sensor:
  Ax =  0.287g
  Ay = -0.094g
  Az = -0.928g
corrected:
  Ax = -0.006g
  Ay = 0.006g
  Az = 0.022g

---

See also How to convert accelerometer values to path/coordinates

Low-pass filter

One of the low-pass filters software algorithm is described on http://en.wikipedia.org/wiki/Low-pass_filter. Here is the example of Python implementation:

# Low-pass filter
import random

RC = 0.05 # time constant (inertion)
DT = 0.02 # sampling time

A = DT/( RC + DT)
#A = 0.45
xk_1 = 0
print "Measured;Filtered"
for i in xrange(0, 200):
    xm = random.randint(0, 2)
    # Both are equals:
    #xk = A*xm + (1-A)*xk_1
    xk = xk_1 + A * (xm - xk_1)
    print "%d;%f" % (xm, xk)
    xk_1 = xk

With RC (tau) we can control inertia of the filter

RC=0.5

RC=0.05

Here is the example how looks low-pass filter with tilt angles (F-16 seems to be more stable):

HMC6343 demo 1 from bapcyk on Vimeo.

If you try to filtering tilt angles then you can see funny effect: turning of 3D model! It's happens due to error in sensor data which filter try to smooth, see image:

Near the scale edges error seems to be small, but angles may be, for ex., 10°, 350°, and filter will smooth their to 10°, 20°, 30°... 340°. And you can see rotation of the model on the screen.
One of the approach to avoid this problem is to normalize angles. For example, 10°, 350° may be converted to 10°, -10°. After such normalization angles great then 360° should be normalized to canonical range. Algorithm is easy:

absdiff = abs(v1 - v0)
if absdiff > 180.:
  d = 360. - absdiff
  if v0 > v1:
    v1 = v0 + d
  else:
    v1 = v0 - d

And after filtering to avoid values great then 360° we can use something like this:

if abs(v) >= 360.:
  v = v % 360.