a
SANSORS.cpp@0:c308751fd941, 2015-04-21 (annotated)
- Committer:
- pfe
- Date:
- Tue Apr 21 10:22:08 2015 +0000
- Revision:
- 0:c308751fd941
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pfe | 0:c308751fd941 | 1 | #include "SANSORS.h" |
pfe | 0:c308751fd941 | 2 | Sansors::Sansors(PinName sda, PinName scl){ |
pfe | 0:c308751fd941 | 3 | i2c_ = new I2C(sda, scl); |
pfe | 0:c308751fd941 | 4 | //100KHz, as specified by the datasheet. |
pfe | 0:c308751fd941 | 5 | i2c_->frequency(100000); |
pfe | 0:c308751fd941 | 6 | } |
pfe | 0:c308751fd941 | 7 | void Sansors::CAP_ALT_TAS_Init(){ |
pfe | 0:c308751fd941 | 8 | //i2c.frequency (100000) ;//void frequency(int hz); |
pfe | 0:c308751fd941 | 9 | //------------ init Compass HMC //Continuous mode, periodic set/reset, 20Hz measurement rate. |
pfe | 0:c308751fd941 | 10 | i2c_Data[0] = HMC6352_CONTINUOUS; |
pfe | 0:c308751fd941 | 11 | i2c_Data[1] = 0x01; |
pfe | 0:c308751fd941 | 12 | i2c_Data[2] = 20; |
pfe | 0:c308751fd941 | 13 | addr=AdSensorALT; |
pfe | 0:c308751fd941 | 14 | i2c_->write(addr, i2c_Data, 3); // Send command |
pfe | 0:c308751fd941 | 15 | //------------- Get Alt Init |
pfe | 0:c308751fd941 | 16 | addr=AdSensorALT; |
pfe | 0:c308751fd941 | 17 | i2c_Data[0] = 0x07; // Send "read data" command to sensor |
pfe | 0:c308751fd941 | 18 | i2c_->write(addr, i2c_Data, 1); // Send command string |
pfe | 0:c308751fd941 | 19 | wait_ms(20); // attendre 20mS avant de relancer |
pfe | 0:c308751fd941 | 20 | i2c_->read(addr, i2c_Data, 2); |
pfe | 0:c308751fd941 | 21 | OldALT = (((int)i2c_Data[0] << 8) | (int)i2c_Data[1]);//*((unsigned short*)i2c_Data)&0xffff; //altitude en décimètre |
pfe | 0:c308751fd941 | 22 | OldALT = OldALT - 3000; |
pfe | 0:c308751fd941 | 23 | //---------------------------- |
pfe | 0:c308751fd941 | 24 | wait_ms(1); //attendre 1mS avant de relancer |
pfe | 0:c308751fd941 | 25 | //------------- Get Offset TAS |
pfe | 0:c308751fd941 | 26 | addr=AdSensorTAS; |
pfe | 0:c308751fd941 | 27 | i2c_Data[0] = 0x01; // Send "read data" command to sensor |
pfe | 0:c308751fd941 | 28 | i2c_->write(addr, i2c_Data, 1); // Send command string |
pfe | 0:c308751fd941 | 29 | wait_us(400);//attendre 400µS avant de relancer |
pfe | 0:c308751fd941 | 30 | i2c_->read(addr, i2c_Data, 2); |
pfe | 0:c308751fd941 | 31 | offset_TAS=(0xff00 & (((unsigned char)i2c_Data[1]) << 8)) | ((unsigned char)i2c_Data[0]&0xff); |
pfe | 0:c308751fd941 | 32 | } |
pfe | 0:c308751fd941 | 33 | signed short Sansors::GetCAP(){ |
pfe | 0:c308751fd941 | 34 | unsigned int CAP10; |
pfe | 0:c308751fd941 | 35 | char Error; |
pfe | 0:c308751fd941 | 36 | Error = 0; |
pfe | 0:c308751fd941 | 37 | addr = AdSensorCAP; |
pfe | 0:c308751fd941 | 38 | i2c_Data[0] = 0x41; // Send "read data" command to sensor |
pfe | 0:c308751fd941 | 39 | Error=i2c_->write(addr, i2c_Data, 1); // Send command string |
pfe | 0:c308751fd941 | 40 | wait_ms(1); //attendre 20mS avant de relancer |
pfe | 0:c308751fd941 | 41 | Error=Error+i2c_->read(addr, i2c_Data, 2); |
pfe | 0:c308751fd941 | 42 | CAP10 = (((int)i2c_Data[0] << 8) | (int)i2c_Data[1]); |
pfe | 0:c308751fd941 | 43 | if (Error==0) return CAP10; |
pfe | 0:c308751fd941 | 44 | return -1; |
pfe | 0:c308751fd941 | 45 | } |
pfe | 0:c308751fd941 | 46 | |
pfe | 0:c308751fd941 | 47 | signed short Sansors::GetALT(){ |
pfe | 0:c308751fd941 | 48 | int ALT; |
pfe | 0:c308751fd941 | 49 | char Error; |
pfe | 0:c308751fd941 | 50 | Error = 0; |
pfe | 0:c308751fd941 | 51 | addr=AdSensorALT; |
pfe | 0:c308751fd941 | 52 | i2c_Data[0] = 0x07; // Send "read data" command to sensor |
pfe | 0:c308751fd941 | 53 | Error=i2c_->write(addr, i2c_Data, 1); // Send command string |
pfe | 0:c308751fd941 | 54 | wait_ms(1); //attendre 20mS avant de relancer |
pfe | 0:c308751fd941 | 55 | Error=Error+i2c_->read(addr, i2c_Data, 2); |
pfe | 0:c308751fd941 | 56 | ALT = (((int)i2c_Data[0] << 8) | (int)i2c_Data[1]);//*((unsigned short*)i2c_Data)&0xffff; //altitude en décimètre |
pfe | 0:c308751fd941 | 57 | ALT = ALT-3000; |
pfe | 0:c308751fd941 | 58 | //ALT = (ALT+OldALT)/2; // calcul moyenne pour réduire le bruit |
pfe | 0:c308751fd941 | 59 | //OldALT = ALT; |
pfe | 0:c308751fd941 | 60 | if (Error==0) return ALT;//*3.28084; |
pfe | 0:c308751fd941 | 61 | return -1; |
pfe | 0:c308751fd941 | 62 | } |
pfe | 0:c308751fd941 | 63 | signed int Sansors::GetTAS(){ |
pfe | 0:c308751fd941 | 64 | float TAS_f; |
pfe | 0:c308751fd941 | 65 | unsigned int TASbrut; |
pfe | 0:c308751fd941 | 66 | unsigned int TAS10; |
pfe | 0:c308751fd941 | 67 | char Error; |
pfe | 0:c308751fd941 | 68 | Error = 0; |
pfe | 0:c308751fd941 | 69 | |
pfe | 0:c308751fd941 | 70 | addr=AdSensorTAS; |
pfe | 0:c308751fd941 | 71 | i2c_Data[0] = 0x01; // Send "read data" command to sensor |
pfe | 0:c308751fd941 | 72 | Error = i2c_->write(addr, i2c_Data, 1); // Send command string |
pfe | 0:c308751fd941 | 73 | wait_us(400); //attendre 100µS avant de relancer |
pfe | 0:c308751fd941 | 74 | Error = Error + i2c_->read(addr, i2c_Data, 2); |
pfe | 0:c308751fd941 | 75 | |
pfe | 0:c308751fd941 | 76 | TASbrut = (0xff00 & ( ((unsigned)i2c_Data[1]) << 8)) | ((unsigned char)i2c_Data[0]&0xff); |
pfe | 0:c308751fd941 | 77 | //vitesse = sqrt((float)(vbrut-offset_v)*2*3.6/1.225); |
pfe | 0:c308751fd941 | 78 | TAS_f = sqrt((float)(TASbrut-offset_TAS)*5.87755102); |
pfe | 0:c308751fd941 | 79 | TAS10 = TAS_f*10; |
pfe | 0:c308751fd941 | 80 | if (TAS10>4000)TAS10 = 0; |
pfe | 0:c308751fd941 | 81 | if (Error==0) return TAS10; |
pfe | 0:c308751fd941 | 82 | return -1; |
pfe | 0:c308751fd941 | 83 | } |