![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
invoer/kompas.cpp
- Committer:
- pd0wm
- Date:
- 2011-09-27
- Revision:
- 0:bec310bde899
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; } }