File content as of revision 12:e0adb697fcdb:

#include "IMD.h"

CANBuffer *tx_IMD_Buffer;

IMD::IMD(CANBuffer *can): _p(P1_26) {
    _p.rise(this, &IMD::rise);
    _p.fall(this, &IMD::fall);
    _period = 0.0;
    _pulsewidth = 0.0;
    
    tx_IMD_Buffer = can;
}

void IMD::rise() {
    _period = _t.read();
    _t.reset();
}
    
void IMD::fall() {
    _pulsewidth = _t.read();
}

float IMD::frequency() { return 1/_period; }
float IMD::pulse_width() { return _pulsewidth; }
float IMD::duty() { return _pulsewidth / _period; }

/*
    Status      State
    1           Normal
    2           under voltage
    3           Insulation measurement: good
    4           Insulation measurement: bad
    5           Device error
    6           Connection fault earth
*/
void update_IMD(void const *arg){
    IMD *instance = (IMD *)arg;
    char data[1] = {0};
    while(1){
        float freq = instance->frequency();
        float duty = instance->duty();
        
        if(freq >= 5 && freq <15){
            data[0] = 1;   
        }else if(freq >= 15 && freq < 25){
            data[0] = 2;   
        }else if(freq >= 25 && freq < 35){
            if(duty <= 15){
                data[0] = 3;
            } else if(duty > 85 && duty < 100){
                data[0] = 4;   
            }   
        }else if(freq >= 35 && freq < 45){
            data[0] = 5;   
        }else if(freq >= 45 && freq < 55){
            data[0] = 6;
        }
        
        CANMessage txMessage(TX_IMD_ID, data, 1);
        tx_IMD_Buffer->txWrite(txMessage);
        
        Thread::wait(100);  //10 Hz update    
    }
}

void IMD::start_update(){
    Thread update_thread(update_IMD, this);
}