Revision 23:a34af501ea89, committed 2018-11-30

Comitter:

shimniok

Date:

Fri Nov 30 15:41:05 2018 +0000

Parent:

22:dc54ca6e6eec

Child:

24:46318b2bf974

Commit message:

Disabled PinDetect code (for now); fixed attach() calls in various functions to use new callback class.

Changed in this revision

--- a/Estimation/Matrix/Matrix.cpp	Thu Nov 29 19:45:49 2018 +0000
+++ b/Estimation/Matrix/Matrix.cpp	Fri Nov 30 15:41:05 2018 +0000
@@ -1,235 +0,0 @@
-#include <stdio.h>
-#include "Matrix.h"
-
-unsigned int matrix_error = 0;
-
-void Vector_Cross_Product(float C[3], float A[3], float B[3])
-{
-    C[0] = (A[1] * B[2]) - (A[2] * B[1]);
-    C[1] = (A[2] * B[0]) - (A[0] * B[2]);
-    C[2] = (A[0] * B[1]) - (A[1] * B[0]);
-  
-    return;
-}
-
-void Vector_Scale(float C[3], float A[3], float b)
-{
-    for (int m = 0; m < 3; m++)
-        C[m] = A[m] * b;
-        
-    return;
-}
-
-float Vector_Dot_Product(float A[3], float B[3])
-{
-    float result = 0.0;
-
-    for (int i = 0; i < 3; i++) {
-        result += A[i] * B[i];
-    }
-    
-    return result;
-}
-
-void Vector_Add(float C[3], float A[3], float B[3])
-{
-    for (int m = 0; m < 3; m++)
-        C[m] = A[m] + B[m];
-        
-    return;
-}
-
-void Vector_Add(float C[3][3], float A[3][3], float B[3][3])
-{
-    for (int m = 0; m < 3; m++)
-        for (int n = 0; n < 3; n++)
-            C[m][n] = A[m][n] + B[m][n];
-}
-
-void Matrix_Add(float C[3][3], float A[3][3], float B[3][3])
-{
-    for (int i = 0; i < 3; i++) {
-        for (int j = 0; j < 3; j++) {
-           C[i][j] = A[i][j] + B[i][j];
-        }
-    }
-}
-
-void Matrix_Add(int n, int m, float *C, float *A, float *B)
-{
-    for (int i = 0; i < n*m; i++) {
-       C[i] = A[i] + B[i];
-    }
-}
-
-void Matrix_Subtract(int n, int m, float *C, float *A, float *B)
-{
-    for (int i = 0; i < n*m; i++) {
-       C[i] = A[i] - B[i];
-    }
-}
-
-
-
-// grabbed from MatrixMath library for Arduino
-// http://arduino.cc/playground/Code/MatrixMath
-// E.g., the equivalent Octave script:
-//   A=[x; y; z];
-//   B=[xx xy xz; yx yy yz; zx xy zz]; 
-//   C=A*B;
-// Would be called like this:
-//   Matrix_Mulitply(1, 3, 3, C, A, B);
-//
-void Matrix_Multiply(int m, int p, int n, float *C, float *A, float *B)
-{
-    // A = input matrix (m x p)
-    // B = input matrix (p x n)
-    // m = number of rows in A
-    // p = number of columns in A = number of rows in B
-    // n = number of columns in B
-    // C = output matrix = A*B (m x n)
-    for (int i=0; i < m; i++) {
-        for(int j=0; j < n; j++) {
-            C[n*i+j] = 0;
-            for (int k=0; k < p; k++) {
-                C[i*n+j] += A[i*p+k] * B[k*n+j];
-            }
-        }
-    }
-        
-    return;
-}
-
-void Matrix_Multiply(float C[3][3], float A[3][3], float B[3][3])
-{
-    for (int i = 0; i < 3; i++) {
-        for (int j = 0; j < 3; j++) {
-            C[i][j] = 0;
-            for (int k = 0; k < 3; k++) {
-               C[i][j] += A[i][k] * B[k][j];
-            }
-        }
-    }
-}
-
-
-void Matrix_Transpose(int n, int m, float *C, float *A)
-{
-    for (int i=0; i < n; i++) {
-        for (int j=0; j < m; j++) {
-            C[j*n+i] = A[i*m+j];
-        }
-    }
-}
-
-#define fabs(x) (((x) < 0) ? -x : x)
-
-// grabbed from MatrixMath library for Arduino
-// http://arduino.cc/playground/Code/MatrixMath
-//Matrix Inversion Routine
-// * This function inverts a matrix based on the Gauss Jordan method.
-// * Specifically, it uses partial pivoting to improve numeric stability.
-// * The algorithm is drawn from those presented in 
-//     NUMERICAL RECIPES: The Art of Scientific Computing.
-// * NOTE: The argument is ALSO the result matrix, meaning the input matrix is REPLACED
-void Matrix_Inverse(int n, float *A)
-{
-    // A = input matrix AND result matrix
-    // n = number of rows = number of columns in A (n x n)
-    int pivrow=0;   // keeps track of current pivot row
-    int k,i,j;        // k: overall index along diagonal; i: row index; j: col index
-    int pivrows[n]; // keeps track of rows swaps to undo at end
-    float tmp;        // used for finding max value and making column swaps
-    
-    for (k = 0; k < n; k++) {
-        // find pivot row, the row with biggest entry in current column
-        tmp = 0;
-        for (i = k; i < n; i++) {
-            if (fabs(A[i*n+k]) >= tmp) {    // 'Avoid using other functions inside abs()?'
-                tmp = fabs(A[i*n+k]);
-                pivrow = i;
-            }
-        }
-        
-        // check for singular matrix
-        if (A[pivrow*n+k] == 0.0f) {
-            matrix_error |= SINGULAR_MATRIX;
-            //fprintf(stdout, "Inversion failed due to singular matrix");
-            return;
-        }
-        
-        // Execute pivot (row swap) if needed
-        if (pivrow != k) {
-            // swap row k with pivrow
-            for (j = 0; j < n; j++) {
-                tmp = A[k*n+j];
-                A[k*n+j] = A[pivrow*n+j];
-                A[pivrow*n+j] = tmp;
-            }
-        }
-        pivrows[k] = pivrow;    // record row swap (even if no swap happened)
-        
-        tmp = 1.0f/A[k*n+k];    // invert pivot element
-        A[k*n+k] = 1.0f;        // This element of input matrix becomes result matrix
-        
-        // Perform row reduction (divide every element by pivot)
-        for (j = 0; j < n; j++) {
-            A[k*n+j] = A[k*n+j]*tmp;
-        }
-        
-        // Now eliminate all other entries in this column
-        for (i = 0; i < n; i++) {
-            if (i != k) {
-                tmp = A[i*n+k];
-                A[i*n+k] = 0.0f;  // The other place where in matrix becomes result mat
-                for (j = 0; j < n; j++) {
-                    A[i*n+j] = A[i*n+j] - A[k*n+j]*tmp;
-                }
-            }
-        }
-    }
-    
-    // Done, now need to undo pivot row swaps by doing column swaps in reverse order
-    for (k = n-1; k >= 0; k--) {
-        if (pivrows[k] != k) {
-            for (i = 0; i < n; i++) {
-                tmp = A[i*n+k];
-                A[i*n+k] = A[i*n+pivrows[k]];
-                A[i*n+pivrows[k]] = tmp;
-            }
-        }
-    }
-    return;
-}
-
-
-void Matrix_Copy(int n, int m, float *C, float *A)
-{
-    for (int i=0; i < n*m; i++)
-        C[i] = A[i];
-}
-
-void Matrix_print(int n, int m, float *A, const char *name)
-{
-    fprintf(stdout, "%s=[", name);
-    for (int i=0; i < n; i++) {
-        for (int j=0; j < m; j++) {
-            fprintf(stdout, "%5.5f", A[i*m+j]);
-            if (j < m-1) fprintf(stdout, ", ");
-        }
-        if (i < n-1) fprintf(stdout, "; ");
-    }
-    fprintf(stdout, "]\n");
-}  
-
-
-void Vector_Print(float A[3], const char *name)
-{
-    fprintf(stdout, "%s=[ ", name);
-    for (int i=0; i < 3; i++)
-        fprintf(stdout, "%5.5f ", A[i]);
-    fprintf(stdout, "]\n");
-    
-    return;
-}
-

--- a/PinDetect.lib	Thu Nov 29 19:45:49 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/AjK/code/PinDetect/#cb3afc45028b

--- a/Schedule/Schedule.cpp	Thu Nov 29 19:45:49 2018 +0000
+++ b/Schedule/Schedule.cpp	Fri Nov 30 15:41:05 2018 +0000
@@ -38,7 +38,7 @@
         _max = max;
         _mode = m;
         float slope = ((float) stop - (float) start) / (float) max;
-        for (int i=0; i <= max; i++) {
+        for (tick i=0; i <= max; i++) {
             _schedule[i] = ((int) (slope*(float)i)) + start;
         }
     }

--- a/Sensors/Camera/Camera.cpp	Thu Nov 29 19:45:49 2018 +0000
+++ b/Sensors/Camera/Camera.cpp	Fri Nov 30 15:41:05 2018 +0000
@@ -11,7 +11,7 @@
 
 void Camera::start()
 {
-    serial.attach(this, &Camera::receive, Serial::RxIrq);
+    serial.attach(callback(this, &Camera::receive), Serial::RxIrq);
     
     // ping
     // should now get ACK\r or NACK\r

--- a/Sensors/GPS/Sirf3.cpp	Thu Nov 29 19:45:49 2018 +0000
+++ b/Sensors/GPS/Sirf3.cpp	Fri Nov 30 15:41:05 2018 +0000
@@ -28,7 +28,7 @@
 {
     reset_available();
     setBaud(4800);
-    serial.attach(this, &Sirf3::recv_handler, Serial::RxIrq);
+    serial.attach(callback(this, &Sirf3::recv_handler), Serial::RxIrq);
 }
 
 void Sirf3::disable(void)

--- a/Sensors/GPS/Ublox6.cpp	Thu Nov 29 19:45:49 2018 +0000
+++ b/Sensors/GPS/Ublox6.cpp	Fri Nov 30 15:41:05 2018 +0000
@@ -36,7 +36,8 @@
 void Ublox6::enable(void)
 {
     reset_available();
-    serial.attach(this, &Ublox6::recv_handler, Serial::RxIrq);
+    serial.attach(callback(this, &Ublox6::recv_handler), Serial::RxIrq);
+    //attach (T *object, R(T::*member)(A1))
 }
 
 void Ublox6::disable(void)

--- a/Sensors/GPS/Venus638flpx.cpp	Thu Nov 29 19:45:49 2018 +0000
+++ b/Sensors/GPS/Venus638flpx.cpp	Fri Nov 30 15:41:05 2018 +0000
@@ -30,7 +30,7 @@
 void Venus638flpx::enable(void)
 {
     reset_available();
-    serial.attach(this, &Venus638flpx::recv_handler, Serial::RxIrq);
+    serial.attach(callback(this, &Venus638flpx::recv_handler), Serial::RxIrq);
 }
 
 void Venus638flpx::disable(void)

--- a/Sensors/IncrementalEncoder/IncrementalEncoder.cpp	Thu Nov 29 19:45:49 2018 +0000
+++ b/Sensors/IncrementalEncoder/IncrementalEncoder.cpp	Fri Nov 30 15:41:05 2018 +0000
@@ -2,8 +2,8 @@
 
 IncrementalEncoder::IncrementalEncoder(PinName pin):  _lastTicks(0),  _ticks(0), _new(false), _interrupt(pin) {
     _interrupt.mode(PullNone); // default is pulldown but my encoder board uses a pull-up and that just don't work
-    _interrupt.rise(this, &IncrementalEncoder::_incRise); 
-    _interrupt.fall(this, &IncrementalEncoder::_incFall); 
+    _interrupt.rise(callback(this, &IncrementalEncoder::_incRise)); 
+    _interrupt.fall(callback(this, &IncrementalEncoder::_incFall)); 
     _t.start();
     _t.reset();
     _lastTime = _t.read_us();

--- a/UI/Beep/Beep.cpp	Thu Nov 29 19:45:49 2018 +0000
+++ b/UI/Beep/Beep.cpp	Fri Nov 30 15:41:05 2018 +0000
@@ -25,7 +25,7 @@
 
     _pwm.period(1.0/freq);
     _pwm.write(0.5);            // 50% duty cycle - beep on
-    toff.attach(this,&Beep::nobeep, time);   // time to off
+    toff.attach(callback(this, &Beep::nobeep), time);   // time to off
 }
 
 

--- a/UI/Buttons/Buttons.cpp	Thu Nov 29 19:45:49 2018 +0000
+++ b/UI/Buttons/Buttons.cpp	Fri Nov 30 15:41:05 2018 +0000
@@ -1,18 +1,18 @@
 #include "devices.h"
 #include "Buttons.h"
-#include "PinDetect.h"
 
-PinDetect nextButton(BUTTONNEXT);
-PinDetect selectButton(BUTTONSELECT);            // Input selectButton
-PinDetect prevButton(BUTTONPREV);
+//PinDetect nextButton(BUTTONNEXT);
+//PinDetect selectButton(BUTTONSELECT);            // Input selectButton
+//PinDetect prevButton(BUTTONPREV);
 
-Buttons::Buttons(void): which(0), pressed(false)
+Buttons::Buttons(void): 
+    which(0), pressed(false)
 {
 }
 
 void Buttons::init()
 {
-
+/*
     // Set up button (plugs into two GPIOs, active low
     selectButton.mode(PullUp);
     selectButton.setSamplesTillAssert(50);
@@ -31,6 +31,7 @@
     prevButton.setAssertValue(0); // active low logic
     prevButton.setSampleFrequency(50); // us
     prevButton.attach_asserted( this, &Buttons::prevPressed );
+*/
 }
 
 void Buttons::nextPressed() 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/UI/Buttons/PinDetect.cpp	Fri Nov 30 15:41:05 2018 +0000
@@ -0,0 +1,154 @@
+/*
+    by Michael Shimniok
+    
+    based on PinDetect Copyright (c) 2010 Andy Kirkham
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in
+    all copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+    THE SOFTWARE.
+*/
+
+#if 0
+
+#include "PinDetect.h"
+
+/** initialise class */
+void PinDetect::init(PinName p, PinMode m) {
+    _sampleTime              = PINDETECT_SAMPLE_PERIOD;
+    _samplesTillAssert       = PINDETECT_ASSERT_COUNT;
+    _samplesTillHeld         = 0;
+    _samplesTillAssertReload = PINDETECT_ASSERT_COUNT;
+    _samplesTillHeldReload   = PINDETECT_HOLD_COUNT;
+    _assertValue             = PINDETECT_PIN_ASSTERED;
+
+    _in = new DigitalIn( p );
+    _in->mode( m );
+    _prevState = _in->read();
+    _ticker = new Ticker;
+}
+
+
+void PinDetect::isr(void) {
+/*
+    int currentState = _in->read();
+
+    if ( currentState != _prevState ) {
+        if ( _samplesTillAssert == 0 ) {
+            _prevState = currentState;
+            _samplesTillHeld = _samplesTillHeldReload;
+            if ( currentState == _assertValue )
+                _callbackAsserted.call();
+            else
+                _callbackDeasserted.call();
+        }
+        else {
+            _samplesTillAssert--;
+        }
+    }
+    else {
+        _samplesTillAssert = _samplesTillAssertReload;
+    }
+
+    if ( _samplesTillHeld ) {
+        if ( _prevState == currentState ) {
+            _samplesTillHeld--;
+            if ( _samplesTillHeld == 0 ) {
+                if ( currentState == _assertValue )
+                    _callbackAssertedHeld.call();
+                else
+                    _callbackDeassertedHeld.call();
+            }
+        }
+        else {
+            _samplesTillHeld = 0;
+        }
+    }
+    */
+}
+
+
+PinDetect::PinDetect(PinName p) {
+    init( p, PullDown );
+}
+
+PinDetect::PinDetect(PinName p, PinMode m) {
+    init( p, m );
+}
+
+PinDetect::~PinDetect() {
+    if ( _ticker )  delete( _ticker );
+    if ( _in )      delete( _in );
+}
+
+void PinDetect::setSampleFrequency(int i) {
+    _sampleTime = i;
+    _prevState  = _in->read();
+    //_ticker->attach_us( this, &PinDetect::isr, _sampleTime );
+}
+
+void PinDetect::setAssertValue (int i) {
+     _assertValue = i & 1; 
+}
+
+void PinDetect::setSamplesTillAssert(int i) {
+    _samplesTillAssertReload = i; 
+}
+
+void PinDetect::setSamplesTillHeld(int i) { 
+    _samplesTillHeldReload = i; 
+}
+
+void PinDetect::mode(PinMode m) { 
+    _in->mode( m ); 
+}
+
+void PinDetect::attach_asserted(Callback function) {
+  _callbackAsserted = function;
+}
+
+template<typename T>
+void PinDetect::attach_asserted(T *object, void (T::*member)(void)) {
+//    _callbackAsserted.attach( object, member );
+}
+
+void PinDetect::attach_deasserted(void (*function)(void)) {
+//    _callbackDeasserted.attach( function );
+}
+
+template<typename T>
+void PinDetect::attach_deasserted(T *object, void (T::*member)(void)) {
+//    _callbackDeasserted.attach( object, member );
+}
+
+void PinDetect::attach_asserted_held(void (*function)(void)) {
+//    _callbackAssertedHeld.attach( function );
+}
+template<typename T>
+void PinDetect::attach_asserted_held(T *object, void (T::*member)(void)) {
+//    _callbackAssertedHeld.attach( object, member );
+}
+
+void PinDetect::attach_deasserted_held(void (*function)(void)) {
+//    _callbackDeassertedHeld.attach( function );
+}
+
+template<typename T>
+void PinDetect::attach_deasserted_held(T *object, void (T::*member)(void)) {
+//    _callbackDeassertedHeld.attach( object, member );
+}
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/UI/Buttons/PinDetect.h	Fri Nov 30 15:41:05 2018 +0000
@@ -0,0 +1,412 @@
+/*
+    by Michael Shimniok
+    
+    based on PinDetect Copyright (c) 2010 Andy Kirkham
+
+    Permission is hereby granted, free of charge, to any person obtaining a copy
+    of this software and associated documentation files (the "Software"), to deal
+    in the Software without restriction, including without limitation the rights
+    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+    copies of the Software, and to permit persons to whom the Software is
+    furnished to do so, subject to the following conditions:
+
+    The above copyright notice and this permission notice shall be included in
+    all copies or substantial portions of the Software.
+
+    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+    THE SOFTWARE.
+*/
+
+#if 0
+
+#ifndef AJK_PIN_DETECT_H
+#define AJK_PIN_DETECT_H
+
+#include "mbed.h"
+
+#define PINDETECT_PIN_ASSTERED   1
+#define PINDETECT_SAMPLE_PERIOD 20000
+#define PINDETECT_ASSERT_COUNT  1
+#define PINDETECT_HOLD_COUNT    50
+
+/** PinDetect adds mechanical switch debouncing to DigitialIn and interrupt callbacks.
+ *
+ * This is done by sampling the specified pin at regular intervals and detecting any
+ * change of state ( 0 -> 1 or 1 -> 0 ). When a state change is detected the attached
+ * callback handler is called. Additionally, if the pin stays in the same state after
+ * a state change for a defined period of time, an extra callback is made allowing a
+ * program to detect when a "key is pressed and held down" rather than a momentary
+ * key/switch press.
+ *
+ * All parameters are customisable which include:-
+ *  <ul>
+ *  <li> The sampling frequency. </li>
+ *  <li> The number of continuous samples until a state change is detected. </li>
+ *  <li> The number of continuous samples until a key is assumed held after a state change. </li>
+ *  <li> The logic level which is assumed to be asserted (0volts or +volts). </li>
+ *  </ul>
+ *
+ * Only callbacks that have been attached will be called by the library.
+ *
+ * Example:
+ * @code
+ * #include "mbed.h"
+ * #include "PinDetect.h"
+ *
+ * PinDetect  pin( p30 );
+ * DigitialOut led1( LED1 );
+ * DigitialOut led2( LED2 );
+ * DigitialOut led3( LED3 );
+ * DigitialOut led4( LED4 );
+ *
+ * void keyPressed( void ) {
+ *     led2 = 1;
+ *     led3 = 0;
+ *     led4 = 0;
+ * }
+ *
+ * void keyReleased( void ) {
+ *     led2 = 0;
+ *     led3 = 0;
+ *     led4 = 0;
+ * }
+ *
+ * void keyPressedHeld( void ) {
+ *     led3 = 1;
+ * }
+ *
+ * void keyReleasedHeld( void ) {
+ *     led4 = 1;
+ * }
+ *
+ * int main() {
+ *
+ *     pin.mode( PullDown );
+ *     pin.attach_asserted( &keyPressed );
+ *     pin.attach_deasserted( &keyReleased );
+ *     pin.attach_asserted_held( &keyPressedHeld );
+ *     pin.attach_deasserted_held( &keyReleasedHeld );
+ *
+ *     // Sampling does not begin until you set a frequency.
+ *     // The default is 20ms. If you want a different frequency
+ *     // then pass the period in microseconds for example, for 10ms :-
+ *     //     pin.setSampleFrequency( 10000 );
+ *     //
+ *     pin.setSampleFrequency(); // Defaults to 20ms.
+ *
+ *     while( 1 ) {
+ *         led1 = !led1;
+ *         wait( 0.2 );
+ *     }
+ * }
+ * @endcode
+ *
+ * This example will flash led1 in a similar to a standard starting program.
+ *
+ * Applying a "1" (switch on) to pin 30 will switch on led2, removing the "1" to "0"
+ * (switch off) led2 goes out. Holding the "switch" at one for one second will switch
+ * on led3. An unasserted P30 (switched off) will, after one second illuminate led4
+ * when the deasserted calledback is called.
+ *
+ * The above is a very basic introduction. For more details:-
+ * @see example.h
+ */
+class PinDetect {
+
+protected:
+    DigitalIn   *_in;
+    Ticker      *_ticker;
+    int         _prevState;
+    int         _currentStateCounter;
+    int         _sampleTime;
+    int         _assertValue;
+    int         _samplesTillAssertReload;
+    int         _samplesTillAssert;
+    int         _samplesTillHeldReload;
+    int         _samplesTillHeld;
+    //FunctionPointer _callbackAsserted;
+    //FunctionPointer _callbackDeasserted;
+    //FunctionPointer _callbackAssertedHeld;
+    //FunctionPointer _callbackDeassertedHeld;
+
+    /** initialise class
+     *
+     * @param PinName p is a valid pin that supports DigitalIn
+     * @param PinMode m The mode the DigitalIn should use.
+     */
+    void init(PinName p, PinMode m); 
+
+    /** The Ticker periodic callback function
+     */
+    void isr(void);
+
+public:
+
+    friend class Ticker;
+
+    /** PinDetect constructor
+     *
+     * By default the PinMode is set to PullDown.
+     *
+     * @see http://mbed.org/handbook/DigitalIn
+     * @param p PinName is a valid pin that supports DigitalIn
+     */
+    PinDetect(PinName p);
+
+    /** PinDetect constructor
+     *
+     * @see http://mbed.org/handbook/DigitalIn
+     * @param PinName p is a valid pin that supports DigitalIn
+     * @param PinMode m The mode the DigitalIn should use.
+     */
+    PinDetect(PinName p, PinMode m);
+
+    /** PinDetect destructor
+     */
+    ~PinDetect();
+
+    /** Set the sampling time in microseconds.
+     *
+     * @param int The time between pin samples in microseconds.
+     */
+    void setSampleFrequency(int i = PINDETECT_SAMPLE_PERIOD);
+
+    /** Set the value used as assert.
+     *
+     * Defaults to 1 (ie if pin == 1 then pin asserted).
+     *
+     * @param int New assert value (1 or 0)
+     */
+    void setAssertValue(int i = PINDETECT_PIN_ASSTERED);
+
+    /** Set the number of continuous samples until assert assumed.
+     *
+     * Defaults to 1 (1 * sample frequency).
+     *
+     * @param int The number of continuous samples until assert assumed.
+     */
+    void setSamplesTillAssert(int i);
+
+    /** Set the number of continuous samples until held assumed.
+     *
+     * Defaults to 50 * sample frequency.
+     *
+     * @param int The number of continuous samples until held assumed.
+     */
+    void setSamplesTillHeld(int i);
+
+    /** Set the pin mode.
+     *
+     * @see http://mbed.org/projects/libraries/api/mbed/trunk/DigitalInOut#DigitalInOut.mode
+     * @param PinMode m The mode to pass on to the DigitalIn
+     */
+    void mode(PinMode m);
+
+    /** Attach a callback function
+     *
+     * @code
+     *
+     * DigitalOut led1( LED1 );
+     * PinDetect pin( p30 );
+     *
+     * void myCallback( void ) {
+     *   led1 = 1;
+     * };
+     *
+     * main() {
+     *     pin.attach_asserted( &myCallback );
+     * }
+     *
+     * @endcode
+     *
+     * Call this function when a pin is asserted.
+     * @param function A C function pointer
+     */
+    void attach_asserted(Callback function);
+
+    /** Attach a callback object/method
+     *
+     * @code
+     *
+     * class Bar {
+     *   public:
+     *     void myCallback( void ) { led1 = 1; }
+     * };
+     *
+     * DigitalOut led1( LED1 );
+     * PinDetect pin( p30 );
+     * Bar bar;
+     *
+     * main() {
+     *     pin.attach_asserted( &bar, &Bar::myCallback );
+     * }
+     *
+     * @endcode
+     *
+     * Call this function when a pin is asserted.
+     * @param object An object that conatins the callback method.
+     * @param method The method within the object to call.
+     */
+    template<typename T>
+    void attach_asserted(T *object, void (T::*member)(void));
+
+    /** Attach a callback function
+     *
+     * @code
+     *
+     * DigitalOut led1( LED1 );
+     * PinDetect pin( p30 );
+     *
+     * void myCallback( void ) {
+     *   led1 = 0;
+     * };
+     *
+     * main() {
+     *     pin.attach_deasserted( &myCallback );
+     * }
+     *
+     * @endcode
+     *
+     * Call this function when a pin is deasserted.
+     * @param function A C function pointer
+     */
+    void attach_deasserted(void (*function)(void));
+
+    /** Attach a callback object/method
+     *
+     * @code
+     *
+     * class Bar {
+     *   public:
+     *     void myCallback( void ) { led1 = 0; }
+     * };
+     *
+     * DigitalOut led1( LED1 );
+     * PinDetect pin( p30 );
+     * Bar bar;
+     *
+     * main() {
+     *     pin.attach_deasserted( &bar, &Bar::myCallback );
+     * }
+     *
+     * @endcode
+     *
+     * Call this function when a pin is deasserted.
+     * @param object An object that conatins the callback method.
+     * @param method The method within the object to call.
+     */
+    template<typename T>
+    void attach_deasserted(T *object, void (T::*member)(void));
+
+    /** Attach a callback function
+     *
+     * @code
+     *
+     * DigitalOut led2( LED2 );
+     * PinDetect pin( p30 );
+     *
+     * void myCallback( void ) {
+     *   led2 = 1;
+     * };
+     *
+     * main() {
+     *     pin.attach_asserted_held( &myCallback );
+     * }
+     *
+     * @endcode
+     *
+     * Call this function when a pin is asserted and held.
+     * @param function A C function pointer
+     */
+    void attach_asserted_held(void (*function)(void));
+
+    /** Attach a callback object/method
+     *
+     * @code
+     *
+     * class Bar {
+     *   public:
+     *     void myCallback( void ) { led2 = 0; }
+     * };
+     *
+     * DigitalOut led2( LED2 );
+     * PinDetect pin( p30 );
+     * Bar bar;
+     *
+     * main() {
+     *     pin.attach_asserted_held( &bar, &Bar::myCallback );
+     * }
+     *
+     * @endcode
+     *
+     * Call this function when a pin is asserted and held.
+     * @param object An object that conatins the callback method.
+     * @param method The method within the object to call.
+     */
+    template<typename T>
+    void attach_asserted_held(T *object, void (T::*member)(void));
+
+    /** Attach a callback function
+     *
+     * @code
+     *
+     * DigitalOut led3( LED3 );
+     * PinDetect pin( p30 );
+     *
+     * void myCallback( void ) {
+     *   led3 = 1;
+     * };
+     *
+     * main() {
+     *     pin.attach_deasserted_held( &myCallback );
+     * }
+     *
+     * @endcode
+     *
+     * Call this function when a pin is deasserted and held.
+     * @param function A C function pointer
+     */
+    void attach_deasserted_held(void (*function)(void));
+
+    /** Attach a callback object/method
+     *
+     * @code
+     *
+     * class Bar {
+     *   public:
+     *     void myCallback( void ) { led3 = 0; }
+     * };
+     *
+     * DigitalOut led3( LED3 );
+     * PinDetect pin( p30 );
+     * Bar bar;
+     *
+     * main() {
+     *     pin.attach_deasserted_held( &bar, &Bar::myCallback );
+     * }
+     *
+     * @endcode
+     *
+     * Call this function when a pin is deasserted and held.
+     * @param object An object that conatins the callback method.
+     * @param method The method within the object to call.
+     */
+    template<typename T>
+    void attach_deasserted_held(T *object, void (T::*member)(void));
+
+    /** operator int()
+     *
+     * Read the value of the pin being sampled.
+     */
+    operator int() { return _in->read(); }
+
+};
+
+#endif
+
+#endif

--- a/globals.h	Thu Nov 29 19:45:49 2018 +0000
+++ b/globals.h	Fri Nov 30 15:41:05 2018 +0000
@@ -15,7 +15,7 @@
 #include "SerialGraphicLCD.h"
 
 extern Steering steering;
-extern Serial pc;
+//extern Serial pc;
 extern SerialGraphicLCD lcd;
 extern Buttons keypad;
 

--- a/logging/logging.cpp	Thu Nov 29 19:45:49 2018 +0000
+++ b/logging/logging.cpp	Fri Nov 30 15:41:05 2018 +0000
@@ -1,273 +0,0 @@
-#include "SystemState.h"
-#include "globals.h"
-#include "logging.h"
-#include "SDFileSystem.h"
-#include "SerialGraphicLCD.h"
-
-// TODO 2 set up logging out of low priority interrupt handler
-
-//SDFileSystem sd(p5, p6, p7, p8, "log"); // mosi, miso, sclk, cs
-static FILE *logp;
-
-Timer logtimer;
-//extern int bufCount;
-
-/*
-void logData( const SystemState s ) {
-    unsigned char buf[512]; // for now we really only need ~256 bytes but in case I add more to state...
-    unsigned char *state = (unsigned char *) &s;
-    //unsigned int t1, t2, t3;
-    //logtimer.start();
-    //logtimer.reset();
-    if (logp) {
-        //t1 = logtimer.read_us();
-        encode(state, sizeof(s), buf, 0); // infinite line size
-        //t2 = logtimer.read_us();
-        fputs((char *) buf, logp);
-        fputs("\n", logp);
-        bufCount--;
-        fprintf(stdout, "bufCount: %d\n", bufCount);
-        //t3 = logtimer.read_us();
-        //fprintf(stdout, "%d %d\n", t3-t2, t2-t1);
-    }
-}
-*/
-
-// from Arduino source
-size_t printNumber(FILE *f, unsigned long n)
-{
-    char buf[8 * sizeof(long) + 1]; // Assumes 8-bit chars plus zero byte.
-    char *str = &buf[sizeof(buf) - 1];
-
-    *str = '\0';
-
-    do {
-        unsigned long m = n;
-        n /= 10;
-        char c = m - 10 * n;
-        *--str = c + '0';
-    } while(n);
-
-    return fputs(str, f);
-}
-
-// from Arduino source
-size_t printInt(FILE *f, long n)
-{
-    int t = 0;
-    if (n < 0) {
-        t = fputc('-', f);
-        n = -n;
-    }
-    return printNumber(f, n) + t;
-}
-
-// from Arduino source
-size_t printFloat(FILE *f, double number, uint8_t digits)
-{
-    size_t n=0;
-
-    if (isnan(number)) return fputs("nan", f);
-    if (isinf(number)) return fputs("inf", f);
-    if (number > 4294967040.0) return fputs("ovf", f);  // constant determined empirically
-    if (number <-4294967040.0) return fputs("ovf", f);  // constant determined empirically
-
-    // Handle negative numbers
-    if (number < 0.0) {
-        n += fputc('-', f);
-        number = -number;
-    }
-
-    // Round correctly so that print(1.999, 2) prints as "2.00"
-    double rounding = 0.5;
-    for (uint8_t i=0; i < digits; ++i)
-        rounding /= 10.0;
-
-    number += rounding;
-
-    // Extract the integer part of the number and print it
-    unsigned long int_part = (unsigned long)number;
-    double remainder = number - (double)int_part;
-    n += printInt(f, int_part);
-
-    // Print the decimal point, but only if there are digits beyond
-    if (digits > 0) {
-        n += fputc('.', f);
-    }
-
-    // Extract digits from the remainder one at a time
-    while (digits-- > 0) {
-        remainder *= 10.0;
-        int toPrint = int(remainder);
-        n += fputc(toPrint+'0', f);
-        remainder -= toPrint;
-    }
-
-    return n;
-}
-
-
-// If I use arduino style print routines, logging takes ~1000 / ~8000 usec
-// the big sprintf takes ~ 700-750 usec all by itself
-void logData( SystemState *s )
-{
-    //char buf[256];
-    //unsigned int t1, t2;
-    //logtimer.start();
-    //logtimer.reset();
-    //t1 = logtimer.read_us();
-
-    if (s) {
-        printInt(logp, s->millis);
-        fputc(',',logp);
-        printFloat(logp, s->current, 2);
-        fputc(',',logp);
-        printFloat(logp, s->voltage, 2);
-        fputc(',',logp);
-        for (int q=0; q < 3; q++) {
-            printFloat(logp, s->gyro[q], 6);
-            fputc(',',logp);
-        }
-        printInt(logp, s->gTemp);
-        fputc(',',logp);
-        for (int q=0; q < 3; q++) {
-            printInt(logp, s->a[q]);
-            fputc(',',logp);
-        }
-        /*
-        for (int q=0; q < 3; q++) {
-            printInt(logp, s->m[q]);
-            fputc(',',logp);
-        }
-        */
-        printFloat(logp, s->gHeading, 2);
-        fputc(',',logp);
-
-        // GPS 1
-        fprintf(logp, "%.7f,%.7f,", s->gpsLatitude, s->gpsLongitude);
-        //printFloat(logp, s->gpsLatitude, 7);
-        //fputc(',',logp);
-        //printFloat(logp, s->gpsLongitude, 7);
-        //fputc(',',logp);
-        printFloat(logp, s->gpsCourse_deg, 2);
-        fputc(',',logp);
-        printFloat(logp, s->gpsSpeed_mps, 2);
-        fputc(',',logp);
-        printFloat(logp, s->gpsHDOP, 1);
-        fputc(',',logp);
-        printInt(logp, s->gpsSats);
-        fputc(',',logp);
-        // Encoders
-        printFloat(logp, s->lrEncDistance, 7);
-        fputc(',',logp);
-        printFloat(logp, s->rrEncDistance, 7);
-        fputc(',',logp);
-        printFloat(logp, s->lrEncSpeed, 2);
-        fputc(',',logp);
-        printFloat(logp, s->rrEncSpeed, 2);
-        fputc(',',logp);
-        printFloat(logp, s->encHeading, 2);
-        fputc(',',logp);
-        // Estimates
-        printFloat(logp, s->estHeading, 2);
-        fputc(',',logp);
-        printFloat(logp, s->estLagHeading, 2);
-        fputc(',',logp);
-        printFloat(logp, s->estLatitude,  7);
-        fputc(',',logp);
-        printFloat(logp, s->estLongitude, 7);
-        fputc(',',logp);
-        printFloat(logp, s->estX, 4);
-        fputc(',',logp);
-        printFloat(logp, s->estY, 4);
-        fputc(',',logp);
-        // Nav
-        printInt(logp, s->nextWaypoint);
-        fputc(',',logp);
-        printFloat(logp, s->bearing, 2);
-        fputc(',',logp);
-        printFloat(logp, s->distance, 3);
-        fputc(',',logp);
-        printFloat(logp, s->steerAngle, 3);
-        fputc(',',logp);
-        printFloat(logp, s->errHeading, 3);
-        fputc(',',logp);
-        printFloat(logp, s->LABrg, 2);
-        fputc(',',logp);
-        printFloat(logp, s->LAx, 4);
-        fputc(',',logp);
-        printFloat(logp, s->LAy, 4);
-        fputc('\n',logp);
-        fflush(logp);
-
-        //t2 = logtimer.read_us();
-        //fprintf(stdout, "%d\n", t2-t1);
-    }
-
-    return;
-}
-
-
-FILE *openlog(const char *prefix)
-{
-    FILE *fp = 0;
-    char myname[64];
-
-    pc.printf("Opening file...\n");
-
-    while (fp == 0) {
-        sprintf(myname, "%s/test.txt", LOGDIR);
-        if ((fp = fopen(myname, "w")) == 0) {
-            pc.printf("Waiting for filesystem to come online...");
-            wait(0.200);
-            lcd.pos(0,1);
-            lcd.printf("%-16s", "Waiting for fs");
-        }
-    }
-    fclose(fp);
-
-    for (int i = 0; i < 1000; i++) {
-        sprintf(myname, "%s/%s%03d.csv", LOGDIR, prefix, i);
-        if ((fp = fopen(myname, "r")) == 0) {
-            break;
-        } else {
-            fclose(fp);
-        }
-    }
-    fp = fopen(myname, "w");
-    if (fp == 0) {
-        pc.printf("file write failed: %s\n", myname);
-    } else {
-
-        // TODO 3 set error message, get rid of writing to terminal
-
-        //status = true;
-        pc.printf("opened %s for writing\n", myname);
-        lcd.pos(0,1);
-        lcd.printf("%-16s", myname);
-    }
-
-    return fp;
-}
-
-
-// Find the next unused filename of the form logger##.csv where # is 0-9
-//
-bool initLogfile()
-{
-    bool status = false;
-
-    logp = openlog("log");
-
-    if (logp != 0) {
-        status = true;
-        //fprintf(logp, "s.millis, s.current, s.voltage, s.gx, s.gy, s.gz, s.gTemp, s.ax, s.ay, s.az, s.mx, s.my, s.mz, s.gHeading, s.cHeading, s.roll, s.pitch, s.yaw, s.gpsLatitude, s.gpsLongitude, s.gpsCourse, s.gpsSpeed, s.gpsHDOP, s.lrEncDistance, s.rrEncDistance, s.lrEncSpeed, s.rrEncSpeed, s.encHeading, s.estHeading, s.estLatitude, s.estLongitude, s.estNorthing, s.estEasting, s.estX, s.estY, s.nextWaypoint, s.bearing, s.distance, s.gbias, s.errAngle, s.leftRanger, s.rightRanger, s.centerRanger, s.crossTrackErr\n");
-    }
-
-    return status;
-}
-
-void closeLogfile(void)
-{
-    if (logp) fclose(logp);
-}