File content as of revision 0:bec310bde899:

#include "kompas.h"

Kompas::Kompas():compass(p9, p10) {
    koers = 0;
    prev = 0;
    cal = 0;

    wait(0.5);

    char compass_out[3];
    compass_out[0] = 0xF1;
    compass_out[1] = 0x05;
    compass_out[1] = 0x02;
    compass.write(ADDR,compass_out,3);
    wait_ms(1);
    refresh.attach(this,&Kompas::update,0.1);



}

void Kompas::update(void) {
    //wat variabeles om als zend en ontvang buffer te fungeren
    char compass_out[1];
    char compass_in[6];
    int i;

    //als het kompas niet bezig met calibreren kan er gelezen worden
    if (cal == 0) {

        //zet 0x50(="sample heading") in zend buffer
        compass_out[0] = 0x50;
        //stuur deze buffer over de bus
        compass.write(ADDR,compass_out,1);

        //geef het kompas even de tijd om de opdracht uit te voeren
        wait_ms(1);

        //Daarna kunnen er 6 8bits waardes uitgelezen worden
        compass.read(ADDR,compass_in,6);


        //Deze 6 8 bits waardes moeten omgerekend worden naar 3 16 bits waardes
        int compass_heading = (int)compass_in[0] << 8 | (int)compass_in[1];
        int compass_pitch = (int)compass_in[2] << 8 | (int)compass_in[3];
        int compass_roll = (int)compass_in[4] << 8 | (int)compass_in[5];
        int n = compass_heading / 10;


        int verschil = n - prev;
        if (verschil > 180)
            verschil -= 360;
        if (verschil < -180)
            verschil += 360;

        if (abs(verschil) > 60) {
            if (verschil > 0)
                prev += 20;
            else
                prev -= 20;
        } else {
        
        
            //n is ok, nu gemiddelde berekenen
            
            //buffer doorschuiven
            for (i = 0;i != 9; i++)
                opslag[i] = opslag[i+1];
                
            opslag[9] = n;
            
            //gemiddelde berekenen
            int som = 0;
            for (i = 0;i != 10; i++)
                som += opslag[i];
            
            koers = (som / 20);
            koers *= 2;
            
            
            

        }


    } else
        koers =  0;
}
int Kompas::get(void) {
    return koers;

}

void Kompas::simulate(int inc) {
    koers += inc;
    if (koers > 360)
        koers -= 360;
    if (koers < 0)
        koers += 360;
}

void Kompas::startcal(void) {
    char compass_out[1];
    if (cal == 0) {
        cal = 1;
        compass_out[0] = 0x71;
        compass.write(ADDR,compass_out,1);
    }
}

void Kompas::stopcal(void) {
    char compass_out[1];
    if (cal == 1) {
        compass_out[0] = 0x7E;
        compass.write(ADDR,compass_out,1);
        cal = 0;
    }
}