File content as of revision 0:fc6fa085d591:

/*
 * read_filter_emg.cpp
 *
 *  Created on: Oct 14, 2015
 *      Author: User
 */

#include "mbed.h"
#include "read_filter_emg.h"
//included for fabs() function
#include <math.h>
//EMG::EMG(AnalogIn aI_, double &v1_, double &v2_)

//EMG constructor
EMG::EMG(PinName aI_, double &v1_, double &v2_)
{
    //initialise the variables of the EMG instance
    //  ??????  NOT SURE HOW THIS SECTION SHOULD WORK EXACTLY, WHAT SHOULD I DEFINE, WHAT SHOULD I 'RENAME'
    //          E.G aI = aI_, v1 = v1_...
    //
    AnalogIn aI(aI_);
    /* two past values of v1 and v2 initialised in main are
        passed to the function as they need to be kept track of
      cannot be reset each time as filtering would be incorrect
    */
    v1 = v1_;
    v2 = v2_;
    //initialised to zero, updated in sample()
    input_sample = 0;
    //initialised to zero, calculated in filter function using input_sample
    filtered_sample = 0;
};

void EMG::sample()
{
    //change value of sample to input of the EMG
    //should i use the reference?
    input_sample = aI.read();
}

//general biquad filter that can be called in all the filter functions
double EMG::biquad(double u, double &v1, double &v2, const double a1,
                    const double a2, const double b0, const double b1, const double b2)
{
    double v = u * a1*v1 * a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    //values of v2 and v1 are updated, as they are passed by reference
    //they update globally
    v2 = v1; v1 = v;
    return y;
}

double EMG::highpass_filter(double input)
{
    /* high pass filter consists of three cascaded biquads
    blow coefficients for those three biquads */
    //first high pass biquad
    const double highp1_a1 = -0.67538034389;
    const double highp1_a2 = 0.12769255668;
    const double highp1_b0 = 1.00000000000;
    const double highp1_b1 = -2.00000000000;
    const double highp1_b2 = 1.00000000000;

    //second high pass biquad
    const double highp2_a1 = -0.76475499450;
    const double highp2_a2 = 0.27692273367;
    const double highp2_b0 = 1.00000000000;
    const double highp2_b1 = -2.00000000000;
    const double highp2_b2 = 1.00000000000;

    //third high pass biquad
    const double highp3_a1 = -0.99216561242;
    const double highp3_a2 = 0.65663360837;
    const double highp3_b0 = 1.00000000000;
    const double highp3_b1 = -2.00000000000;
    const double highp3_b2 = 1.00000000000;

    //input to each filter is output of the filter before(excl. first which uses input_sample)
    /* NOT SURE IF PASSING V1 AND V2 IS CORRECT,
    WILL IT UPDATE IN THE MEMORY POSITION SO THAT
    V1 IS CHANGED GLOBALLY */
    double y1 = EMG::biquad(input, v1, v2, highp1_a1, highp1_a2, highp1_b0, highp1_b1, highp1_b2);
    double y2 = EMG::biquad(y1, v1, v2, highp2_a1, highp2_a2, highp2_b0, highp2_b1, highp2_b2);
    double y3 = EMG::biquad(y2, v1, v2, highp3_a1, highp3_a2, highp3_b0, highp3_b1, highp3_b2);

    return y3;
}

//need to implement
double rectify(double input)
{
    input = fabs(input);
    return input;
}
double EMG::lowpass_filter(double input)
{
    /* lowpass filter consists of three cascaded biquads
    below the coefficients for those three biquads */
    //first high pass biquad
    const double lowp1_a1 = -1.05207469728;
    const double lowp1_a2 = 0.28586907478;
    const double lowp1_b0 = 1.00000000000;
    const double lowp1_b1 = 2.00000000000;
    const double lowp1_b2 = 1.00000000000;

    //second high pass biquad
    const double lowp2_a1 = -1.16338171052;
    const double lowp2_a2 = 0.42191097989;
    const double lowp2_b0 = 1.00000000000;
    const double lowp2_b1 = 2.00000000000;
    const double lowp2_b2 = 1.00000000000;

    //third high pass biquad
    const double lowp3_a1 = -1.42439823874;
    const double lowp3_a2 = 0.74093118112;
    const double lowp3_b0 = 1.00000000000;
    const double lowp3_b1 = 2.00000000000;
    const double lowp3_b2 = 1.00000000000;

    //input to each filter is output of the filter before(excl. first which uses input_sample)
    /* NOT SURE IF PASSING V1 AND V2 IS CORRECT,
    WILL IT UPDATE IN THE MEMORY POSITION SO THAT V1 IS
    CHANGED GLOBALLY */
    double y1 = EMG::biquad(input, v1, v2, lowp1_a1, lowp1_a2, lowp1_b0, lowp1_b1, lowp1_b2);
    double y2 = EMG::biquad(y1, v1, v2, lowp2_a1, lowp2_a2, lowp2_b0, lowp2_b1, lowp2_b2);
    double y3 = EMG::biquad(y2, v1, v2, lowp3_a1, lowp3_a2, lowp3_b0, lowp3_b1, lowp3_b2);

    return y3;
}

double EMG::filter(double input)
{
    /* function passes the input through the three filters
    returns the final output value as filtered sample
    this is used in check_state() function to determine state of system
    */
    double y1 = highpass_filter(input);
    double y2 = rectify(y1);
    double y3 = lowpass_filter(y3);
    filtered_sample = y3;

    return filtered_sample;
}