bool LIS3MDL::init(deviceType device, sa1State sa1)
{
// perform auto-detection unless device type and SA1 state were both specified
if (device == device_auto || sa1 == sa1_auto)
{
// check for LIS3MDL if device is unidentified or was specified to be this type
if (device == device_auto || device == device_LIS3MDL)
{
// check SA1 high address unless SA1 was specified to be low
if (sa1 != sa1_low && testReg(LIS3MDL_SA1_HIGH_ADDRESS, WHO_AM_I) == LIS3MDL_WHO_ID)
{
sa1 = sa1_high;
if (device == device_auto) { device = device_LIS3MDL; }
}
// check SA1 low address unless SA1 was specified to be high
else if (sa1 != sa1_high && testReg(LIS3MDL_SA1_LOW_ADDRESS, WHO_AM_I) == LIS3MDL_WHO_ID)
{
sa1 = sa1_low;
if (device == device_auto) { device = device_LIS3MDL; }
}
}
// make sure device and SA1 were successfully detected; otherwise, indicate failure
if (device == device_auto || sa1 == sa1_auto)
{
return false;
}
}
_device = device;
switch (device)
{
case device_LIS3MDL:
address = (sa1 == sa1_high) ? LIS3MDL_SA1_HIGH_ADDRESS : LIS3MDL_SA1_LOW_ADDRESS;
break;
}
return true;
}
int main(int argc, char *argv[])
{
QCoreApplication a(argc, argv);
/*Failed to bind if this two exist at the same time*/
TestLogin testReg(false);
if (!testReg.testReady ())
{
return 1;
}
return a.exec();
}
bool LSM303::init(deviceType device, sa0State sa0)
{
// determine device type if necessary
if (device == device_auto)
{
if (testReg(D_SA0_HIGH_ADDRESS, WHO_AM_I) == D_WHO_ID)
{
// device responds to address 0011101 with D ID; it's a D with SA0 high
device = device_D;
sa0 = sa0_high;
}
else if (testReg(D_SA0_LOW_ADDRESS, WHO_AM_I) == D_WHO_ID)
{
// device responds to address 0011110 with D ID; it's a D with SA0 low
device = device_D;
sa0 = sa0_low;
}
// Remaining possibilities: DLHC, DLM, or DLH. DLHC seems to respond to WHO_AM_I request the
// same way as DLM, even though this register isn't documented in its datasheet, so instead,
// guess if it's a DLHC based on acc address (Pololu boards pull SA0 low on DLM and DLH;
// DLHC doesn't have SA0 but uses same acc address as DLH/DLM with SA0 high).
else if (testReg(NON_D_ACC_SA0_HIGH_ADDRESS, CTRL_REG1_A) != TEST_REG_NACK)
{
// device responds to address 0011001; guess that it's a DLHC
device = device_DLHC;
sa0 = sa0_high;
}
// Remaining possibilities: DLM or DLH. Check acc with SA0 low address to make sure it's responsive
else if (testReg(NON_D_ACC_SA0_LOW_ADDRESS, CTRL_REG1_A) != TEST_REG_NACK)
{
// device responds to address 0011000 with DLM ID; guess that it's a DLM
sa0 = sa0_low;
// Now check WHO_AM_I_M
if (testReg(NON_D_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID)
{
device = device_DLM;
}
else
{
device = device_DLH;
}
}
else
{
// device hasn't responded meaningfully, so give up
return false;
}
}
// determine SA0 if necessary
if (sa0 == sa0_auto)
{
if (device == device_D)
{
if (testReg(D_SA0_HIGH_ADDRESS, WHO_AM_I) == D_WHO_ID)
{
sa0 = sa0_high;
}
else if (testReg(D_SA0_LOW_ADDRESS, WHO_AM_I) == D_WHO_ID)
{
sa0 = sa0_low;
}
else
{
// no response on either possible address; give up
return false;
}
}
else if (device == device_DLM || device == device_DLH)
{
if (testReg(NON_D_ACC_SA0_HIGH_ADDRESS, CTRL_REG1_A) != TEST_REG_NACK)
{
sa0 = sa0_high;
}
else if (testReg(NON_D_ACC_SA0_LOW_ADDRESS, CTRL_REG1_A) != TEST_REG_NACK)
{
sa0 = sa0_low;
}
else
{
// no response on either possible address; give up
return false;
}
}
}
_device = device;
// set device addresses and translated register addresses
switch (device)
{
case device_D:
acc_address = mag_address = (sa0 == sa0_high) ? D_SA0_HIGH_ADDRESS : D_SA0_LOW_ADDRESS;
translated_regs[-OUT_X_L_M] = D_OUT_X_L_M;
translated_regs[-OUT_X_H_M] = D_OUT_X_H_M;
translated_regs[-OUT_Y_L_M] = D_OUT_Y_L_M;
translated_regs[-OUT_Y_H_M] = D_OUT_Y_H_M;
translated_regs[-OUT_Z_L_M] = D_OUT_Z_L_M;
translated_regs[-OUT_Z_H_M] = D_OUT_Z_H_M;
break;
case device_DLHC:
acc_address = NON_D_ACC_SA0_HIGH_ADDRESS; // DLHC doesn't have SA0 but uses same acc address as DLH/DLM with SA0 high
mag_address = NON_D_MAG_ADDRESS;
translated_regs[-OUT_X_H_M] = DLHC_OUT_X_H_M;
translated_regs[-OUT_X_L_M] = DLHC_OUT_X_L_M;
translated_regs[-OUT_Y_H_M] = DLHC_OUT_Y_H_M;
translated_regs[-OUT_Y_L_M] = DLHC_OUT_Y_L_M;
translated_regs[-OUT_Z_H_M] = DLHC_OUT_Z_H_M;
translated_regs[-OUT_Z_L_M] = DLHC_OUT_Z_L_M;
break;
case device_DLM:
acc_address = (sa0 == sa0_high) ? NON_D_ACC_SA0_HIGH_ADDRESS : NON_D_ACC_SA0_LOW_ADDRESS;
mag_address = NON_D_MAG_ADDRESS;
translated_regs[-OUT_X_H_M] = DLM_OUT_X_H_M;
translated_regs[-OUT_X_L_M] = DLM_OUT_X_L_M;
translated_regs[-OUT_Y_H_M] = DLM_OUT_Y_H_M;
translated_regs[-OUT_Y_L_M] = DLM_OUT_Y_L_M;
translated_regs[-OUT_Z_H_M] = DLM_OUT_Z_H_M;
translated_regs[-OUT_Z_L_M] = DLM_OUT_Z_L_M;
break;
case device_DLH:
acc_address = (sa0 == sa0_high) ? NON_D_ACC_SA0_HIGH_ADDRESS : NON_D_ACC_SA0_LOW_ADDRESS;
mag_address = NON_D_MAG_ADDRESS;
translated_regs[-OUT_X_H_M] = DLH_OUT_X_H_M;
translated_regs[-OUT_X_L_M] = DLH_OUT_X_L_M;
translated_regs[-OUT_Y_H_M] = DLH_OUT_Y_H_M;
translated_regs[-OUT_Y_L_M] = DLH_OUT_Y_L_M;
translated_regs[-OUT_Z_H_M] = DLH_OUT_Z_H_M;
translated_regs[-OUT_Z_L_M] = DLH_OUT_Z_L_M;
break;
}
return true;
}
bool L3G::init(deviceType device, sa0State sa0)
{
int id;
// perform auto-detection unless device type and SA0 state were both specified
if (device == device_auto || sa0 == sa0_auto)
{
// check for L3GD20H, D20 if device is unidentified or was specified to be one of these types
if (device == device_auto || device == device_D20H || device == device_D20)
{
// check SA0 high address unless SA0 was specified to be low
if (sa0 != sa0_low && (id = testReg(D20_SA0_HIGH_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR)
{
// device responds to address 1101011; it's a D20H or D20 with SA0 high
sa0 = sa0_high;
if (device == device_auto)
{
// use ID from WHO_AM_I register to determine device type
device = (id == D20H_WHO_ID) ? device_D20H : device_D20;
}
}
// check SA0 low address unless SA0 was specified to be high
else if (sa0 != sa0_high && (id = testReg(D20_SA0_LOW_ADDRESS, WHO_AM_I)) != TEST_REG_ERROR)
{
// device responds to address 1101010; it's a D20H or D20 with SA0 low
sa0 = sa0_low;
if (device == device_auto)
{
// use ID from WHO_AM_I register to determine device type
device = (id == D20H_WHO_ID) ? device_D20H : device_D20;
}
}
}
// check for L3G4200D if device is still unidentified or was specified to be this type
if (device == device_auto || device == device_4200D)
{
if (sa0 != sa0_low && testReg(L3G4200D_SA0_HIGH_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID)
{
// device responds to address 1101001; it's a 4200D with SA0 high
device = device_4200D;
sa0 = sa0_high;
}
else if (sa0 != sa0_high && testReg(L3G4200D_SA0_LOW_ADDRESS, WHO_AM_I) == L3G4200D_WHO_ID)
{
// device responds to address 1101000; it's a 4200D with SA0 low
device = device_4200D;
sa0 = sa0_low;
}
}
// make sure device and SA0 were successfully detected; otherwise, indicate failure
if (device == device_auto || sa0 == sa0_auto)
{
return false;
}
}
_device = device;
// set device address
switch (device)
{
case device_D20H:
case device_D20:
address = (sa0 == sa0_high) ? D20_SA0_HIGH_ADDRESS : D20_SA0_LOW_ADDRESS;
break;
case device_4200D:
address = (sa0 == sa0_high) ? L3G4200D_SA0_HIGH_ADDRESS : L3G4200D_SA0_LOW_ADDRESS;
break;
}
return true;
}
//~ must return a zero on any successful completion of command
//~ if unsuccessful, return a 1 which is an abend interrupt, the OS
//~ will remove the process from the processor and kill it.
int Processor::perform(string command){
vector<string> tokens;
//~ tokenize is defined within Tokenize.h It tokenizes the
//~ string and inserts the tokens in the returned vector.
tokens = Tokenize(command, " \t");
//~ cout << "performing command: " << command << endl;
//~ for(uint i = 0; i < tokens.size(); i++){
//~ cout << "\ttoken " << i << ": " << tokens[i] << endl;
//~ }
if(tokens[0] == "noop"){
//~ do nothing
return 0;
}
else if(tokens[0] == "jmp"){
//~ Just change the PC, it is that simple
//~ this one should really take an address, a
//~ register, or an indirect address
//~ printf("jumping to %s\n",tokens[1].c_str());
PC = atoi(tokens[1].c_str());
return 0;
}else if(tokens[0] == "mov"){
//~ MOV AL, 61h
//~ Move (really a copy) the hexadecimal value '61' into
//~ the processor register known as "AL". (The h-suffix means
//~ hexadecimal or = 97 in decimal) */
if(testReg(tokens[1])){
return 1; //this is the interrupt that should occur
}else{ //given a bad command (abbend)
setReg(tokens[1], (word)atoi(tokens[2].c_str()));
return 0;
}
}else if(tokens[0] == "inc"){
if(testReg(tokens[1])){
return 1; // returned int for bad command
}else{
word regVal = getReg(tokens[1]);
regVal++;
setReg(tokens[1],regVal);
return 0;
}
}else if(tokens[0] == "dec"){
if(testReg(tokens[1])){
return 1; // returned int for bad command
}else{
word regVal = getReg(tokens[1]);
regVal--;
//~ printf("decrementing %s. ",tokens[1].c_str());
//~ printf("It is now %d\n",(int)regVal);
setReg(tokens[1],regVal);
return 0;
}
}else if(tokens[0] == "add"){
if(testReg(tokens[1]) || testReg(tokens[2])){
return 1;// returned int for bad command
}else{
int addFrom = getReg(tokens[2]);
int addTo = getReg(tokens[1]);
setReg(tokens[1],addFrom+addTo);
return 0;
}
}else if(tokens[0] == "mul"){
if(testReg(tokens[1]) || testReg(tokens[2])){
return 1;
}else{
int multFrom = getReg(tokens[2]);
int multTo = getReg(tokens[1]);
setReg(tokens[1],multFrom * multTo);
return 0;
}
}else if(tokens[0] == "div"){
if(testReg(tokens[1]) || testReg(tokens[2]) || getReg(tokens[2] )== 0){
return 1;//this is an abend
}else{
int divFrom = getReg(tokens[2]);
int divTo = getReg(tokens[1]);
setReg(tokens[1],divFrom * divTo);
return 0;
}
}else if(tokens[0] == "jz"){
//~ if the first register is zero jump to the address in sec reg
//~ printf("got a jz\n");
if(testReg(tokens[1]) || testReg(tokens[2])) {
return 1;//this is an abend
}else{
//~ printf("checking whether %s is zero\n",tokens[1].c_str());
//~ printf("its contents is %d\n",(int)getReg(tokens[1]));
if((int)getReg(tokens[1]) == 0){
PC = getReg(tokens[2]);
}
return 0;
}
}else if(tokens[0] == "end"){
return 2;// returned int for normal end of program
}else{
cout << "simProc reports unrecognized command: " << command << endl;
return 1;
}
}
bool LSM303::init(deviceType device, sa0State sa0)
{
// perform auto-detection unless device type and SA0 state were both specified
if (device == device_auto || sa0 == sa0_auto)
{
// check for LSM303D if device is unidentified or was specified to be this type
if (device == device_auto || device == device_D)
{
// check SA0 high address unless SA0 was specified to be low
if (sa0 != sa0_low && testReg(D_SA0_HIGH_ADDRESS, WHO_AM_I) == D_WHO_ID)
{
// device responds to address 0011101 with D ID; it's a D with SA0 high
device = device_D;
sa0 = sa0_high;
}
// check SA0 low address unless SA0 was specified to be high
else if (sa0 != sa0_high && testReg(D_SA0_LOW_ADDRESS, WHO_AM_I) == D_WHO_ID)
{
// device responds to address 0011110 with D ID; it's a D with SA0 low
device = device_D;
sa0 = sa0_low;
}
}
// check for LSM303DLHC, DLM, DLH if device is still unidentified or was specified to be one of these types
if (device == device_auto || device == device_DLHC || device == device_DLM || device == device_DLH)
{
// check SA0 high address unless SA0 was specified to be low
if (sa0 != sa0_low && testReg(DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR)
{
// device responds to address 0011001; it's a DLHC, DLM with SA0 high, or DLH with SA0 high
sa0 = sa0_high;
if (device == device_auto)
{
// use magnetometer WHO_AM_I register to determine device type
//
// DLHC seems to respond to WHO_AM_I request the same way as DLM, even though this
// register isn't documented in its datasheet. Since the DLHC accelerometer address is the
// same as the DLM with SA0 high, but Pololu DLM boards pull SA0 low by default, we'll
// guess that a device whose accelerometer responds to the SA0 high address and whose
// magnetometer gives the DLM ID is actually a DLHC.
device = (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID) ? device_DLHC : device_DLH;
}
}
// check SA0 low address unless SA0 was specified to be high
else if (sa0 != sa0_high && testReg(DLM_DLH_ACC_SA0_LOW_ADDRESS, CTRL_REG1_A) != TEST_REG_ERROR)
{
// device responds to address 0011000; it's a DLM with SA0 low or DLH with SA0 low
sa0 = sa0_low;
if (device == device_auto)
{
// use magnetometer WHO_AM_I register to determine device type
device = (testReg(DLHC_DLM_DLH_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID) ? device_DLM : device_DLH;
}
}
}
// make sure device and SA0 were successfully detected; otherwise, indicate failure
if (device == device_auto || sa0 == sa0_auto)
{
return false;
}
}
_device = device;
// set device addresses and translated register addresses
switch (device)
{
case device_D:
acc_address = mag_address = (sa0 == sa0_high) ? D_SA0_HIGH_ADDRESS : D_SA0_LOW_ADDRESS;
translated_regs[-OUT_X_L_M] = D_OUT_X_L_M;
translated_regs[-OUT_X_H_M] = D_OUT_X_H_M;
translated_regs[-OUT_Y_L_M] = D_OUT_Y_L_M;
translated_regs[-OUT_Y_H_M] = D_OUT_Y_H_M;
translated_regs[-OUT_Z_L_M] = D_OUT_Z_L_M;
translated_regs[-OUT_Z_H_M] = D_OUT_Z_H_M;
break;
case device_DLHC:
acc_address = DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS; // DLHC doesn't have configurable SA0 but uses same acc address as DLM/DLH with SA0 high
mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
translated_regs[-OUT_X_H_M] = DLHC_OUT_X_H_M;
translated_regs[-OUT_X_L_M] = DLHC_OUT_X_L_M;
translated_regs[-OUT_Y_H_M] = DLHC_OUT_Y_H_M;
translated_regs[-OUT_Y_L_M] = DLHC_OUT_Y_L_M;
translated_regs[-OUT_Z_H_M] = DLHC_OUT_Z_H_M;
translated_regs[-OUT_Z_L_M] = DLHC_OUT_Z_L_M;
break;
case device_DLM:
acc_address = (sa0 == sa0_high) ? DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS : DLM_DLH_ACC_SA0_LOW_ADDRESS;
mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
translated_regs[-OUT_X_H_M] = DLM_OUT_X_H_M;
translated_regs[-OUT_X_L_M] = DLM_OUT_X_L_M;
translated_regs[-OUT_Y_H_M] = DLM_OUT_Y_H_M;
translated_regs[-OUT_Y_L_M] = DLM_OUT_Y_L_M;
translated_regs[-OUT_Z_H_M] = DLM_OUT_Z_H_M;
translated_regs[-OUT_Z_L_M] = DLM_OUT_Z_L_M;
break;
case device_DLH:
acc_address = (sa0 == sa0_high) ? DLHC_DLM_DLH_ACC_SA0_HIGH_ADDRESS : DLM_DLH_ACC_SA0_LOW_ADDRESS;
mag_address = DLHC_DLM_DLH_MAG_ADDRESS;
translated_regs[-OUT_X_H_M] = DLH_OUT_X_H_M;
translated_regs[-OUT_X_L_M] = DLH_OUT_X_L_M;
translated_regs[-OUT_Y_H_M] = DLH_OUT_Y_H_M;
translated_regs[-OUT_Y_L_M] = DLH_OUT_Y_L_M;
translated_regs[-OUT_Z_H_M] = DLH_OUT_Z_H_M;
translated_regs[-OUT_Z_L_M] = DLH_OUT_Z_L_M;
break;
}
return true;
}