První sada nově testovaných senzorů se dá zakoupit jako vývojový kit od firmy Sparkfun Electronics. Modul byl zakoupen již dříve a dnes se v nabídce obchodu už nenachází. Uvádím zde tedy alespoň odkaz na novější verzi tohoto modulu (9 Degrees of Freedom). Liší se pouze mírně modifikovanou verzí magnetometru. Modul se připojí ke stávající inerciální jednotce pomocí sběrnice I2C. Přímo na desce modulu jsou dokonce již osazeny i pull-up rezistory.
 |
| Novější varianta Sparkfun 9DOF modulu Zdroj: http://www.sparkfun.com |
Komunikace po sběrnici I2C
Implementace knihovny pro komunikaci po sběrnici je silně závislá na použitém mikrokontroléru. Já si napsal vlastní pro mikrokontrolér LPC2368. Její kód je příliš dlouhý na to, abych ho zde vypisoval, proto uvedu jen deklarace funkcí s velmi stručným popisem.
/*!
\brief I2C single byte read routine
\details
\param devaddr 8 bit device address
\param regaddr 8 bit register address
\param data Pointer to address where readed byte will be stored
\return Returns 0 in case of error in comunication with sensor, 1 otherwise
*/
BOOL i21c_read(uint32_t devaddr, uint32_t regaddr, uint8_t * data);
BOOL i21c_read_seq(uint32_t devaddr, uint32_t regaddr, volatile uint8_t * data, uint32_t size);/*!
\brief I2C single byte write routine
\details
\param devaddr 8 bit device address
\param regaddr 8 bit register address
\param data Byte to write
\return Returns 0 in case of error in comunication with sensor, 1 otherwise
*/
BOOL i21c_write(uint32_t devaddr, uint32_t regaddr, uint8_t data);
BOOL i21c_write_seq(uint32_t devaddr, uint32_t regaddr, uint8_t * data, uint32_t size);
Akcelerometr Analog Devices ADXL345
Senzor ADXL345 je tříosý akcelerometr s až 13 bitovým rozlišením a nastavitelným rozsahem až do ±16 g (nastavitelný rozsah je pouze pro 10 bitové rozlišení) [1]. Při maximálním 13 bitovém rozlišení má senzor citlivost 3.9 mg/LSB (LSB - least significant bit). Senzor komunikuje po sběrnici I2C a to buď rychlostí 100 kHz nebo 400 kHz a má adresu 0xA6. Poslední bit této adresy je určen logickou úrovní na pinu SDO/ALT ADDRESS (12). Možnosti využití senzoru jsou značně omezené díky tomu, že je z něj na modulu vyvedeno pouze rozhraní I2C a další piny jsou nedostupné. Mezi tyto nedostupné piny patří například rozhraní SPI či piny přerušení, které mimo jiné indikující volný pád nebo jednoduché či dvojité poklepání na senzor (viz datasheet).
Při inicializaci je nutné pouze nastavit Measure Bit v registru POWER_CTL pro probuzení senzoru ze standby módu do měřícího kontinuálního módu. Dále můžeme nastavit frekvenci s jakou bude senzor měřit a rozlišení.
#define ADXL345_DEV_ADDR (0xA6)
#define ADXL345_DATA_ADDR (0x32)
#define ADXL345_REGISTER_PWRCTL (0x2D)
#define ADXL345_PWRCTL_MEASURE (1 << 3)
#define ADXL345_REGISTER_BWRATE (0x2C)
#define ADXL345_REGISTER_DATAFORMAT (0x31)
#define ADXL345_FULL_RES_BIT (1 << 3)
typedef enum {
adxl345_datarate_f100Hz = 0x0A,
adxl345_datarate_f200Hz = 0x0B,
adxl345_datarate_f400Hz = 0x0C,
adxl345_datarate_f800Hz = 0x0D
} adxl345_data_rate;
typedef enum {
adxl345_resolution_2g = 0x00,
adxl345_resolution_4g = 0x01,
adxl345_resolution_8g = 0x02,
adxl345_resolution_16g = 0x03,
adxl345_resolution_full = 0xFF
} adxl345_resolution;
/*! \brief Initialization of sensor
\details
\param freq Data rate of sensing
\param res Resolution
\return Returns 0 in case of error in comunication with sensor, 1 otherwise
*/
BOOL init_adxl345(adxl345_data_rate freq, adxl345_resolution res) {
BOOL result;
//Set measure bit in PWR_CTL register to start measuring
result = i21c_write(ADXL345_DEV_ADDR, ADXL345_REGISTER_PWRCTL, ADXL345_PWRCTL_MEASURE);
//Set date rate to 200 Hz
result &= i21c_write(ADXL345_DEV_ADDR, ADXL345_REGISTER_BWRATE, freq);
//Set resolution
if(res == 0xFF)
// +-16 g resolution, 13 bit, 4 mg/LSB
result &= i21c_write(ADXL345_DEV_ADDR, ADXL345_REGISTER_DATAFORMAT, ADXL345_FULL_RES_BIT);
else
result &= i21c_write(ADXL345_DEV_ADDR, ADXL345_REGISTER_DATAFORMAT, res);
return result;
}
Čtení naměřených dat ze senzoru se provádí čtením šesti po sobě následující registrů, ve kterých je pro jednotlivé naměřené hodnoty pro dané osy uloženo vždy nejprve spodních 8 bitů a následně zbývající horní bity. Následující ukázka funkce pro čtení naměřených dat ze senzoru je jistě názornější než slovní popis.
/*! \brief Reading of sensor data
\details Reads acceleration in all three axes os sensor and rotate them to coordinate system of inertial unit
\param accelerometer_data[] Array where will be stored readed data (readed direct from register of sensor)
\return Returns 0 in case of error in comunication with sensor, 1 otherwise
*/
BOOL read_data_adxl345(int16_t accelerometer_data[]) {
BOOL result;
uint8_t readed_data[6];
result = i21c_read_seq(ADXL345_DEV_ADDR, ADXL345_DATA_ADDR, readed_data, 6);
accelerometer_data[0] = (int16_t)((readed_data[1]<<8) | readed_data[0]); // x
accelerometer_data[1] = (int16_t)((readed_data[3]<<8) | readed_data[2]); // y
accelerometer_data[2] = (int16_t)((readed_data[5]<<8) | readed_data[4]); // z
return result;
}
Elektronický gyroskop InvenSense ITG-3200
Elektronický gyroskop ITG-3200 je 16 bitový tříosý mikromechanický (MEMS) senzor úhlové rychlosti [2]. Rozlišení senzoru je neměnné a nabývá hodnoty ±2000 °/s při citlivosti 14.375 LSB/(°/s). Senzor má v sobě integrován digitální dolnopropustní filtr a teploměr. Podobně jako u akcelerometru senzor umožňuje i komunikaci po SPI, ale ne v případě tohoto modulu. Senzor opět umožňuje komunikaci po sběrnici I2C rychlostí 100 kHz, případně 400 kHz a má 8 bitovou adresu 0xD0. Připojením pinu AD0 na jinou logickou úroveň může být změněn LSB této adresy.
Inicializace senzoru spočívá pouze v nastavení dolnopropustního filtru a vzorkovací frekvence nebo přesněji nastavení děličky základní frekvence od níž je ta vzorkovací odvozena. Základní frekvence též závisí na nastavení dolnopropustního filtru. Pokud je dolnopropustní filtr nastaven na 256 Hz, je základní frekvence 8 kHz. Pro jiné hodnoty dolnopropustního filtru nabývá vždy základní frekvence hodnoty 1 kHz. Senzor funguje v kontinuálním měřícím režimu.
#define ITG3200_DEV_ADDR (0xD0)
#define ITG3200_TEMP_ADDR (0x1B)
#define ITG3200_DATA_ADDR (0x1D)
#define ITG3200_REGISTER_DLPF_FS (0x16)
#define ITG3200_FULLSCALE (0x03 << 3)
#define ITG3200_REGISTER_SAMPLE_RATE_DIVIDER (0x15)
typedef enum {
itg3200_low_pass_f256Hz,
itg3200_low_pass_f188Hz,
itg3200_low_pass_f98Hz,
itg3200_low_pass_f42Hz,
itg3200_low_pass_f20Hz,
itg3200_low_pass_f10Hz,
itg3200_low_pass_f5Hz
} itg3200_low_pass_filter;
/*!
\brief Initialize gyro sensor
\details Sets low pass filter, frequence of sampling
\param lp_freq Frequency of low pass filter
\param divider Divide base sampling freqency. Base sample frequency is 8 kHz for 256 Hz lowpass filter and 1 kHz for others.
\return Returns 0 in case of error in comunication with sensor, 1 otherwise
*/
BOOL init_itg3200(itg3200_low_pass_filter lp_freq, DWORD divider) {
BOOL result;
// set divider of base sample frequency
result = i21c_write(ITG3200_DEV_ADDR, ITG3200_REGISTER_SAMPLE_RATE_DIVIDER, divider);
// set low pass filter and full scale resolution of 2000°/s (the only possible)
result &= i21c_write(ITG3200_DEV_ADDR, ITG3200_REGISTER_DLPF_FS, ITG3200_FULLSCALE | lp_freq);
return result;
}
Čtení je podobné jako v předchozím případě s jedním drobným rozdílem. Vzorkovací frekvence je odvozena od vnitřního oscilátoru, který má přesnost 2 %, což není mnoho. Pokud tedy nastavíme vzorkovací frekvenci na 200 Hz a čteme data ze senzoru také s frekvencí 200 Hz, stává se, že senzor ještě nemá nová data připravena a my načteme stejná data jako v minulé iteraci. To je důvod, proč je ve funkci pro čtení zahrnuta i čekací smyčka, která kontroluje příznak připravenosti nových dat v registrech senzoru.
/*! \brief Reading of sensor data
* \details Reads measured data from sensor in all three axes of sensor and rotate them to coordinate system of inertial unit
*
* \param gyro_data[] Array where will be stored readed data (readed direct from register of sensor)
* \return Returns 0 in case of error in comunication with sensor, 1 otherwise
*/
BOOL read_data_itg3200(int16_t gyro_data[]) {
BOOL result = 1;
uint8_t readed_data[6];
uint8_t new_data_available;
// wait until new data is ready
do {
result &= i21c_read(ITG3200_DEV_ADDR, 26, &new_data_available);
} while(new_data_available & 1);
result &= i21c_read_seq(ITG3200_DEV_ADDR, ITG3200_DATA_ADDR, readed_data, 6);
gyro_data[0] = -(int16_t)((readed_data[0]<<8) | readed_data[1]); //x
gyro_data[1] = (int16_t)((readed_data[2]<<8) | readed_data[3]); //y
gyro_data[2] = (int16_t)((readed_data[4]<<8) | readed_data[5]); //z
return result;
}
Magnetometr Honeywell HMC5843
Magnetometr HMC5843 je tříosý 12 bitový senzor magnetického pole (magnetické indukce) [3]. Zjednodušeně se dá říci, že převádí změnu rezistivity anizotropních magnetorezistorů na změnu magnetické indukce (anizotropní magnetorezistory mají závislou rezistivitu na magnetické indukci). Jako ostatní senzory osazené na modulu komunikuje po sběrnici I2C a má pevně danou adresu 0x3D. Opět podporuje rychlost komunikace 100 kHz i 400 kHz. Maximální rozlišení senzoru je ±4 gauss (±400 μT) a maximální vzorkovací frekvence je 50 Hz.
Inicializace senzoru spočívá nastavení vzorkovací frekvence a kontinuálního měřícího módu. Senzor je totiž ve výchozím stavu nastaven na pouhé jedno měření po kterém přejde do sleep módu.
#define HMC5843_DEV_ADDR (0x3C)
#define HMC5843_DATA_ADDR (0x03)
#define HMC5843_REGISTER_CTRLA (0x00)
#define HMC5843_REGISTER_MEASMODE (0x02)
#define HMC5843_MEASMODE_CONT (0x00)
typedef enum {
hmc5843_datarate_05Hz,
hmc5843_datarate_1Hz,
hmc5843_datarate_2Hz,
hmc5843_datarate_5Hz,
hmc5843_datarate_10Hz,
hmc5843_datarate_20Hz,
hmc5843_datarate_50Hz
} hmc5843_datarate;
/*! \brief Initialization of sensor
\details
\param data_rate Sampling rate
\return Returns 0 in case of error in comunication with sensor, 1 otherwise
*/
BOOL init_hmc5843(hmc5843_datarate data_rate) {
BOOL result;
//set up data rate
result = i21c_write(HMC5843_DEV_ADDR, HMC5843_REGISTER_CTRLA, (data_rate << 2));
//set up continuous measurement mode
result &= i21c_write(HMC5843_DEV_ADDR, HMC5843_REGISTER_MEASMODE, HMC5843_MEASMODE_CONT);
return result;
}
Čtení dat ze senzoru je opět skoro totožné s těmi předchozími./*! \brief Reading of sensor data
* \details Reads measured data from sensor in all three axes of sensor and rotate them to coordinate system of inertial unit
*
* \param magnetometer_data[] Array where will be stored readed data (readed direct from register of sensor)
* \return Returns 0 in case of error in comunication with sensor, 1 otherwise
*/
BOOL read_data_hmc5843(int16_t magnetometer_data[]) {
BOOL result;
uint8_t readed_data[6];
result = i21c_read_seq(HMC5843_DEV_ADDR, HMC5843_DATA_ADDR, readed_data, 6);
magnetometer_data[0] = (int16_t)((readed_data[0]<<8) | readed_data[1]); //x
magnetometer_data[1] = (int16_t)((readed_data[2]<<8) | readed_data[3]); //y
magnetometer_data[2] = -(int16_t)((readed_data[4]<<8) | readed_data[5]); //z
return result;
}
Zdroje
[1] Datasheet (ADXL345)[2] Datasheet (ITG-3200)
[3] Datasheet (HMC5883L)
Žádné komentáře:
Okomentovat