diff --git a/Makefile b/Makefile index c6ea63e..58df68b 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,18 @@ -SOURCES = algebra.scm cruisingplot.scm computation.scm draw.scm frequency.scm glshortcuts.scm sensor.scm gps.scm history.scm net.scm plot.scm sound.scm spherical.scm units.scm weather.scm wind.scm +# Copyright (C) 2010 Sean D'Epagnier +# +# 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. -OBJECTS = $(SOURCES:.scm=.o) +SOURCES = algebra.scm cruisingplot.scm computation.scm draw.scm frequency.scm sensor.scm gps.scm magnetometer.scm history.scm net.scm plot.scm sound.scm spherical.scm units.scm weather.scm wind.scm wifiaimer.scm utilities.scm ahrs.scm options.scm relay.scm config.scm tiltcompensation.scm matrix.scm infix2prefix.scm leastsquares.scm autopilot.scm types.scm filter.scm task.scm + +CSOURCES = geomag70/geomag70.c libgps.c + +CFLAGS = -g +LDFLAGS = -L/usr/local/lib -lgps + +OBJECTS = $(SOURCES:.scm=.o) $(CSOURCES:.c=.o) all: cruisingplot @@ -8,8 +20,8 @@ all: cruisingplot csc -c $< cruisingplot: $(OBJECTS) - csc -o crusingplot $(OBJECTS) + csc -o cruisingplot $(OBJECTS) $(LDFLAGS) clean: - rm -f cruisingplot + rm -f cruisingplot $(OBJECTS) diff --git a/ahrs.scm b/ahrs.scm new file mode 100644 index 0000000..e8575ba --- /dev/null +++ b/ahrs.scm @@ -0,0 +1,196 @@ +;; Copyright (C) 2011 Sean D'Epagnier +;; +;; 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 ahrs)) +(declare (uses sensor options computation infix2prefix)) + +(use srfi-1 environments) + +(define (sensor-9dof-setup device) + (let ((sensor-names '(accelerometer accelerometer accelerometer + gyroscope gyroscope gyroscope + magnetometer magnetometer magnetometer ))) + (let ((sensor-indexes (map sensor-new-index sensor-names))) + (let-values (((i o) (open-serial-device device 38400))) + ; set to output all sensors + (task-create + (lambda () + (let init ((index 0)) + (cond ((< index 8) + (display "4" o) + (flush-output o) + (task-sleep .05) + (init (+ index 1))))) + + (make-line-reader + (lambda () i) + (lambda (line) + (let ((values (string-split line ","))) + (cond ((< (length values) 2) #t) ; startup lines (no comma) and empty ok + ((and (= (length values) 11) + (equal? (list-ref values 0) "$") + (equal? (list-ref values 10) "#")) + (map (lambda (name index value) + (sensor-update `(,name ,index) (string->number value))) + sensor-names sensor-indexes (cdr values))) + (else (very-verbose "malformed line from 9dof: " line)))))))))))) + +; basic ahrs combines accelerometers magnetometers and gyroscopes +; the parameters are lists of indices of which sensors to use, or +; 'all to use all available sensors + +; If the list is null, then no sensors are used, this is allowed, +; but results in loss of features + +(define (default-ahrs-options) + (create-options + (list + (make-indicies-verifier 'accelerometers 'accelerometer) + (make-indicies-verifier 'magnetometers 'magnetometer) + (make-indicies-verifier 'gyroscopes 'gyroscope) + (make-discrete-verifier 'filter "which algorithm to use" 'complementary '(complementary ekf ukf)) + (make-number-verifier 'period "period of updates" .1 .001 1000)) + (string-append + " Create ahrs which updates twice a second, using only even axis accelerometers " + "and odd axis magnetometers, (all available gyroscopes however)\n" + " '--ahrs=accelerometers=(0 2 4 6 8),magnetometers=(1 3 5 7),period=.5'\n" + " Create an ahrs which only uses accelerometers and magnetometers\n" + " '--ahrs=gyroscopes=()'\n") + #f)) + +(define ahrses 0) +(type-register 'ahrs '(pitch roll yaw pitchrate rollrate yawrate vx vy vz ax ay az timestamp deviation)) + +(define (make-ahrs-from-string arg) + (let ((options (default-ahrs-options)) + (lastahrsupdate #f)) + (define time (start-timer)) + (if arg (parse-basic-options-string options arg)) + + ; make ahrs -> ahrs.0 + (if (zero? ahrses) (computation-register 'ahrs (lambda () (computation-calculate 'ahrs.0)))) + + (computation-register (string->symbol (string-append "ahrs." (number->string ahrses))) + (lambda () lastahrsupdate)) + + (verbose "starting ahrs task " ahrses) + (set! ahrses (+ ahrses 1)) + + (start-periodic-task + (options 'period) + (let ((filter (case (options 'type) + ((complementary) (make-complementary-filter options))))) + (lambda () + (let ((gps-speed (computation-calculate 'gps-speed)) + (gyros (map computation-calculate (options 'gyroscopes)))) + 1 + ))) + 'ahrs-update))) + + +;; for over-determined calibration: + +; What I have is: + +; X = x +; Y = a*x + b*y +; Z = c*x + d*y + e*z +; W = f*x + g*y + h*z + +; X Y Z and W are the measurements from the sensors. x, y and z are the +; truth values (unknown) and a-h are calibration coefficients. To +; apply the calibration you need the matrix inverse of the above which +; is fairly obvious. + + +(define quad-axis-measurements '()) +(define (quad-axis-record measurements) + (set! quad-axis-measurements (cons measurements quad-axis-measurements))) + +(define (quad-axis-recalculate) + (print "quad axis state: " + (call/cc (lambda (bail) + (with-exception-handler + (lambda _ (bail #f)) + compute-quad-least-squares))))) + + +; grind(expand(eliminate( +; [X = x + Bx, +; Y = a*x + b*y + By, +; Z = c*x + d*y + e*z, + Bz +; W = f*x + g*y + h*z, + Bw], [x, y, z]))); + +; With some substitution you can get the truth equation: +; +; b*h*Z-d*h*Y+e*g*Y+a*d*h*X-b*c*h*X-a*e*g*X+b*e*f*X-b*e*W+By*d*h-a*Bx*d*h +; +b*Bx*c*h-b*Bz*h-By*e*g+a*Bx*e*g-b*Bx*e*f+b*Bw*e + +; d/da = d*h*X - e*g*X - Bx*d*h + Bx*e*g +; d/db = h*Z - c*h*X + e*f*X - e*W + Bx*c*h - Bz*h - Bx*e*f + Bw*e +; d/dc = b*Bx*h - b*h*X +; d/dd = -h*Y + a*h*X + By*h - a*Bx*h +; d/de = g*Y - a*g*X + b*f*X - b*W - By*g + a*Bx*g - b*Bx*f + b*Bw +; d/df = b*e*X - b*Bx*e +; d/dg = e*Y - a*e*X - By*e + a*Bx*e +; d/dh = b*Z - d*Y + a*d*X - b*c*X + By*d - a*Bx*d + b*Bx*c - b*Bz +; d/dBx = - a*d*h + b*c*h + a*e*g - b*e*f +; d/dBy = d*h - e*g +; d/dBz = -b*h +; d/dBw = b*e + +(define quad-jacobian-partials + (map string-infix->prefix + '("d * h * X - e * g * X - Bx * d * h + Bx * e * g" + "h * Z - c * h * X + e * f * X - e * W + Bx * c * h - Bz * h - Bx * e * f + Bw * e" + "b * Bx * h - b * h * X" + " - h * Y + a * h * X + By * h - a * Bx * h" + "g * Y - a * g * X + b * f * X - b * W - By * g + a * Bx * g - b * Bx * f + b * Bw" + "b * e * X - b * Bx * e" + "e * Y - a * e * X - By * e + a * Bx * e" + "b * Z - d * Y + a * d * X - b * c * X + By * d - a * Bx * d + b * Bx * c - b * Bz" + " - a * d * h + b * c * h + a * e * g - b * e * f" + "d * h - e * g" + " - b * h" + "b * e"))) + +(define quad-residual + (string-infix->prefix "b * h * Z - d * h * Y + e * g * Y + a * d * h * X - b * c * h * X - a * e * g * X + b * e * f * X - b * e * W + By * d * h - a * Bx * d * h + b * Bx * c * h - b * Bz * h - By * e * g + a * Bx * e * g - b * Bx * e * f + b * Bw * e")) + + +(define (build-quad-jacobian-residual-row state measurements) + (let ((env (scheme-report-environment 5))) + (for-each (lambda (symbol value) + (environment-extend! env symbol value)) + '(a b c d e f g h Bx By Bz Bw) + (map first (matrix->list state))) + (environment-extend! env 'X (first measurements)) + (environment-extend! env 'Y (second measurements)) + (environment-extend! env 'Z (third measurements)) + (environment-extend! env 'W (fourth measurements)) + (list + (map (lambda (exp) (eval exp env)) quad-jacobian-partials) + (eval quad-residual env)))) + +(define (quad-complete? state) + (let ((d (matrix-ref (matrix* (matrix-transpose state) state) 0 0))) + (verbose "d: " d) + (< d 1e-1))) + + +(define (quad-least-squares) + (least-squares-iterate (matrix-transpose (matrix '((0 1 -1 1 1 1 1 1 0 0 0 0)))) + build-quad-jacobian-residual-row quad-complete? 10)) + +(computation-register 'force + (string-append "The magtitude of the accelerometers combined force, " + "non-moving yields 1 (for 1 gravity)") + '(accelerometer) + (lambda () + (magnitude (sensor-query 'accelerometer)))) + +;(computation-register 'pitch diff --git a/algebra.scm b/algebra.scm index 55610cc..10d34b4 100644 --- a/algebra.scm +++ b/algebra.scm @@ -5,6 +5,10 @@ ;; License as published by the Free Software Foundation; either ;; version 3 of the License, or (at your option) any later version. +(declare (unit algebra)) + +(declare (uses srfi-1)) + ; some linear algebra functions (define (square x) (* x x)) @@ -26,3 +30,70 @@ (apply + (apply map * vecs))) (define average (lambda args (/ (fold + 0 args) (length args)))) +(define rms (lambda args (sqrt (apply average (map square args))))) + +(define Pi (* 2 (asin 1))) +(define (r2d x) (* (/ 180 Pi) x)) +(define (d2r x) (* (/ Pi 180) x)) + +; from +=- Pi +(define (phase-resolve phase) + (let ((phase (remainder phase (* 2 Pi)))) + (cond ((< phase (- Pi)) (+ phase (* 2 Pi))) + ((> phase Pi) (- phase (* 2 Pi))) + (else phase)))) + +; from 0 to 2Pi +(define (phase-resolve-positive phase) + (let ((phase (remainder phase (* 2 Pi)))) + (+ phase (if (negative? phase) (* 2 Pi) 0)))) + +; from +- 180 +(define (phase-resolve-degrees phase) + (r2d (phase-resolve (d2r phase)))) + +; from 0 to 360 +(define (phase-resolve-positive-degrees phase) + (r2d (phase-resolve-positive (d2r phase)))) + +(define (newtons-method state build-jacobian-residual-row iterations) + (let each-iteration ((state state) + (iteration 0)) + (if (= iteration iterations) state + (let ((HZ (build-jacobian-residual-row state))) + (let ((H (first HZ)) + (Z (second HZ))) + (each-iteration (map (lambda (state diff) + (- state (if (= 0 diff) + (begin (print "reached local minima at " + state " nudge .01") -.01) + (/ Z (abs diff))))) + state H) + (+ iteration 1))))))) + +(define (solve-quadratic-method a b c) + (/ (- (sqrt (- (* b b) (* 4 a c))) b) (* 2 a))) + +; 0 = a*x^2 + b*x + c +(define (solve-quadratic initial-value a b c) + (first (newtons-method `(,initial-value) (lambda (state) (let ((x (first state))) + (list (list (+ (* 2 a x) b)) + (+ (* a x x) (* b x) c)))) + 100))) + +; 0 = a*x^3 + b*x^2 + c*x + d +(define (solve-cubic initial-value a b c d) + (first (newtons-method `(,initial-value) (lambda (state) (let ((x (first state))) + (list (list (+ (* 3 a x) (* 2 b x) c)) + (+ (* a x x x) (* b x x) (* c x) d)))) + 100))) + +(define (integrator pos step derivative) + (* step (derivative pos))) + +(define (runge-kutta-2 pos step derivative) + (* step (/ (+ (derivative pos) + (derivative (+ pos step))) 2))) + +;(define (runge-kutta-4 pos step derivative) + diff --git a/autopilot.scm b/autopilot.scm new file mode 100644 index 0000000..e13b5fc --- /dev/null +++ b/autopilot.scm @@ -0,0 +1,102 @@ +;; Copyright (C) 2011 Sean D'Epagnier +;; +;; 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 autopilot)) + +; The goal is to keep the boat on course by measuring it's motion, +; then estimating what the motion will be in the future with various +; helm changes. The basic forces involved: +; +; Water Helm from Keel and Rudder pushing water +; waterhelm(waterspeed, waterdirection, rudderposition) = +; waterspeed*rudderconstant*rudderposition - keelconstant*heading' +; +; Weather Helm from sails pushing against air +; + weatherhelm(windspeed, winddirection) + current/bias +; +; the forces turning the boat are the total of +; heading'' = waterhelm + weatherhelm +; +;; We can predict the heading for sailboats with integration of the forces +;; turning the boat: +; +; heading(t+1) = heading(t) + heading'(t)*t +; heading'(t+1) = heading'(t) + heading''(t)*t +; +; the yawmoment is the inertia of yawing the vessel, to be predicte +; +; we will try a simple quadratic and estimate weatherhelm +; when sail trim, wind speed, or direction change, these values change +; weatherhelm(windspeed, winddirection) = a*windspeed^2 + b*winddirection + c +; +; the rudder constant varies from vessel to vessel +; rudderhelm(headingrate, rudderposition, waterspeed) = rudderconstant*waterspeed*rudderposition +; could add optional term: kappa*rudderspeed to account for lateral water movement from helm change +; +; since we only command the rudderspeed on or off, we can integrate to get position +; the position can saturate, so we can apply that as well +; rudderposition = rudderspeed*time, saturate(rudderposition, +maxrudder, -maxrudder) +; +; to command autopilot, simply calculate the predicted heading at T in the future +; where T is the minimum autopilot movement with rudder speed set to +- and 0, and determine +; which result has the closest heading to the desired heading and is not yawing. + +; to determine which move to make, apply critical to underdampening depending +; on how hard we want to work the autopilot + + +; m*f''(x) + c*f'(x) + k*x = 0 + + +; for roll pitch and heave we can model like a pendulum: +; M(t) - Mx = Ia +; +; where M(t) is the external healing moment, Mx is the resisting moment (bouancy and ballast) +; I is the moment of intertia, and a is the angulat acceleration + +; + +(define (create-autopilot arg) + (define options + (create-options + (append `(,(make-number-verifier 'gain "gain in feedback" 1 0 10) + ,(make-number-verifier 'gain "max turn speed (deg/s)" 10 0 100) + ,(make-discrete-verifier 'filter "what autopilot to use" 'basic '(basic derivative)) + ,(make-sensor-indicies-verifier "index of which ahrs to use" 'ahrs 'ahrs) + ,(make-number-verifier 'update-rate "how often to command motor in hz" 2 .1 100)) + ,(make-number-verifier 'wind-angle "apparent wind angle to hold" 0 -180 180)) + (motor-options))) + + (start-periodic-task + (/ (options 'update-rate)) + (case (options 'filter) + ((basic) (autopilot-basic options)) + ((derivative (autopilot-derivative options))) + (else (error "unrecognized autopilot filter: " (options 'filter)))))) + + + + +;; basic autopilot feedback loop +; error = wind direction - desired wind direction +; command = error * gain / rate +(define (basic-autopilot options) + (let ((error (- (computation-calculate 'wind-direction) (options 'wind-angle)))) + (motor-command (/ (* error (options 'gain)) (options 'update-rate))))) + +;; derivative autopilot uses the rate of change of heading (gyro) to do a better +; job of heading correction +(define (derivative-autopilot options) + (let ((error (- (computation-calculate 'wind-direction) (options 'wind-angle))) + (ahrs (computation-calculate 'ahrs))) + (let ((yaw-rate (type-field-get 'ahrs 'yawrate ahrs)) + (desired-yaw-rate (saturate (* (options 'gain) error) + (- (options 'max-turn-rate)) (options 'max-turn-rate)))) + (let ((rate-error (- yaw-rate desired-yaw-rate))) + (motor-command (/ (+ (* rate-error motor-constant)) + (/ (options 'update-rate)))))))) + diff --git a/complementaryfilter.scm b/complementaryfilter.scm new file mode 100644 index 0000000..e4b3ba6 --- /dev/null +++ b/complementaryfilter.scm @@ -0,0 +1,22 @@ +;; Copyright (C) 2010 Sean D'Epagnier +;; +;; 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. + +;; The complementary filter is simpler and easier to tune than kalman +;; filter for attitude estimation. Also less computation + +; theta'(t) = a*gyro + b +; theta(0) = 0 +; theta(t+1) = theta(t) + theta'(t) +; +; we are trying to find +(define (gyroscope-bias) + +(define (complementary-filter a b + +(define (make-complementary-filter ) + (let ((attitude #f)) + (lambda () diff --git a/computation.scm b/computation.scm index d23390f..97fb47b 100644 --- a/computation.scm +++ b/computation.scm @@ -8,6 +8,9 @@ ; various values can be computed from the raw sensors ; these values may not be computed all the time +(declare (unit computation)) +(declare (uses utilities)) + (use srfi-1 srfi-69 environments) (define computations (make-hash-table)) @@ -16,41 +19,95 @@ ; sensors are a list of the sensors needed for this calculation ; to succeed ; calculation is a thunk that returns the computed value -(define (computation-register name info calculation) +(define (computation-register name info sensors calculation) + (if (hash-table-exists? computations name) + (warning "re-registering computation: " name) + (verbose "registered computation: " name " (" info ")")) (hash-table-set! computations name - (list info calculation))) + (list info sensors calculation))) + +; give the name which should return the computation for name.0 +(define (computation-register-first-alias name sensors) + (let*((zsname (string-append (symbol->string name) ".0")) + (zname (string->symbol zsname))) + (cond ((hash-table-exists? computations name) ; already without index? move it to first index + (verbose "moving computation: " name " -> " zsname) + (hash-table-set! computations zname (hash-table-ref computations name)) + (hash-table-delete! computations name))) + (computation-register name (string-append "alias for " zsname) sensors + (lambda () (computation-calculate zname))))) ; print the computation info for all the registered computations (define (computation-info) (hash-table-walk computations (lambda (name data) - (print name " -- " (first data))))) + (if (apply sensor-contains? (second data)) + (print name " -- " (first data)))))) ; shorthand way of reaching computations -; certain computations may be buffered within a certain period to (define (computation-calculate name) (if (hash-table-exists? computations name) (let ((data (hash-table-ref computations name))) - ((second data))) - (error "Cannot perform unregistered computation" name))) + (if (apply sensor-contains? (second data)) + ((third data)) #f)) + (error "Cannot perform unregistered computation " name))) ; return a thunk which evaluates a given expression which may use ; symbols which are registered computations which are evaluated and substituted +; if the return is false, then the expression could not be computed (define (computations-revaluate . exps) - (let ((symbols (delete-duplicates - (let each-expr ((exps exps)) - (cond ((and (symbol? exps) - (hash-table-exists? computations exps)) - (list exps)) - ((and (not (null? exps)) (list? exps)) - (append (each-expr (car exps)) (each-expr (cdr exps)))) - (else '())))))) - (lambda () - (let ((env (scheme-report-environment 5))) - (for-each (lambda (symbol) - (environment-extend! env symbol (computation-calculate symbol))) - symbols) - (map (lambda (exp) (eval exp env)) exps))))) + (let ((symbols '())) + (define (symbols-recompute) + (set! symbols + (delete-duplicates + (let each-expr ((exps exps)) + (cond ((and (symbol? exps) + (hash-table-exists? computations exps)) + (list exps)) + ((and (not (null? exps)) (list? exps)) + (append (each-expr (car exps)) (each-expr (cdr exps)))) + (else '())))))) + (define (evaluate) + (let ((env (scheme-report-environment 5))) + (call/cc (lambda (cont) + (for-each (lambda (symbol) + (let ((value (computation-calculate symbol))) + (if (not value) (cont #f)) + (environment-extend! env symbol value))) + symbols) + (map (lambda (exp) (eval exp env)) exps))))) + (lambda () + (call/cc (lambda (bail) + (with-exception-handler + (lambda _ + (symbols-recompute) + (bail + (call/cc (lambda (bail) + (with-exception-handler + (lambda _ (bail #f)) + evaluate))))) + evaluate)))))) + +(define (computation-unique-index name) + (if (not (hash-table-exists? computations name)) + 0 + (let each-index ((index 1)) + (let ((sym (string->symbol (string-append (symbol->string name) "." (number->string index))))) + (if (hash-table-exists? computations sym) + (each-index (+ index 1)) + index))))) + +(define (computation-indexed-name name index) + (string->symbol (string-append (symbol->string name) "." (number->string index)))) + +(define (computation-register-unique-name name info sensors calculation) + (let ((index (computation-unique-index name))) + (print "unique index: " index) + (cond ((zero? index) (computation-register name info sensors calculation)) + (else + (if (= 1 index) (computation-register-first-alias name sensors)) + (computation-register (computation-indexed-name name index) info sensors calculation))))) ; basic computations -(computation-register 'time "time in seconds since start" current-time) +(computation-register 'time "time in seconds since start" '() elapsed-seconds) + diff --git a/config.scm b/config.scm new file mode 100644 index 0000000..82b72a2 --- /dev/null +++ b/config.scm @@ -0,0 +1,102 @@ +;; Copyright (C) 2010 Sean D'Epagnier +;; +;; 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 config)) + +(use args glut) + +(define config-file #f) + +(define (usage) + (print "Usage: " (car (argv)) " [options...]") + (newline) + (print (args:usage opts)) + (print "Report bugs to sean at depagnier dot com") + (exit)) + +(define output-file #f) + +(define opts + (list + (args:make-option (a ahrs) (optional: "OPTIONS") "Create an ahrs which runs kalman filter algorithms. Invoke -a help for more information." + (make-ahrs-from-string arg)) + (args:make-option (C config) (required: "FILENAME") "include a config file which contentss specifies command line options" + (with-input-from-file arg + (lambda () + (let loop () + (let ((line (read-line))) + (if (not (eof-object? line)) + (let ((space (string-index line #\space))) + (if space + (args:parse + (list (string-append "-" (string-take line space)) + (string-drop line (+ space 1))) + opts) + (error "invalid configuration line" line)) + (loop)))))))) + + (args:make-option (c client) (required: "HOST") "Connect to a remote host to obtain sensor data" + (sensor-net-client arg)) + (args:make-option (d debug) (required: "EXPRESSION1[,EXPESSION2...]") + "Perform the given computation at 1hz and print to stdout, help for info" + (debug arg)) + (args:make-option (f filter) (required: "EXPRESSION1[,options]") + "Filter this computation, help for info" + (create-filter arg)) + (args:make-option (g gps) (optional: "DEVICE") "device or url for gps data, if none is specified, then the default gpsd location (localhost:2947) is tried." + (gps-setup arg)) + (args:make-option (h help) #:none "Display this text" (usage)) + (args:make-option (i input) (required: "FILENAME,options") + "Use a file with sensor data as input (replay), -i help for info" + (sensor-replay-logfile arg)) + (args:make-option (m magnetometer) (required: "DEVICE") "device or url for mag data." + (magnetometer-setup arg)) + (args:make-option (o output) (required: "FILENAME") + "Write all input sensor data to a log file log for future replay" + (verbose "adding output log file: " (if (equal? arg "-") "" arg)) + (if (equal? arg "-") + (sensor-log-to-port (current-output-port)) + (sensor-log-to-file arg))) + (args:make-option (p plot) (required: "PLOT") "Plot computations. Invoke with -p help for more information." + (set! setup (lambda () + (sleep 1) + (plots-setup plots))) + (set! plots (append plots `(,(lambda () (create-plot-from-string arg)))))) + (args:make-option (s server) (optional: "PORT") + (string-append "Run a server listening on the specified port or " + (number->string net-default-port) + " by default") + (net-server arg)) + (args:make-option (v verbose) #:none "Print debugging info, -vv for extra info" + (cond ((eq? verbose nice-print) + (verbose "Extra Debugging output " + (if (eq? very-verbose verbose) "already " "") + "enabled") + (set! very-verbose verbose)) + (else + (set! verbose nice-print) + (verbose "Debugging output enabled")))) + (args:make-option (w weather) (required: "DEVICE") "device or url for weather data" + (weather-setup arg)) + + (args:make-option (2 gps-plot) (optional: "ARGS") "draw gps plot display." + (set! plots (append plots `(,(lambda () (create-gps-plot-from-string arg)))))) + (args:make-option (3 relay) (required: "RELAY") "Device to use for relay control" + (relay-setup arg)) + (args:make-option (4 wifiaimer) (required: "options") + "Device to control servo to aim antenna, -wifiaimer help for more info" + (create-wifi-aimer arg)) + (args:make-option (6 tiltcompensate) #:none "Enable tilt-compensation routines" + (tilt-compensation-setup)) + (args:make-option (9 9DOF) (required: "DEVICE") "device to use for sparkfun 9DOF" + (sensor-9dof-setup arg)) + )) + +(define (config-setup) + (very-verbose "Configuration setup") + (args:parse (command-line-arguments) opts) + (verbose "Configuration complete.")) diff --git a/cruisingplot.scm b/cruisingplot.scm index 89f306f..17d0c96 100644 --- a/cruisingplot.scm +++ b/cruisingplot.scm @@ -9,91 +9,15 @@ ;; ;; chicken-setup opengl glut syntax-case pos args -(use srfi-1 srfi-4 posix srfi-18 srfi-12 srfi-13) +(use srfi-1 srfi-4 posix srfi-18 srfi-12 srfi-13 srfi-69 glut args) -(define (read-from-string string) (with-input-from-string string read)) +(declare (uses net plot gps weather wind magnetometer computation utilities leastsquares ahrs config relay tiltcompensation wifiaimer filter task)) -(define (die . args) - (apply print "Error: " args) - (exit 1)) +(define (setup) (lambda () #t)) -(define (warning . args) - (apply print "Warning: " args)) +(define (help) + (print "Crusing plot.. no help available, ")) -(define warning-once - (let ((table (make-hash-table))) - (lambda args - (cond ((not (hash-table-exists? table args)) - (hash-table-set! table args #t) - (apply warning args)))))) - -(define replayrate 1) - -(define current-time - (let ((start (current-milliseconds))) - (lambda () (* replayrate (/ (- (current-milliseconds) start) 1000))))) - -(use args) - -(define (usage) - (print "Usage: " (car (argv)) " [options...]") - (newline) - (print (args:usage opts)) - (print "Report bugs to geckosenator at gmail.") - (exit 1)) - -(define output-file #f) -(define noshow #f) - -(define verbose (lambda _ #t)) - -(define (push-exit-handler handler) - (exit-handler (let ((old-handler (exit-handler))) (lambda () (handler) (old-handler))))) - -(define plots '()) - -(define opts - (list - (args:make-option (c client) (required: "HOST") "Connect to a remote host to obtain data" - (net-add-client arg)) - (args:make-option (g gps) (required: "DEVICE") "device or url for gps data, if none is specified, then the default gpsd location (localhost:2947) is tried." - (gps-setup arg)) - (args:make-option (h help) #:none "Display this text" (usage)) - (args:make-option (i input) (required: "FILENAME") "Use a file log for replay" - (verbose "replaying data from log file '" arg "' at " replayrate "x speed") - (sensor-replay-logfile arg replayrate)) - (args:make-option (m magnetometer) (required: "DEVICE") "device or url for mag data." - (magnetometer-setup arg)) - (args:make-option (o output) (required: "FILENAME") - "Write all input sensor data to a log file log for future replay" - (verbose "setting output log file to " (if (eq? arg "-") "" arg)) - (set! sensor-log-port - (if (eq? arg "-") - (current-output-port) - (open-output-file arg)))) - (args:make-option (r replayrate) (required: "RATE") "Rate to replay the log (0 for infinite)" - (set! replayrate (string->number arg))) - (args:make-option (p plot) (required: "PLOT") "Plot setup invoke with -p help for info" - (set! plots (append plots (list arg)))) - (args:make-option (settings) (required: "FILENAME") "Read and write runtime settings from file" - (settings-read arg) - (push-exit-handler (lambda () (settings-write arg)))) - (args:make-option (s server) (optional: "PORT") - (string-append "Run a server listening on the specified port or " - (number->string net-default-port) - " by default") - (net-server arg)) - (args:make-option (v verbose) #:none "Print debugging info" - (set! verbose print)) - (args:make-option (w weather) (required: "DEVICE") "device or url for weather data" - (weather-setup arg)) - )) - -(args:parse (command-line-arguments) opts) - -(if (null? plots) (let x () (thread-sleep! 1) (x))) - -(glut:InitDisplayMode (+ glut:DOUBLE glut:RGB glut:ALPHA)) -(plots-setup plots) -(glut:IdleFunc (lambda () (thread-sleep! .01))) -(glut:MainLoop) +(config-setup) +(setup) +(task-schedule-loop) diff --git a/draw.scm b/draw.scm index 28775a5..4dad8c5 100644 --- a/draw.scm +++ b/draw.scm @@ -5,6 +5,8 @@ ;; License as published by the Free Software Foundation; either ;; version 3 of the License, or (at your option) any later version. +(declare (unit draw)) + (define (draw-vector-arrow a b) (let ((m (map average a b)) (n `(,(* .15 (- (cadr a) (cadr b))) diff --git a/filter.scm b/filter.scm new file mode 100644 index 0000000..6e4362a --- /dev/null +++ b/filter.scm @@ -0,0 +1,51 @@ +;; Copyright (C) 2011 Sean D'Epagnier +;; +;; 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 () (last value))) + (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)) + (fc (if c (first c) #f)) + (lp ((if (eq? (options 'type) 'lowpass) * /) + (current-task-period) (options 'frequency)))) + (set! value + (let each-order ((value value) + (fc fc)) + (if (null? value) '() + (let ((filtered-value + (if (not (first value)) fc + (case (options 'type) + ((lowpass) (+ (* lp fc) (* (- 1 lp) (first value)))) + (else (error "unknown filter type" (options 'type))))))) + (cons filtered-value + (each-order (cdr value) filtered-value)))))))) + ))) diff --git a/frequency.scm b/frequency.scm index 89927f1..885641c 100644 --- a/frequency.scm +++ b/frequency.scm @@ -5,6 +5,8 @@ ;; License as published by the Free Software Foundation; either ;; version 3 of the License, or (at your option) any later version. +(declare (unit frequency)) + (use srfi-1) ; convert data into to the frequency domain diff --git a/glshortcuts.scm b/glshortcuts.scm index 1484b0d..be4606d 100644 --- a/glshortcuts.scm +++ b/glshortcuts.scm @@ -5,7 +5,7 @@ ;; License as published by the Free Software Foundation; either ;; version 3 of the License, or (at your option) any later version. -(use gl) + ; make syntax for various opengl features to make them nicer (define-syntax glBegin (syntax-rules () diff --git a/gps.scm b/gps.scm index f6d7165..531c970 100644 --- a/gps.scm +++ b/gps.scm @@ -5,35 +5,84 @@ ;; License as published by the Free Software Foundation; either ;; version 3 of the License, or (at your option) any later version. -;; This file handles reading data from a gps, either raw or through gpsd +(declare (unit gps)) +(declare (uses sensor units types utilities)) + +(use easyffi gl glu glut) + +(include "glshortcuts.scm") + +;; This file handles reading data from a gps through libgps + +(foreign-parse " +extern int gps_read(struct gps_data_t *gpsdata); +extern bool gps_waiting(struct gps_data_t *gpsdata); +extern int gps_stream(struct gps_data_t *gpsdata, + unsigned int flags, + /*@null@*/void *); + +extern struct gps_data_t *libgps_open(const char *host, const char *port); +extern int libgps_close(struct gps_data_t *); +extern void libgps_read(struct gps_data_t *data, double results[6]); +") + +(define DEFAULT_GPSD_PORT "2947") +(define WATCH_ENABLE 1) (type-register 'gps '(time latitude longitude altitude heading speed)) - ;; open the gps device, or connect to the gpsd server -(define (gps-setup device) - (call/cc (lambda (return) - (with-exception-handler - (lambda _ - (print "GPS initialization failed on: " device ": " _) - (return #f)) - (lambda () - (let* ((split (string-split device ":")) - (port (if (null? (cdr split)) - 2947 - (string->number (cadr split))))) - (verbose "Trying to connect to gpsd server at " (car split) - " port " port) - (let-values ([(i o) (tcp-connect (car split) port) ]) - (verbose "Success connecting to gps on " (car split)) - (let ((index (sensor-index-count 'gps))) - (verbose "reporting as gps " index) - (write "w\n" o) ; put in query mode - (make-line-reader i - (lambda (line) - (let ((report (parse-gpsd-line line))) - (if report - (sensor-update `(gps ,index) report)))))) - (return #t)))))))) +(define (register-gps-computations) + (computation-register-first-alias 'gps '(gps)) + + (computation-register 'gps-latitude "latitude in degrees from gps" '(gps) + (lambda () (sensor-field-get 'gps 'latitude))) + + (computation-register 'gps-longitude "longitude in degrees from gps" '(gps) + (lambda () (sensor-field-get 'gps 'longitude))) + + (computation-register 'gps-altitude "altitude in meters from gps" '(gps) + (lambda () (sensor-field-get 'gps 'altitude))) + + (computation-register 'gps-speed "speed in knots from gps" '(gps) + (lambda () (m/s->knots (sensor-field-get 'gps 'speed)))) + + (computation-register 'gps-heading "heading in degrees from gps" '(gps) + (lambda () (sensor-field-get 'gps 'heading))) + + (computation-register 'gps-time "gps timestamp" '(gps) + (lambda () (sensor-field-get 'gps 'time)))) + + + ;; open the gps host, or connect to the gpsd server +(define (gps-setup host) + (let* ((split (string-split (if host host "localhost") ":")) + (port (if (null? (cdr split)) + DEFAULT_GPSD_PORT + (cadr split)))) + (verbose "Trying to connect to gpsd server at " + (car split) " port " port) + (let ((data (libgps_open (car split) port))) + (if (not data) + (error "GPS initialization failed on " (string-append (car split) ":" port)) + (let ((index (sensor-new-index 'gps))) + (verbose "Success connecting to gps on " + (car split) " reporting as gps " index) + (if (zero? index) (register-gps-computations)) + + (gps_stream data WATCH_ENABLE #f) + (push-exit-handler (lambda () (verbose "Closing gps " index) (libgps_close data))) + + (create-periodic-task + (string-append "gps " (number->string index) " task") .1 + (lambda () + (cond ((gps_waiting data) + (gps_read data) + (let ((results (make-f64vector 6 0))) + (libgps_read data results) + (if (not (nan? (second (f64vector->list results)))) + (sensor-update `(gps ,index) (f64vector->list results)))))) + + ))))))) (define (parse-gpsd-line line) (define (parse-gpsd-report line) @@ -75,7 +124,7 @@ (+ (floor (/ val 100)) (/ (remainder val 100) 60))) (if (< fix 1) ; hacked to turn on without DGPS - (print "warning, only normal GPS not DGPS fix, discarding") + (warning "only normal GPS not DGPS fix, discarding") (begin (if (equal? "S" ns) (set! north (- north))) @@ -83,24 +132,110 @@ (set! east (- east))) (list time north east alt hd 0)))))))))) -(computation-register 'gps-speed "speed in knots from gps" - (lambda () (m/s->knots (sensor-field-get 'gps 'speed)))) - -(computation-register 'gps-heading "heading in degrees from gps" - (lambda () (sensor-field-get 'gps 'heading))) - -(computation-register 'gps-time "gps timestamp" - (lambda () (sensor-field-get 'gps 'time))) +;; now hack the geomag c program in place +(foreign-parse "extern int geomag70(char *modelfilename, + double latitude, double longitude, double alt, + int day, int month, double year, + double results[14]);"); ;; Routines for International Geomagnetic Reference Field ; provide magnetic inclination, declination, and field strength given gps input -(define (compute-IGRF date lattitude longitude altitude) - 1 -) - -(computation-register 'magnetic-inclination "The inclination of the magtitude of the field, calculated from gps position, or with no gps, measured from angle of magnetometer and accelerometer" - (lambda () 0)) -(computation-register 'magnetic-declination "The declination of the magnetic field, calculated from gps position, and date" - (lambda () 0)) -(computation-register 'field-strength "The declination of the magnetic field, calculated from gps position, and date" - (lambda () 0)) +; +; results gives DIHXYZF and respective rate of change for each in nT + +(define (compute-gps-IGRF) + (let ((results (make-f64vector 14 0)) + (date (current-date))) + (if (= -1 (geomag70 + "geomag70/IGRF11.COF" + (sensor-field-get 'gps 'latitude) + (sensor-field-get 'gps 'longitude) + (sensor-field-get 'gps 'altitude) + (date-day date) (date-month date) (date-year date) + results)) + (warning "geomag70 failed")) + results)) + +(computation-register 'magnetic-declination "The declination of the magnetic field, calculated from gps position, and date" '(gps) + (lambda () (f64vector-ref (compute-gps-IGRF) 0))) + +(computation-register 'magnetic-inclination "The inclination of the magtitude of the field, calculated from gps position, or with no gps, measured from angle of magnetometer and accelerometer" '(gps) + (lambda () (f64vector-ref (compute-gps-IGRF) 1))) + +(computation-register 'magnetic-fieldstrength "The declination of the magnetic field, calculated from gps position, and date" '(gps) + (lambda () (f64vector-ref (compute-gps-IGRF) 6))) + +(define gps-plot #f) + +(define (create-gps-plot-from-string arg) + (glut:ReshapeFunc + (lambda (w h) + (gl:Viewport 0 0 w h) + ; setup projection matrix + (gl:MatrixMode gl:PROJECTION) + (gl:LoadIdentity))) + + (glut:DisplayFunc + (let ((lastspd 0) (lasthdg 0)) + (lambda () + (define (draw-print . args) + (glLetMatrix + (let each-char ((l (string->list (with-output-to-string (lambda () (for-each display args)))))) + (cond ((not (null? l)) + (glut:StrokeCharacter glut:STROKE_MONO_ROMAN (car l)) + (each-char (cdr l))))))) + + (gl:ClearColor 0 0 0 0) + (gl:Clear gl:COLOR_BUFFER_BIT) + + (glColor 1 1 1) + (glLetMatrix + (gl:LineWidth 4) + (gl:Translated -1 .9 0) + (gl:Scaled .001 .001 .001) + + (let ((spd (computation-calculate 'gps-speed)) + (hdg (computation-calculate 'gps-heading))) + (draw-print "Speed: " (round-to-places spd 2) " knts") + (gl:Translated 0 -200 0) + (draw-print "Accel: " (round-to-places (- spd lastspd) 2)) + (set! lastspd spd) + + (gl:Translated 0 -200 0) + (draw-print "Heading: " (round-to-places hdg 2)) + (gl:Translated 0 -200 0) + (draw-print "Turn Rt: " (round-to-places (- hdg lasthdg) 2)) + (set! lasthdg hdg))) + + (glut:SwapBuffers) + (glut:TimerFunc 1000 glut:PostWindowRedisplay (glut:GetWindow))))) + + (glut:KeyboardFunc + (lambda (key x y) + (case key + ((#\esc #\q) (exit)) + ((#\f) (glut:FullScreen))) + (glut:PostRedisplay))) + + (glut:SpecialFunc + (lambda (key x y) + (if (glut-HasModifiers glut:ACTIVE_SHIFT) + (let ((rs 1)) + (cond + ((= key glut:KEY_LEFT) (RotateAfter rs 0 1 0)) + ((= key glut:KEY_RIGHT) (RotateAfter rs 0 -1 0)) + ((= key glut:KEY_UP) (RotateAfter rs 1 0 0)) + ((= key glut:KEY_DOWN) (RotateAfter rs -1 0 0)) + ((= key glut:KEY_PAGE_UP) (RotateAfter rs 0 0 1)) + ((= key glut:KEY_PAGE_DOWN) (RotateAfter rs 0 0 -1)))) + (let ((ts 1)) + (cond + ((= key glut:KEY_LEFT) (gl:Translatef ts 0 0)) + ((= key glut:KEY_RIGHT) (gl:Translatef (- ts) 0 0)) + ((= key glut:KEY_UP) (gl:Translatef 0 (- ts) 0)) + ((= key glut:KEY_DOWN) (gl:Translatef 0 ts 0)) + ((= key glut:KEY_PAGE_UP) (set-zoom .5)) + ((= key glut:KEY_PAGE_DOWN) (set-zoom 2)) + ))) + (glut:PostRedisplay)))) + diff --git a/history.scm b/history.scm index 295ec7f..9ec8c32 100644 --- a/history.scm +++ b/history.scm @@ -5,6 +5,8 @@ ;; License as published by the Free Software Foundation; either ;; version 3 of the License, or (at your option) any later version. +(declare (unit history)) + ; historys store a list of lists of numbers of data ; are procedures that take a symbol for the operation: ; dump - return a list of history pairs ((T1 value1 .. valuem) ... (Tn value1 ... valuem)) @@ -32,8 +34,11 @@ (cons (car history) (each-history (cdr history))))))) ; recalculate min and max (let ((history-values (map second history))) - (set! minvalues (apply map min history-values)) - (set! maxvalues (apply map max history-values)))) + (cond ((not (null? history-values)) + (set! minvalues (fold (lambda args (apply map min args)) + (car history-values) (cdr history-values))) + (set! maxvalues (fold (lambda args (apply map max args)) + (car history-values) (cdr history-values))))))) ((apply-clippings) (let ((mins (first args)) (maxs (second args))) diff --git a/infix2prefix.scm b/infix2prefix.scm new file mode 100644 index 0000000..dd5fd7e --- /dev/null +++ b/infix2prefix.scm @@ -0,0 +1,122 @@ +;; +;; Copyright (C) 2007 Sean D'Epagnier All Rights Reserved. +;; +;; Meteor is free software; you can redistribute it and/or +;; modify it under the terms of the GNU Library General Public +;; License as published by the Free Software Foundation; either +;; version 2 of the License, or (at your option) any later version. +;; +;; Meteor is distributed in the hope that it will be useful, +;; but WITHOUT ANY WARRANTY; without even the implied warranty of +;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +;; GNU General Public License for more details. +;; +;; You should have received a copy of the GNU Library General Public +;; License along with this library; if not, write to the Free +;; Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +;; + +(declare (unit infix2prefix)) + +;; Convert infix strings into prefix expressions +;; Only + - * / operators supported + +;; eg: (string-infix->prefix "1*(2+3)*4") +;; Value: (* 1 (+ 2 3) 4) + +;; eg: (infix->prefix '(1 + 2 + 3)) +;; Value: (+ 1 2 3) + +;; Has a repl loop as an infix calculator: +;; #;9> (calc) +;; > 1+2*3+4 +;; = 11 +;; > quit + +;; right now there is a hack to support "-1+1", it would be (+ (- 0 1) 1) because +;; term-optimizer.scm does not support (- 1) + +(define (operator? exp) + (let test ((ops '(+ - * /))) + (cond ((null? ops) #f) + ((equal? exp (car ops)) #t) + (else (test (cdr ops)))))) + +; need - above + because of how I deal with unary - operator +(define (op-order op) + (case op ((+) 0) ((-) 1) ((* /) 2))) + +(define (op-order< x y) + (< (op-order x) (op-order y))) + +(define (infix->prefix exp) + (define (scan-op op args exp) +; (print "scan-op " op " " args " " exp) + (if (null? exp) + (cons (cons op args) '()) + (let ((x (car exp))) + (if (operator? x) + (if op + (cond ((equal? x op) (scan-exp op args (cdr exp))) + ((op-order< x op) + (scan-exp x (list (cons op args)) (cdr exp))) + ((op-order< op x) + (let ((end-arg (car args)) + (body-args (cdr args))) + (let ((val (scan-exp x (list end-arg) (cdr exp)))) + (scan-op op (append (if (equal? (caar val) op) + (cdar val) + (list (car val))) + body-args ) + + (cdr val))))) + (else (scan-exp x (list (cons op args)) (cdr exp)))) + (scan-exp x args (cdr exp))) + (error "Expected operator"))))) + (define (scan-exp op args exp) +; (print "scan-exp " op " " args " " exp) + (if (and (eq? op '-) (null? exp)) + (list (cons '- (if (= (length args) 1) args (list (cons '+ args))))) + (let ((x (car exp))) + (cond ((pair? x) (scan-op op (append args (list (infix->prefix x))) (cdr exp))) + ((operator? x) (error "Unexpected operator")) + (else (scan-op op (append (list x) args) (cdr exp))))))) + (let ((expr (car (scan-exp #f '() (reverse exp))))) + (if (car expr) expr (cadr expr)))) + +;; bonus function +(define (prefix->postfix exp) + (cond ((null? exp) '()) + ((pair? exp) (append (map prefix->postfix (cdr exp)) + (list (prefix->postfix (car exp))))) + (else exp))) + +; (+ (* a b) c) => a*b + c +; (+ a (* b c)) => a + b*c + +;; takes a string and puts spaces around certain characters +(define (space-op str) + (list->string + (let loop1 ((l (string->list str))) + (define seps (string->list "+-*/()")) + (if (null? l) '() + (let loop2 ((s seps)) + (cond ((null? s) (cons (car l) (loop1 (cdr l)))) + ((equal? (car s) (car l)) (append (list #\space (car l) #\space) + (loop1 (cdr l)))) + (else (loop2 (cdr s))))))))) + +;; take an infix expression as a string and give it in prefix as a symbol +(define (string-infix->prefix str) + (infix->prefix (read (open-input-string + (string-append "(" (space-op str) ")"))))) + +;; calculator mode +(define (calc) + (display "> ") + (let ((line (read-line))) + (cond ((not (equal? "quit" line)) + (display "= ") + (display (eval (string-infix->prefix line))) + (newline) + (calc))))) diff --git a/kalman.scm b/kalman.scm new file mode 100644 index 0000000..0d341a8 --- /dev/null +++ b/kalman.scm @@ -0,0 +1,114 @@ +;; Copyright (C) 2010 Sean D'Epagnier +;; +;; 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 kalman)) +; In the kalman filter, the following matrices are used +; with these dimensions: +; n = number of states +; m = number of measurements +; +; A: nxn BK: nxm P: nxn H: mxn R: mxm Z: mx1 X: nx1 + +(define (kalman-state name initial-value initial-covarience + time-update-calculation measurement-update-calculation) + (list value covarience + (list name initial-value initial-covarience + time-update-calculation measurement-update-calculation))) + +; reset a stat to initial values +(define (kalman-state-reset state) + (let ((s (third state))) + (list (second s) (third s) s))) + +; return the name of a state +(define (kalman-state-name state) + (first (third state))) + +; Create a kalman measurement where calculation is a thunk +; returning the measurement + +; kalman calculations take the input value as well as the states +(define (kalman-measurement name calculation + +; create a kalman filter from these states and measurements +(define (kalman-filter states measurements) + (let ((n (length states)) + (m (length measurements))) + (append `(,states measurements) + (if (null? prev) + + + (define (kalman-time-update filter Q) +; X = AX + BU +; P = APA' + Q + (let* ((X (matrix+ (matrix* A X) (matrix* B U))) + (P (matrix+ (matrix* A P (matrix-transpose A)) Q))) + + (define (kalman-measurement-update filter H V R Z) + ; K = PH'(HPH' + VRV')^-1 + ; X = X + K*Z + ; P = (I - KH)P(I - KH)' + KRK' + (let* ((K (matrix* P (matrix-transpose H) + (matrix-invert (matrix+ (matrix* H P (matrix-transpose H)) + (matrix* V R (matrix-transpose V))))))) + (X (matrix+ X (matrix* K Z))) + (d (matrix- (matrix-identity n) (matrix* K H))) + (P (matrix+ (matrix* d P (matrix-transpose d)) (matrix* K R (matrix-transpose K))))) + + +(define (make-kalman-state initial-value process-noise time-update) + (list initial-value process-noise time-update)) + +; update +(define (make-kalman-measurement measurement-update) + + +(define (unscented-scale alpha kappa L) + (- (* alpha alpha (+ L kappa)) L)) + +; W(0) = scale / (scale + L) +; W(i) = 1 / (2 * (scale + L)) i = 1..2L +(define (unscented-mean-weights alpha beta kappa L) + (cons (/ scale (+ L scale)) + (make-list (* 2 L) (/ (* 2 (+ L scale)))))) + +; W(0) = scale / (scale + L) + (beta - alpha^2 + 1) +; W(i) = 1 / (2 * (scale + L)) i = 1..2L +(define (unscented-covariance-weights alpha beta kappa L) + (cons (+ (/ scale (+ L scale)) (- beta (square alpha)) 1) + (make-list (* 2 L) (/ (* 2 (+ L scale)))))) + +(define (unscented-mean sigma-points) + (let*((L (length sigma-points)) + (weights (unscented-mean-weights alpha beta kappa L))) + (fold x +; scale matrix square root by (alpha^2*(L + kappa) + L) to give a good spread +(define (unscented-sigma-points mean covariance) + (let*((L (length mean)) + (scale ) + (covariance_root (matrix-cholesky-decomposition covariance)) + + +; x contains initial state +; Px contains covarience of x +; y contains 2L+1 sigma points +; Py contains covarience of y +; l = a^2(L + k) - L, scaling parameter +; a determines spread of sigma points usually from 1 to 1e-4 +; k is a secondary parameter usually 0 or 3 - L +; X = avg(x), and for i from 0 to L, avg(x) +- sqrt((L+l)Px)i + +(define (unscented-transformation x Px + + +(define (unscented-sigma-points->mean-covarience + +(define (unscented-kalman-filter states measurements) + +; calculate test using states for position and velocity + +(define (kalman-test diff --git a/leastsquares.scm b/leastsquares.scm new file mode 100644 index 0000000..9f34a11 --- /dev/null +++ b/leastsquares.scm @@ -0,0 +1,60 @@ +;; Copyright (C) 2010 Sean D'Epagnier +;; +;; 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 leastsquares)) +(declare (uses matrix)) + +; return (HH')^-1*H'Z' +(define (least-squares-apply H Z) + (matrix* (let ((i (matrix* (matrix-transpose H) H))) + (let ((ii (matrix-inverse i))) + ii)) + (matrix-transpose H) Z)) + +; the functions build build-jacobian-row takes state and measurements +(define (compute-least-squares state build-jacobian-residual-row measurements) + (let ((HZ (map (lambda (measurement) + (build-jacobian-residual-row state measurement)) + measurements))) + (list (matrix (map car HZ)) (matrix (map cdr HZ))))) + +; complete? is a procedure which takes the state and may determine to stop iteration +; and/or modify the state +(define (least-squares-iterate initial-state build-jacobian-residual-row measurements + complete? max-iterations) + (let each-iteration ((state initial-state) (iteration 0)) + (let ((HZ (compute-least-squares state build-jacobian-residual-row measurements))) + (let* ((nstate (matrix+ state (apply least-squares-apply HZ)))) + (print "nstate " nstate) + (cond ((complete? nstate (second HZ)) nstate) + ((> iteration max-iterations) (verbose "max iters reached") #f) + (else (each-iteration nstate (+ iteration 1)))))))) + +;(define (regression data . partials) + + +; least squares to fit data +; use y = a*x + b +;(define (linear-regression data) +; (let ((state '(1 1))) +; (let ((update +; (compute-least-squares (lambda (state measurement) +; (list (list (first measurement) 1) +; (- (second measurement) +; (* (first state) (first measurement)) +; (second state) +; ))) +; state +; data))) +; (matrix->list (matrix+ (matrix (list state)) update))))) +;) + +; use y = a*x^2 + b*x + c +;(define (quadratic-regression data) + +;y = a*x^3 + b*x^2 + c*x + d +;(define (cubic-regression data) diff --git a/magnetometer.scm b/magnetometer.scm index 713c6a2..c003eaf 100644 --- a/magnetometer.scm +++ b/magnetometer.scm @@ -8,50 +8,116 @@ ;; This file handles reading from a magnetometer (and optional accelerometer) ;; of the digitalsurveyinstruments design, perform calibration on the unit -(type-register 'accelerometer '(x y z)) -(type-register 'magnetometer '(x y z)) +(declare (unit magnetometer)) +(declare (uses sensor)) (define (dsi-reader i o rate) - (display "set /sensors/accel/outputrate " o) - (display rate o) (newline o) - (display "set /sensors/mag/outputrate " o) - (display rate o) (newline o) + (let ((accel-sensor-indexes (map sensor-new-index '(accelerometer accelerometer accelerometer))) + (mag-sensor-indexes (map sensor-new-index '(magnetometer magnetometer magnetometer)))) +; (display "set /sensors/accel/outputrate " o) +; (display rate o) (newline o) +; (display "set /sensors/mag/outputrate " o) +; (display rate o) (newline o) (let read-loop () (make-line-reader - i (lambda (line) - (let ((words (string-split line))) - (if (= (length words) 4) - (case (car words) - (("accel:") - (sensor-update 'accelerometer (map string->number (cdr words)))) - (("mag:") - (sensor-update 'magnetometer (map string->number (cdr words))))))))))) + (lambda () i) + (lambda (line) + (if (eof-object? line) + (begin (verbose "magnetometer end of file: we will try to reconnect") + (let-values (((ni no) (try-opening-serial-devices + '("/dev/ttyACM0" "/dev/ttyACM1" "/dev/ttyACM2" "/dev/ttyACM3") + 38400))) + (cond ((and ni no) + (set! i ni) (set! o no)) + (else + (for-each (lambda (sensor sensor-indexes) + (for-each (lambda (sensor-index) + (sensor-update (list sensor + sensor-index) #f)) + sensor-indexes)) + '(accelerometer magnetometer) + (list accel-sensor-indexes mag-sensor-indexes)) + (task-sleep 1))))) + + (let ((words (string-split line))) + (if (= (length words) 4) + (let ((sensor (cond ((equal? (car words) "accel:") + `(accelerometer ,accel-sensor-indexes)) + ((equal? (car words) "mag:") + `(magnetometer ,mag-sensor-indexes)) + (else #f))) + (sensor-values (map string->number (cdr words)))) + (if sensor + (for-each (lambda (sensor-value sensor-index) + (sensor-update (list (first sensor) sensor-index) sensor-value)) + sensor-values (second sensor)) + (warning-once "unrecognized sensor: " sensor))))))))))) + (define (magnetometer-setup device) (let-values (((i o) (open-serial-device device 38400))) - (thread-start! (lambda () (dsi-reader i o 2))))) - + (create-periodic-task "magnetomter task" .1 + (lambda () (dsi-reader i o 8))))) -(computation-register 'magnetic-heading "The heading derived from the magnetometer" +(computation-register 'magnetic-heading "The heading derived from the magnetometer" '(magnetometer) (lambda () 0)) (computation-register - 'heading "The true heading derived from the magnetometer and declination, if no magnetometer is specified, gps heading is used, with a warning when below 5knots" - (if (sensor-contains? 'magnetometer) - (let ((c (computations-revaluate 'magnetic-heading 'declination))) - (lambda () (apply - (c)))) - (let ((c (computations-revaluate 'gps-heading 'gps-speed))) - (lambda () (let ((d (c))) - (if (< (second d) 5) (warning-once "using gps heading for heading, " - "but gps speed is low so it may " - "be very inaccurate.")) - (first d)))))) - -(computation-register 'magnetic-magnitude "The magtitude of the field" + 'heading "The true heading derived from the magnetometer and declination, if no magnetometer is specified, gps heading is used" '(gps) + (lambda () + (if (sensor-contains? 'magnetometer) + (- (computation-calculate 'magnetic-heading) + (computation-calculate 'declination)) + (begin (warning-once "using gps heading for heading, " + "this may be very inaccurate.") + (computation-calculate 'gps-heading))))) + +(computation-register 'magnetic-magnitude "The magnitude of the field" '(magnetometer) (lambda () (magnitude (sensor-query 'magnetometer)))) -(computation-register 'force "The magtitude of the accelerometers combined force, non-moving yields 1 (for 1 gravity)" - (lambda () - (magnitude (sensor-query 'accelerometer)))) +; take pairs of x and y data for flat mag calibration +; (c*(x-a))^2 + (c*(d*(y-b) + e*(x-a)))^2 = 1 +; +; a and b are bias for x and y axis (initially set to average of data) +; c is overall scale (initially 1/25000) +; d is relative scale difference between x and y (initially 1) +; e is cross coupling term (initially 0) +; all terms should be constant except e changes with local magnetic field strength + +(define (flat-mag-apply calibration x y) + (let-values (((a b c d e) (apply values (first (matrix->list (matrix-transpose calibration)))))) + (list + (* c (- x a)) + (* c (+ (* d (- y b)) (* e (- x a))))))) + +(define (build-flat-mag-jacobian-residual-row state measurement) + (very-verbose "build-flat-mag-jacobian-residual-row: " state " " measurement) + (let-values (((a b c d e) (apply values (first (matrix->list (matrix-transpose state))))) + ((x y) (apply values measurement))) + (let ((x-a (- x a)) + (y-b (- y b)) + (c^2 (* c c))) + (list (list (+ (* -2 c^2 e (+ (* d y-b) (* e x-a))) + (* -2 c^2 x-a)) + (* -2 c^2 d (+ (* d y-b) (* e x-a))) + (* 2 c (+ (square (+ (* d y-b) (* e x-a))) + (square x-a))) + (* 2 c^2 y-b (+ (* d y-b) (* e x-a))) + (* 2 c^2 x-a (+ (* d y-b) (* e x-a)))) + (- (+ (square (* c x-a)) + (square (* c (+ (* d y-b) (* e x-a)))) + -1)))))) + +(define (flat-mag-complete? state residuals) + (let ((dev (apply rms (first (matrix->list (matrix-transpose residuals)))))) + (verbose "flat-mag-complete? dev: " dev) + (if (> dev .1) #f #t))) + +(define (flat-mag-calibrate mag-log) + (let ((avg-x (apply average (map first mag-log))) + (avg-y (apply average (map second mag-log)))) + (least-squares-iterate (matrix-transpose (matrix `((,avg-x ,avg-y ,(/ 25000) 1 0)))) + build-flat-mag-jacobian-residual-row + mag-log flat-mag-complete? 10))) diff --git a/matrix.scm b/matrix.scm new file mode 100644 index 0000000..cd3dae0 --- /dev/null +++ b/matrix.scm @@ -0,0 +1,194 @@ +;; Copyright (C) 2010 Sean D'Epagnier +;; +;; 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. + +; I define additional utility functions to matrix-utils +; for matrices which sits on top of lapack blas etc.. +; makes it easier for me to read + +(declare (unit matrix)) + +(use srfi-1 srfi-4-utils blas matrix-utils atlas-lapack) + +(define-utility-matrices f64) + +(define matrix-rows first) +(define matrix-cols second) +(define matrix-vector third) +(define matrix-trans fourth) ; is the matrix transposed? + ; this allows O(1) transposing + +(define (vector->matrix n m vector) + (list n m vector blas:NoTrans)) + +(define (vector->square-matrix vector) + (let ((n (sqrt (f64vector-length vector)))) + (if (not (integer? n)) (error "invalid vector size to vector->square-matrix" vector)) + (vector->matrix n n vector))) + +(define (vector->matrix-trans n m vector trans) + (list n m vector trans)) + +(define zero-matrix (vector->matrix 0 0 (f64vector))) + +(define (zeroed-matrix r c) + (vector->matrix r c (matrix-zeros r c))) + +(define (matrix l) + (let ((n (length l))) + (if (= 0 n) + zero-matrix + (let ((m (length (car l)))) + (for-each (lambda (l) (if (not (= m (length l))) + (error "non-rectangular lists in matrix"))) + (cdr l)) + (vector->matrix n m (apply f64vector (apply append l))))))) + +(define (matrix-identity r . c) + (if (null? c) + (vector->matrix r r (matrix-eye r r)) + (vector->matrix r (car c) (matrix-eye r (car c))))) + +(define (matrix-ref m row col) + (f64vector-ref (matrix-vector m) + (if (eq? blas:NoTrans (matrix-trans m)) + (+ (* row (matrix-cols m)) col) + (+ (* col (matrix-rows m)) row)))) + +(define (matrix->list m) + (map (lambda (row) + (map (lambda (col) + (matrix-ref m row col)) + (sequence 0 (- (matrix-cols m) 1)))) + (sequence 0 (- (matrix-rows m) 1)))) + +(define (matrix-transpose m) + (list (matrix-cols m) (matrix-rows m) (matrix-vector m) + (if (eq? blas:NoTrans (matrix-trans m)) + blas:Trans + blas:NoTrans))) + +(define (matrix+ . matrices) + (define (matrix+sub a c) + (let ((a (if (eq? (matrix-trans c) blas:NoTrans) a (matrix-transpose a)))) + (if (not (and (= (matrix-rows a) (matrix-rows c)) + (= (matrix-cols a) (matrix-cols c)))) + (error "dimension mis-match in matrices supplied to matrix+" a c)) + (let ((b (matrix-identity (matrix-cols a) (matrix-cols a)))) + (vector->matrix-trans (matrix-rows a) (matrix-cols b) + (blas:dgemm blas:RowMajor (matrix-trans a) blas:NoTrans + (matrix-rows a) (matrix-cols b) (matrix-cols a) + 1 (matrix-vector a) (matrix-vector b) 1 (matrix-vector c)) + (matrix-trans c))))) + (cond ((null? matrices) zero-matrix) + ((null? (cdr matrices)) (car matrices)) + (else (apply matrix+ (cons (matrix+sub (car matrices) (cadr matrices)) + (cddr matrices)))))) + +(define (matrix-neg m) + (vector->matrix-trans (matrix-rows m) (matrix-cols m) + (f64vector-map - (matrix-vector m)) (matrix-trans m))) + +(define (matrix- . matrices) + (cond ((null? matrices) zero-matrix) + ((null? (cdr matrices)) (matrix-neg (car matrices))) + (else (matrix+ (car matrices) (matrix-neg (apply matrix+ (cdr matrices))))))) + +(define (matrix-map proc m) + (vector->matrix-trans (matrix-rows m) (matrix-cols m) + (f64vector-map proc (matrix-vector m)) + (matrix-trans m))) + +(define (matrix* . matrices) + (define (matrix*sub a b) + (cond ((and (number? a) (number? b)) (* a b)) + ((number? a) (matrix-map (lambda (n) (* a n)) b)) + ((number? b) (matrix-map (lambda (n) (* b n)) a)) + ((not (= (matrix-cols a) (matrix-rows b))) + (error "dimension mis-match in matrices supplied to matrix*" a b)) + (else + (let ((c (matrix-zeros (matrix-rows a) (matrix-cols b)))) + (vector->matrix (matrix-rows a) (matrix-cols b) + (blas:dgemm blas:RowMajor (matrix-trans a) (matrix-trans b) + (matrix-rows a) (matrix-cols b) (matrix-cols a) + 1 (matrix-vector a) (matrix-vector b) 0 c)))))) + (cond ((null? matrices) zero-matrix) + ((null? (cdr matrices)) (car matrices)) + (else (apply matrix* (cons (matrix*sub (car matrices) (cadr matrices)) + (cddr matrices)))))) + +(define (matrix-square? m) + (= (matrix-rows m) (matrix-cols m))) + +(define (matrix-vector-index m row col) + (if (eq? blas:NoTrans (matrix-trans m)) + (+ (* row (matrix-cols m)) col) + (+ (* col (matrix-rows m)) row))) + +(define (matrix-ref m row col) + (f64vector-ref (matrix-vector m) (matrix-vector-index m row col))) + +(define (matrix-set! m row col value) + (f64vector-set! (matrix-vector m) (matrix-vector-index m row col) value)) + +(define (matrix-remove-row m row) + (matrix (let each-row ((rows (matrix->list m)) (r 0)) + (if (null? rows) '() + (let ((rest (each-row (cdr rows) (+ r 1)))) + (if (= row r) rest (cons (car rows) rest))))))) + +(define (matrix-remove-col m col) + (matrix-transpose (matrix-remove-row (matrix-transpose m) col))) + +(define (matrix-determinant m) + (cond ((not (= (matrix-rows m) (matrix-cols m))) + (error "matrix-determinant failed on non-square matrix" m)) + ((= (matrix-rows m) 1) (matrix-ref m 0 0)) + (else + (let ((n (matrix-remove-row m 0))) + (apply + (map (lambda (col) + (* (if (even? col) 1 -1) (matrix-ref m 0 col) + (matrix-determinant (matrix-remove-col n col)))) + (sequence 0 (- (matrix-cols m) 1)))))))) + +(define (matrix-inverse m) + (if (not (matrix-square? m)) (error "cannot perform inverse on non-square matrix" m)) + (let-values (((A pivot) (atlas-lapack:dgetrf blas:RowMajor (matrix-rows m) + (matrix-cols m) (matrix-vector m)))) + (vector->matrix (matrix-rows m) (matrix-cols m) + (atlas-lapack:dgetri blas:RowMajor (matrix-rows m) A pivot)))) + +; use cholesky decomposition on matrix A returns L +; A = LL' +; A must be square, symmetric, invertable, positive and definate +; Ljj = sqrt(Ajj - sum(Ljk^2) k = 1 to j-1)) +; Lij = (Aij - sum(LikLjk k = 1 to j-1)) / Ljj +; return #f if no decomp can be found +(define (matrix-cholesky-decomposition A) + (call/cc + (lambda (ret) + (let* ((n (matrix-rows A)) + (L (zeroed-matrix n n)) + (square (lambda (x) (* x x)))) + (for-each + (lambda (j) + (matrix-set! + L j j (let ((val (- (matrix-ref A j j) + (fold + 0 (map (lambda (k) + (square (matrix-ref L j k))) + (sequence 0 (- j 1))))))) + (if (not (positive? val)) (ret #f) (sqrt val)))) + (for-each (lambda (i) + (matrix-set! + L i j (/ (- (matrix-ref A i j) + (fold + 0 (map (lambda (k) + (* (matrix-ref L i k) + (matrix-ref L j k))) + (sequence 0 (- j 1))))) + (matrix-ref L j j)))) + (sequence (+ j 1) (- n 1)))) + (sequence 0 (- n 1))) + (ret L))))) diff --git a/motor.scm b/motor.scm new file mode 100644 index 0000000..b395b0e --- /dev/null +++ b/motor.scm @@ -0,0 +1,30 @@ +;; Copyright (C) 2011 Sean D'Epagnier +;; +;; 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. + +; very simple motor control driver where ascii number is sent over a serial port +; command a motor for a specified number of milliseconds (+ or - for forward +; or backwards) + +(declare (unit motor)) + +(define (motor-options) + `(,(make-string-verifier "serial device of motor" 'motordevice "/dev/ttyUSB0") + ,(make-baud-verifier "serial baud rate to use" 'serialbaud 19200))) + +(define (motor-open motor-options) + (let-values (((motor-input motor-output) + (open-serial-device (motor-options 'motordevice) (motor-options 'serialbaud)))) + (list motor-input motor-output motor-options))) + +(define motor-input first) +(define motor-output second) +(define motor-options third) + +(define (motor-command motor command) + (verbose "motor at " ((motor-options motor) 'motor-device) " command: " command) + (write (inexact->exact (round command)) motor) + (newline motor)) diff --git a/net.scm b/net.scm index 5f62b69..a8d3b84 100644 --- a/net.scm +++ b/net.scm @@ -7,42 +7,56 @@ ; This type of networking provides both client and server capabilities and ; gives unlimited control. The basic mechanic is all scheme expressions -; delivered over the socket are evaluated remotely. +; delivered over the socket are evaluated remotely, and the results sent back +; over the network + +(declare (unit net)) (use tcp srfi-18) (define net-default-port 23227) -(define (net-repl) +; given an input and output port, in a loop, +; read from input, evaluate, and write to output +(define (net-repl i o) (call/cc (lambda (cont) (with-exception-handler (lambda _ (cont #f)) - (lambda () (write (eval (read))) (newline))))) - (net-repl)) + (lambda () + (let loop () + (with-input-from-port i + (lambda () (with-output-to-port o + (lambda () + (write (eval (read))) + (newline))))) + (task-sleep .001) (loop))))))) -; run a server on a specified port +; start a server on a specified port ; the server handles many clients and simply performs evaluation (define (net-server port) - (thread-start! + (create-task (lambda () (let ((port (if port port net-default-port))) - (verbose "starting server thread on port " port) + (verbose "starting server task on port " port) (let ((listener (tcp-listen port))) (let accept-loop () (let-values (((i o) (tcp-accept listener))) - (thread-start! - (lambda () (with-input-from-port i - (lambda () (with-output-to-port o - net-repl)))))) + (create-task (lambda () (net-repl i o)))) (accept-loop))))))) -; connect to a server at a given address -(define (net-add-client address expression) +; connect to a server at a given address, write expression, and call receiver +; repeatedly for each lien +(define (net-add-client address expression receiver) (let-values (((i o) (if (string-contains address ":") (tcp-connect address) (tcp-connect address net-default-port)))) - (thread-start! (lambda () ; handle return requests - (with-input-from-port i - net-repl))))) + (create-task (lambda () ; handle return requests + (verbose "connected to server") + (write expression o) + (newline o) + (let loop () (receiver (read i)) (loop)))) + + ; delay after connecting to server to allow some data + (task-sleep .5))) diff --git a/options.scm b/options.scm new file mode 100644 index 0000000..b32b16b --- /dev/null +++ b/options.scm @@ -0,0 +1,148 @@ +;; Copyright (C) 2010 Sean D'Epagnier +;; +;; 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 options)) + +; Verifiers for options +; these store the name, default value, and a method to verify a new value +; and update it +(define (make-verifier name help default verify) + (lambda (op) + (case op + ((name) name) + ((help) (print name " -- " help " (default " default ")")) + ((default) (verify default)) + ((verify) verify)))) + +(define (make-number-verifier name help default min max) + (make-verifier + name help default + (lambda (num) + (cond ((< num min) (print "Value given for option '" name "' is below minimum of " min) min) + ((> num max) (print "Value given for option '" name "' is above maximum of " max) max) + (else num))))) + +(define (make-integer-verifier name help default min max) + (make-verifier name help default + (lambda (num) + (if (not (integer? num)) + (die "non-integer value: " num " given for " name) + (((make-number-verifier name help default min max) 'verify) num))))) + +(define (make-discrete-verifier name help default validvalues) + (make-verifier + name (apply string-append help " Values: " (map symbol->string validvalues)) + default + (lambda (value) + (if (member value validvalues) + value + (die "value " value " for option " name + " invalid. Valid values:\n" validvalues))))) + +(define (make-baud-verifier name help default) + (make-discrete-verifier + name help default + '(300 600 1200 2400 4800 9600 19200 38400 57600))) + +(define (make-boolean-verifier name help default) + (make-verifier + name (string-append help " (true or false)") default + (lambda (value) + (cond ((boolean? value) value) + ((eq? value 'true) #t) + ((eq? value 'false) #f) + (else (error "boolean option " name " must be either true or false\n")))))) + +(define (make-string-verifier name help default) + (make-verifier + name help default + (lambda (value) + (cond ((string? value) value) + ((symbol? value) (symbol->string value)) + (else (error "string option " name " requires string value\n")))))) + +(define (make-color-verifier default) + (make-verifier 'color + (string-append + "can be by name, eg:\n" + " red green blue cyan magenta\n" + " yellow gray black white\n" + " or can be a normalized triple, for red:\n" + " (1 0 0)\n") + default parse-color)) + +(define (make-bounds-verifier) + (make-verifier 'bounds + (string-append + "minimum and maximum values for each axis (rectangular), eg:\n" + " ((-10 10) (-5 5))\n") + '() + (lambda (value) + (cond ((eq? value 'auto) '()) + (else value))))) + +(define (make-sensor-indicies-verifier sensors-name sensor-name) + (make-verifier sensors-name + "list of indicies of sensors to use or all default all" + (lambda (value) + (letrec ((and (lambda args (cond ((null? args) #t) + ((car args) (apply and (cdr args))) + (else #f))))) + (cond ((eq? value 'all) (sensor-indicies sensor-name)) + ((not (and (list? value) (apply and (map number? value)))) + (error "value must be list of indicies for" sensors-name)) + (else value)))))) + +(define (create-options verifiers examples parent-options) + (let ((values (alist->hash-table + (map (lambda (verifier) (cons (verifier 'name) (verifier 'default))) verifiers))) + (verifiers (alist->hash-table + (map (lambda (verifier) (cons (verifier 'name) (verifier 'verify))) verifiers)))) + (lambda (op . args) + (cond + ((eq? op 'help) + (hash-table-for-each + values + (lambda (name value) (print name ": " value))) + (print "Examples: " examples) + (if parent-options (parent-options 'help))) + ((eq? op 'update) + (let ((value (if (null? (cdr args)) 'true (second args)))) + (cond ((hash-table-exists? values (first args)) + (hash-table-set! values (first args) + ((hash-table-ref verifiers (first args)) value))) + (parent-options (apply parent-options op args)) + (else (error "cannot update unrecognized option" (car args) + (hash-table-ref values op)))))) + ((hash-table-exists? values op) + (hash-table-ref values op)) + (parent-options (parent-options op)) + (else (error "unrecognized option" op)))))) + +(define (options-help options arg) + (cond ((equal? arg "help") + (print "options include:") + (options 'help) + (exit)))) + +; parse basic option splitting and parsing where options are separated by +; , and values assignned with = +(define (parse-basic-split-options-string options args) + (if (not (null? args)) (options-help options (first args))) + (for-each (lambda (option) + (apply options 'update + (map read-from-string (string-split option "=")))) + args)) + +(define (parse-basic-options-string options arg) + (parse-basic-split-options-string options (string-split arg ","))) + +(define (parse-basic-arg-options-string options arg) + (options-help options arg) + (let ((options-string (string-split arg ","))) + (parse-basic-split-options-string options (cdr options-string)) + (car options-string))) diff --git a/plot.scm b/plot.scm index 0785292..f0cda94 100644 --- a/plot.scm +++ b/plot.scm @@ -9,20 +9,25 @@ ; the expressions are evaluated with the scheme eval function ; and are used to calculate values of the data, ; as well as the new minimum and maximum boundaries needed. -; The special variables are given: -; T - current time -; (data 'data-key 'field) Calculates the value as needed -; eg: (wind speed)n -; Otherwise normal lisp constructs are allowed +; +; Otherwise normal lisp expressions are allowed + +(declare (unit plot)) +(declare (uses history computation utilities options)) (use srfi-1 srfi-69 gl glu glut) +(include "glshortcuts.scm") + +(define plots '()) + ; an axis is a list with 3 thunks that calculate the value, the min and the max (define (parse-color value) (case value ((white) '(1 1 1)) ((yellow) '(1 1 0)) ((green) '(0 1 0)) ((blue) '(0 0 1)) ((red) '(1 0 0)) ((magenta) '(1 0 1)) - ((cyan) '(0 1 1)) ((gray) '(.5 .5 .5)) ((black) '(0 0 0)) + ((cyan) '(0 1 1)) ((gray) '(.5 .5 .5)) ((orange) '(1 .5 0)) + ((black) '(0 0 0)) (else (if (and (list? value) (= 3 (length value))) value (error "Unrecognized color" value))))) @@ -36,92 +41,27 @@ (lambda () (display arg)))) args))))) -(define (round-decimals value places) - (let ((f (expt 10 places))) - (/ (round (* value f)) f))) - -; Verifiers for plot and trace options -; these store the name, default value, and a method to verify a new value -; and update it -(define (make-verifier name default verify) - (lambda (op) - (case op - ((name) name) - ((default) (verify default)) - ((verify) verify)))) - -(define (make-number-verifier name default min max) - (make-verifier - name default - (lambda (num) - (cond ((< num min) (print "Value given for option '" name "' is below minimum of " min) min) - ((> num max) (print "Value given for option '" name "' is above maximum of " max) max) - (else num))))) - -(define (make-discrete-verifier name default validvalues) - (make-verifier - name default - (lambda (value) - (if (member value validvalues) - value - (die "value " value "for option " name - "invalid. Valid values include:\n" validvalues))))) - -(define (make-boolean-verifier name default) - (make-verifier - name default - (lambda (value) - (cond ((boolean? value) value) - ((eq? value 'true) #t) - ((eq? value 'false) #f) - (else (error "boolean option " name " must be either true or false\n")))))) - -(define (make-color-verifier default) - (make-verifier 'color default parse-color)) - -(define (make-bounds-verifier) - (make-verifier 'bounds '() - (lambda (value) - (cond ((eq? value 'auto) '()) - (else value))))) - -(define (create-options verifiers parent-options) - (let ((values (alist->hash-table - (map (lambda (verifier) (cons (verifier 'name) (verifier 'default))) verifiers))) - (verifiers (alist->hash-table - (map (lambda (verifier) (cons (verifier 'name) (verifier 'verify))) verifiers)))) - (lambda (op . args) - (if (eq? op 'update) - (let ((value (if (null? (cdr args)) 'true (second args)))) - (cond ((hash-table-exists? values (first args)) - (hash-table-set! values (first args) - ((hash-table-ref verifiers (first args)) value))) - (parent-options (apply parent-options op args)) - (else (error "cannot update unrecognized option" (car args) - (hash-table-ref values op))))) - (if (hash-table-exists? values op) - (hash-table-ref values op) - (error "unrecognized option" op)))))) - (define (default-plot-options) (create-options (list (make-bounds-verifier) - (make-number-verifier 'fov 90 0 180) - (make-number-verifier 'gridcount 5 0 100) - (make-boolean-verifier 'gridnumbering 'true) - (make-number-verifier 'framerate 4 .001 1000)) + (make-number-verifier 'fov "field of view, angle in degrees for 3d projection" 90 0 180) + (make-number-verifier 'gridcount "set number of grid lines" 6 0 100) + (make-number-verifier 'period "how often to update this trace in seconds" .25 0 1000000) + (make-boolean-verifier 'gridnumbering "enable or disable grid numbering" 'true) + (make-number-verifier 'framerate "rate to render te plot" 4 .001 1000) + (make-number-verifier 'timeout "how long to keep data in history, 0 for forever" 0 0 100000000) + (make-discrete-verifier 'type "type of plot" '2d '(2d 3d polar))) + "no examples yet" #f)) (define (default-trace-options default-color plot-options) (create-options (list (make-color-verifier default-color) - (make-number-verifier 'period 1 0 1000000) - (make-number-verifier 'thickness 2 0 100) - (make-number-verifier 'timeout 0 0 100000000) - (make-discrete-verifier 'type '2d '(2d 3d polar)) - (make-discrete-verifier 'mode 'lines '(points lines))) + (make-number-verifier 'thickness "how fat to draw" 2 0 100) + (make-discrete-verifier 'mode "plot mode" 'lines '(points lines))) + "no examples yet" plot-options)) ; a trace is like the plot of one variable, it renders each time invoked @@ -132,36 +72,50 @@ (else (error "invalid axis count for plot" (length axes)))) (let ((history (create-history))) - (thread-start! (lambda () - (let ((computations (apply computations-revaluate axes))) - (let loop () - (history 'update - (let ((c (computations)) - (type (options 'type))) - (case type - ((2d 3d) c) - ((polar) `(,(* (first c) (cos (second c))) - ,(* (first c) (sin (second c))))) - (else (error "unknown plot type" type))))) - (thread-sleep! (options 'period)) - (loop))))) + (let ((computations (apply computations-revaluate axes))) + (create-periodic-task + `(trace-update ,axes) + (options 'period) + (lambda () + (let ((c (computations)) + (type (options 'type))) + (if c (history 'update + (case type + ((2d 3d) c) + ((polar) `(,(* (first c) (sin (deg2rad (second c)))) + ,(* (first c) (cos (deg2rad (second c)))))) + (else (error "unknown plot type" type))))))) + )) (lambda (op . args) (case op ((axis-count) (length axes)) ((bounds) (zip (history 'min) (history 'max))) ((options) options) + ((apply-timeout) (if (> (options 'timeout) 0) (history 'apply-timeout (options 'timeout)))) ((display) ; draw using opengl + + ; hack to avoid thick points and lines wrapping to the left side of screen + (glLetMatrix + (gl:LoadIdentity) + (gl:ClipPlane gl:CLIP_PLANE0 + (f64vector -1 0 0 + (- 1 (/ (- (options 'thickness) 1) + (glut:Get glut:WINDOW_WIDTH)))))) + + (gl:Enable gl:CLIP_PLANE0) + + + (apply glColor (options 'color)) (glBegin (case (options 'mode) ((points) (gl:PointSize (options 'thickness)) gl:POINTS) ((lines) (gl:LineWidth (options 'thickness)) gl:LINE_STRIP) (else (error "unknown plot mode" mode))) (for-each (lambda (values) - (apply gl:Color3f (options 'color)) (apply glVertex values)) (history 'dump))) - (if (> (options 'timeout) 0) - (history 'apply-timeout (options 'timeout)))))))) + + (gl:Disable gl:CLIP_PLANE0)))))) ; take take min, max pairs, and give overall min max (define (bounds-union . bounds) @@ -169,8 +123,6 @@ (apply max (map second bounds)))) (define (find-plot-bounds defaultbounds traces) -; (print "bounds " defaultbounds traces) - (let ((axis-count ((car traces) 'axis-count))) (let each-bound ((defaultbounds defaultbounds) (tracebounds (remove null? (map (lambda (trace) (trace 'bounds)) traces)))) @@ -186,12 +138,8 @@ (each-bound (cdr defaultbounds) (map cdr tracebounds)))))))) (define (plot-display options traces) - (gl:Clear gl:COLOR_BUFFER_BIT) + (gl:Clear gl:COLOR_BUFFER_BIT) - ; update bounds and set the modelview matrix - (gl:MatrixMode gl:MODELVIEW) - (gl:LoadIdentity) - (let ((bounds (find-plot-bounds (options 'bounds) traces))) (if (>= (length bounds) 2) (let ((left (first (first bounds))) @@ -201,46 +149,102 @@ (near (if (< (length bounds) 3) -1 (first (third bounds)))) (far (if (< (length bounds) 3) 1 (second (third bounds))))) + (gl:MatrixMode gl:MODELVIEW) (gl:LoadIdentity) + (gl:Ortho left right top bottom near far) - + ; Draw the grid lines (gl:Enable gl:LINE_STIPPLE) (gl:LineStipple 1 17) + (gl:LineWidth 2) - (let ((hspacing (/ (- right left) (+ 1 (options 'gridcount))))) - (let each-hgridline ((offset (+ left hspacing))) - (cond ((< offset (- right (/ hspacing 10))) - (glColor .6 .6 .6) - (glBegin gl:LINES - (glVertex offset top) - (glVertex offset bottom)) - (cond ((options 'gridnumbering) - (glColor 1 1 1) - (glRasterPos offset (/ (+ bottom (* 99 top)) 100)) - (glutPrint (round-decimals offset 3)))) - (each-hgridline (+ offset hspacing)))))) - - (let ((vspacing (/ (- bottom top) (+ 1 (options 'gridcount))))) - (let each-vgridline ((offset (+ top vspacing))) - (cond ((< offset (- bottom (/ vspacing 10))) - (glColor .6 .6 .6) - (glBegin gl:LINES - (glVertex left offset) - (glVertex right offset)) - (cond ((options 'gridnumbering) - (glColor 1 1 1) - (glRasterPos (/ (+ (* 99 left) right) 100) - (- offset (/ vspacing 10))) - (glutPrint (round-decimals offset 3)))) - (each-vgridline (+ offset vspacing)))))) - - (gl:Disable gl:LINE_STIPPLE) + (case (((first traces) 'options) 'type) + ((2d 3d) + (let ((hspacing (/ (- right left) (+ 1 (options 'gridcount))))) + (let each-hgridline ((offset (+ left hspacing))) + (cond ((< offset (- right (/ hspacing 10))) + (glColor .6 .6 .6) + (glBegin gl:LINES + (glVertex offset top) + (glVertex offset bottom)) + (cond ((options 'gridnumbering) + (glColor 1 1 1) + (glRasterPos offset (/ (+ bottom (* 99 top)) 100)) + (glutPrint (round-to-places offset 3)))) + (each-hgridline (+ offset hspacing)))))) + + (let ((vspacing (/ (- bottom top) (+ 1 (options 'gridcount))))) + (let each-vgridline ((offset (+ top vspacing))) + (cond ((< offset (- bottom (/ vspacing 10))) + (glColor .6 .6 .6) + (glBegin gl:LINES + (glVertex left offset) + (glVertex right offset)) + (cond ((options 'gridnumbering) + (glColor 1 1 1) + (glRasterPos (/ (+ (* 99 left) right) 100) + (- offset (/ vspacing 10))) + (glutPrint (round-to-places offset 3)))) + (each-vgridline (+ offset vspacing))))))) + ((polar) + (let ((rspacing (/ (sqrt (+ (square (- bottom top)) + (square (- right left)))) + (+ (options 'gridcount) 1))) + (aspacing (/ (* 2 pi) (options 'gridcount))) + (number-angle (atan (+ top bottom) (+ left right)))) + (let each-rgridline ((offset (sqrt (+ (if (negative? (* top bottom)) + 0 + (min (square top) (square bottom))) + (if (negative? (* left right)) + 0 + (min (square left) (square right)))))) + (rcount 0)) + (cond ((<= rcount (options 'gridcount)) + (glColor .6 .6 .6) + (glBegin gl:LINE_LOOP + (let each-theta ((theta 0)) + (cond ((< theta (* 2 pi)) + (glVertex (* offset (sin theta)) (* offset (cos theta))) + (each-theta (+ theta .1)))))) + + (cond ((options 'gridnumbering) + (glColor 1 1 1) + (glRasterPos (* offset (cos number-angle)) + (* offset (sin number-angle))) + (glutPrint (round-to-places offset 3)))) + (each-rgridline (+ offset rspacing) (+ rcount 1))))) + + (let each-agridline ((offset 0)) + (cond ((< offset (* 2 pi)) + (glColor .6 .6 .6) + (glBegin gl:LINES + (glVertex 0 0) + + (let ((max-r (sqrt (max + (+ (square left) (square top)) + (+ (square right) (square top)) + (+ (square left) (square bottom)) + (+ (square right) (square bottom)))))) + (glVertex (* max-r (cos offset)) (* max-r (sin offset))))) + (each-agridline (+ offset aspacing)))))))) + + (gl:Disable gl:LINE_STIPPLE) - (gl:Translatef 0 0 (- near))))) - - ; Draw the traces - (for-each (lambda (trace) (trace 'display)) traces)) + (gl:Translatef 0 0 (- near)) + ; Draw the traces + + (for-each (lambda (trace) (trace 'display)) traces) + + ; Do any additional plot rendering + (for-each (lambda (display) (display left right top bottom)) plot-extra-display) + + )))) + +(define plot-extra-display '()) + +(define (plot-add-extra-display-function thunk) + (set! plot-extra-display (cons thunk plot-extra-display))) ; the plot may have multiple instances of various axes, therefore a list ; which of lists of axes is used. Options for the plot for the first element @@ -257,35 +261,26 @@ (if (any (lambda (trace) (eq? ((trace 'options) 'type) '3d)) traces) (glu:Perspective (options 'fov) (/ w h) .1 100)))) - (glut:DisplayFunc - (let ((last-frame-time (current-milliseconds)) - (period (round (/ 1000 (options 'framerate))))) - (lambda () - (let ((frame-time (current-milliseconds))) - ; redisplay again at the framerate - (cond ((> frame-time (+ last-frame-time (* 2 period))) - (warning-once "system too SLOW to render at " - "the framerate set for plot") - (set! last-frame-time frame-time)) - ((> frame-time (+ last-frame-time period)) - (set! last-frame-time (+ last-frame-time period)) - (plot-display options traces) - (glut:SwapBuffers))) - - (glut:TimerFunc (- (+ last-frame-time period) frame-time) - glut:PostWindowRedisplay - (glut:GetWindow)))))) + (glut:DisplayFunc + (lambda () + (for-each (lambda (trace) (trace 'apply-timeout)) traces) + (plot-display options traces) + (glut:SwapBuffers))) + + (let ((win (glut:GetWindow))) + (create-periodic-task "plot redraw task" (/ (options 'framerate)) + (lambda () (glut:PostWindowRedisplay win)))) (glut:KeyboardFunc (lambda (key x y) (case key ((#\esc #\q) (exit)) - ((#\f) (glut:FullScreen))) + ((#\f) (glut:FullScreenToggle))) (glut:PostRedisplay))) (glut:SpecialFunc (lambda (key x y) - (if (glut-HasModifiers glut:ACTIVE_SHIFT) + (if (= (bitwise-and (glut:GetModifiers) glut:ACTIVE_SHIFT) glut:ACTIVE_SHIFT) (let ((rs 1)) (cond ((= key glut:KEY_LEFT) (RotateAfter rs 0 1 0)) @@ -304,83 +299,50 @@ ((= key glut:KEY_PAGE_DOWN) (set-zoom 2)) ))) (glut:PostRedisplay)))) - -(define (plot-help) - (print "Plot setup in the form: TRACE0[!TRACE1]...[!TRACEn][,option0]...[,optionn]\n" - "options for type of plot include:\n" - " bounds minimum and maximum values for each axis (rectangular), eg:\n" - " ((-10 10) (-5 5)) (default auto)\n" - " fov field of view, angle in degrees for 3d projection (default 90)\n" - " gridcount set number of grid lines (default 5)\n" - " gridnumbering enable or disable grid numbering\n" - " framerate rate to render the plot (default 4)\n" - "The form for TRACE is:\n" - " AXIS0[:AXIS1]...[:AXISn][,option0=value][,option1=value]...[optionn=value]\n" - " color -- can be by name, eg:\n" - " red green blue cyan magenta\n" - " yellow gray black white (default white)\n" - " or can be a normalized triple, for red:\n" - " (1 0 0)\n" - " period -- how often to update this trace in seconds (default 1)\n" - " type 2d (default), 3d, polar\n" - " mode points or lines (default lines)\n" - " thickness -- how fat to draw (default 2)\n" - " timeout how long to keep data in history, 0 for never (default 0)\n" - "Example of windspeed plot:\n" - " --plot windspeed\n" - "Here, only 1 axis is given, so time is automatically added.\n" - "Another example:\n" - " --plot speedintowind,color=yellow;speed,color=blue\n" - "Some more examples:\n" - "-p speedintowind:winddirection,mode=points;heading,mode=points,type=polar\n")) (define (create-plot-from-string arg) - (if (eq? arg "help") (plot-help) - (let ((plotoptions (default-plot-options))) - (create-plot - plotoptions - (map (lambda (trace-string default-color) - (let ((traceoptions (default-trace-options default-color plotoptions))) - (create-trace - traceoptions - (map (lambda (axis-string) - (let ((options-string (string-split axis-string ","))) - (for-each (lambda (option) - (apply traceoptions 'update - (map read-from-string (string-split option "=")))) - (cdr options-string)) - (read-from-string (car options-string)))) - (string-split trace-string ":"))))) - (string-split arg ";") - '(white red green blue yellow cyan magneta gray)))))) + (let ((plot-options (default-plot-options)) + (split-arg (string-split arg ";")) + (trace-colors '(white red green blue yellow cyan magenta gray orange))) + (if (> (length split-arg) (length trace-colors)) + (error "too many traces specified for plot")) + + (create-plot + plot-options + (map (lambda (trace-string default-color) + (let ((traceoptions (default-trace-options default-color plot-options))) + (create-trace + traceoptions + (map (lambda (axis-string) + (read-from-string (parse-basic-arg-options-string traceoptions axis-string))) + (string-split trace-string ":"))))) + split-arg trace-colors)))) (define glutMainWindow #f) -(define (plots-setup args) +(define (plots-setup plots) + (glut:InitDisplayMode (+ glut:DOUBLE glut:RGB glut:ALPHA)) (set! glutMainWindow (glut:CreateWindow "cruisingplot")) - (if (= (length args) 1) - (create-plot-from-string (car args)) - (let* ((plot-count (length args)) + (if (= (length plots) 1) + ((car plots)) + (let* ((plot-count (length plots)) (plots-x-count (ceiling (sqrt plot-count))) (plots-y-count (ceiling (/ plot-count plots-x-count)))) (let ((plots - (let each-plot ((args args) (x 0) (y 0)) - (if (null? args) '() + (let each-plot ((plots plots) (x 0) (y 0)) + (if (null? plots) '() (cons (list (let ((win (glut:CreateSubWindow glutMainWindow 0 0 64 64))) - (create-plot-from-string (car args)) + ((car plots)) win) x y) (if (< x (- plots-x-count 1)) - (each-plot (cdr args) (+ x 1) y) - (each-plot (cdr args) 0 (+ y 1)))))))) + (each-plot (cdr plots) (+ x 1) y) + (each-plot (cdr plots) 0 (+ y 1)))))))) (glut:SetWindow glutMainWindow) - - (glut:DisplayFunc - (lambda () - (gl:Clear gl:COLOR_BUFFER_BIT) - (glut:SwapBuffers))) - + + (glut:DisplayFunc (lambda () #t)) + (glut:ReshapeFunc (lambda (w h) (gl:Viewport 0 0 w h) @@ -391,4 +353,19 @@ (glut:PositionWindow (* (second plot) x-spacing) (* (third plot) y-spacing)) (glut:ReshapeWindow x-spacing y-spacing)) - plots)))))))) + plots))))))) + +; This is a bug workaround, for some reason glut:IdleFunc is never +; called with multiple sub windows, and the only way to get it to work +; is to create a thread for the glut main loop. With only 1 plot we +; can avoid creating threads all together + + (if (= (length plots) 1) + (let ((scheduler (create-task-scheduler #t))) + (glut:IdleFunc (lambda () + (scheduler) + )) + (glut:MainLoop))) + + (glut:IdleFunc (lambda () (thread-sleep! .01))) + (thread-start! glut:MainLoop)) diff --git a/quaternion.scm b/quaternion.scm new file mode 100644 index 0000000..3a5de91 --- /dev/null +++ b/quaternion.scm @@ -0,0 +1,117 @@ +;; Copyright (C) 2010 Sean D'Epagnier +;; +;; 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 quaternion)) + +(define (quaternion+ . args) + (apply map + args)) + +(define quaternion-identity '(1 0 0 0)) + +(define (quaternion* . args) + (case (length args) + ((0) quaternion-identity) + ((1) (first args)) + ((2) (let ((q0 (first args)) (q1 (second args))) + (let ((q00 (first q0)) (q01 (second q0)) (q02 (third q0)) (q03 (fourth q0)) + (q10 (first q1)) (q11 (second q1)) (q12 (third q1)) (q13 (fourth q1))) + `(,(- (* q00 q10) (* q01 q11) (* q02 q12) (* q03 q13)) + ,(+ (* q00 q11) (* q01 q10) (* q02 q13) (- (* q03 q12))) + ,(+ (* q00 q12) (- (* q01 q13)) (* q02 q10) (* q03 q11)) + ,(+ (* q00 q13) (* q01 q12) (- (* q02 q11)) (* q03 q10)))))) + (else (apply quaternion* (quaternion* (first args) (second args)) (cddr args))))) + +(define (quaternion-conjugate q) + (cons (car q) (vector- (cdr q)))) + +(define (apply-quaternion-to-vector q v) + (let ((w (cons 0 v))) + (cdr (quaternion* q w (quaternion-conjugate q))))) + +; create a quaternion from an angle and vector it rotates about +(define (angle-vector->quaternion angle v) + (let ((n (vector-magnitude v))) + (let ((fac (if (zero? n) 0 (/ (sin (/ angle 2)) n)))) + (cons (cos (/ angle 2)) + (vector-scale fac v))))) + +; give the quaternion rotation from 3 vector a to 3 vector b +(define (vector-vector->quaternion a b) + (angle-vector->quaternion (acos (min 1 (/ (vector-dot a b) (sqrt (* (vector-magnitude^2 a) + (vector-magnitude^2 b)))))) + (vector-cross a b))) + +(define (quaternion a b c d) (list a b c d)) + +; same as 4 vector +(define quaternion-magnitude vector-magnitude) + +; same as 4 vector +(define quaternion-normalize vector-normalize) + +; angle of rotation of quaternion +(define (quaternion-angle q) + (* 2 (acos (first q)))) + +; angle of rotation given just 3 imaginary parts +(define (quaternion-imaginary-vector-angle v) + (* 2 (asin (vector-magnitude v)))) + +; convert quaternion rotation to 3x3 discrete cosine matrix rotation +(define (quaternion->discrete-cosine-matrix q) + (let ((q0 (first q)) (q1 (second q)) (q2 (third q)) (q3 (fourth q))) + (let ((q0^2 (square q0)) (q1^2 (square q1)) (q2^2 (square q2)) (q3^2 (square q3))) + (matrix + `((,(+ q0^2 q1^2 (- q2^2) (- q3^2)) ,(* 2 (- (* q1 q2) (* q0 q3))) ,(* 2 (+ (* q1 q3) (* q0 q2)))) + (,(* 2 (+ (* q1 q2) (* q0 q3))) ,(+ q0^2 (- q1^2) q2^2 (- q3^2)) ,(* 2 (- (* q2 q3) (* q0 q1)))) + (,(* 2 (- (* q1 q3) (* q0 q2))) ,(* 2 (+ (* q2 q3) (* q0 q1))) ,(+ q0^2 (- q1^2) (- q2^2) q3^2))))))) + +; convert 3x3 discrete cosine matrix rotation to quaternion +(define (discrete-cosine-matrix->quaternion m) + (let ((q0 (/ (sqrt (+ 1 (matrix-ref m 0 0) (matrix-ref m 1 1) (matrix-ref m 2 2))) 2))) + `(,q0 + ,(/ (- (matrix-ref m 2 1) (matrix-ref m 1 2)) (* 4 q0)) + ,(/ (- (matrix-ref m 0 2) (matrix-ref m 2 0)) (* 4 q0)) + ,(/ (- (matrix-ref m 1 0) (matrix-ref m 0 1)) (* 4 q0))))) + +; convert a quaternion rotation to pitch roll and yaw +(define (quaternion->pitch-roll-yaw q) + (let ((q0 (first q)) (q1 (second q)) (q2 (third q)) (q3 (fourth q))) + (let ((q0^2 (square q0)) (q1^2 (square q1)) (q2^2 (square q2)) (q3^2 (square q3))) + `(,(atan (* 2 (+ (* q0 q1) (* q2 q3))) (+ q0^2 (- q1^2) (- q2^2) q3^2)) + ,(asin (* 2 (- (* q0 q2) (* q1 q3)))) + ,(atan (* 2 (+ (* q1 q2) (* q0 q3))) (+ q0^2 q1^2 (- q2^2) (- q3^2))))))) + +; convert pitch roll and yaw angles to quaternion rotation +(define (pitch-roll-yaw->quaternion p r y) + (let ((p2 (/ p 2)) (r2 (/ r 2)) (y2 (/ y 2))) + (let ((sp2 (sin p2)) (sr2 (sin r2)) (sy2 (sin y2)) + (cp2 (cos p2)) (cr2 (cos r2)) (cy2 (cos y2))) + `(,(+ (* cp2 cr2 cy2) (* sp2 sr2 sy2)) + ,(- (* sp2 cr2 cy2) (* cp2 sr2 sy2)) + ,(+ (* cp2 sr2 cy2) (* sp2 cr2 sy2)) + ,(- (* cp2 cr2 sy2) (* sp2 sr2 cy2)))))) + +; use rotate a vector around a quaternion using two different means of computation +; to test library correctness +(define (test-quaternion-on-vector q v) + (print "q applied to v : " (apply-quaternion-to-vector q v)) + (print "q converted to matrix multiplied by v: " + (row-matrix->vector + (matrix* (quaternion->discrete-cosine-matrix q) + (vector->row-matrix v))))) + +; given an orientation quaternion, and pitch roll, yaw rates relative to +; this orientation, calculate the quaternion rotation rate +(define (quaternion-rate quat p q r) + (vector-scale (/ 2) (row-matrix->vector + (matrix* + (matrix `(( 0 ,(- p) ,(- q) ,(- r)) + (,p 0 , r ,(- q)) + (,q ,(- r) 0 , p ) + (,r ,q ,(- p) 0 ))) + (vector->row-matrix quat))))) diff --git a/relay.scm b/relay.scm new file mode 100644 index 0000000..c0b7597 --- /dev/null +++ b/relay.scm @@ -0,0 +1,29 @@ +;; Copyright (C) 2010 Sean D'Epagnier +;; +;; 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. + +;; Control relays connected to virtual comm port + +(declare (unit relay)) + +(define relay-ports '()) + +(define (relay-setup device) + (verbose "relay-setup " device) + (let-values (((i o) (open-serial-device device 9600))) + (set! relay-ports (append relay-ports (list o))))) + +(define (relay-set index on?) + (let ioloop ((index index) (relay-ports relay-ports)) + (cond ((null? relay-ports) (error "relay index invalid" index)) + ((> index 8) (ioloop (- index 8) (cdr relay-ports))) + (else + (with-output-to-port (car relay-ports) + (lambda () + (write-char (integer->char 255)) + (write-char (integer->char index)) + (write-char (integer->char (if on? 1 0))) + (flush-output))))))) diff --git a/sailingefficiency.scm b/sailingefficiency.scm index 55e19ac..7e6f293 100644 --- a/sailingefficiency.scm +++ b/sailingefficiency.scm @@ -14,7 +14,7 @@ ; updates fields by running-function which takes fields (C A N) ; C is the new data value -; A is the previous average value +; A is the previous value ; N is the count of updates (define (install-running-calculator running-name running-function name field #!optional (index 0)) (data-register-callback @@ -55,4 +55,5 @@ (define (new-wind-direction-speed-plot) (data-register-callback -(define (calculate-hull-speed +(define (calculate-hull-speed-knots LWLft) + (* 1.34 (sqrt LWLft))) diff --git a/sailspeed.scm b/sailspeed.scm new file mode 100644 index 0000000..b989be6 --- /dev/null +++ b/sailspeed.scm @@ -0,0 +1,151 @@ +;; Copyright (C) 2011 Sean D'Epagnier +;; +;; 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. + +;; Use wind speed and direction along with gps speed to build a database +;; storing collected data. This gives an idea of boat performance with +;; a given sail configuration, and lets the user know if they are sheeted wrong + + +(declare (unit sailspeed)) +(declare (uses algebra matrix plot utilities)) + +;; store database as vector of wind speeds at each knot of wind 0-19 +;; of wind directions (each degree) +;; of max boat speed through water + +(define sailspeed-max-windspeed 20) + +(define sailspeed-database + (make-vector (+ 1 sailspeed-max-windspeed) (make-vector 360 #f))) + +(define (sailspeed-database-ref wind-speed wind-direction) + (vector-ref (vector-ref sailspeed-database wind-speed) wind-direction)) + +(define (sailspeed-database-true-wind-speed wind-speed wind-direction) + (first (sailspeed-database-ref wind-speed wind-direction))) + +(define (sailspeed-database-true-wind-direction wind-speed wind-direction) + (second (sailspeed-database-ref wind-speed wind-direction))) + +(define (sailspeed-database-water-speed wind-speed wind-direction) + (third (sailspeed-database-ref wind-speed wind-direction))) + +(define (sailspeed-database-set! wind-speed wind-direction + true-wind-speed true-wind-direction water-speed) + (vector-set! (vector-ref sailspeed-database wind-speed) wind-direction + `(true-wind-speed true-wind-direction water-speed))) + +(define (relative-wind-from-angle-of-attack angle-of-attack wind-speed water-speed water-direction) + (vector+ `(0 ,water-direction) (course+ `(,wind-speed ,(- angle-of-attack)) `(,water-speed 0)))) + + +(define (sailspeed-database-update angle-of-attack wind-speed water-speed water-direction) + (sailspeed-database-set! angle-of-attack wind-speed + true-wind-speed true-wind-direction + (max water-speed + (sailspeed-database-water-speed wind-speed wind-direction)))) + +; The sailboat transform: +; 2 +; / (sin(a/2) \ +; sin(a0) * sin(a) * | ------- | = VW * eta +; \ sin(a0-a) / +; +; a0 – real wind direction relatively to boat +; +; a – apparent wind direction relatively to boat +; VW – real wind velocity +; eta – sailing boat's slowly changing function of many parameters (1/velocity units), +; which incorporates most of the boat's and rigging's specific Physics. +; +(define (sailspeed-compute-sailboat-transform) + +; Find boat speed in terms of transform +; +; VW * sin(a0 - a) +; VB = ---------------- +; sin(a) +(define (sailspeed-sailboat-transform-boat-speed a0 a VW) + (/ (* VW (sin (deg2rad (- a0 a)))) + (sin (deg2rad a)))) + + +; compute optimal course, or courses if tacking/jibing is needed to achieve highest +; velocity made good + +(define (sailspeed-optimal-course true-wind) + (let each-angle ((best-angle 0) (best-vmg 0) (current-angle 0)) + (if (= current-angle 360) best-angle + (let ((best-speed (sailspeed-database-water-speed wind-speed current-angle))) + (if best-speed + (let ((vmg (* best-speed (cos (* (if upwind? 1 -1) (deg2rad best-vmg)))))) + (if (> vmg best-vmg) + (each-angle current-angle vmg (+ current-angle 1)) + (each-angle best-angle best-vmg (+ current-angle 1)))) + (each-angle best-angle best-vmg (+ current-angle 1))))))) + +(define (sailspeed-optimal-tack-angle wind-speed) + (sailspeed-optimal-velocity-made-good #t wind-speed)) + +(define (sailspeed-optimal-jibe-angle wind-speed) + (sailspeed-optimal-velocity-made-good #f wind-speed)) + +(define (sailspeed-database-print wind-speed-step wind-direction-step) + (let ((wind-speeds (sequence 1 sailspeed-max-wind wind-speed step)) + (wind-directions (sequence 0 360 wind-direction-step))) + (display "\t") + (for-each + (lambda (wind-speed) (display wind-speed) (display "\t")) + wind-speeds) + + (for-each + (lambda (wind-direction) + (for-each + (lambda (wind-speed) + (display wind-direction) + (display "\t") + (display (sailspeed-database-ref wind-speed wind-direction)) + (newline)) + wind-speeds)) + wind-directions))) + +(define stability-time 10) ; 10 seconds is a stable reading + +(define (make-stable-reading thunk tolerance) + (let ((stable-sum (thunk)) + (stable-count 1) + (stable-timer (start-timer))) + (lambda () + (let ((reading (thunk)) + (stable-reading (/ stable-sum stable-count))) + (set! stable-count (+ stable-count 1)) + (set! stable-sum (+ stable-sum reading)) + (cond ((> (abs (- reading stable-reading)) tolerance) + (set! stable-timer (start-timer)) + (set! stable-sum 0) + (set! stable-count 0))) + (if (> (stable-timer) stability-time) + stable-sum #f))))) + +(define sailspeed-update + (let ((stable-wind-speed (make-stable-reading (lambda () (computation-calculate 'wind-speed)) 3)) + (stable-wind-direction (make-stable-reading (lambda () (computation-calculate 'wind-direction)) 10)) + (stable-water-speed (make-stable-reading (lambda () (computation-calculate 'water-speed)) 1)) + (stable-heading (make-stable-reading (lambda () (computation-calculate 'water-heading)) 10)) + (stable-gps-speed (make-stable-reading (lambda () (computation-calculate 'gps-speed)) 3)) + (stable-gps-heading (make-stable-reading (lambda () (computation-calculate 'gps-heading)) 10))) + (lambda () + (let ((wind-speed (stable-wind-speed)) + (wind-direction (stable-wind-direction)) + (water-speed (stable-water-speed)) + (heading (stable-heading)) + (gps-speed (stable-gps-speed)) + (gps-heading (stable-gps-heading))) + (if (and wind-speed wind-direction water-speed heading gps-speed gps-heading) + (let ((true-wind (calculate-true-wind heading wind-speed wind-direction gps-speed gps-heading))) + (sailspeed-database-update wind-speed wind-direction + water-speed (first true-wind) (second true-wind)))))))) diff --git a/sensor.scm b/sensor.scm index c9954aa..c772fbe 100644 --- a/sensor.scm +++ b/sensor.scm @@ -4,13 +4,23 @@ ;; 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. + + ;; This file handles storing values (primarily from sensors) ;; values can be installed, queried, and callbacks can be installed as well ;; this should work even across a network -;(use srfi-69) +(declare (unit sensor)) + +(use srfi-69) + +(define sensor-log-ports '()) -(define sensor-log-port #f) +(define (sensor-log-to-port port) + (set! sensor-log-ports (cons port sensor-log-ports))) + +(define (sensor-log-to-file filename) + (sensor-log-to-port (open-output-file filename))) (define sensors (make-hash-table)) @@ -29,6 +39,16 @@ (define (sensor-index name-key) (if (list? name-key) (cadr name-key) 0)) +; get a list if indicies of of sensors by name +(define (sensor-indicies name) + (let each-value ((values (hash-table->alist sensors))) + (cond ((null? values) '()) + ((eq? (caar values) name) + (cons (cadar values) + (each-value (cdr values)))) + (else + (each-value (cdr values)))))) + ; get the proper hash key for a name or key (define (sensor-key name-key) (list (sensor-name name-key) (sensor-index name-key))) @@ -37,15 +57,24 @@ (define (sensor-query name-key) (let ((key (sensor-key name-key))) (if (hash-table-exists? sensors key) - (car (hash-table-ref sensors key)) + (hash-table-ref sensors key) (error "sensor-query on nonexistant key " key)))) ; Open a file and automatically update values from it -(define (sensor-replay-logfile filename rate) - (thread-start! +(define (sensor-replay-logfile arg) + (let ((options (create-options + (list (make-number-verifier 'rate "rate of replay" 1 .001 1000)) + (string-append " Set input log file, and option\n") + #f))) + (let ((filename (parse-basic-arg-options-string options arg))) + (verbose "replaying data from log file '" filename "' at " (options 'rate) "x speed") + + (create-task + (string-append "replay task for file: " filename) (lambda () (with-input-from-file filename (lambda () + (let ((sensor-key-mapping (make-hash-table))) (let each-line () (let ((line (read-line))) (if (not (eof-object? line)) @@ -54,27 +83,57 @@ (let ((time (car split)) (key (cadr split)) (value (caddr split))) - (thread-sleep! (/ (- time (current-time)) replayrate)) - (sensor-update key value))) + (task-sleep! (- (/ time (options 'rate)) (elapsed-seconds))) + (if (hash-table-exists? sensor-key-mapping key) + (sensor-update (hash-table-ref sensor-key-mapping key) value) + (let ((new-key `(,(sensor-name key) + ,(sensor-new-index (sensor-name key))))) + (verbose "replay sensor from file '" filename "' " key " -> " new-key) + (hash-table-set! sensor-key-mapping key new-key)))))))) (each-line)) (verbose "log file '" filename "' complete"))))))))) +(define (make-raw-sensor-computation name index) + (computation-register (string->symbol (string-append (symbol->string name) + "." + (number->string index))) + (string-append (symbol->string name) " " (number->string index)) + `(,name) + (lambda () (sensor-query `(,name ,index))))) + ; whenever a value needs to be updated, call this ; if the table did not have this value, it is now added (define (sensor-update name-key value) - (verbose "sensor update: " name-key " " value) - (if sensor-log-port - (with-output-to-port - sensor-log-port - (lambda () - (print `(,(current-time) ,name-key ,value))))) - - (let ((key (sensor-key name-key))) - (if (not (hash-table-exists? sensors key)) - (hash-table-set! sensors key (list value)) - (let ((ref (hash-table-ref sensors key))) - (set-car! ref value) - (for-each (lambda (callback) (callback)) (cdr ref)))))) +; (very-verbose "sensor update: " name-key " " value) + (set! sensor-log-ports + (let each-port ((ports sensor-log-ports)) + (cond ((null? ports) '()) + (else + (if (call/cc + (lambda (cont) + (with-exception-handler + (lambda _ (cont #f)) + (lambda () + (with-output-to-port (car ports) + (lambda () (print `(,(elapsed-seconds) ,name-key ,value)))) + (cont #t))))) + (cons (car ports) (each-port (cdr ports))) + (each-port (cdr ports))))))) + + ; add computation for raw sensor data + (if (not (hash-table-exists? sensors (sensor-key name-key))) + (make-raw-sensor-computation (sensor-name name-key) (sensor-index name-key))) + + ; store value locally + (hash-table-set! sensors (sensor-key name-key) value)) + + +(define (sensor-net-client address) + (net-add-client address + '(let ((port (current-output-port))) (sensor-log-to-port port) 'ok) + (lambda (data) + (if (not (eq? data 'ok)) + (sensor-update (second data) (third data)))))) ; return the number of entrees in the table with this name (define (sensor-index-count name) @@ -83,120 +142,81 @@ (index-loop (+ index 1)) index))) +; register a new sensor index without updating it yet, return the index +(define (sensor-new-index name) + (let ((index (sensor-index-count name))) + (make-raw-sensor-computation name index) + (hash-table-set! sensors `(,name ,index) #f) + index)) + ; return a list of valid keys (define (sensor-query-keys) (hash-table-keys sensors)) -; the callback function is called with the updated sensor whenever it is updated -; eg: (sensor-install-callback 'gps (lambda (gpssensor) (print "gpssensor: " gpssensor))) -(define (sensor-register-callback name-keys callback) - (let ((keys (map sensor-key name-keys))) - (cond ((let all ((keys keys)) - (cond ((null? keys) #t) - ((hash-table-exists? sensors (car keys)) (all (cdr keys))) - (else #f))) - (let ((refs (map (lambda (key) (hash-table-ref sensors key)) keys))) - (for-each (lambda (key ref) - (hash-table-set! sensors key - (cons (car ref) (cons (lambda () - (callback (map car refs)) - (cdr ref)))))) - keys refs)) - #t) - (net-client ; attempt to install a remote callback on the server - (hash-table-set! sensors key (list #f callback)) ; local callback - (sensor-register-remote-callback key) - #t) - (else - (verbose "failed to register callback for " key) - #f)))) - -; register a callback remotely with the server to provide this name-key -(define (sensor-register-remote-callback name-key) - (let ((name (sensor-name name-key)) - (index (sensor-index name-key))) - (let ((net-index (- index (sensor-index-count name)))) - (verbose "configuring server to send updates for " key) - (if (not net-client) - (error "sensor-register-remote-callback called without server connection")) - (with-output-to-port - `(if (not (sensor-install-callback - ,(name net-index) - (let ((port (current-output-port))) - (lambda (sensor) - (write (list 'sensor-update ,key sensor) port))))) - (write (list 'display "server failed to provide " ,key) port)) - net-client)))) - - - -;;; types for fields within each physical sensor - -(define types (make-hash-table)) - -; provide a means to extract elements from a list by name -; handler should take the subfield and a value -(define (type-register name fields) - (hash-table-set! types name fields)) - -; generic means to operate on the type -(define (type-field-apply name field values func) - (let each-def ((definition (hash-table-ref types name)) - (values values)) - (cond - ((null? definition) (error "Field " field " does not exist in " name)) - ((eq? field (car definition)) (func values)) - (else (each-def (cdr definition) (cdr values)))))) - -; get the right field from a list -(define (type-field-get name field values) - (type-field-apply name field values car)) - -; set a field in a list -(define (type-field-set! name field values value) - (type-field-apply name field values (lambda (values) (set-car! values value)))) - ;;;; line reader used by various specific sensors ;; handles reading data from input sensors and executing callback functions (define readers '()) (define (make-reader port proc end?) - (thread-start! + (create-periodic-task + (string-append "reader for " (port->string port)) .05 + (let ((cur "")) + (lambda () + (if (char-ready? port) + (let ((c (read-char port))) + (call/cc + (lambda (flushed) + (cond ((end? c (lambda () + (set! cur "") + (flushed #t))) + (proc cur) + (set! cur "")) + (else (set! cur (string-append cur (string c))))))))))))) + + +(define (make-line-reader port-proc proc) + (create-periodic-task .05 (lambda () - (let ((cur "")) - (let loop () - (if (char-ready? port) - (let ((c (read-char port))) - (call/cc - (lambda (flushed) - (cond ((end? c (lambda () - (set! cur "") - (flushed #t))) - (proc cur) - (set! cur "")) - (else (set! cur (string-append cur (string c)))))))) - (thread-sleep! .1)) - (loop)))))) - -(define (make-line-reader port proc) - (make-reader port proc (lambda (c flush) (equal? c #\newline)))) + (let loop () + (if (char-ready? (port-proc)) + (let ((l (read-line (port-proc)))) + (proc l) + (loop))))))) + (define (sensor-field-get name-key field) (if (sensor-contains? name-key) (type-field-get (sensor-name name-key) field - (sensor-query name-key)) - 0)) - + (sensor-query name-key)) 0)) + +(define (open-serial-device-with-handler device baud handler) + (call/cc (lambda (bail) + (verbose "Trying to open serial device at " device) + (let ((ret (with-exception-handler + (lambda _ + (apply bail (handler _))) + (lambda () + (let ((cmd (string-append "stty -F " device " " (number->string baud) + " ignbrk -icrnl -opost -onlcr -isig" + " -icanon -iexten -echo -echoe -echok" + " -echoctl -echoke"))) + (verbose "executing shell command: " cmd) + (system cmd)) + (list (open-input-file device) + (open-output-file device)))))) + (verbose "Success opening " device) + (apply values ret))))) + (define (open-serial-device device baud) - (call/cc - (lambda (return) - (with-exception-handler - (lambda _ - (print "device initialization failed on: " device ": " _) - (return #f)) - (lambda () - (system (string-append "stty -F " device " " (number->string baud))) - (verbose "Trying to open serial device at " device) - (verbose "Success opening " device) - (values (open-input-file device) - (open-output-file device))))))) + (open-serial-device-with-handler + device baud (lambda _ + (print "device initialization failed on: " device ": " _) + (exit)))) + +(define (try-opening-serial-devices devices baud) + (if (null? devices) + (values #f #f) + (let-values (((i o) (open-serial-device-with-handler (car devices) baud + (lambda _ '(#f #f))))) + (if (and i o) (values i o) + (try-opening-serial-devices (cdr devices) baud))))) diff --git a/sheetcontrol.scm b/sheetcontrol.scm new file mode 100644 index 0000000..5bfe93e --- /dev/null +++ b/sheetcontrol.scm @@ -0,0 +1,17 @@ +;; Copyright (C) 2010 Sean D'Epagnier +;; +;; 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. + + +;; The goal is to take input from various sources, and over time and find the +;; optimal adjustment update. The most important and obvious input is wind +;; direction data, and is the only required input + +;; simple corellation between position, wind direction and wind speed as well +;; as a wide varience to take care of factors like + +(define (simple-sheet-control) + ( diff --git a/sound.scm b/sound.scm index 7edd0e1..4dff487 100644 --- a/sound.scm +++ b/sound.scm @@ -5,6 +5,8 @@ ;; License as published by the Free Software Foundation; either ;; version 3 of the License, or (at your option) any later version. +(declare (unit sound)) + ; Support various sound drivers (define (sound-play-wav-file filename) (warning "No sound driver loaded, using alsaplayer... ") @@ -13,7 +15,7 @@ (define (sound-init driver) (case driver ((openal) - (use openal) +; (use openal) (let ((device (alc:OpenDevice #f)) (context (alc:CreateContext device #f))) (alc:MakeContextCurrent context) diff --git a/spherical.scm b/spherical.scm index 6810882..c142723 100644 --- a/spherical.scm +++ b/spherical.scm @@ -5,6 +5,9 @@ ;; License as published by the Free Software Foundation; either ;; version 3 of the License, or (at your option) any later version. +(declare (unit spherical)) + +(declare (uses algebra)) ;; This file contains routines for computations in spherical coordinates @@ -12,11 +15,17 @@ (define (spherical-distance lat1 lon1 lat2 lon2) (acos (+ (* (sin lat1) (sin lat2)) (* (cos lat1) (cos lat2) (cos (- lon1 lon2)))))) +(define (spherical-distance-degrees lat1 lon1 lat2 lon2) + (spherical-distance (d2r lat1) (d2r lon1) (d2r lat2) (d2r lon2))) + ;; the initial heading for a great circle course from point 1 to point 2 (define (spherical-heading lat1 lon1 lat2 lon2) (remainder (atan (* (sin (- lon1 lon2)) (cos lat2)) (- (* (cos lat1) (sin lat2)) (* (sin lat1) (cos lat2) (cos (- lon1 lon2))))) - (* 2 pi))) + (* 2 Pi))) + +(define (spherical-heading-degrees lat1 lon1 lat2 lon2) + (r2d (spherical-heading (d2r lat1) (d2r lon1) (d2r lat2) (d2r lon2)))) ;; return the latitude and longitude of the normal axis of the great circle (define (spherical-great-circle lat1 lon1 lat2 lon2) @@ -35,7 +44,7 @@ (asin (+ (* (sin lat) (cos d)) (* (cos lat) (sin d) (cos tc)))) (remainder (- lon (atan (* (sin tc) (sin d) (cos lat) (- (cos d) (* (sin lat1) (sin lat)))))) - (* 2 pi)))) + (* 2 Pi)))) ;; give the maximum latitude of a great circle given a latitude and course ;; derived from clairaut's formula: @@ -51,9 +60,9 @@ (glBegin GL_LINE_LOOP (let ((z (sin lat)) (r (cos lat)) - (astep (/ (* 2 pi) steps))) + (astep (/ (* 2 Pi) steps))) (let each-angle ((a 0)) - (cond ((< a (* 2 pi)) + (cond ((< a (* 2 Pi)) (gl:Vertex3f (* r (sin a)) (* r (cos a)) z) (each-angle (+ a astep)))))))) @@ -61,9 +70,9 @@ (gl:PushMatrix) (gl:Rotated lon 0 0 1) (glBegin GL_LINE_LOOP - (let ((astep (/ (* 2 pi) steps))) + (let ((astep (/ (* 2 Pi) steps))) (let each-angle ((a 0)) - (cond ((< a (* 2 pi)) + (cond ((< a (* 2 Pi)) (gl:Vertex3f 0 (* r (sin a)) (* r (cos a))) (each-angle (+ a astep))))))) (gl:PopMatrix)) diff --git a/task.scm b/task.scm new file mode 100644 index 0000000..4841563 --- /dev/null +++ b/task.scm @@ -0,0 +1,143 @@ +;; Copyright (C) 2011 Sean D'Epagnier +;; +;; 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 task)) +(declare (uses srfi-1 srfi-18 utilities)) + +; list of tasks +(define tasks '()) + +(define task-info first) +(define task-period second) +(define task-sleep-until third) +(define task-thunk fourth) +(define task-continuation fifth) + +(define current-task #f) ; task currently running +(define current-task-cont #f) ; continuation to exit current task + +(define (task-set-period! task period) + (set-car! (cdr task) period)) + +(define (task-set-sleep-until! task sleep-until) + (set-car! (cddr task) sleep-until)) + +(define (task-set-continuation! task cont) + (set-car! (cddddr task) cont)) + +; sleep for the task period if seconds not specified +(define (task-sleep . seconds) + (if (not current-task) (error "task-sleep called from non-task")) + (call/cc (lambda (cont) + (task-set-continuation! current-task (lambda () (cont #t))) + (current-task-cont (if (null? seconds) #t (first seconds)))))) + +(define (task-exit) + (if (not current-task) (error "task-exit called from non-task")) + (current-task-cont 'exit)) + +; Run thunk every period seconds (resolution milliseconds) +(define (create-periodic-task info period thunk) + (very-verbose "create-periodic-task " info " " tasks) + (set! tasks (cons (list info period 0 thunk thunk) tasks))) + +; Run the thunk as a task, when it returns the task exits +(define (create-task info thunk) + (create-periodic-task info 0 + (lambda () thunk + (task-exit)))) + +; could be replaced with a heap if there were many tasks +; returns sorted task list +(define (insert-periodic-task task tasks) + (cond ((null? tasks) (list task)) + ((< (task-sleep-until task) (task-sleep-until (car tasks))) + (cons task tasks)) + (else + (cons (car tasks) (insert-periodic-task task (cdr tasks)))))) + +(define (create-task-scheduler sleep-while-idle) + (let ((timer (start-timer)) + (total-run-time 0) + (total-sleep-time 0) + (last-total-run-time 1) + (task-time 0)) + + (create-periodic-task + "task-stats-task" 1 + (let ((last-time 0)) + (lambda () + (let ((time (timer))) + (let ((elapsed (- time last-time))) + (set! last-time time) + (define (time-percentage t) + (string-append (if (zero? elapsed) "N/A" + (number->string (round-to-places (* 100 (/ t elapsed)) 2))) + "%")) + (verbose "<" (length tasks) " tasks>" + " task-time: " (time-percentage total-run-time) + " non-task: " (time-percentage (- elapsed total-run-time total-sleep-time)) + " sleep: " (time-percentage total-sleep-time)) + + (set! last-total-run-time total-run-time) + (set! total-run-time 0) + (set! total-sleep-time 0) + + ))))) + + (lambda () + (cond ((null? tasks) + (print "No tasks exist, exiting.") + (exit))) + (let ((task (first tasks))) + (cond ((< (task-sleep-until task) (timer)) + (set! current-task task) + + (let ((ret (call/cc (lambda (cont) + (set! current-task-cont cont) + (set! task-time (timer)) + ((task-continuation task)) + (task-set-continuation! task (task-thunk task)) + 'return)))) + (let ((run-time (- (timer) task-time))) + (set! current-task #f) + (set! total-run-time (+ total-run-time run-time)) + (cond ((eq? ret 'exit) + (verbose "task " (task-info task) " exited") + (set! tasks (cdr tasks))) ; delete this task + (else + (cond ((and (> task-time 5) ; dont do this first 5 seconds + (> run-time (/ (task-period task) (length tasks))) + (> (- (timer) (task-sleep-until task)) + (* 2 (task-period task)))) + (verbose "task " (task-info task) " took too long " + " " (round-to-places run-time 5) " seconds" + " doubling period to " (* 2 (task-period task))) + (task-set-period! task (* 2 (task-period task))))) + (task-set-sleep-until! task + (if (number? ret) + (+ (timer) ret) + (+ (task-sleep-until task) + (task-period task)))) + (set! tasks (insert-periodic-task task (cdr tasks))) + ))))) + + (else ; sleep it off + (let ((sleep-start-time (timer))) + (sleep (if sleep-while-idle + (- (task-sleep-until task) sleep-start-time) + .001)) + (let ((sleep-time (- (timer) sleep-start-time))) + (set! total-sleep-time (+ total-sleep-time sleep-time)))))))))) + +(define (task-schedule-loop) + (let ((scheduler (create-task-scheduler #t))) + (let loop () (scheduler) (loop)))) + +(define (current-task-period) + (if current-task (task-period current-task) + (error "call to current-task-period outside of task"))) diff --git a/term-optimizer.scm b/term-optimizer.scm new file mode 100644 index 0000000..92035b5 --- /dev/null +++ b/term-optimizer.scm @@ -0,0 +1,533 @@ +;; Copyright (c) 2007 Alex Shinn. All rights reserved. +;; BSD-style license: http://synthcode.com/license.txt + +;; Simple optimizer for large arithmetic expressions. Applies the +;; distributive law and factors out common subexpressions. + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;; This code is R5RS, plus SORT, PRINT and ->STRING from Chicken. + +;; To compile in Chicken, "csc -s -O2 term-optimizer.scm" will give you +;; a shared library you can load to provide the following procedures: +;; +;; (optimize-expr expr) +;; +;; Returns an optimized version of the symbolic expression `expr', +;; which should be a mathematical expression in terms of symbols, +;; numeric literals, and the operators: +, -, *, /. +;; +;; (optimize-expr '(+ (* a b) (* a c))) +;; => (* a (+ b c)) +;; +;; (pp-expr/C expr [name [type]]) +;; +;; Writes the symbolic s-expression `expr' (an expression of the +;; above form, optionally with LET's) to the current-output-port as a +;; C expression. If `name' is specified, writes it as a C function +;; definition using the given name. Types are specified as `type', +;; which defaults to "double." + +;; History: +;; Version 0.3 (2007/02/20) - constant folding, refactoring (* -1 x) +;; Version 0.2 (2007/02/20) - more optimizations +;; Version 0.1 (2007/02/19) - initial version + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + +(require-extension (srfi 1)) + +;; Compute the cost of an expression, an sexp allowing LET and +;; arithmetic operators. An example of scaling operators is given +;; (typically DIV costs more than MUL costs more than ADD). Currently +;; assumes intermediate computations are free (not necessarily so if +;; not all values fit in registers), and registers and immediates are +;; equally fast (true for x86, RISC usually requires some overhead for +;; loading the values). +(define (expr-cost x) + (if (pair? x) + (if (eq? 'let (car x)) + (fold + (expr-cost (caddr x)) (map expr-cost (map cadr (cadr x)))) + (fold + + ;; (* (case (car x) ((/) 4) ((*) 3) (else 1))) + (- (length (cdr x)) 1) + (map expr-cost (cdr x)))) + 0)) + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;; expression utilities + +;; number < symbol < list +(define (sub-expr< a b) + (cond + ((number? a) (or (not (number? b)) (< a b))) + ((number? b) #f) + ((symbol? a) (or (not (symbol? b)) (stringstring a) (symbol->string b)))) + ((symbol? b) #f) + ((pair? a) + (or (not (pair? b)) + (let lp ((a a) (b b)) + (cond + ((null? a) (not (null? b))) + ((null? b) #f) + ((sub-expr< (car a) (car b)) #t) + ((sub-expr< (car b) (car a)) #f) + (else (lp (cdr a) (cdr b))))))) + (else (not (pair? b))))) + +;; just sorts the terms lexicographically +(define (normalize-sub-expr x) + (if (pair? x) + (cons (car x) (sort (map normalize-sub-expr (cdr x)) sub-expr<)) + x)) + +;; assumes already normalized +(define (unique-exprs ls) + (map cdr (group-with-counts (sort ls sub-expr<)))) + +(define (delete1 x ls) + (let lp ((ls2 ls) (rev '())) + (cond ((null? ls2) ls) + ((equal? x (car ls2)) (append-reverse rev (cdr ls2))) + (else (lp (cdr ls2) (cons (car ls2) rev)))))) + +(define (cartesian-product lol) + (if (null? lol) + (list '()) + (let ((l (car lol)) + (rest (cartesian-product (cdr lol)))) + (append-map + (lambda (x) + (map (lambda (sub-prod) (cons x sub-prod)) rest)) + l)))) + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + +(define (all-op-combinations op ls) + (if (not (pair? ls)) + '() + (let lp1 ((x (car ls)) (ys (cdr ls)) (pairs '())) + (if (null? ys) + (reverse pairs) + (let lp2 ((ls ys) (pairs pairs)) + (if (null? ls) + (lp1 (car ys) (cdr ys) pairs) + (lp2 (cdr ls) (cons (list op x (car ls)) pairs)))))))) + +(define (all-possible-sub-exprs x) + (if (pair? x) + (case (car x) + ((+ *) (append (all-op-combinations (car x) (cdr x)) + (append-map all-possible-sub-exprs (cdr x)))) + ((- /) (append-map all-possible-sub-exprs (cdr x))) + (else (error "unknown operation" (car x)))) + '())) + +(define (group-with-counts ls) + (if (null? ls) + '() + (let lp ((x (car ls)) (ls (cdr ls)) (count 1) (res '())) + (cond + ((null? ls) (reverse (cons (cons count x) res))) + ((equal? x (car ls)) (lp x (cdr ls) (+ count 1) res)) + (else (lp (car ls) (cdr ls) 1 (cons (cons count x) res))))))) + +(define (sub-expr-stats x) + (group-with-counts + (sort + (map normalize-sub-expr (all-possible-sub-exprs x)) + sub-expr<))) + +;; attempt to extract all elements from ls1 in ls2, returns the +;; remaining elements in ls2, or #f if ls1 isn't a subset +(define (can-extract ls1 ls2) + (let lp1 ((ls1 ls1) (ls2 ls2)) + (if (null? ls1) + ls2 + (let lp2 ((ls ls2) (rev '())) + (cond + ((null? ls) #f) + ((equal? (car ls1) (car ls)) + (lp1 (cdr ls1) (append (reverse rev) (cdr ls)))) + (else (lp2 (cdr ls) (cons (car ls) rev)))))))) + +(define (replace-var x name to) + (let lp ((x x)) + (cond + ((pair? x) + (if (eq? 'let (car x)) + (let ((vars (map car (cadr x))) + (vals (map lp (map cadr (cadr x))))) + (let ((body (if (memq name vars) (cddr x) (map lp (cddr x))))) + (cons 'let (cons (map list vars vals) body)))) + (map lp x))) + ((eq? x name) to) + (else x)))) + +(define (replace-sub-expressions x from to) + (let ((op (car from)) (args (cdr from))) + (let lp ((x x)) + (if (pair? x) + (let ((x (map lp x))) + (if (eq? (car x) op) + (cond + ((can-extract args (cdr x)) + => (lambda (rest) + (if (pair? rest) + (let ((tail (lp (cons op rest)))) + (cons op (cons to (if (pair? tail) + (cdr tail) + (list tail))))) + to))) + (else x)) + x)) + x)))) + +(define *temp-count* 0) + +;; FIXME: greedy - not necessarily optimal +(define (eliminate-sub-expressions x) + (let lp ((x x)) + (if (not (pair? x)) + x + (let ((stats (sort (sub-expr-stats x) (lambda (a b) (> (car a) (car b)))))) + (if (<= (caar stats) 1) + x + (let ((tmp (string->symbol + (string-append "t" (number->string *temp-count*))))) + (set! *temp-count* (+ *temp-count* 1)) + `(let ((,tmp ,(cdar stats))) + ,(lp (replace-sub-expressions x (cdar stats) tmp))))))))) + +(define (inverse-op x) + (case x ((+) '-) ((-) '+) ((*) '/) ((/) '*) (else #f))) + +(define (symbol-fold x) + (cond + ;; handle canceled terms + ((and (pair? x) (memq (car x) '(/ -))) + (if (memq (cadr x) (cddr x)) + (let ((others (delete1 (cadr x) (cddr x)))) + (let ((res (if (eq? (car x) '-) 0 1))) + (if (null? others) + res + (constant-fold (cons (car x) (cons res) others))))) + x)) + ;; maybe (* x 2) => (+ x x), (+ x y x z x) => (+ (* x 3) y z) etc... + (else x))) + +(define (constant-fold x) + (if (pair? x) + (case (car x) + ((let) + (cons 'let + (cons (map (lambda (v) (list (car v) (constant-fold (cadr v)))) (cadr x)) + (map constant-fold (cddr x))))) + ((+ * / -) + (symbol-fold + (let ((args (map constant-fold (cdr (flatten-expr x))))) + (receive (lits others) (partition number? args) + (if (and (pair? lits) (pair? (cdr lits))) + (let ((res (eval (if (and (memq (car x) '(/ -)) (not (number? (car args)))) + (cons (inverse-op (car x)) lits) + (cons (car x) lits)) + (scheme-report-environment 5)))) + (cond + ((null? others) + res) + ((and (memq (car x) '(/ -)) (not (number? (car args)))) + (cons (car x) (cons (car others) (cons res (cdr others))))) + (else + (cons (car x) (cons res others))))) + (cons (car x) args)))))) + (else + (map constant-fold x))) + x)) + +(define (flatten-expr x) + (if (pair? x) + (case (car x) + ((let) + (cons 'let (cons (map (lambda (v) (list (car v) (flatten-expr (cadr v)))) (cadr x)) + (map flatten-expr (cddr x))))) + ((+ *) + (receive (same other) + (partition (lambda (y) (and (pair? y) (eq? (car x) (car y)))) + (map flatten-expr (cdr x))) + (cons (car x) (append other (apply append (map cdr same)))))) + (else (map flatten-expr x))) + x)) + +(define (refactor*-1 x) + (if (pair? x) + (case (car x) + ((let) (cons 'let (cons (map (lambda (v) (list (car v) (refactor*-1 (cadr v)))) + (cadr x)) + (map refactor*-1 (cddr x))))) + ((+) + (receive (neg pos) + (partition (lambda (y) (and (pair? y) (eq? '* (car y)) (memv -1 y))) + (map refactor*-1 (cdr x))) + (if (pair? neg) + (let* ((neg (map (lambda (y) (delete1 -1 y)) neg)) + (neg-sum (if (pair? (cdr neg)) (cons '+ neg) (car neg)))) + (if (null? pos) + (list '* -1 neg-sum) + (list '- + (if (pair? (cdr pos)) (cons '+ pos) (car pos)) + neg-sum))) + x))) + (else (map refactor*-1 x))) + x)) + +(define (used-more-than-once? sym x) + (let ((count 0)) + (call-with-current-continuation + (lambda (return) + (let lp ((x x)) + (if (pair? x) + (if (eq? 'let (car x)) + (begin + (for-each lp (map cadr (cadr x))) + (if (not (memq sym (map car (cadr x)))) + (for-each lp (cddr x)))) + (for-each lp x)) + (if (eq? x sym) + (begin + (set! count (+ count 1)) + (if (>= count 2) + (return #t)))))) + #f)))) + +(define (remove-extra-temps x) + (if (pair? x) + (if (eq? 'let (car x)) + (let lp ((ls (cadr x)) (body (remove-extra-temps (caddr x))) (vars '())) + (if (null? ls) + (if (null? vars) + body + `(let ,(reverse vars) ,body)) + (let ((var (caar ls)) (val (remove-extra-temps (cadar ls)))) + (if (or (number? val) (not (used-more-than-once? var body))) + (lp (cdr ls) (replace-var body var val) vars) + (lp (cdr ls) body (cons (list var val) vars)))))) + (map remove-extra-temps x)) + x)) + +(define (renumber-temps x) + (let ((i 0)) + (let lp ((x x)) + (if (pair? x) + (if (eq? 'let (car x)) + (let* ((tmps (map (lambda (v) + (let ((tmp (string->symbol (string-append "t" (number->string i))))) + (set! i (+ i 1)) + tmp)) + (cadr x))) + (vals (map-in-order lp (map cadr (cadr x))))) + (let lp ((ls (map car (cadr x))) + (tmps tmps) + (vals vals) + (body (map-in-order lp (cddr x))) + (vars '())) + (if (null? ls) + (cons 'let (cons (reverse vars) body)) + (lp (cdr ls) + (cdr tmps) + (cdr vals) + (replace-var body (car ls) (car tmps)) + (cons (list (car tmps) (car vals)) vars))))) + (map-in-order lp x)) + x)))) + +;; FIXME: doesn't explore all combinations, need to expand fully first +(define (all-distributions x) + (cond + ((not (pair? x)) + (list x)) + ((eq? 'let (car x)) + (error "being lazy about let factoring")) + ((not (any pair? (cdr x))) ; no sub-exprs to distribute + (list x)) + ((eq? '+ (car x)) + (let ((terms (cdr x))) + ;; loop over all possible factors + (let lp1 ((factors (unique-exprs (append-map cdr (cdr x)))) + (res (list x))) + (if (null? factors) + res + (let ((a (car factors))) + (let lp2 ((ls terms) (used '()) (unused '())) + (if (null? ls) + (if (or (null? used) (null? (cdr used))) + (lp1 (cdr factors) res) + ;; yay, we can apply the distributive law! + (let ((dist (list '* a (cons '+ used)))) + (if (null? unused) + (lp1 (cdr factors) (cons dist res)) + (lp1 (cdr factors) (cons (cons '+ (cons dist unused)) res))))) + (cond + ((and (pair? (car ls)) + (eq? '* (caar ls)) + (pair? (cdar ls)) + (can-extract (list a) (cdar ls))) + => (lambda (rem) + (if (null? (cdr rem)) + (lp2 (cdr ls) (cons (car rem) used) unused) + (lp2 (cdr ls) (cons (cons '* rem) used) unused)))) + (else (lp2 (cdr ls) used (cons (car ls) unused))))))))))) + (else ; other arbitrary operator + (cartesian-product (map all-distributions x))))) + +;; Searches all refactorings via the distributive law for the most +;; optimal formulation according to EXPR-COST. +(define (optimize-expr x) + (define (opt1-expr x) + (refactor*-1 (flatten-expr (constant-fold (eliminate-sub-expressions x))))) + (set! *temp-count* 0) + (refactor*-1 + (flatten-expr + (renumber-temps + (remove-extra-temps + (let opt ((x x)) + (if (pair? x) + (if (eq? 'let (car x)) + ;; FIXME: let handling is sub-optimal, but not used as input + (cons (car x) + (cons (map (lambda (v) (list (car v) (opt (cdr v)))) + (cadr x)) + (map opt (cddr x)))) + (let ((ls (all-distributions x)) + (best (opt1-expr x))) + (let lp ((ls ls) (cost (expr-cost best)) (best best)) + (if (null? ls) + best + (let* ((expr2 (opt1-expr (car ls))) + (cost2 (expr-cost expr2))) + (if (< cost2 cost) + (lp (cdr ls) cost2 expr2) + (lp (cdr ls) cost best))))))) + x))))))) + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + +(define (call-with-ids x proc) + (let ((res + (let lp ((x x) (bound '(* + - /)) (local '()) (free '())) + (cond + ((pair? x) + (if (eq? 'let (car x)) + (let ((tmp (lp (cons '+ (map cadr (cadr x))) bound local free))) + (lp (caddr x) + (lset-union eq? (map car (cadr x)) bound) + (lset-union eq? (map car (cadr x)) (car tmp)) + (cdr tmp))) + (let lp2 ((ls x) (local local) (free free)) + (if (null? ls) + (cons local free) + (let ((tmp (lp (car ls) bound local free))) + (lp2 (cdr ls) (car tmp) (cdr tmp))))))) + ((symbol? x) + (if (and (not (memq x bound)) (not (memq x free))) + (cons local (cons x free)) + (cons local free))) + (else + (cons local free)))))) + (proc (sort (car res) sub-expr<) (sort (cdr res) sub-expr<)))) + +;; FIXME: doesn't break long lines +(define (pp-expr/C x . o) + (define (prec a) (case a ((*) 1) ((/) 2) ((+) 3) ((-) 4) (else 5))) + (define (prec< a b) (< (prec a) (prec b))) + (define (prec!= a b) (not (= (prec a) (prec b)))) + (let* ((name (and (pair? o) (car o))) + (type (or (and (pair? o) (pair? (cdr o)) (cadr o)) 'double)) + (indent (if name " " ""))) + (define (pp x line? prec) + (if line? (display indent)) + (cond + ((and (pair? x) (eq? 'let (car x))) + (pp (last x) line? prec)) + ((pair? x) + (if (prec!= prec (car x)) (display "(")) + (let* ((op (car x)) + (str (string-append " " (->string op) " "))) + (pp (cadr x) #f op) + (for-each + (lambda (y) (display str) (pp y #f op)) + (cddr x))) + (if (prec!= prec (car x)) (display ")"))) + (else + (write x))) + (if line? (display ";\n"))) + (call-with-ids x + (lambda (local free) + (if name + (begin + (display type) (display " ") (display name) (display " (") + (display (string-intersperse + (map (lambda (x) (string-append (->string type) " " x)) + (map ->string free)) + ", ")) + (display ") {\n"))) + (if (pair? local) + (begin + (display indent) + (display type) (display " ") + (display (string-intersperse (map ->string local) ", ")) + (display ";\n"))) + ;; write out let's + (let lp ((x x)) + (cond + ((and (pair? x) (eq? 'let (car x))) + (let ((vals (map lp (map cadr (cadr x))))) + (for-each + (lambda (name val) + (display indent) (display name) (display " = ") + (pp val #f '=) (display ";\n")) + (map car (cadr x)) + vals) + (last (map lp (cddr x))))) + ((pair? x) + (map lp x)) + (else + x))) + ;; write return value + (display indent) (display "return ") + (pp x #f 'top) (display ";\n") + (if name (display "}\n")))))) + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;; Example: + +;; (-D*D*F*F + 2*C*D*F*G - C*C*G*G + D*D*E*H - 2*B*D*G*H + A*G*G*H +;; - 2*C*D*E*I + 2*B*D*F*I + 2*B*C*G*I - 2*A*F*G*I - B*B*I*I +;; + A*E*I*I + C*C*E*J - 2*B*C*F*J + A*F*F*J + B*B*H*J - A*E*H*J) +;; / (C*C*E - 2*B*C*F + A*F*F + B*B*H - A*E*H) + +;; rewrote subtractions as * -1, need to consider that in optimizations +(define test-expr + '(/ (+ (* -1 D D F F) (* 2 C D F G) (* -1 C C G G) (* D D E H) (* -2 B D G H) + (* A G G H) (* -2 C D E I) (* 2 B D F I) (* 2 B C G I) (* -2 A F G I) + (* -1 B B I I) (* A E I I) (* C C E J) (* -2 B C F J) (* A F F J) + (* B B H J) (* -1 A E H J)) + (+ (* C C E) (* -2 B C F) (* A F F) (* B B H) (* -1 A E H)))) + +;; testing in scheme +(define (make-fun x) + (call-with-ids x (lambda (local free) (eval `(lambda ,free ,x) (scheme-report-environment 5))))) + +;; (define f1 (make-fun test-expr)) +;; (define f2 (make-fun (optimize-expr test-expr))) + +'(do ((i 0 (+ i 1))) + ((= i 100)) + (let ((r (lambda () (exact->inexact (/ (random 100) (+ 1 (random 100))))))) + (let ((a (r)) (b (r)) (c (r)) (d (r)) (e (r)) (f (r)) (g (r)) (h (r)) (i (r)) (j (r))) + (let ((x (f1 a b c d e f g h i j)) + (y (f2 a b c d e f g h i j))) + (if (> (abs (if (zero? y) x (- 1 (/ x y)))) 0.1) + (print "FAIL on inputs " (list a b c d e f g h i j) ": expected " x " got " y)))))) + +;; if you want to spit it back out to C +;; (pp-expr/C (optimize-expr test-expr) 'FOO 'double) + diff --git a/test.scm b/test.scm deleted file mode 100644 index a60dddc..0000000 --- a/test.scm +++ /dev/null @@ -1,9 +0,0 @@ -(use srfi-1 srfi-69) - -(define computations (make-hash-table)) - -(define (computation-register name info calculation) - (hash-table-set! computations name - (list info calculation))) - -(computation-register 'time "time in seconds since start" current-time) diff --git a/tiltcompensation.scm b/tiltcompensation.scm new file mode 100644 index 0000000..4287b84 --- /dev/null +++ b/tiltcompensation.scm @@ -0,0 +1,338 @@ +;; Copyright (C) 2010 Sean D'Epagnier +;; +;; 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. + +;; Read from intertial sensors (accelerometer and gyroscope..) and command +;; electric motors to counteract this motion + +(declare (unit tiltcompensation)) +(declare (uses algebra matrix plot utilities)) + +(use gl glu) + +(include "glshortcuts.scm") + +(define tilt-history '()) + +(define (update-history) + (let ((cur-time (computation-calculate 'time)) + (value (computation-calculate 'accelerometer.5))) + (if value + (set! tilt-history + (cons (list cur-time value) + (let new-history ((tilt-history tilt-history)) + (if (or (null? tilt-history) + (< (caar tilt-history) (- cur-time 4))) + '() + (cons (car tilt-history) (new-history (cdr tilt-history)))))))))) + +(define (compute-amplitude-period-phase-bias bail) + (define noise-level 8) + (define min-amplitude (* noise-level 2)) + (if (null? tilt-history) (bail #f)) + (let ((avg (apply average (map cadr tilt-history)))) + (define (last-history-cross tilt-history bail) + (let loop ((tilt-history tilt-history) + (cross #f)) + (cond ((null? tilt-history) + (verbose "no rolling detected") + (bail #f)) + ((< (abs (- (cadar tilt-history) avg)) noise-level) + (loop (cdr tilt-history) #t)) + ((and cross + (> (abs (- (cadar tilt-history) avg)) noise-level)) + tilt-history) + (else (loop (cdr tilt-history) #f))))) + + (define (sub-history tilt-history stop) + (if (eq? tilt-history stop) '() + (cons (car tilt-history) (sub-history (cdr tilt-history) stop)))) + + (let* ((a (last-history-cross tilt-history bail)) + (b (last-history-cross a bail)) + (c (last-history-cross b bail))) + (let* ((period (abs (- (caar a) (caar c)))) + (amplitude + (let ((values (map cadr (sub-history a c)))) + (/ (- (apply max values) + (apply min values)) 2))) + (phase (/ (remainder + (- (caar tilt-history) + (if (> (cadar a) avg) (caar a) (caar b))) + period) period))) + (verbose "amplitude: " amplitude) + (verbose "period: " period) + (verbose "phase: " phase) + (matrix `((,amplitude) (,period) (,phase) (,avg))))))) + +; Truth equation +; measurement = amplitude*sin( 2*Pi*time / period + phase) + bias + +; Kalman States: amplitude, period, phase, bias +; measurement +; Z = X[0]*sin(2*Pi * time / X[1] + X[2]) + X[3]; +; partials: +; dZ/dX[0] = sin(2*Pi*time/X[1]+X[2]) +; dZ/dX[1] = -2*X[0]*Pi*time*cos(2*Pi*time/X[1]+X[2])/X[1]^2; +; dZ/dX[2] = X[0]*cos(2*Pi*time/X[1]+X[2]) +; dZ/dX[3] = 1; +(define (build-tilt-jacobian-residual-row state measurements) + (let ((X0 (matrix-ref state 0 0)) (X1 (matrix-ref state 1 0)) + (X2 (matrix-ref state 2 0)) (X3 (matrix-ref state 3 0))) + `((,(sin (+ (* 2 Pi time (/ X1)) X2)) + ,(* -2 Pi X0 time (cos (+ (* 2 Pi time (/ X1)) X2)) (/ X1) (/ X1)) + ,(* X0 (cos (+ (* 2 Pi time (/ X1)) X2))) + ,1) + ,(- value (* X0 (sin (+ (* 2 Pi time (/ X1)) X2))) X3)))) + +(define (tilt-complete? state) + ; force phase to be from -Pi to Pi + (matrix-set! state 2 0 (phase-resolve (matrix-ref state 2 0))) + + ; force amplitude to be positive by flipping period and phase + (cond ((negative? (matrix-ref state 0 0)) + (matrix-set! state 0 0 (- (matrix-ref state 0 0))) + (matrix-set! state 1 0 (- (matrix-ref state 1 0))) + (matrix-set! state 2 0 (- (matrix-ref state 2 0))))) + + ; force period to be > 3 + (cond ((< (matrix-ref state 1 0) 3) + (matrix-set! state 1 0 3))) + + (let ((d (matrix-ref (matrix* (matrix-transpose state) state) 0 0))) + (verbose "d: " d) + (< d 1e-1))) + +(define (compute-amplitude-period-phase-bias-least-squares) + (let* ((rawdata (map second tilt-history)) + (minval (apply min rawdata)) + (maxval (apply max rawdata)) + (avgval (apply average rawdata))) + (least-squares-iterate (matrix `((,(- maxval minval)) (1) (1) (,avgval))) + 10 tilt-constraints tilt-finished? + built-tilt-jacobian-row build-tilt-residual-row))) + + +; Truth equation +; measurement = amplitude*sin( 2*Pi*time / period + phase) + bias +; +; amplitude = sqrt(a^2 + b^2) +; phase = atan(a, b) +; measurement = a*sin( 2*Pi*frequency*time) + b*cos( 2*Pi*frequency*time) + bias +; +; states are: a, b, frequency, bias +; +; dZ/dX[0] = sin( 2*Pi*X[2]*time) +; dZ/dX[1] = cos( 2*Pi*X[2]*time) +; dZ/dX[2] = 2*X[0]*Pi*time*cos(2*X[2]*Pi*time)-2*X[1]*Pi*time*sin(2*X[2]*Pi*time) +; dZ/dX[3] = 1; +(define (build-polar-tilt-jacobian-row state measurements) + (let ((X0 (matrix-ref state 0 0)) (X1 (matrix-ref state 1 0)) + (X2 (matrix-ref state 2 0)) (X3 (matrix-ref state 3 0)) + (time (first measurements))) + (list (sin (* 2 Pi time X2)) + (cos (* 2 Pi time X2)) + (* 2 Pi time (- (* X0 (cos (* 2 Pi X2 time))) (* X1 (sin (* 2 Pi X2 time))))) + 1))) + +(define (build-polar-tilt-residual-row state measurements) + (let ((X0 (matrix-ref X 0 0)) (X1 (matrix-ref X 1 0)) + (X2 (matrix-ref X 2 0)) (X3 (matrix-ref X 3 0)) + (time (first measurements)) (value (second measurements))) + (list (- value (* X0 (sin (+ (* 2 Pi time (/ X1)) X2))) X3)))) + +(define (command-relays phase-percent duty-cycle) + (cond ((> phase-percent duty-cycle) + (relay-set 1 #t)) + ((> phase-percent 0) + (relay-set 2 #t)) + ((> (+ phase-percent 1) duty-cycle) + (relay-set 1 #f)) + (else + (relay-set 2 #f)))) + +(define (command-relays-off) + (relay-set 1 #f) + (relay-set 2 #f)) + +(define (run-primitive-tilt-compensation) + (call/cc (lambda (bail) (compute-amplitude-period-phase-bias bail)))) + + +(define (run-tilt-compensation) + (call/cc (lambda (bail) + (with-exception-handler + (lambda _ (bail #f)) + (lambda () (compute-amplitude-period-phase-bias-least-squares)))))) + +(define tilt-comp-last #f) + +(computation-calculate 'time) + +(define min-amplitude 6) +(define relay-phase-shift (* Pi 2)) + +(define (tilt-compensation-setup) + (start-periodic-task + "tilt compensation task" .15 + (lambda () + (update-history) + + (let ((A (run-tilt-compensation))) + (print "A: " A) + (cond (A (set! tilt-comp-last A)) + (tilt-comp-last + ; if we failed to converge, decay amplitude + (matrix-set! tilt-comp-last 0 0 (* (matrix-ref tilt-comp-last 0 0) .99))))) + + (print "tilt-comp-last: " tilt-comp-last) + + (if (and (not (null? relay-ports)) tilt-comp-last) + (let ((amplitude (matrix-ref tilt-comp-last 0 0)) + (period (matrix-ref tilt-comp-last 1 0)) + (phase (matrix-ref tilt-comp-last 2 0)) + (time (computation-calculate 'time))) + (let ((cur-phase (+ (/ (* 2 Pi time) period) phase relay-phase-shift))) + (let ((phase-percent (/ (phase-resolve cur-phase) Pi))) + (if (> amplitude min-amplitude) + (command-relays phase-percent .25) + (command-relays-off))))))) + + 'tilt-compensation-update)) + +(plot-add-extra-display-function + (lambda (left right top bottom) + (let ((state tilt-comp-last +; (run-tilt-compensation) +; (matrix '((1) (1) (0) (0))) + )) + (if state + (glBegin gl:LINE_STRIP + (gl:LineWidth 3) + (gl:Color3f 1 0 0) + +; plot ; amplitude*sin( 2*Pi*time / period + phase) + bias + + (for-each (lambda (time) + (glVertex time + (+ (* (matrix-ref state 0 0) + (sin (+ (* 2 Pi time (/ (matrix-ref state 1 0))) + (matrix-ref state 2 0)))) + (matrix-ref state 3 0)))) + (sequence left right .1)) + ))))) + + +(use numbers srfi-1) +(define Pi (* 2 (asin 1))) +(define (sequence from to . step) + (if (> from to) '() (cons from (apply sequence (+ from (if (null? step) 1 (car step))) to step)))) + +(define i (sqrt -1)) +(define (square x) (* x x)) + +(define (discrete-fourier-transform data) + (let ((N (length data))) + (map (lambda (k) + (fold + 0 (map (lambda (n) + (* (list-ref data n) (exp (/ (- (* 2 Pi i k n)) N)))) + (sequence 0 (- N 1))))) + (sequence 0 (- N 1))))) + + +(define (discrete-fourier-transform-sub k data) + (let ((N (length data))) + (fold + 0 (map (lambda (n) + (* (list-ref data n) (exp (/ (- (* 2 Pi i k n)) N)))) + (sequence 0 (- N 1)))))) + +(define (inverse-discrete-fourier-transform data) + (let ((N (length data))) + (map (lambda (k) + (/ + (fold + 0 (map (lambda (n) + (* (list-ref data n) (exp (/ (* 2 Pi i k n) N)))) + (sequence 0 (- N 1)))) + N)) + (sequence 0 (- N 1))))) + +(define (linear-interpolate time h1 h2) + (let ((t1 (car h1)) (v1 (cadr h1)) + (t2 (car h2)) (v2 (cadr h2))) + (+ v2 (* (/ (- time t2) (- t1 t2)) (- v1 v2))))) + +; take history as a list of (time value) lists and +; use simple linear interpolation between pairs to +; build a discrete history (with equal time spacing) +(define (build-discrete-history history time-step) + (if (or (null? history) (zero? time-step)) '() + (let*((start (first history)) + (end (last history)) + (start-time (car start)) + (end-time (car end)) + (overall-time (- start-time end-time))) + (let each-step ((time start-time) + (history history)) + (let ((next-history (cdr history))) + (cond ((or (null? next-history) (< time end-time)) '()) + ((< time (caar next-history)) (each-step time next-history)) + (else (cons (linear-interpolate time (car history) (car next-history)) + (each-step (- time time-step) history))))))))) + +(define (history-frequency-amplitude-phase history) + (let ((max-frequency 20)) + (let*((discrete-history (build-discrete-history history (/ max-frequency))) + (dft (discrete-fourier-transform discrete-history)) + (N (length dft)) + (frequency-step (/ max-frequency (- N 1)))) + (map (lambda (dft-value frequency) + (list frequency + (* (if (zero? frequency) 1 2) (/ (complex-amplitude dft-value) N)) + (+ (complex-phase dft-value) (/ Pi 2)))) + dft (sequence (exact->inexact 0) max-frequency frequency-step))))) + +; get rid of frequencies > 1hz +(define (remove-high-frequencies history-frequency-amplitude-phase) + (cond ((null? history-frequency-amplitude-phase) '()) + ((< (first (car history-frequency-amplitude-phase)) 1) + (cons (car history-frequency-amplitude-phase) + (remove-high-frequencies (cdr history-frequency-amplitude-phase)))) + (else '()))) + +; print estimated frequency, period, amplitude, and bias of signal +(define (history-frequency-short-info history-frequency-amplitude-phase) + (let*((bias (cadar history-frequency-amplitude-phase)) + (frequency (/ (fold + 0 (map (lambda (frequency-amplitude-phase) + (* (first frequency-amplitude-phase) + (second frequency-amplitude-phase))) + (cdr history-frequency-amplitude-phase))) + (fold + 0 (map second (cdr history-frequency-amplitude-phase))))) + (period (/ frequency)) + (amplitude (sqrt (fold + 0 (map (lambda (frequency-amplitude-phase) + (square (second frequency-amplitude-phase))) + (cdr history-frequency-amplitude-phase)))))) + (print "frequency " frequency " period " period " amplitude " amplitude " bias " bias))) + + +; print information about frequency, period, amplitude, and phase of history data +(define (history-frequency-long-info history-frequency-amplitude-phase) + (for-each (lambda (history-value) + (let ((frequency (first history-value)) + (amplitude (second history-value)) + (phase (third history-value))) + (let ((period (if (zero? frequency) "DC" (/ (round (* 100 (/ frequency))) 100)))) + (if (and (> amplitude 1e-5)) + (print "frequency " frequency " period " period + " amplitude " amplitude " phase " (if (zero? frequency) "N/A" phase)))))) + history-frequency-amplitude-phase)) + +(define (make-sin-history period amplitude phase bias start-time stop-time time-step) + (let each-time ((time start-time)) + (if (> time stop-time) '() + (cons `(,time ,(+ (* amplitude (sin (+ (* 2 Pi (/ time period)) phase))) bias)) + (each-time (+ time time-step)))))) + +;(define (compute-tilt-fourier-transform) diff --git a/types.scm b/types.scm new file mode 100644 index 0000000..6cae21d --- /dev/null +++ b/types.scm @@ -0,0 +1,39 @@ +;; Copyright (C) 2011 Sean D'Epagnier +;; +;; 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. + +;;; types for fields within a list +(declare (unit types)) + +(define types (make-hash-table)) + +; provide a means to extract elements from a list by name +; handler should take the subfield and a value +(define (type-register name fields) + (hash-table-set! types name fields)) + +; generic means to operate on the type +(define (type-field-apply name field values func) + (let each-def ((definition (hash-table-ref types name)) + (values values)) + (cond + ((null? definition) (error "Field " field " does not exist in " name)) + ((eq? field (car definition)) (func values)) + (else (each-def (cdr definition) (cdr values)))))) + +; get the right field from a list +(define (type-field-get name field values) + (type-field-apply name field values car)) + +; set a field in a list +(define (type-field-set! name field values value) + (type-field-apply name field values (lambda (values) (set-car! values value)))) + +(define (type-field-print name values) + (for-each (lambda (field value) + (display field) (display ": ") (display value) (display " ")) + (hash-table-ref types name) values) + (newline)) diff --git a/units.scm b/units.scm index b459c4a..e5f5691 100644 --- a/units.scm +++ b/units.scm @@ -5,6 +5,8 @@ ;; License as published by the Free Software Foundation; either ;; version 3 of the License, or (at your option) any later version. +(declare (unit units)) + (define (mph->knots x) (* x .868976242)) (define (m/s->knots x) (* x 1.94384449)) diff --git a/utilities.scm b/utilities.scm new file mode 100644 index 0000000..919c13f --- /dev/null +++ b/utilities.scm @@ -0,0 +1,187 @@ +;; Copyright (C) 2010 Sean D'Epagnier +;; +;; 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 utilities)) +(declare (uses plot)) + +(define (sequence from to . step) + (if (> from to) '() (cons from (apply sequence (+ from (if (null? step) 1 (car step))) to step)))) + +(define (square x) + (* x x)) + +(define (cube x) + (* x x x)) + +(define (complex-abs c) + (sqrt (+ (square (real-part c)) (square (imag-part c))))) + +(define (complex-phase c) + (atan (imag-part c) (real-part c))) + +(define (nan? x) + (not (= x x))) + +; this works for all real and imaginary infinities +(define (infinity? x) + (let ((y (complex-abs x))) + (and (= y y (+ y 1))))) + +(define (read-from-string string) (with-input-from-string string read)) + +(define last-show #t) +(define (nice-print . args) + (if last-show (display "\n")) + (apply print args) + (set! last-show #f)) + +(define (show . args) + (if last-show (display "\r")) + (for-each (lambda (arg) (display arg)) args) + (if (not last-show) (display "\n")) + (set! last-show #t)) + +(define (die . args) + (apply nice-print "Error: " args) + (exit)) + +(define (warning . args) + (apply nice-print "Warning: " args)) + +(define warning-once + (let ((table (make-hash-table))) + (lambda args + (cond ((not (hash-table-exists? table args)) + (hash-table-set! table args #t) + (apply warning args)))))) + +(define (start-timer . replayrate) + (let ((start (current-milliseconds))) + (lambda () (* (if (null? replayrate) 1 (car replayrate)) + (/ (- (current-milliseconds) start) 1000))))) + +(define elapsed-seconds (start-timer)) + +(define (noop . _) #t) +(define verbose noop) +(define very-verbose noop) + +(define sleep thread-sleep!) + +(define (push-exit-handler handler) + (exit-handler (let ((old-handler (exit-handler))) + (lambda _ (handler) (apply old-handler _))))) + +(define (port->string port) + (with-output-to-string (lambda () (write (port))))) + +(define (debug arg) + (create-periodic-task + (string-append "debug '" arg "' task") 1 + (let ((exps (map read-from-string (string-split arg ",")))) + (let ((comp (apply computations-revaluate exps))) + (lambda () + (cond ((equal? arg "help") + (nice-print "possible values which can be used in a debugging expression:") + (computation-info) + (exit))) + + (let ((values (comp))) + (cond (values + (for-each (lambda (exp value) + (if (not (eq? exp (car exps))) (display "\t")) + (display exp) + (display ": ") + (display value)) + exps values) + (newline))))))))) + +(define (make-run-once proc) + (let ((run #f)) + (lambda () + (if run + (error "proc run more than once" proc) + (let () + (set! run #t) + (proc)))))) + +(define (round-to-places n places) + (let ((factor (expt 10 places))) + (/ (round (* n factor)) factor))) + +(define (saturate value minimum maximum) + (min minimum (max maximum value))) + +(define (round-toward-zero n) + (if (positive? n) (floor n) (ceiling n))) + +(define (integer->name n) + (if (not (integer? n)) (error "integer->name recieved non-integer" n)) + (define (integer->name-append n) + (if (zero? n) "" (string-append " " (integer->name n)))) + (if (negative? n) + (string-append "negative " (integer->name (abs n))) + (let each-div ((divs '((1e15 "quadrillion") ((1e12 "trillion") (1e9 "billion") + (1e6 "million") (1e3 "thousand") (1e2 "hundred"))))) + (cond ((null? divs) ; under 100 now + (if (< n 20) + (case (inexact->exact n) + ((0) "zero") ((1) "one") ((2) "two") ((3) "three") ((4) "four") + ((5) "five") ((6) "six") ((7) "seven") ((8) "eight") ((9) "nine") + ((10) "ten") ((11) "eleven") ((12) "twelve") ((13) "thirteen") ((14) "fourteen") + ((15) "fifthteen") ((16) "sixteen") ((17) "seventeen") ((18) "eighteen") ((19) "nineteen") + (else (error "unsuported number" n))) + (string-append (case (inexact->exact (floor (/ n 10))) + ((2) "twenty") ((3) "thirty") ((4) "fourty") ((5) "fifty") + ((6) "sixty") ((7) "seventy") ((8) "eighty") ((9) "ninety") + (error "failed decoding in integer->number" (floor (/ n 10)))) + (integer->name-append (remainder n 10))))) + ((>= n (caar divs)) + (string-append (integer->name (floor (/ n (caar divs)))) + " " (cadar divs) + (integer->name-append (remainder n (caar divs))))) + (else (each-div (cdr divs))))))) + +(define (number->name n) + (cond ((not (number? n)) (error "number->name requires number, given" n)) + ((not (= n n)) "NaN") + ((not (zero? (imag-part n))) + (string-append (number->name (real-part n)) " and " + (number->name (imag-part n)) " imaginary")) + (else + (let* ((r (round-toward-zero n)) + (f (- n r))) + (string-append (integer->name r) + (if (zero? f) "" + (let start ((s (string->list (number->string f)))) + (cond ((null? s) (error "number->name failed decoding fraction")) + ((equal? (car s) #\.) + (apply string-append " point" + (map (lambda (c) + (string-append + " " + (integer->name + (string->number + (string c))))) + (cdr s)))) + (else (start (cdr s))))))))))) + +(define (base-integer->primary-name n) + (case n ((1) "first") ((2) "second") ((3) "third") ((5) "fifth") + (else (let ((sn (integer->name n))) + (string-append + (case (string-ref sn (- (string-length sn) 1)) + ((#\e #\t) (substring sn 0 (- (string-length sn) 1))) + (else sn)) + "th"))))) + +; convert 1 to "first", 2 to "second", 3 to "third" etc.. +(define (integer->primary-name n) + (let* ((sn (remainder n 10)) (bn (- n sn))) + (if (or (< (remainder n 100) 20) (zero? (remainder n 10))) + (base-integer->primary-name n) + (string-append (integer->name bn) " " (base-integer->primary-name sn))))) diff --git a/vector.scm b/vector.scm index 59df442..3dab9ed 100644 --- a/vector.scm +++ b/vector.scm @@ -8,27 +8,60 @@ ; Not scheme vectors, these are used in calculations for movement ; a "course" is the polar version with heading and speed +(define (vector-magnitude^2 vec) + (apply + (map square vec))) + (define (vector-magnitude vec) - (sqrt (apply + (map square vec)))) + (sqrt (vector-magnitude^2 vec))) (define (vector-normalize vec) - (let ((mag (magnitude vec))) + (let ((mag (vector-magnitude vec))) (map (lambda (x) (/ x mag)) vec))) +(define (vector+ . vecs) + (apply map + vecs)) + +(define (vector- . vecs) + (apply map - vecs)) + +(define (vector-scale factor v) + (map (lambda (w) (* factor w)) v)) + (define (vector-dot . vecs) (apply + (apply map * vecs))) -; uh, yeah we support higher order cross products +; we support all dimensions of cross products +; 1 2d vector given will yield an orthogonal vector +; two 3d vectors can be supplied to gain a third orthogonal, or +; three 4d vectors etc... (define (vector-cross . vecs) - (if ())) + (if (not (apply = (+ (length vecs) 1) (map length vecs))) + (error "invalid dimensions given to vector-cross, requires n vectors of n+1 dimension" vecs)) + (let ((m (matrix vecs))) + (map (lambda (col) + (* (if (even? col) 1 -1) + (matrix-determinant (matrix-remove-col m col)))) + (sequence 0 (length vecs))))) ; These vector operations only apply to 2D vectors (define (course->vector course) - (list (* (car course) (cos (cadr course))) - (* (car course) (sin (cadr course))))) + (list (* (car course) (cos (deg2rad (cadr course)))) + (* (car course) (sin (deg2rad (cadr course)))))) (define (vector->course vector) (list (vector-magnitude vector) (vector-heading vector))) (define (vector-heading vector) - (atan (cadr vector) (car vector))) + (rad2deg (atan (cadr vector) (car vector)))) + +(define (course+ . courses) + (vector->course (apply vector+ (map course->vector courses)))) + +(define (course- . courses) + (vector->course (apply vector- (map course->vector courses)))) + +(define (vector->row-matrix v) + (matrix (map list v))) + +(define (row-matrix->vector m) + (map first (matrix->list m))) diff --git a/water.scm b/water.scm new file mode 100644 index 0000000..51c422f --- /dev/null +++ b/water.scm @@ -0,0 +1,76 @@ +;; Copyright (C) 2011 Sean D'Epagnier +;; +;; 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. + +;; This file converts units from weather sensors and calculates +;; True and apparent wind speed in knots and direction in radians + +(declare (unit water)) + +(use srfi-1) + +(define (water-speed) + (warning-once "no water speed data supported, assuming no current") + (computation-calculate 'gps-speed)) + +(define (water-direction) + (warning-once "no water speed data supported, assuming no current") + (let ((heading (computation-calculate 'heading)) + (gps-heading (computation-calculate 'gps-heading))) + (phase-resolve-degrees (- gps-heading heading)))) + +(define (water-heading) + (+ (computation-calculate 'heading) + (water-direction))) + +;; calculate the true win heading and speed +;; apparent = course + true +(computation-register 'water-speed "speed in knots thru water" + '(water) + (lambda () (water-speed))) + +(computation-register 'water-direction "relative direction water is flowing past us, indicates side flow (slip)" + '(water) + (lambda () (water-direction))) + +(computation-register 'water-heading "heading we are actually moving through water" + '(water) + (lambda () (water-heading))) + +; find the course of water over ground +(define (water-current) + (let ((heading (computation-calculate 'heading)) + (water-speed (computation-calculate 'water-speed)) + (water-heading (computation-calculate 'water-heading)) + (gps-speed (computation-calculate 'gps 'speed)) + (gps-direction (computation-calculate 'gps 'heading))) + (course- `(,water-speed ,water-heading) + `(,gps-speed ,gps-heading)))) + +(computation-register 'water-current-speed "speed water is moving relative to ground" + '(gps) + (first (water-current))) + +(computation-register 'water-current-direction "direction water is moving relative to ground" + '(gps) + (second (water-current))) + +; the course we are moving through the water +(define (water-course) + `(,(water-speed) ,(water-heading))) + +; find the course of wind over water +(define (wind-course-over-water) + (course- (true-wind) (true-water))) + +(computation-register 'angle-of-attack + "The angle between the true wind and the movement through the water" + (lambda () + (let ((wow (wind-course-over-water)) + (wc (water-course))) + (phase-resolve-degrees + (- (second wow) (second wc))))))) + diff --git a/waves.scm b/waves.scm new file mode 100644 index 0000000..52515d9 --- /dev/null +++ b/waves.scm @@ -0,0 +1,10 @@ +; Korteweg-de Vries Equation +; +; df/dt + d^3f/dx^3 + 6*f*df/dx = 0 +; +; f(x, t) = w(x-c*t) c is phase speed +; +; -c*dw/dx + d^3*w/dx^3 + 6*w*dw/dx = 0 +; or +; 3*w^2 + d^2w/dx^2 - cw = A +; diff --git a/weather.scm b/weather.scm index 0432c76..be716a7 100644 --- a/weather.scm +++ b/weather.scm @@ -5,6 +5,8 @@ ;; License as published by the Free Software Foundation; either ;; version 3 of the License, or (at your option) any later version. +(declare (unit weather)) + ;; This file handles reading wind speed and direction data from ;; a davis vantage pro 2 weather station @@ -13,48 +15,49 @@ wind-speed wind-speed-10-min-avg wind-direction humidity rain-rate)) -(define (davis-weather-reader i o) - (let read-loop () - (display "LOOP 1\n" o) - (let ((timeout (+ (current-milliseconds) 2))) - (let packet ((data '())) - (cond ((> (current-milliseconds) timeout) - (read-loop)) - ((char-ready? i) - (let ((count (length data)) - (c (read-char i))) +(define (davis-weather-reader i o index) + (create-periodic-task + (string-append "davis-weather-reader " (number->string index)) 1.5 + (lambda () + (display "LOOP 1\n" o) + (let ((timeout (+ (current-milliseconds) 2))) + (let packet ((data '())) + (cond ((> (current-milliseconds) timeout) + (task-sleep)) + ((char-ready? i) + (let ((count (length data)) + (c (read-char i))) (cond ((or (and (= count 0) (not (eq? c #\L))) (and (= count 1) (not (eq? c #\O))) (and (= count 2) (not (eq? c #\O)))) (packet data)) ((= count 98) - (parse-loop-packet (reverse data)) - (thread-sleep! 1.5) - (read-loop)) + (parse-loop-packet (reverse data) index)) (else (packet (cons c data)))))) (else - (thread-sleep! .1) - (packet data))))))) + (task-sleep .1) + (packet data)))))))) (define (weather-setup device) (let-values (((i o) (open-serial-device device 19200))) - (thread-start! (lambda () (davis-weather-reader i o))))) + (davis-weather-reader i o (sensor-index-count 'weather)))) -(define (parse-loop-packet line) +(define (parse-loop-packet line index) + (very-verbose "parse-loop-packet " line index) (define (char-at ref) (char->integer (list-ref line ref))) (define (short-at ref) (+ (* 256 (char-at (+ ref 1))) (char-at ref))) - (sensor-update 'weather - `(,(short-at 7) ; barometer - ,(F->C (/ (short-at 9) 10)) ; inside-temperature - ,(/ (char-at 11) 100) ; inside-humidity - ,(F->C (/ (short-at 12) 10)) ; temperature - ,(mph->knots (char-at 14)) ; wind-speed - ,(mph->knots (char-at 15)) ; wind-speed-10-min-avg - ,(short-at 16) ; wind-direction - ,(/ (char-at 33) 100) ; humidity - ,(/ (short-at 41) 100) ; rain-rate - ))) + (sensor-update `(weather ,index) + `(,(short-at 7) ; barometer + ,(F->C (/ (short-at 9) 10)) ; inside-temperature + ,(/ (char-at 11) 100) ; inside-humidity + ,(F->C (/ (short-at 12) 10)) ; temperature + ,(mph->knots (char-at 14)) ; wind-speed + ,(mph->knots (char-at 15)) ; wind-speed-10-min-avg + ,(short-at 16) ; wind-direction + ,(/ (char-at 33) 100) ; humidity + ,(/ (short-at 41) 100) ; rain-rate + ))) diff --git a/wifiaimer.scm b/wifiaimer.scm new file mode 100644 index 0000000..b56fd1e --- /dev/null +++ b/wifiaimer.scm @@ -0,0 +1,203 @@ +;; Copyright (C) 2011 Sean D'Epagnier +;; +;; 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 wifiaimer)) +(declare (uses computation utilities)) + +(use srfi-1 srfi-13 srfi-18 posix) + +; given and update and calibration we solve for command +; the motor rotates the antenna 1 degree every 30 milliseconds, +; but it takes half milliseconds +(define (track-speed-calculate desired-update) + (min 5000 (max -5000 + (* desired-update 50)))) + +(define (create-wifi-aimer arg) + (let ((scan-speed 200) + (scan-direction -1) + (statistics-timer (start-timer)) + (motor-command-total 0) + (misalignment-total 0) + (misalignment-count 0)) + + (define (make-state-verifier) + (make-discrete-verifier 'state "initial state" 'tracking '(scanning tracking))) + + (define options + (create-options + `(,(make-string-verifier 'wifidevice "wlan device to use" "wlan20") + ,(make-number-verifier 'unwrap "if ccables get twisted do this to unwrap n counts" 0 -2 2) + ,(make-number-verifier 'heading "heading to hold" 0 -180 180) + ,(make-number-verifier 'beamwidth "tolerated heading mismatch" 8 0 180) + ,(make-state-verifier)) + "-wifiaimer wifidevice=wlan0,motordevice=/dev/ttyS0,serialbaud=9600" + (create-motor-options))) + + (define mag-calibration (matrix-transpose (matrix '((5551 + -12832.0247508799 + 4.00245833922293e-05 + 0.963022408978104 + -0.116278162422756))))) + + (define (scan-heading-min-max-from-headings headings) + (let ((mins (map (lambda (heading) (if heading heading 180)) headings)) + (maxs (map (lambda (heading) (if heading heading -180)) headings))) + (let ((min-heading (apply min mins)) + (max-heading (apply max maxs))) + (cond ((> (- max-heading min-heading) 180) + (list max-heading (+ min-heading 360))) + ((> min-heading max-heading) #f) + (else (list min-heading max-heading)))))) + + ; return association status + (define (read-wifi-info) + (let ((wifidevice (options 'wifidevice)) + (tempfilename "/tmp/cruisingplotiwconfig")) + (system (string-append "bash -c \"iwconfig " wifidevice " &> " tempfilename "\"")) + (let ((winfo (read-all tempfilename))) + (cond ((string-contains winfo "No such device") + (warning-once "wifi device: " wifidevice " does not exist") + #f) + ((string-contains winfo "Not-Associated") #f) + (else #t))))) + + (define (apply-heading mag-x mag-y) + (let ((cal (flat-mag-apply mag-calibration mag-x mag-y))) + (rad2deg (atan (second cal) (first cal))))) + + (if (equal? arg "help") (wifiaimer-help)) + (parse-basic-options-string options arg) + + (let ((motor (motor-open options)) + (motor-position 0)) + (define (get-heading) + (let ((heading (computation-calculate 'heading))) + (cond (heading heading) + (else (warning-once "heading not calculated, assuming stationary") + 0)))) + (start-periodic-task + "wifi aimer task" 1 + (let ((scan-start #f) + (min-associated-mag-x #f) + (max-associated-mag-y #f) + (min-associated-mag-x #f) + (max-associated-mag-y #f) + (mag-log '()) + (state (case (options 'state) + ((scanning) 'start-scan) + ((tracking) 'tracking))) + (antenna-heading 0) + (last-antenna-heading 0) + (antenna-heading-wraps 0) + (scan-heading (options 'heading)) + ) + (lambda () + (let ((associated (read-wifi-info)) + (mag-x (sensor-query '(magnetometer 0))) + (mag-y (sensor-query '(magnetometer 1)))) + (verbose "Wifiaimer associated: " associated) + (very-verbose "maglog " mag-log) + + ; statistics + (cond ((>= (statistics-timer) 60) + (print "\nwifiaimerstats: movement " + (round-to-places (/ motor-command-total 1000) 2) + " seconds or " + (round-to-places (/ motor-command-total (statistics-timer) 10) 2) + "% of time") + (print "average misalignment: " + (if (zero? misalignment-count) "N/A" + (round-to-places (/ misalignment-total misalignment-count) 2)) + " degrees") + (set! misalignment-total 0) + (set! misalignment-count 0) + (set! motor-command-total 0) + (set! statistics-timer (start-timer)))) + + (case state + ((start-scan) ; move motor to start of scan, and wait until it is there + (set! scan-start (start-timer)) + (set! min-associated-mag-x #f) + (set! min-associated-mag-y #f) + (set! max-associated-mag-x #f) + (set! max-associated-mag-y #f) + (set! mag-log '()) + (set! scan-direction (- scan-direction)) + (set! state 'scanning)) + ((scanning) + (if (>= (scan-start) 120) + (set! state 'scan-complete)) + (set! motor-command-total (+ motor-command-total (abs command))) + (motor-command motor (* scan-speed scan-direction)) + (cond ((and associated mag-x mag-y) + (cond ((or (not min-associated-mag-x) (< mag-x min-associated-mag-x)) + (set! min-associated-mag-x (list mag-x mag-y))) + ((< mag-y min-associated-mag-y) + (set! min-associated-mag-y (list mag-x mag-y))) + ((> mag-x max-associated-mag-x) + (set! max-associated-mag-x (list mag-x mag-y))) + ((> mag-y max-associated-mag-y) + (set! max-associated-mag-y (list mag-x mag-y)))))) + + (if (and mag-x mag-y) + (set! mag-log (cons (list mag-x mag-y) mag-log)))) + ((scan-complete) + (print "Wifiaimer scan complete") + (set! mag-calibration (flat-mag-calibrate mag-log)) + (print "mag-calibration: " mag-calibration) + (let ((headings + (map (lambda (mag-xy) (if mag-xy + (map (lambda (mag-xy) + (apply apply-heading mag-xy)) + (apply flat-mag-apply mag-calibration mag-xy)) + #f)) + `(,min-associated-mag-x + ,min-associated-mag-y + ,max-associated-mag-x + ,max-associated-mag-y)))) + (verbose "headings " headings) + (let ((heading-min-max (scan-heading-min-max-from-headings headings))) + (cond (heading-min-max + (verbose "Detected wifi heading range: " heading-min-max) + (set! scan-heading (apply average heading-min-max))))) + (cond (scan-heading + (set! state 'tracking)) + (else + (print "Wifiaimer scan failed, repeating") + (set! state 'start-scan))))) + ((tracking) + (if (and mag-x mag-y) + (let ((antenna-heading (apply-heading mag-x mag-y))) + (verbose "wifiaimer antenna-heading " antenna-heading " wraps " antenna-heading-wraps) + + ; calculate error, and update motor to correct it + (let ((antenna-total-heading + (+ antenna-heading (* antenna-heading-wraps 360)))) + (let ((antenna-misalignment (- antenna-total-heading scan-heading))) + (show "Wifiaimer antenna " (round-to-places antenna-total-heading 2) + " misalignment " (round-to-places antenna-misalignment 2)) + (set! misalignment-total (+ misalignment-total (abs antenna-misalignment))) + (set! misalignment-count (+ misalignment-count 1)) + + (if (> (abs antenna-misalignment) (options 'beamwidth)) + (let ((command + (track-speed-calculate antenna-misalignment))) + (motor-command command) + )))) + + ; did we wrap? record it + (cond ((and (> antenna-heading 90) (< last-antenna-heading -90)) + (set! antenna-heading-wraps (+ antenna-heading-wraps 1))) + ((and (< antenna-heading -90) (> last-antenna-heading 90)) + (set! antenna-heading-wraps (- antenna-heading-wraps 1)))) + + (set! last-antenna-heading antenna-heading))) + ))))) + + + 'wifi-aimer)))) diff --git a/wind.scm b/wind.scm index af49cfb..52b0475 100644 --- a/wind.scm +++ b/wind.scm @@ -8,6 +8,8 @@ ;; This file converts units from weather sensors and calculates ;; True and apparent wind speed in knots and direction in radians +(declare (unit wind)) + (use srfi-1) ; with some simple filters it is easier to read plots @@ -16,49 +18,58 @@ (let ((last #f)) (lambda (update) (set! last (if last - (+ (* gamma speed) (* (- 1 gamma) last)) - speed)) + (+ (* gamma update) (* (- 1 gamma) last)) + update)) + last))) + +(define (make-directional-low-pass-filter) + (let ((last #f)) + (lambda (update) + (let ((next (phase-resolve-degrees update))) + (set! last (if last + (rad2deg (car (wind-direction-Yamartino-method + (list (deg2rad last) + (deg2rad next))))) + next))) last))) + ;; calculate the true wind direction and speed ;; apparent = course + true (computation-register 'wind-speed "speed in knots of apparant wind" - (let ((filter (make-low-pass-filter .5))) - (lambda () - (filter (sensor-field-get 'weather 'wind-speed))))) + '(weather) + (lambda () + (sensor-field-get 'weather 'wind-speed))) -(computation-register 'relative-wind-direction "relative direction in degrees on vessel" +(computation-register 'wind-direction "direction in degrees of apparant wind, relative to heading" '(weather) (lambda () (sensor-field-get 'weather 'wind-direction))) -(computation-register 'wind-direction "direction in degrees of apparant wind, -after heading is subtracted" - (let ((c (computations-revaluate 'relative-wind-direction 'heading))) - (lambda () - (sensor-field-get 'weather 'wind-direction)))) - -; compute true wind speed and direction -(define (true-wind) - (let ((wind-speed (sensor-field-get 'weather 'wind-speed)) - (wind-direction ((computation-revaluate 'wind-direction))) - (gps-speed (sensor-field-get 'gps 'speed)) - (gps-direction (deg2rad (sensor-field-get 'gps 'heading)))) - (let ((wx (* wind-speed (cos wind-direction))) - (wy (* wind-speed (sin wind-direction))) - (gx (* gps-speed (cos gps-direction))) - (gy (* gps-speed (sin gps-direction)))) - (let ((sx (- wx gx)) - (sy (- wy gy))) - (list (magnitude (list sx sy)) (rad2deg (atan sy sx))))))) - -(computation-register 'true-wind-speed "The true wind speed in knots." +; find the course wind is taking over ground +(define (calculate-true-wind heading wind-speed wind-direction gps-speed gps-heading) + (course- `(,wind-speed ,(+ heading wind-direction)) + `(,gps-speed ,gps-heading))) + +(define (current-true-wind) + (calculate-true-wind (computation-calculate 'heading) + (computation-calculate 'wind-speed) + (computation-calculate 'wind-direction) + (computation-calculate 'gps-speed) + (computation-calculate 'gps-heading))) + +(computation-register 'true-wind-speed "The true wind speed in knots." '(gps weather) (lambda () (first (true-wind)))) -(computation-register 'true-wind-direction "The true wind direction in degrees." +(computation-register 'true-wind-heading "The true wind heading in degrees." '(gps weather) (lambda () (second (true-wind)))) +(computation-register 'true-wind-direction "The true wind direction relative to heading in degrees." + '(gps weather) + (lambda () + (- (computation-calculate 'true-wind-heading) (computation-calculate 'heading)))) + ; calculate average wind direction and deviation from a list of ; wind directions (in radians) using the Yamartino method (define (wind-direction-Yamartino-method directions) @@ -68,3 +79,16 @@ after heading is subtracted" (atan sa ca) (let ((e (sqrt (- 1 (* sa sa) (* ca ca))))) (* (asin e) (+ 1 (* (- (/ 2 (sqrt 3)) 1) (* e e e)))))))) + + +; we can smooth wind direction to get more useful measurement using the magnetic +; heading to improve bandwidth +; +; we need proper filter for heading with resolver +; truewinddirectionsmoothed = tc*(magneticheading+winddirection) + (1-tc)*truewinddirectionsmoothed +; +; then we can achieve wind direction bandwidth as fast as magnetometer heading +; winddirectionmagneticbandwidthsmoothed = truewinddirectionsmoothed - magneticheading + + +