ahmed ahmed / CAP_ALT_TAS

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?

UserRevisionLine numberNew 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 }