bool AD5933_Class::getComplexTetra(int millisDelay, double gainFactor, double vPhaseShift, double iPhaseShift, double &impVal, double &phase) {
int rVoltage, iVoltage, rCurrent, iCurrent;
int ctrReg = getByte(0x80); // Get the content of Control Register and put it into ctrReg
delay(millisDelay);
digitalWrite(IV_MUX, LOW); // Voltage measurement
delay(millisDelay);
if (setCtrMode(REPEAT_FREQ, ctrReg) == false) {
return false;
}
getComplexRawOnce(rVoltage, iVoltage);
delay(millisDelay);
digitalWrite(IV_MUX, HIGH); // Current measurement
delay(millisDelay);
if (setCtrMode(REPEAT_FREQ, ctrReg) == false) {
return false;
}
getComplexRawOnce(rCurrent, iCurrent);
impVal = gainFactor * (getMag(rVoltage, iVoltage) / getMag(rCurrent, iCurrent));
phase = (atan2(iVoltage, rVoltage) - vPhaseShift) - (atan2(iCurrent, rCurrent) - iPhaseShift);
return true;
}
void MPU9150::getMagnetoScaled(float *mx, float *my, float *mz) {
int16_t m1, m2, m3;
getMag(&m1, &m2, &m3);
*mx = normalizeMag(m1);
*my = normalizeMag(m2);
*mz = normalizeMag(m3);
}
int main(void)
{
init();
startUp();
for (int i = 1; i < 4; i++) LEEnabel(i);
while (1)
{
fireMode = getFireMode();
newMagSize = getMag(); //save the Mag size
if(newMagSize == -1)
{
newMag = 0; //Mag-slot empty
ammoCounter = -1;
}
if(((newMag == 0) & !(newMagSize == -1))) //Mag-slot not empty anymore
{
ammoCounter = newMagSize;
newMag = 1;
}
if(adcOn == 0) display(ammoCounter);
if(adcOn == 1)
{
volt |= (ADCL);
volt |= (ADCH << 8);
volt = volt *15;
volt = volt /1023;
display(volt);
}
displaySpeed(speed, motorOn);
setSpeed(speed, motorOn);
}
}
// SETTERS
Vector2& Vector2::setMag(double m)
{
// Check for zero mag
if (getMag() == 0)
{
x = 0;
y = 0;
mag = 0;
}
else
{
double oldMag = getMag();
x /= oldMag;
y /= oldMag;
x *= m;
y *= m;
}
_mag();
return *this;
}
bool AD5933_Class::getImpedance(double gainFactor, double &impVal) {
while ( (getStatusReg() & 0x02) != 0x02 ); // Wait until measurement is complete.
int rComp, iComp;
byte impData[4];
blockRead(0x94, 4, impData);
rComp = impData[0] * 16 * 16 + impData[1];
iComp = impData[2] * 16 * 16 + impData[3];
double magSq = getMag(rComp, iComp);
impVal = gainFactor / magSq;
return true;
}
bool AD5933_Class::getGainFactorS_Set(double cResistance, int avgNum, double *gainFactorArr, double *pShiftArr)
{
int ctrReg = getByte(0x80);
if (setCtrMode(STAND_BY, ctrReg) == false)
{
return false;
}
if (setCtrMode(INIT_START_FREQ, ctrReg) == false)
{
return false;
}
if (setCtrMode(START_FREQ_SWEEP, ctrReg) == false)
{
return false;
}
int t1 = 0;
int rImag, rReal;
double mag;
while ( (getStatusReg() & 0x04) != 0x04) // Loop while if the entire sweep in not complete
{
double tSumMag = 0, tSumPhase = 0;
for (int t2 = 0; t2 < avgNum; t2++)
{
getComplexRawOnce(rReal, rImag); // Only for real and Imag components.
tSumMag += getMag(rReal, rImag);
tSumPhase += atan2(rImag, rReal);
if (setCtrMode(REPEAT_FREQ, ctrReg) == false)
{
return false;
}
}
//Serial.println(t1);
gainFactorArr[t1] = (tSumMag / avgNum) * cResistance;
pShiftArr[t1] = tSumPhase / avgNum;
if (setCtrMode(INCR_FREQ, ctrReg) == false)
{
return false;
}
t1++;
}
if (setCtrMode(POWER_DOWN, ctrReg) == false)
{
return false;
}
return true; // Succeed!
}
bool AD5933_Class::getGainFactorC(double cResistance, int avgNum, double &gainFactor, double &pShift, bool retStandBy)
{
int ctrReg = getByte(0x80); // Get the content of Control Register and put it into ctrReg
if (setCtrMode(STAND_BY, ctrReg) == false)
{
return false;
}
if (setCtrMode(INIT_START_FREQ, ctrReg) == false)
{
return false;
}
//delay(delayTimeInit);
if (setCtrMode(START_FREQ_SWEEP, ctrReg) == false)
{
return false;
}
int t1 = 0, rImag, rReal;
double tSum = 0, tSumP = 0;
while (t1 < avgNum) // Until reached pre-defined number for averaging.
{
getComplexRawOnce(rReal, rImag);
tSum += getMag(rReal, rImag);
tSumP += atan2(rImag, rReal);
if (setCtrMode(REPEAT_FREQ, ctrReg) == false)
{
return false;
}
t1++;
}
double mag = tSum / (double)avgNum;
pShift = tSumP / (double)avgNum;
if (retStandBy == false)
{
gainFactor = mag * cResistance;
return true;
}
if ( setCtrMode(STAND_BY, ctrReg) == false)
{
return false;
}
resetAD5933();
// Gain Factor is different from one of the datasheet in this program. Reciprocal Value.
gainFactor = mag * cResistance;
return true;
}
/*
* Hidden Function to get magnitude value of impedance measurement. (It does not wait.)
* Returns the magnitude of impedance value
*/
double AD5933_Class::getMagValue() {
int rComp, iComp;
byte impData[4];
blockRead(0x94, 4, impData);
rComp = impData[0] * 16 * 16 + impData[1];
iComp = impData[2] * 16 * 16 + impData[3];
double result = getMag(rComp, iComp); // Calculating magnitude.
#if LOGGING3
printer->print("getMagValue - Resistance Magnitude is ");
printer->println(result);
#endif
return result;
}
bool AD5933_Class::getComplexOnce(double gainFactor, double pShift, double &vReal, double &vComp, double &impVal, double &phase)
{
while ( (getStatusReg() & 0x02) != 0x02 ); // Wait until measurement is complete.
int rComp, iComp;
byte impData[4];
blockRead(0x94, 4, impData);
rComp = impData[0] * 16 * 16 + impData[1];
iComp = impData[2] * 16 * 16 + impData[3];
double magSq = getMag(rComp, iComp);
impVal = gainFactor / magSq;
phase = atan2(iComp, rComp) - pShift;
vReal = impVal * cos(phase);
vComp = impVal * sin(phase);
return true;
}
double AD5933_Class::getMagValue() {
// Hidden Function to get magnitude value of impedance measurement. (It does not wait.)
// TODO: Rewrite this function with using block read function.
int rComp, iComp;
//rComp = getRealComp(); // Getting Real Component
//iComp = getImagComp(); // Getting Imaginary Component
byte impData[4];
blockRead(0x94, 4, impData);
rComp = impData[0] * 16 * 16 + impData[1];
iComp = impData[2] * 16 * 16 + impData[3];
double result = getMag(rComp, iComp); // Calculating magnitude.
#if LOGGING3
printer->print("getMagValue - Resistance Magnitude is ");
printer->println(result);
#endif
return result;
}
void ImuAdis16448::processMeasurements() {
while (1) {
boost::this_thread::interruption_point();
//get the newest measurement
Measurement::Ptr meas = Sensor::measurement_queue_.pop();
//VISENSOR_DEBUG("in imu_adis thread %d\n", meas.buffersize);
//check for missed frames
// TODO(schneith): use a timer to check for missed frames (currently we only detected sequences of MISSED-RECEIVED-MISSED, not MISSED-MISSED,...)
if( checkTimestamp(meas->timestamp) )
{
//publish an empty missed frame
ViImuMsg::Ptr missed_imu_ptr = boost::make_shared<ViImuMsg>();
missed_imu_ptr->imu_id = Imu::imu_id_;
publishImuData(missed_imu_ptr, ViErrorCodes::MEASUREMENT_DROPPED);
}
// //TODO adapt
// create new shared pointer for the imu message
ViImuMsg::Ptr imu_msg_ptr = boost::make_shared<ViImuMsg>();
// Add imu information to the msg
imu_msg_ptr->imu_id = Imu::imu_id_;
imu_msg_ptr->timestamp=meas->timestamp;
imu_msg_ptr->timestamp_host = meas->timestamp_host;
imu_msg_ptr->timestamp_fpga_counter = meas->timestamp_fpga_counter;
// get imu data
getGyro(meas->data.get(), &imu_msg_ptr->gyro[0]);
getAcc(meas->data.get(), &imu_msg_ptr->acc[0]);
getMag(meas->data.get(), &imu_msg_ptr->mag[0]);
getBaro(meas->data.get(), &imu_msg_ptr->baro);
publishImuData(imu_msg_ptr, ViErrorCodes::NO_ERROR);
}
}
void Vector2D::setMag(double newMag)
{
double mRatio = newMag / getMag();
xComp *= mRatio;
yComp *= mRatio;
}
bool AD5933_Class::getGainFactorTetra(double calResistance, int avgNum, double &gainFactor, double &vPShift, double &iPShift, bool retStandBy){
int ctrReg = getByte(0x80); // Get the content of Control Register and put it into ctrReg
if (setCtrMode(STAND_BY, ctrReg) == false){
return false;
}
if (setCtrMode(INIT_START_FREQ, ctrReg) == false){
return false;
}
if (setCtrMode(START_FREQ_SWEEP, ctrReg) == false){
return false;
}
int index = 0, iVoltage = 0, rVoltage = 0, iCurrent = 0, rCurrent = 0;
double voltageSum = 0, currentSum = 0, voltagePhaseSum = 0, currentPhaseSum = 0;
delay(100);
digitalWrite(IV_MUX, LOW); //Toggle current / voltage multiplexer to voltage
delay(100);
if (setCtrMode(REPEAT_FREQ, ctrReg) == false) {
return false;
}
// Until reached pre-defined number for averaging.
while (index < avgNum) {
getComplexRawOnce(rVoltage, iVoltage);
voltageSum += getMag(rVoltage, iVoltage);
voltagePhaseSum += atan2(iVoltage, rVoltage);
if (setCtrMode(REPEAT_FREQ, ctrReg) == false) {
return false;
}
index++;
}
delay(100);
digitalWrite(IV_MUX, HIGH); // Toggle current / voltage multiplexer to current
delay(100); // delay for mux to settle
index = 0; // Reset index for current
if (setCtrMode(REPEAT_FREQ, ctrReg) == false) {
return false;
}
while (index < avgNum) {
getComplexRawOnce(rCurrent, iCurrent);
currentSum += getMag(rCurrent, iCurrent);
currentPhaseSum += atan2(iCurrent, rCurrent);
if (setCtrMode(REPEAT_FREQ, ctrReg) == false) {
return false;
}
index++;
}
double voltageMag = voltageSum / ((double) avgNum);
double currentMag = currentSum / ((double) avgNum);
vPShift = voltagePhaseSum / ((double) avgNum);
iPShift = currentPhaseSum / ((double) avgNum);
if (retStandBy == false){
gainFactor = calResistance * (currentMag / voltageMag);
return true;
}
if ( setCtrMode(STAND_BY, ctrReg) == false){
return false;
}
resetAD5933();
gainFactor = calResistance * (currentMag / voltageMag);
return true;
}
bool AD5933_Class::getGainFactorsTetraSweep(double cResistance, int avgNum, double *gainFactorArr, double *vShiftArr, double *cShiftArr){
int ctrReg = getByte(0x80);
if (setCtrMode(STAND_BY, ctrReg) == false)
{
return false;
}
if (setCtrMode(INIT_START_FREQ, ctrReg) == false)
{
return false;
}
if (setCtrMode(START_FREQ_SWEEP, ctrReg) == false)
{
return false;
}
int arrayIndex = 0;
while ( (getStatusReg() & 0x04) != 0x04) { // While the fequency sweep isn't complete
//////
int averageIndex = 0, iVoltage = 0, rVoltage = 0, iCurrent = 0, rCurrent = 0;
double voltageSum = 0, currentSum = 0, voltagePhaseSum = 0, currentPhaseSum = 0;
digitalWrite(IV_MUX, LOW); //Toggle current / voltage multiplexer to voltage
delay(10);
if (setCtrMode(REPEAT_FREQ, ctrReg) == false) {
return false;
}
// Until reached pre-defined number for averaging.
while (averageIndex < avgNum) {
getComplexRawOnce(rVoltage, iVoltage);
voltageSum += getMag(rVoltage, iVoltage);
voltagePhaseSum += atan2(iVoltage, rVoltage);
if (setCtrMode(REPEAT_FREQ, ctrReg) == false) {
return false;
}
averageIndex++;
}
digitalWrite(IV_MUX, HIGH); // Toggle current / voltage multiplexer to current
delay(10); // delay for mux to settle
averageIndex = 0; // Reset index for current
if (setCtrMode(REPEAT_FREQ, ctrReg) == false) {
return false;
}
while (averageIndex < avgNum) {
getComplexRawOnce(rCurrent, iCurrent);
currentSum += getMag(rCurrent, iCurrent);
currentPhaseSum += atan2(iCurrent, rCurrent);
if (setCtrMode(REPEAT_FREQ, ctrReg) == false) {
return false;
}
averageIndex++;
}
double voltageMag = voltageSum / ((double) avgNum);
double currentMag = currentSum / ((double) avgNum);
gainFactorArr[arrayIndex] = cResistance * (currentMag / voltageMag);
vShiftArr[arrayIndex] = voltagePhaseSum / ((double) avgNum);
cShiftArr[arrayIndex] = currentPhaseSum / ((double) avgNum);
//////
if (setCtrMode(INCR_FREQ, ctrReg) == false){
return false;
}
arrayIndex++;
}
if (setCtrMode(POWER_DOWN, ctrReg) == false){
return false;
}
return true; // Succeed!
}
void Vector2D::normalize()
{
xComp /= getMag();
yComp /= getMag();
}
bool AD5933_Class::getGainFactorS_TP(double cResistance, int avgNum, double startFreq, double endFreq, double &GF_Init, double &GF_Incr, double &PS_Init, double &PS_Incr)
{
int ctrReg = getByte(0x80);
byte numIncrementBackup = numIncrement;
long incrHexBackup = incrHex;
setNumofIncrement(1);
setIncrement(endFreq - startFreq);
if (setCtrMode(STAND_BY, ctrReg) == false)
{
return false;
}
if (setCtrMode(INIT_START_FREQ, ctrReg) == false)
{
return false;
}
if (setCtrMode(START_FREQ_SWEEP, ctrReg) == false)
{
return false;
}
int t1 = 0;
int rImag, rReal;
double sumMag = 0, sumPhase = 0;
for (t1 = 0; t1 < avgNum; t1++)
{
getComplexRawOnce(rReal, rImag); // Only for real and Imag components.
sumMag += getMag(rReal, rImag);
sumPhase += atan2(rImag, rReal);
if (setCtrMode(REPEAT_FREQ, ctrReg) == false)
{
return false;
}
}
GF_Init = (sumMag / avgNum) * cResistance;
PS_Init = sumPhase / avgNum;
setCtrMode(INCR_FREQ, ctrReg);
sumMag = 0;
sumPhase = 0;
for (t1 = 0; t1 < avgNum; t1++)
{
getComplexRawOnce(rReal, rImag); // Only for real and Imag components.
sumMag += getMag(rReal, rImag);
sumPhase += atan2(rImag, rReal);
if (setCtrMode(REPEAT_FREQ, ctrReg) == false)
{
return false;
}
}
GF_Incr = ((sumMag / avgNum) * cResistance - GF_Init) / numIncrementBackup;
PS_Incr = ((sumPhase / avgNum) - PS_Init) / numIncrementBackup;
if (setCtrMode(POWER_DOWN, ctrReg) == false)
{
return false;
}
setNumofIncrement(numIncrementBackup);
setIncrementinHex(incrHexBackup);
return true; // Succeed!
}
double Vector2D::getADelt(Vector2D vec)
{
return acos((*this * vec) / (getMag() * vec.getMag()));
}
