void Gyroscope::getGyroValues(gyroscope_data* data) {
// 1. Read STATUS_REG
// 2. If STATUS_REG(3) = 0 then go to 1
// 3. If STATUS_REG(7) = 1 then some data have been overwritten
// 4. read XYZ
while (!isDataAvailable()) {
delayMicroseconds(10);
}
i2cProtocol.read(L3G4_Address, L3G4_OUT_X_L | L3G4_AUTOINC, 6);
while (i2cProtocol.available() < 6) {
}
uint8_t xLSB = i2cProtocol.receive(); //read OUT_X_L_A (LSB)
uint8_t xMSB = i2cProtocol.receive(); //read OUT_X_H_A (MSB)
uint8_t yLSB = i2cProtocol.receive(); //read OUT_Y_L_A (LSB)
uint8_t yMSB = i2cProtocol.receive(); //read OUT_Y_H_A (MSB)
uint8_t zLSB = i2cProtocol.receive(); //read OUT_Z_L_A (LSB)
uint8_t zMSB = i2cProtocol.receive(); //read OUT_Z_H_A (MSB)
data->r = ((xMSB << 8) | xLSB);
data->p = ((yMSB << 8) | yLSB);
data->y = ((zMSB << 8) | zLSB);
}
void frameGenerator() {
uint8_t getByte,i=0,nbytes=0;
if(isDataAvailable(&bufferOut)==1) {
disableTxInterrupt;
do {
//
getByte=getFromBuffer(&bufferOut);
if(getByte<=FUNCTION_ID) {
setToBuffer(FRAME_START,&bufferTx);
txDataLength++;
setToBuffer(getByte,&bufferTx);
txDataLength++;
getByte=getFromBuffer(&bufferOut);
setToBuffer(getByte,&bufferTx);
txDataLength++;
} else {
setToBuffer(FRAME_START,&bufferTx);
txDataLength++;
if(getByte==getMeasureResponse) {
getByte=getMeasure;
}
setToBuffer(getByte,&bufferTx);
txDataLength++;
nbytes=getFromBuffer(&bufferOut);
setToBuffer(nbytes,&bufferTx);
txDataLength++;
for(i=0; i<nbytes; i++) {
getByte=getFromBuffer(&bufferOut);
setToBuffer(getByte,&bufferTx);
txDataLength++;
}
}
} while(isDataAvailable(&bufferOut)==1);
txDataLength=txDataLength-1;
SendByte(getFromBuffer(&bufferTx));
}
}
interrupt VectorNumber_Vsci1tx void TxInterrupt (void) { //rutina para enviar datos por interrupciones
(void)statusSerialRegister1; //borra flags de interrupcion
if(txDataLength>0&&isDataAvailable(&bufferTx)) {
enableTxInterrupt;
serialData=getFromBuffer(&bufferTx);
txDataLength--;
} else {
disableTxInterrupt;
}
return;
}
void processIncommingCarMessages(uint16_t count)
{
tXBeePacket * pkt;
uint16_t i = 0;
while (isDataAvailable() && i++ < count)
{
pkt = getPkt();
uint16_t address = pkt-> getRemoteAddress();
char* message = pkt->getData();
char compare[4] = {message[0], message[1], message[2], 0};
//message format: SV ##.##
// 0123
float measurment = atof(&message[3]);
if(! strcmp(compare, "SV ") )
{
//process voltage stuff
ChargeManager::updateVehicleVoltage(address, measurment);
}
else if(! strcmp(compare, "SA ") )
{
//process current stuff
ChargeManager::updateVehicleCurrent(address, measurment);
}
else
{
Serial.print("RX Invalid XBee Packet from: ");
Serial.print(address);
Serial.print(", :: ");
Serial.println(pkt->getData());
}
/**
Serial.print("XBee Receive: ");
Serial.print(pkt->getLength());
Serial.print(" bytes from: ");
Serial.print(pkt->getRemoteAddress());
Serial.println(".");
Serial.println(pkt->getData());
Serial.println("");
*/
if(pkt)
delete(pkt);
pkt = 0;
}
}