Revision 0:7a4d80e7ea81, committed 2015-02-21

Comitter:

t_tatsuoka

Date:

Sat Feb 21 19:28:57 2015 +0000

Child:

1:4172c4324c2d

Commit message:

Ver. 1.0.1 Published initial version

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FilterTest.cpp	Sat Feb 21 19:28:57 2015 +0000
@@ -0,0 +1,217 @@
+/**
+ *  @file       FilterTest.cpp
+ *  @brief      Calculate filters
+ *              HPF: 1st order / LPF: 2nd order / Notch: 2nd order
+ *  @date       2015.02.22
+ *  @version    1.0.1
+ */
+#include "FilterTest.h"
+
+/** Constructor
+*/
+FilterTest::FilterTest ()
+{
+    set_hpf_coef(INIT_HB, INIT_HA);
+    set_lpf_coef(INIT_LB, INIT_LA1, INIT_LA2);
+    set_brf_coef(INIT_NB, INIT_NA1, INIT_NA2);
+    reset_hpf_buf();
+    reset_lpf_buf();
+    reset_brf_buf();
+}
+
+/** Calculate filter
+ *  @param      val         Input value
+ *  @param      hpf_on      High pass filter enable
+ *  @param      lpf_on      Low pass filter enable
+ *  @param      brf_on      Notch filter enable
+ *  @return                 Output value
+ */
+double FilterTest::calc(double val, int hpf_on, int lpf_on, int brf_on)
+{
+    double retVal = val;
+    /* High pass filter */
+    if(hpf_on) {
+        retVal = hpf(retVal);
+    } else {
+        reset_hpf_buf();
+    }
+    /* Low pass filter */
+    if(lpf_on) {
+        retVal = lpf(retVal);
+    } else {
+        reset_lpf_buf();
+    }
+    /* Notch (Band reject) filter */
+    if(brf_on) {
+        retVal = brf(retVal);
+    } else {
+        reset_brf_buf();
+    }
+    return retVal;
+}
+
+/** Reset delay buffers for high pass filter
+*/
+void FilterTest::reset_hpf_buf()
+{
+    _hw = 0.0;
+}
+
+/** Reset delay buffers for low pass filter
+*/
+void FilterTest::reset_lpf_buf()
+{
+    _lw1 = 0.0;
+    _lw2 = 0.0;
+}
+
+/** Reset delay buffers for notch filter
+*/
+void FilterTest::reset_brf_buf()
+{
+    _nw1 = 0.0;
+    _nw2 = 0.0;
+}
+
+/** Set coefficient for HPF
+ *  @param      hb          Numerator cofficient
+ *  @param      ha          Denominator cofficient
+ *  @retval     true        OK
+ *  @retval     false       NG
+ */
+bool FilterTest:: set_hpf_coef(double hb, double ha)
+{
+    if(hb > 1.0) {
+        return false;
+    } else if((ha > 1.0)||(ha < -1.0)) {
+        return false;
+    } else {
+        _hb = hb;
+        _ha = ha;
+        reset_hpf_buf();
+        return true;
+    }
+}
+
+/** Set coefficient for LPF
+ *  @param      lb          Numerator cofficient
+ *  @param      la1         Denominator cofficient 1
+ *  @param      la2         Denominator cofficient 2
+ *  @retval     true        OK
+ *  @retval     false       NG
+ */
+bool FilterTest:: set_lpf_coef(double lb, double la1, double la2)
+{
+    if(lb > 1.0) {
+        return false;
+    } else if((la1 > 2.0)||(la1 < -2.0)) {
+        return false;
+    } else if(la2 > 1.0) {
+        return false;
+    } else {
+        _lb = lb;
+        _la1 = la1;
+        _la2 = la2;
+        reset_lpf_buf();
+        return true;
+    }
+}
+
+/** Set coefficient for BRF
+ *  @param      nb          Numerator cofficient
+ *  @param      na1         Denominator cofficient 1
+ *  @param      na2         Denominator cofficient 2
+ *  @retval     true        OK
+ *  @retval     false       NG
+ */
+bool FilterTest:: set_brf_coef(double nb, double na1, double na2)
+{
+    if(nb > 1.0) {
+        return false;
+    } else if((na1 > 2.0)||(na1 < -2.0)) {
+        return false;
+    } else if(na2 > 1.0) {
+        return false;
+    } else {
+        _nb = nb;
+        _na1 = na1;
+        _na2 = na2;
+        reset_brf_buf();
+        return true;
+    }
+}
+
+/** High pass filter (1st order)
+ *  @param      x      Input value
+ *  @return            Output value
+ *
+ *       hb   v  +
+ *  x ---I>---+---O-------+--- y
+ *            | hw|-      |
+ *            |  [z]      |
+ *            |   |   ha  |
+ *            +---O---<I--+
+ */
+double FilterTest::hpf(double x)
+{
+    double v, y;
+
+    v = _hb * x;
+    y = v - _hw;
+    _hw = v + _ha * y;
+    return y;
+}
+
+/** Low pass filter (2nd order)
+ *  @param      x      Input value
+ *  @return            Output value
+ *
+ *      lb  v
+ *  x --I>--+-----O-------+--- y
+ *          |  lw1|       |
+ *          |    [z]      |
+ *          |  2  |  -la1 |
+ *          +-I>--O---<I--+
+ *          |  lw2|       |
+ *          |    [z]      |
+ *          |     |  -la2 |
+ *          +-----O---<I--+
+ */
+double FilterTest::lpf(double x)
+{
+    double v, y;
+
+    v = _lb * x;
+    y = v + _lw1;
+    _lw1 = 2 * v - _la1 * y + _lw2;
+    _lw2 = v - _la2 * y;
+    return y;
+}
+
+/** Notch filter (Band reject filter) (2nd order)
+ *  @param      x      Input value
+ *  @return            Output value
+ *
+ *       nb  v
+ *  x -+-I>--+----O-------+--- y
+ *     |     | nw1|       |
+ *     |     |   [z]      |
+ *    +| na1 |    |       |
+ *     O-I>-------O       |
+ *    -|     | nw2|       |
+ *     |     |   [z]      |
+ *     |     |    |  -na2 |
+ *     |     +----O---<I--+
+ *     |                  |
+ *     +------------------+
+ */
+double FilterTest::brf(double x)
+{
+    double v, y;
+
+    v = _nb * x;
+    y = v + _nw1;
+    _nw1 = _na1 * ( x - y ) + _nw2;
+    _nw2 = v - _na2 * y;
+    return y;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FilterTest.h	Sat Feb 21 19:28:57 2015 +0000
@@ -0,0 +1,70 @@
+/**
+ *  @file       FilterTest.h
+ *  @brief      Header file for FilterTest.cpp
+ *  @date       2015.02.22
+ *  @version    1.0.1
+ */
+#ifndef _INC_FilterTest
+#define _INC_FilterTest
+
+#include "mbed.h"
+
+/* High pass filter fc = 0.5 Hz, fs = 1000 Hz */
+#define INIT_HB (0.998431665916719)    /* HPF numerator coefficient */
+#define INIT_HA (-0.996863331833438)   /* HPF denominator coefficient */
+
+/* Low pass filter fc = 30 Hz, fs = 1000 Hz */
+#define INIT_LB (0.007820208033497)    /* LPF numerator coefficient */
+#define INIT_LA1 (-1.734725768809275)  /* LPF denominator coefficient 1 */
+#define INIT_LA2 (0.766006600943264)   /* LPF denominator coefficient 2 */
+
+/* Notch filter fc = 50 Hz, fs = 1000 Hz */
+#define INIT_NB (0.820675769028781)    /* BRF numerator coefficient */
+#define INIT_NA1 (-1.561018075800720)  /* BRF denominator coefficient 1 */
+#define INIT_NA2 (0.641351538057563)   /* BRF denominator coefficient 2 */
+
+// /* Notch filter fc = 60 Hz, fs = 1000 Hz */
+// #define INIT_NB (0.793459754030595)    /* BRF numerator coefficient */
+// #define INIT_NA1 (-1.475480443592650)  /* BRF denominator coefficient 1 */
+// #define INIT_NA2 (0.586919508061190)   /* BRF denominator coefficient 2 */
+
+/**  Filter test
+ */
+class FilterTest
+{
+
+public:
+    FilterTest();
+    double calc(double val, int hpf_on, int lpf_on, int brf_on);
+    bool set_hpf_coef(double hb, double ha);
+    bool set_lpf_coef(double lb, double la1, double la2);
+    bool set_brf_coef(double nb, double na1, double na2);
+
+private:
+
+    /* Coefficient */
+    double      _hb;                /* High pass filter numerator coefficient */
+    double      _ha;                /* High pass filter denominator coefficient */
+    double      _lb;                /* Low pass filter numerator coefficient */
+    double      _la1;               /* Low pass filter denominator coefficient 1 */
+    double      _la2;               /* Low pass filter denominator coefficient 2 */
+    double      _nb;                /* Notch filter numerator coefficient */
+    double      _na1;               /* Notch filter denominator coefficient 1 */
+    double      _na2;               /* Notch filter denominator coefficient 2 */
+
+    /* Delay buffer */
+    double      _hw;                /* High pass filter delay buffer */
+    double      _lw1;               /* Low pass filter delay buffer 1 */
+    double      _lw2;               /* Low pass filter delay buffer 2 */
+    double      _nw1;               /* Notch filter delay buffer 1 */
+    double      _nw2;               /* Notch filter delay buffer 2 */
+
+    /* Member functions */
+    void reset_hpf_buf();
+    void reset_lpf_buf();
+    void reset_brf_buf();
+    double hpf(double x);
+    double lpf(double x);
+    double brf(double x);
+};
+#endif    /* INC_FilterTest */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice.lib	Sat Feb 21 19:28:57 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/USBDevice/#0ca6eeb54b97

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Feb 21 19:28:57 2015 +0000
@@ -0,0 +1,170 @@
+/**
+ *  @file       Main.cpp
+ *  @brief      Send 1ch waveform data to PC via USB serial
+ *  @date       2015.02.22
+ *  @version    1.0.1
+ */
+#include "mbed.h"
+#include "USBSerial.h"
+#include "FilterTest.h"
+
+#define ON      (1)
+#define OFF     (0)
+
+#define LED_ON  (0)
+#define LED_OFF (1)
+
+#define BYTE_MASK     (0xFF)
+#define INT16_MAX     (32767)
+#define INT16_MIN     (-32768)
+
+#define SAMPLING_RATE (0.001)   /* A/D sampling rate (1ms) */
+
+#define PACKET_HEADER (0xAA)
+#define WAVEFORM_ID   (0x01)
+#define PACKET_SIZE   (64)
+#define DATA_NUM      (60)
+#define BUF_NUM       (2)
+#define CNTR_SIZE     (100)
+/*  [ Packet format ]                                           */
+/*        ----------------                                      */
+/*  0x00 | Header byte    | Fixed value (PACKET_HEADER:0xAA)    */
+/*  0x01 | Data type      | ID value    (0x01:Waveform)         */
+/*  0x02 | Packet counter | Count value (0to99 < CNTR_SIZE:100) */
+/*  0x03 | Payload size   | Fixed value (DATA_NUM:60)           */
+/*  0x04 | Data0 (MSBside)| Short type  (signed, big endian)    */
+/*  ...  | ...            |                                     */
+/*  0x3F | Data29(LSBside)|                                     */
+/*        ----------------                                      */
+
+Ticker      sampling;           /* Interval timer for A/D sampling */
+AnalogIn    wave_in(p20);       /* A/D port */
+USBSerial   serial;
+FilterTest  filter;
+
+DigitalIn   hpf_on(p28);        /* Hiph pass filter */
+DigitalIn   lpf_on(p27);        /* Low pass filter */
+DigitalIn   brf_on(p26);        /* Notch (Band reject) filter */
+/*  [ DIP switch ]                                  */
+/*    TG-LPC11U35-501         +3.3V                 */
+/*  | CN1         CN2 |        ---                  */
+/*  |     mbed BD     |         |                   */
+/*  |              13 | p28 -/ -+  Hiph pass filter */
+/*  |              14 | p27 -/ -+  Low pass filter  */
+/*  |              15 | p26 -/ -+  Notch filter     */
+/*  |                 |                             */
+/*  pull-down is default as below.                  */
+/*  http://developer.mbed.org/handbook/DigitalIn    */
+/*  It says "By default, the DigitalIn is setup     */
+/*          with an internal pull-down resistor."   */
+/*  Better to set the pins to pull-up mode and the  */
+/*  switch connected to GND.                        */
+
+DigitalOut  dbg_pin1(p21);      /* for debug */
+DigitalOut  dbg_pin2(p22);      /* for debug */
+DigitalOut  dbg_led1(LED1);     /* for debug */
+DigitalOut  dbg_led2(LED2);     /* for debug */
+
+int32_t idx, toggle, send_flag;
+uint8_t cntr;
+uint8_t buf[BUF_NUM][PACKET_SIZE];
+
+
+/** Initialize buffer
+ *  @param      tgl    buffer toggle index
+ *  @param      ctr    packet counter
+ */
+void init_buf(int32_t tgl, uint8_t ctr)
+{
+    int i;
+    if(tgl>BUF_NUM) {
+        return;
+    }
+    buf[tgl][0] = PACKET_HEADER;
+    buf[tgl][1] = WAVEFORM_ID;
+    buf[tgl][2] = ctr;              /* Packet counter */
+    buf[tgl][3] = DATA_NUM;         /* Payload size */
+    idx = 4;                        /* Start index of waveform */
+    for(i=4; i<PACKET_SIZE; i++) {  /* Initialize array just in case */
+        buf[tgl][i] = 0;
+    }
+}
+
+/** Interval timer for read A/D value
+ */
+void ad_sampling()
+{
+    int32_t wav_temp;
+    dbg_pin1 = ON;      /* Calculation time for filter for debug */
+    dbg_led1 = hpf_on;  /* High pass filter enable status for debug */
+    dbg_led2 = lpf_on;  /* Low pass filter enable status for debug */
+
+    /* Read and filter data */
+    wav_temp = (int32_t)filter.calc( (double)(wave_in.read_u16() - INT16_MAX), hpf_on, lpf_on, brf_on );
+    wav_temp = (wav_temp > INT16_MAX) ? INT16_MAX : wav_temp;   /* Clip ceiling */
+    wav_temp = (wav_temp < INT16_MIN) ? INT16_MIN : wav_temp;   /* Clip floor */
+    buf[toggle][idx] = (uint8_t)((wav_temp >> 8 ) & BYTE_MASK); /* MSB side (big endian) */
+    idx++;
+    buf[toggle][idx] = (uint8_t)(wav_temp & BYTE_MASK);         /* LSB side */
+    idx++;
+
+    dbg_pin1 = OFF; /* for debug */
+
+    /* Switch buffer */
+    if(idx >= PACKET_SIZE) {    /* Counter reached */
+        toggle = !toggle;
+        cntr = (cntr + 1 ) % CNTR_SIZE;
+        init_buf(toggle, cntr);
+        send_flag = ON;          /* Set flag */
+    }
+}
+
+/** Send data packet
+ *  @param      p_buf   Data array
+ *  @param      size    Data length
+ */
+bool send_packet(uint8_t *p_buf, uint16_t size)
+{
+    if(serial.writeable()) {
+        dbg_pin2 = ON;   /* for debug */
+        serial.writeBlock (p_buf, size);    /* Send data via USB */
+        dbg_pin2 = OFF;  /* for debug */
+        return true;
+    } else {
+        return false;
+    }
+}
+
+/** Main function
+ */
+int main()
+{
+    /* Initialization */
+    idx = 0;
+    toggle = 0;
+    cntr = 0;
+    send_flag = 0;
+    init_buf(toggle, cntr);
+    init_buf(!toggle, cntr);
+
+    dbg_pin1 = OFF;
+    dbg_pin2 = OFF;
+    dbg_led1 = LED_OFF;
+    dbg_led2 = LED_OFF;
+
+    /* Start interval timer */
+    sampling.attach(&ad_sampling, SAMPLING_RATE);
+
+    /* Main loop */
+    while(1) {
+        if(send_flag != OFF) {
+            /* Send data */
+            send_packet(buf[!toggle], (uint16_t)PACKET_SIZE);
+
+            /* Disable interrupt */
+            __disable_irq();
+            send_flag = OFF;    /* Clear flag */
+            __enable_irq();
+        }
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Feb 21 19:28:57 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9ad691361fac
\ No newline at end of file