a
SANSORS.cpp
- Committer:
- pfe
- Date:
- 2015-04-21
- Revision:
- 0:c308751fd941
File content as of revision 0:c308751fd941:
#include "SANSORS.h" Sansors::Sansors(PinName sda, PinName scl){ i2c_ = new I2C(sda, scl); //100KHz, as specified by the datasheet. i2c_->frequency(100000); } void Sansors::CAP_ALT_TAS_Init(){ //i2c.frequency (100000) ;//void frequency(int hz); //------------ init Compass HMC //Continuous mode, periodic set/reset, 20Hz measurement rate. i2c_Data[0] = HMC6352_CONTINUOUS; i2c_Data[1] = 0x01; i2c_Data[2] = 20; addr=AdSensorALT; i2c_->write(addr, i2c_Data, 3); // Send command //------------- Get Alt Init addr=AdSensorALT; i2c_Data[0] = 0x07; // Send "read data" command to sensor i2c_->write(addr, i2c_Data, 1); // Send command string wait_ms(20); // attendre 20mS avant de relancer i2c_->read(addr, i2c_Data, 2); OldALT = (((int)i2c_Data[0] << 8) | (int)i2c_Data[1]);//*((unsigned short*)i2c_Data)&0xffff; //altitude en décimètre OldALT = OldALT - 3000; //---------------------------- wait_ms(1); //attendre 1mS avant de relancer //------------- Get Offset TAS addr=AdSensorTAS; i2c_Data[0] = 0x01; // Send "read data" command to sensor i2c_->write(addr, i2c_Data, 1); // Send command string wait_us(400);//attendre 400µS avant de relancer i2c_->read(addr, i2c_Data, 2); offset_TAS=(0xff00 & (((unsigned char)i2c_Data[1]) << 8)) | ((unsigned char)i2c_Data[0]&0xff); } signed short Sansors::GetCAP(){ unsigned int CAP10; char Error; Error = 0; addr = AdSensorCAP; i2c_Data[0] = 0x41; // Send "read data" command to sensor Error=i2c_->write(addr, i2c_Data, 1); // Send command string wait_ms(1); //attendre 20mS avant de relancer Error=Error+i2c_->read(addr, i2c_Data, 2); CAP10 = (((int)i2c_Data[0] << 8) | (int)i2c_Data[1]); if (Error==0) return CAP10; return -1; } signed short Sansors::GetALT(){ int ALT; char Error; Error = 0; addr=AdSensorALT; i2c_Data[0] = 0x07; // Send "read data" command to sensor Error=i2c_->write(addr, i2c_Data, 1); // Send command string wait_ms(1); //attendre 20mS avant de relancer Error=Error+i2c_->read(addr, i2c_Data, 2); ALT = (((int)i2c_Data[0] << 8) | (int)i2c_Data[1]);//*((unsigned short*)i2c_Data)&0xffff; //altitude en décimètre ALT = ALT-3000; //ALT = (ALT+OldALT)/2; // calcul moyenne pour réduire le bruit //OldALT = ALT; if (Error==0) return ALT;//*3.28084; return -1; } signed int Sansors::GetTAS(){ float TAS_f; unsigned int TASbrut; unsigned int TAS10; char Error; Error = 0; addr=AdSensorTAS; i2c_Data[0] = 0x01; // Send "read data" command to sensor Error = i2c_->write(addr, i2c_Data, 1); // Send command string wait_us(400); //attendre 100µS avant de relancer Error = Error + i2c_->read(addr, i2c_Data, 2); TASbrut = (0xff00 & ( ((unsigned)i2c_Data[1]) << 8)) | ((unsigned char)i2c_Data[0]&0xff); //vitesse = sqrt((float)(vbrut-offset_v)*2*3.6/1.225); TAS_f = sqrt((float)(TASbrut-offset_TAS)*5.87755102); TAS10 = TAS_f*10; if (TAS10>4000)TAS10 = 0; if (Error==0) return TAS10; return -1; }