// Rozsireni o naklonomer s kompasem LSM303DLHC
//================================================
#ifdef modul_LSM303DLHC
int16_t ax, ay, az; // tihove zrychleni v kazde ose
int16_t ax_0, ay_0, az_0; // hodnoty z horizontalni kalibrace naklonu
int16_t ax_90, ay_90, az_90; // hodnoty z vertikalni kalibrace naklonu
int16_t prum_ax, prum_ay, prum_az ;
boolean kolisa_x = false;
boolean kolisa_y = false;
boolean kolisa_z = false;
int16_t mx , my , mz ; // magneticka sila v kazde ose
int16_t mag_x_min, mag_y_min, mag_z_min;
int16_t mag_x_max, mag_y_max, mag_z_max;
float posledni_uhel; // posledni zmereny uhel naklonu se zaznamenava jako desetinne cislo a pouzije se pro mereni azimutu
byte last_status; // pro kontrolu stavu I2C komunikace s modulem LSM303DLHC
//----------------------------------------------
// z naklonu a hodnot magnetometru zjisti azimut natoceni krabicky (0 az 359)
int zjisti_azimut(void)
{
nacti_3D_magnet();
mx = map( mx , mag_x_min , mag_x_max , -100 , 100); // prevod na +/- 100 pomoci rozsahu zjisteneho pri kalibraci
my = map( my , mag_y_min , mag_y_max , -100 , 100);
mz = map( mz , mag_z_min , mag_z_max , -100 , 100);
float vaha_my = cos(posledni_uhel * PI / 180); // pri horizontalni poloze se uprednostnuje vypocet s magnetometrem v ose Y
float vaha_mz = sin(posledni_uhel * PI / 180); // pri vertikalni poloze se uprednostnuje vypocet s magnetometrem v ose Z
// v mezipoloze mezi horizontalnim a vertikalnim naklonem se pouzivani obe osy v "COS"/"SIN" pomeru
float my_v = my * vaha_my;
float mz_v = mz * vaha_mz;
float azimut = atan2(mx,(my_v + mz_v));
azimut = (180 * (azimut + PI) / PI); // pootoceni azimutu (kvuli smeru ulozeni cidla) a prevod z radianu na stupne
azimut = azimut - offset_kompasu; // jemne doladeni spatne nalepeneho cidla
if (azimut > 359) azimut = azimut - 360;
if (azimut < 0) azimut = azimut + 360;
azimut = constrain(mapfloat(azimut , 0 , 360 , 360 , 0), 0 , 359) + 0.5; // pro jistotu jeste omezit na rozsah 0 az 359 stupnu a zaokrouhlit
return (int)azimut; // azimut nikdy nebude zaporny, tak staci oseknout desetinnou cast
}
//----------------------------------------------
//----------------------------------------------
// zjisteni uhlu naklonu vcetne filtrovani nestabilnich hodnot
uint16_t uhel(void)
{
long sumax = 0;
long sumay = 0;
long sumaz = 0;
int pole_prumeru_x[11];
int pole_prumeru_y[11];
int pole_prumeru_z[11];
int max_opakovani = 5; // pri kolisani nektereho vzorku v mereni se mohou provest jeste dalsi 4 overovaci mereni
//--------------------------------------
// filtr
boolean merit_x = true;
boolean merit_y = true;
boolean merit_z = true;
while (max_opakovani > 0)
{
max_opakovani --;
sumax = 0;
sumay = 0;
sumaz = 0;
for (byte i = 0; i < 4 ; i ++) // nacteni 4 mereni do poli
{
nacti_3D_zrychleni();
if (merit_x == true) pole_prumeru_x[i] = ax;
if (merit_y == true) pole_prumeru_y[i] = ay;
if (merit_z == true) pole_prumeru_z[i] = az;
delay(60);
}
for (byte i = 0; i < 4 ; i ++) // prumer 4 mereni naklonu
{
sumax = sumax + pole_prumeru_x[i];
sumay = sumay + pole_prumeru_y[i];
sumaz = sumaz + pole_prumeru_z[i];
}
if (merit_x == true) prum_ax = sumax / 4;
if (merit_x == true) prum_ay = sumay / 4;
if (merit_x == true) prum_az = sumaz / 4;
kolisa_x = false;
kolisa_y = false;
kolisa_z = false;
for (byte i = 0; i < 4 ; i ++) // vyhodnoceni extremnich odchylek jednotlivych mereni od prumeru
{
if (merit_x == true and pole_prumeru_x[i] - prum_ax > 200) kolisa_x = true;
if (merit_x == true and pole_prumeru_x[i] - prum_ax < -200) kolisa_x = true;
if (merit_y == true and pole_prumeru_y[i] - prum_ay > 200) kolisa_y = true;
if (merit_y == true and pole_prumeru_y[i] - prum_ay < -200) kolisa_y = true;
if (merit_z == true and pole_prumeru_z[i] - prum_az > 200) kolisa_z = true;
if (merit_z == true and pole_prumeru_z[i] - prum_az < -200) kolisa_z = true;
}
if (kolisa_x == false) merit_x = false;
if (kolisa_y == false) merit_y = false;
if (kolisa_z == false) merit_z = false;
if (kolisa_x == false and kolisa_y == false and kolisa_z == false)
{
break;
}
else
{
delay(50);
}
}
if (max_opakovani == 0 and (kolisa_x == true or kolisa_y == true or kolisa_z == true)) stabilni_naklon = false; // nestabilni mereni naklonu
else stabilni_naklon = true; // stabilni mereni naklonu
// konec filtru
// vystupem jsou zprumerovane hodnoty 'prum_ax', 'prum_ay', 'prum_az', pri kterych se 4 po sobe jdouci vzorky neodchylily od prumeru o vic nez 200 cisel
// pokud by doslo k vetsi odchylce, provede se jeste dalsi 1 az maximalne 4 pokusy.
// kdyz ani po 5 merenich neni hodnota stabilni, vrati se to, co je k dispozici
// Zaroven se vraci jeste znacka do globalni promenne 'stabilni_naklon', ktera je vyuzita ve vodovaze (svitici tecky pri nestabilnim naklonu)
//--------------------------------------
int aktx = prum_ax - ax_0;
int akty = prum_ay - ay_0;
int aktz = prum_az - az_90;
float uhel_z = atan2(aktz, sqrt(square(aktx) + square(akty)))/(PI/180);
uhel_z = mapfloat(uhel_z, 90, 0 , 0 , 90);
if (akty > 0) uhel_z = -uhel_z;
posledni_uhel = uhel_z; // pro testovani stability mereni
if (uhel_z < 0) uhel_z = (uhel_z + 999.95) * 10.0; // zaokrouhleni zaporneho cisla a prevod na kladne cislo (posun o 1000 stupnu)
else uhel_z = (uhel_z + 1000.05) * 10.0; // zaokrouhleni kladneho cisla a posun o 1000 stupnu
return uhel_z ;
}
//----------------------------------------------
//----------------------------------------------
// kalibrace magnetometru nalezenim minimalnich a maximalnich hodnot ve vsech osach
// pro serlinka = true se odesilaji hodnoty pres seriovou linku
// pro serlinka = false se kalibrace provadi pres displej
// v obou pripadech kalibrace konci v okamziku, kdy se po dobu 15 sekund nezmeni minimum a maximum z ani jedne osy
void kalibrace_magnet(boolean serlinka)
{
mag_x_min = 20000;
mag_y_min = 20000;
mag_z_min = 20000;
mag_x_max = -20000;
mag_y_max = -20000;
mag_z_max = -20000;
boolean nulujcasovac = true;
boolean predcasne_ukonceno = false;
unsigned long stabcas = millis();
unsigned int zbyva = 15;
unsigned int posledni_zobrazena_sekunda = 0;
while (millis() - stabcas < 10000) // 10 sekund se neobjevilo zadne nove minimum ani maximum
{
nacti_3D_magnet();
if (mag_x_min > mx)
{
mag_x_min = mx;
nulujcasovac = true;
}
if (mag_x_max < mx)
{
mag_x_max = mx;
nulujcasovac = true;
}
if (mag_y_min > my)
{
mag_y_min = my;
nulujcasovac = true;
}
if (mag_y_max < my)
{
mag_y_max = my;
nulujcasovac = true;
}
if (mag_z_min > mz)
{
mag_z_min = mz;
nulujcasovac = true;
}
if (mag_z_max < mz)
{
mag_z_max = mz;
nulujcasovac = true;
}
if (nulujcasovac == true)
{
stabcas = millis();
nulujcasovac = false;
if (serlinka == true)
{
Serial.print(mag_x_min);
Serial.print(" ; ");
Serial.print(mag_y_min);
Serial.print(" ; ");
Serial.print(mag_z_min);
Serial.print(" ; ");
Serial.print(mag_x_max);
Serial.print(" ; ");
Serial.print(mag_y_max);
Serial.print(" ; ");
Serial.println(mag_z_max);
}
}
else // dobiha 10 sek. casovac
{
zbyva = 10 - ((millis() - stabcas) / 1000);
if (zbyva != posledni_zobrazena_sekunda) // kazdou sekundu se aktualizuje informace o zbyvajicim case do konce kalibrace
{
posledni_zobrazena_sekunda = zbyva;
if (serlinka == true)
{
Serial.print('*');
Serial.println(zbyva);
}
zobraz_cislo(zbyva, 16);
}
}
if (digitalRead(pin_tl_ok) == LOW) // predcasne ukonceni kalibrace stiskem tlacitka [OK]
{
predcasne_ukonceno = true;
break;
}
delay(200);
} // dobehlo 10 sekund, behem kterych se nezaznamenal zadny extrem
if (serlinka == true)
{
Serial.print("mag_x_min: ");
Serial.print(mag_x_min);
Serial.print(" mag_x_max: ");
Serial.println(mag_x_max);
Serial.print("mag_y_min: ");
Serial.print(mag_y_min);
Serial.print(" mag_y_max: ");
Serial.println(mag_y_max);
Serial.print("mag_z_min: ");
Serial.print(mag_z_min);
Serial.print(" mag_z_max: ");
Serial.println(mag_z_max);
}
boolean chyba_kalibrace = false;
if (predcasne_ukonceno == true) chyba_kalibrace = true;
if (abs(mag_x_max - mag_x_min) < 100) chyba_kalibrace = true; // test, jestli je rozdil mezi minimem a maximem dostatecny
if (abs(mag_y_max - mag_y_min) < 100) chyba_kalibrace = true;
if (abs(mag_z_max - mag_z_min) < 100) chyba_kalibrace = true;
if (chyba_kalibrace == true)
{
chyba(9); // problem s kalibraci kompasu ("Err-C")
if (serlinka == true)
{
Serial.println(lng267); // "Chyba kalibrace kompasu"
}
kompas_setup(); // Pri chybe se nactou puvodni konstanty z EEPROM.
}
else // kalibrace kompasu probehla s prijatelnymi vysledky
{
zobraz_text(38); // "SAVE"
tone_X(pin_bzuk,4000, 10); // po uspesne kalibraci bzukne (pokud je pipani povoleno)
delay(300);
EEPROM_write_int(600,mag_x_min); // ulozeni kalibracnich magnetickych konstant do EEPROM
EEPROM_write_int(602,mag_x_max);
EEPROM_write_int(604,mag_y_min);
EEPROM_write_int(606,mag_y_max);
EEPROM_write_int(608,mag_z_min);
EEPROM_write_int(610,mag_z_max);
}
delay(100);
}
//----------------------------------------------
//----------------------------------------------
void naklon_setup(void)
{
naklon_write(CTRL_REG4_A, 0x08); // FS = 00 (+/- 2 g full scale); HR = 1 (high resolution enable)
naklon_write(CTRL_REG1_A, 0x47); // ODR = 0100 (50 Hz ODR); LPen = 0 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
ax_0 = (int16_t) EEPROM_read_int(612); // hodnoty naklonu ve vsech osach pro horizontalni polohu
ay_0 = (int16_t) EEPROM_read_int(614);
az_0 = (int16_t) EEPROM_read_int(616);
ax_90 = (int16_t) EEPROM_read_int(618); // hodnoty naklonu ve vsech osach pro vertikalni polohu
ay_90 = (int16_t) EEPROM_read_int(620);
az_90 = (int16_t) EEPROM_read_int(622);
}
//----------------------------------------------
//----------------------------------------------
void kompas_setup(void)
{
magnet_write(CRA_REG_M, 0x0C); // DO = 011 (7.5 Hz ODR)
magnet_write(CRB_REG_M, 0x20); // GN = 001 (+/- 1.3 gauss full scale)
magnet_write(MR_REG_M , 0x00); // MD = 00 (continuous-conversion mode)
mag_x_min = (int16_t) EEPROM_read_int(600); // z EEPROM se nactou minima a maxima z posledni kalibrace kompasu
mag_x_max = (int16_t) EEPROM_read_int(602);
mag_y_min = (int16_t) EEPROM_read_int(604);
mag_y_max = (int16_t) EEPROM_read_int(606);
mag_z_min = (int16_t) EEPROM_read_int(608);
mag_z_max = (int16_t) EEPROM_read_int(610);
}
//----------------------------------------------
//----------------------------------------------
void default_naklonomer(void)
{
EEPROM_write_int(612, -3216); // prvni nastrel nejakych realnych hodnot. Urcite bude ale nutne provest uvodni kalibraci naklonu.
EEPROM_write_int(614, 638);
EEPROM_write_int(616, 18627);
EEPROM_write_int(618, -4073);
EEPROM_write_int(620, -16203);
EEPROM_write_int(622, 1311);
ax_0 = (int16_t) EEPROM_read_int(612); // hodnoty naklonu ve vsech osach pro horizontalni polohu
ay_0 = (int16_t) EEPROM_read_int(614);
az_0 = (int16_t) EEPROM_read_int(616);
ax_90 = (int16_t) EEPROM_read_int(618); // hodnoty naklonu ve vsech osach pro vertikalni polohu
ay_90 = (int16_t) EEPROM_read_int(620);
az_90 = (int16_t) EEPROM_read_int(622);
}
//----------------------------------------------
//----------------------------------------------
void default_kompas(void)
{
EEPROM_write_int(600,-20000); // prednastavena minima a maxima pro vsechny 3 osy magmetometru. Urcite se ale bude muset provest uvodni kalibrace
EEPROM_write_int(602, 20000);
EEPROM_write_int(604,-20000);
EEPROM_write_int(606, 20000);
EEPROM_write_int(608,-20000);
EEPROM_write_int(610, 20000);
mag_x_min = (int16_t) EEPROM_read_int(600); // z EEPROM se nactou minima a maxima, ktera tam byla ulozena v predchozich radkach
mag_x_max = (int16_t) EEPROM_read_int(602);
mag_y_min = (int16_t) EEPROM_read_int(604);
mag_y_max = (int16_t) EEPROM_read_int(606);
mag_z_min = (int16_t) EEPROM_read_int(608);
mag_z_max = (int16_t) EEPROM_read_int(610);
}
//----------------------------------------------
//----------------------------------------------
// Servisni informace o aktualnich offsetech a krajnich kalibracnich bodech
// Pri parametru 'true' vypise jeste 10x obsah vsech akcelaracnich registru a jejich prepocet na uhel.
void naklonomer_info(boolean test)
{
Serial.print(" aX0:");
Serial.print(ax_0);
Serial.print(" aY0:");
Serial.print(ay_0);
Serial.print(" aZ0:");
Serial.println(az_0);
Serial.print(" aX90:");
Serial.print(ax_90);
Serial.print(" aY90:");
Serial.print(ay_90);
Serial.print(" aZ90:");
Serial.println(az_90);
Serial.print("mag_x_min:");
Serial.print(mag_x_min);
Serial.print(" mag_x_max:");
Serial.println(mag_x_max);
Serial.print("mag_y_min:");
Serial.print(mag_y_min);
Serial.print(" mag_y_max:");
Serial.println(mag_y_max);
Serial.print("mag_z_min:");
Serial.print(mag_z_min);
Serial.print(" mag_z_max:");
Serial.println(mag_z_max);
if (test == true)
{
for (byte i = 0; i < 10 ; i++)
{
nacti_3D_zrychleni();
Serial.print("AX: ");
Serial.print(ax);
Serial.print(" AY: ");
Serial.print(ay);
Serial.print(" AZ: ");
Serial.println(az);
}
}
}
//----------------------------------------------
//----------------------------------------------
// Automaticka kalibrace naklonomeru
// Probiha ve dvou krocich
// - zjisteni zrychleni vsech 3 os akcelerometru pri horizontalni poloze krabicky
// - zjisteni zrychleni vsech 3 os akcelerometru pri vertikalni poloze krabicky
// Vsech 6 16-bitovych hodnot se ulozi do EEPROM na adresy 612 az 623
// Pri zapnuti napajeni jsou hodnoty z teto pameti vybrany a pouzity pro vypocty.
void kalibrace_naklon(void)
{
zobraz_text(47); // "n-CAL" (kalibrace naklonomeru)
Serial.println(lng232); // "Poloz SQM horizontalne"
delay(2000);
zobraz_text(64); // "Hori" (horizontalni kalibrace naklonomeru)
while(Serial.available()) Serial.read();
Serial.println(">>");
while (Serial.available() == 0 and digitalRead(pin_tl_ok) == HIGH) delay(100); // ceka na odklepnuti horizontove polohy, nebo stisk OK
long sumax = 0;
long sumay = 0;
long sumaz = 0;
for (byte i = 0; i < 10 ; i ++)
{
nacti_3D_zrychleni();
sumax = sumax + ax;
sumay = sumay + ay;
sumaz = sumaz + az;
delay(100);
}
ax_0 = sumax / 10;
ay_0 = sumay / 10;
az_0 = sumaz / 10;
Serial.println(lng233); // "Postav SQM svisle"
zobraz_text(65); // "Vert" (vertikalni kalibrace naklonomeru)
delay(2000);
Serial.println(">>");
while(Serial.available()) Serial.read();
while (Serial.available() == 0 and digitalRead(pin_tl_ok) == HIGH) delay(100); // ceka na odklepnuti horizontove polohy, nebo stisk OK
while(Serial.available()) Serial.read() ;
sumax = 0;
sumay = 0;
sumaz = 0;
for (byte i = 0; i < 10 ; i ++)
{
nacti_3D_zrychleni();
sumax = sumax + ax;
sumay = sumay + ay;
sumaz = sumaz + az;
delay(100);
}
ax_90 = sumax / 10;
ay_90 = sumay / 10;
az_90 = sumaz / 10;
boolean chyba_kalibrace = false;
if (abs(ax_90 - ax_0) > 7000) chyba_kalibrace = true; // tato osa by se mela pri naklonu menit malo
if (abs(ay_90 - ay_0) < 10000) chyba_kalibrace = true; // tato osa by se mela pri naklonu menit hodne
if (abs(az_90 - az_0) < 10000) chyba_kalibrace = true; // tato osa by se mela pri naklonu menit hodne
if (chyba_kalibrace == true) // cidlo naklonu je spatne umistene
{
naklonomer_info(true);
Serial.println(lng234); // "Chyba kalibrace naklonu - nastavuji defaultni hodnoty"
delay(2000);
while(Serial.available()) Serial.read();
Serial.println(">>");
while (Serial.available() == 0) delay(100); // ceka na odklepnuti
while(Serial.available()) Serial.read();
default_naklonomer(); // nastaveni defaultnich hodnot
}
else // cilo naklonu je umistene dobre
{
zobraz_text(38); // "SAVE"
delay(400);
EEPROM_write_int(612, ax_0); // zprumerovane hodnoty se ulozi do EEPROM
EEPROM_write_int(614, ay_0);
EEPROM_write_int(616, az_0);
EEPROM_write_int(618, ax_90);
EEPROM_write_int(620, ay_90);
EEPROM_write_int(622, az_90);
zobraz_text(28); // " " (displej zhasne)
Serial.println("\r\nOK\r\n");
}
}
//----------------------------------------------
//----------------------------------------------
// dvouosa vodovaha v horizontalni pozici
void vodovaha_hor(void)
{
byte maskaX[] = {0,0,0,0,0};
byte maskaY[] = {0,0,0,0,0};
uhel();
int pasmo_vyrovnani = 80; // v jak sirokem pasmu hodnot zrychleni se bude hlasit vyrovnany stav
if ((prum_ax - ax_0) > pasmo_vyrovnani) maskaX[3] = 48; // nakloneno moc vpravo
if ((prum_ax - ax_0) < - pasmo_vyrovnani) maskaX[1] = 6; // nakloneno moc vlevo
if (((prum_ax - ax_0) >= - pasmo_vyrovnani) and ((prum_ax - ax_0) <= pasmo_vyrovnani)) // vyrovnano v ose vlevo / vpravo
{
maskaX[3] = 48;
maskaX[1] = 6;
}
if ((prum_ay - ay_0) > pasmo_vyrovnani) // nakloneno moc dopredu
{
maskaY[3] = 8;
maskaY[2] = 8;
maskaY[1] = 8;
}
if ((prum_ay - ay_0) < - pasmo_vyrovnani) // nakloneno moc dozadu
{
maskaY[3] = 1;
maskaY[2] = 1;
maskaY[1] = 1;
}
if (((prum_ay - ay_0) >= - pasmo_vyrovnani) and ((prum_ay - ay_0) <= pasmo_vyrovnani)) // vyrovnano v ose dopredu / dozadu
{
maskaY[3] = 9;
maskaY[2] = 9;
maskaY[1] = 9;
}
for (i = 0; i < 5 ; i++)
{
D_pamet[i] = maskaX[i] | maskaY[i] ;
}
if (stabilni_naklon == false)
{
D_pamet[4] = D_pamet[4] | 128 ; // nestabilni naklon rozsviti tecku na vsech zobrazovacich jednotkach
D_pamet[3] = D_pamet[3] | 128 ;
D_pamet[2] = D_pamet[2] | 128 ;
D_pamet[1] = D_pamet[1] | 128 ;
D_pamet[0] = D_pamet[0] | 128 ;
}
aktualizuj_displej();
delay(20);
}
//----------------------------------------------
//----------------------------------------------
// dvouosa vodovaha v zenitove pozici
void vodovaha_zen(void)
{
byte maskaX[] = {0,0,0,0,0};
byte maskaZ[] = {0,0,0,0,0};
int pasmo_vyrovnani = 80; // v jak sirokem pasmu hodnot zrychleni se bude hlasit vyrovnany stav
uhel();
if (prum_ax > ax_90 + pasmo_vyrovnani) maskaX[4] = 33; // nakloneno moc vpravo
if (prum_ax < ax_90 - pasmo_vyrovnani) maskaX[0] = 3; // nakloneno moc vlevo
if (prum_ax >= ax_90 - pasmo_vyrovnani and prum_ax <= ax_90 + pasmo_vyrovnani) // osa vlevo / vpravo vyrovnana
{
maskaX[4] = 33;
maskaX[0] = 3;
}
if (prum_az > az_90 - pasmo_vyrovnani) maskaZ[2] = 28; // nakloneno moc dozadu (k sobe)(znak 'u')
if (prum_az < az_90 + pasmo_vyrovnani) maskaZ[2] = 35; // nakloneno moc dopredu (od sebe) (znak '^')
if (prum_az >= az_90 - pasmo_vyrovnani and prum_az <= az_90 + pasmo_vyrovnani) maskaZ[2] = 63; // osa dopredu / dozadu vyrovnana
for (i = 0; i < 5 ; i++)
{
D_pamet[i] = maskaX[i] | maskaZ[i] ;
}
if (stabilni_naklon == false)
{
D_pamet[4] = D_pamet[4] | 128 ; // nestabilni naklon rozsviti tecku na vsech zobrazovacich jednotkach
D_pamet[3] = D_pamet[3] | 128 ;
D_pamet[2] = D_pamet[2] | 128 ;
D_pamet[1] = D_pamet[1] | 128 ;
D_pamet[0] = D_pamet[0] | 128 ;
}
aktualizuj_displej();
delay(20);
}
//----------------------------------------------
//----------------------------------------------
// zapis jednoho bajtu do naklonomeru
void naklon_write(byte registr, byte data)
{
Wire.beginTransmission(I2C_ADDR_NAKLON);
Wire.write(registr);
Wire.write(data);
last_status = Wire.endTransmission();
}
//----------------------------------------------
//----------------------------------------------
// zapis jednoho bajtu do magnetometru
void magnet_write(byte registr, byte data)
{
Wire.beginTransmission(I2C_ADDR_KOMPAS);
Wire.write(registr);
Wire.write(data);
last_status = Wire.endTransmission();
}
//----------------------------------------------
//----------------------------------------------
// precte z naklonomeru hodnotu zrychleni ve vsech osach a ulozi je do globalnich promennych ax, ay, az
void nacti_3D_zrychleni(void)
{
Wire.beginTransmission(I2C_ADDR_NAKLON);
// assert the MSB of the address to get the accelerometer
// to do slave-transmit subaddress updating.
Wire.write(OUT_X_L_A | (1 << 7));
last_status = Wire.endTransmission();
Wire.requestFrom(I2C_ADDR_NAKLON, (byte)6);
delay(5);
byte xla = Wire.read();
byte xha = Wire.read();
byte yla = Wire.read();
byte yha = Wire.read();
byte zla = Wire.read();
byte zha = Wire.read();
ax = (int16_t)(xha << 8 | xla);
ay = (int16_t)(yha << 8 | yla);
az = (int16_t)(zha << 8 | zla);
}
//----------------------------------------------
//----------------------------------------------
// precte z magletometru hodnotu magnetickeho pole ve vsech osach a ulozi je do globalnich promennych mx, my, mz
void nacti_3D_magnet(void)
{
Wire.beginTransmission(I2C_ADDR_KOMPAS);
// assert the MSB of the address to get the accelerometer
// to do slave-transmit subaddress updating.
Wire.write(DLHC_OUT_X_H_M | (1 << 7));
last_status = Wire.endTransmission();
Wire.requestFrom(I2C_ADDR_KOMPAS, (byte)6);
byte xhm = Wire.read();
byte xlm = Wire.read();
byte zhm = Wire.read();
byte zlm = Wire.read();
byte yhm = Wire.read();
byte ylm = Wire.read();
mx = (int16_t)(xhm << 8 | xlm);
my = (int16_t)(yhm << 8 | ylm);
mz = (int16_t)(zhm << 8 | zlm);
}
//----------------------------------------------
//----------------------------------------------
// druha mocnina pro velka cisla
int32_t square(int16_t vstup)
{
return 1UL * vstup * vstup;
}
//----------------------------------------------
//----------------------------------------------
// nahrada funkce map() pro desetinna cisla
float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
//----------------------------------------------
//----------------------------------------------
// jen testovaci rychle (a nepresne) zjisteni naklonu
uint16_t rychly_naklon(void)
{
nacti_3D_zrychleni();
int aktx = ax - ax_0;
int akty = ay - ay_0;
int aktz = az - az_90;
float uhel_z = atan2(aktz, sqrt(square(aktx) + square(akty)))/(PI/180);
uhel_z = mapfloat(uhel_z, 90, 0 , 0 , 90);
if (akty > 0) uhel_z = -uhel_z;
posledni_uhel = uhel_z;
uhel_z = (uhel_z + 0.05 + 1000.0) * 10.0; // prevod na kladne cislo (posun o 1000 stupnu), zaokrouhleni na 1 desetinu a prevod na cele cislo
return uhel_z ;
}
//----------------------------------------------
#endif