forked from seandepagnier/cruisingplot
-
Notifications
You must be signed in to change notification settings - Fork 0
/
filter.scm
57 lines (52 loc) · 2.95 KB
/
filter.scm
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
;; Copyright (C) 2011 Sean D'Epagnier <[email protected]>
;;
;; This Program is free software; you can redistribute it and/or
;; modify it under the terms of the GNU General Public
;; License as published by the Free Software Foundation; either
;; version 3 of the License, or (at your option) any later version.
(declare (unit filter))
(declare (uses utilities options))
(define (create-filter arg)
(let* ((options (create-options
(list (make-number-verifier 'update-period "how often to apply" .1 0 1000)
(make-number-verifier 'frequency "filter frequency" 1 0 1000)
(make-number-verifier 'order "filter order" 1 0 10)
(make-discrete-verifier 'type "type of filter" 'lowpass
'(lowpass highpass derivative integral))
(make-string-verifier 'name "name of filter" "filter"))
(string-append " lowpass gps speed: '-f gps-speed,frequency=.1'\n")
#f))
(filter-string (parse-basic-arg-options-string options arg))
(filter (read-from-string filter-string))
(computation (computations-revaluate filter))
(value (make-list (options 'order) #f)))
(computation-register-unique-name (string->symbol (options 'name))
(string-append "filtered value of: " filter-string)
'() (lambda ()
(case (options 'type)
((lowpass) (last value))
((highpass) (let ((c (computation))) (if c (- (first c) (last value)) #f)))
(else (error "unknown filter type" (options 'type))))))
(verbose "created " (integer->primary-name (options 'order)) " order " (options 'type) " filter '"
(options 'name) "' filtering '" filter-string "' @ "
(/ (options 'update-period)) "hz")
(create-periodic-task
`(filter ,filter)
(options 'update-period)
(lambda ()
(let ((c (computation)))
(set! value
(if c (let ((fc (first c))
(lp (*
(current-task-period) (options 'frequency))))
(let each-order ((value value)
(fc fc))
(if (null? value) '()
(let ((filtered-value
(if (not (first value)) fc
(case (options 'type)
((lowpass highpass) (+ (* lp fc) (* (- 1 lp) (first value))))
(else (error "unknown filter type" (options 'type)))))))
(cons filtered-value
(each-order (cdr value) filtered-value))))))
(cons #f (cdr value)))))))))