void MPU9250::read_temp()
{
char response[2];
int16_t bit_data;
float data;
ReadRegs(MPUREG_TEMP_OUT_H,response,2);
bit_data=((int16_t)response[0]<<8)|response[1];
data=(float)bit_data;
temperature=(data/340)+36.53;
}
void MPU9250::read_gyro()
{
char response[6];
int16_t bit_data;
float data;
int i;
ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
for(i=0; i<3; i++) {
bit_data = ((int16_t)response[i*2] << 8) | response[i*2+1];
data = (float)bit_data;
gyroscope_data[i] = (PI / 180) * data / gyro_divider;
}
}
void MPU9250::read_acc()
{
char response[6];
int16_t bit_data;
float data;
int i;
ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
for(i=0; i<3; i++) {
bit_data = ((int16_t)response[i*2] << 8) | response[i*2+1];
data = (float)bit_data;
accelerometer_data[i] = G_SI * data / acc_divider;
}
}
void MPU9250::calib_acc()
{
char response[4];
int temp_scale;
//READ CURRENT ACC SCALE
temp_scale=ReadReg(MPUREG_ACCEL_CONFIG);
set_acc_scale(BITS_FS_8G);
//ENABLE SELF TEST need modify
//temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
ReadRegs(MPUREG_SELF_TEST_X,response,4);
calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4);
calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2);
calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011));
set_acc_scale(temp_scale);
}
void MPU9250::read_mag(){
char response[7];
int16_t bit_data;
float data;
int i;
WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer
bcm2835_delayMicroseconds(10000);
ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7);
//must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
for(i=0; i<3; i++) {
bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
data=(float)bit_data;
magnetometer_data[i]=data*magnetometer_ASA[i];
}
}
void MPU9250::calib_mag(){
char response[3];
float data;
int i;
WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer
WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer
//WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
bcm2835_delayMicroseconds(10000);
//response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C
ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
//response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
for(i=0; i<3; i++) {
data=response[i];
magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
}
}
void MPU9250::read_all(){
char response[21];
int16_t bit_data;
float data;
int i;
//Send I2C command at first
WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer
//must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
//wait(0.001);
ReadRegs(MPUREG_ACCEL_XOUT_H,response,21);
//Get accelerometer value
for(i=0; i<3; i++) {
bit_data = ((int16_t)response[i*2] << 8)|response[i*2+1];
data = (float)bit_data;
accelerometer_data[i] = G_SI * data / acc_divider;
}
//Get temperature
bit_data = ((int16_t)response[i*2] << 8) | response[i*2+1];
data = (float)bit_data;
temperature = ((data - 21) / 333.87) + 21;
//Get gyroscope value
for(i=4; i<7; i++) {
bit_data = ((int16_t)response[i*2] << 8) | response[i*2+1];
data = (float)bit_data;
gyroscope_data[i-4] = (PI / 180) * data / gyro_divider;
}
//Get Magnetometer value
for(i=7; i<10; i++) {
bit_data = ((int16_t)response[i*2+1] << 8) | response[i*2];
data = (float)bit_data;
magnetometer_data[i-7] = data * magnetometer_ASA[i-7];
}
}
bool DebugExecute( uDB_t *buff, ULONG cmd, bool stop_on_module_load )
{
EXCEPTIONREPORTRECORD ex;
ULONG value;
ULONG stopvalue;
ULONG notify = 0;
BOOL got_second_notification;
ULONG fcp;
CONTEXTRECORD fcr;
buff->Cmd = cmd;
value = buff->Value;
if( cmd == DBG_C_Go ) {
value = 0;
}
stopvalue = XCPT_CONTINUE_EXECUTION;
got_second_notification = FALSE;
if( cmd == DBG_C_Stop ) {
stopvalue = XCPT_CONTINUE_STOP;
}
for( ;; ) {
buff->Value = value;
buff->Cmd = cmd;
CallDosDebug( buff );
value = stopvalue;
cmd = DBG_C_Continue;
/*
* handle the preemptive notifications
*/
switch( buff->Cmd ) {
case DBG_N_ModuleLoad:
RecordModHandle( buff->Value );
if( stop_on_module_load )
return( TRUE );
break;
case DBG_N_ModuleFree:
break;
case DBG_N_NewProc:
break;
case DBG_N_ProcTerm:
value = XCPT_CONTINUE_STOP; /* halt us */
notify = DBG_N_ProcTerm;
break;
case DBG_N_ThreadCreate:
break;
case DBG_N_ThreadTerm:
break;
case DBG_N_AliasFree:
break;
case DBG_N_Exception:
ExceptLinear = buff->Addr;
if( buff->Value == DBG_X_STACK_INVALID ) {
value = XCPT_CONTINUE_SEARCH;
break;
}
fcp = buff->Len;
if( buff->Value == DBG_X_PRE_FIRST_CHANCE ) {
ExceptNum = buff->Buffer;
if( ExceptNum == XCPT_BREAKPOINT ) {
notify = DBG_N_Breakpoint;
value = XCPT_CONTINUE_STOP;
break;
} else if( ExceptNum == XCPT_SINGLE_STEP ) {
notify = DBG_N_SStep;
value = XCPT_CONTINUE_STOP;
break;
}
}
//
// NOTE: Going to second chance causes OS/2 to report the
// exception in the debugee. However, if you report
// the fault at the first chance notification, the
// debugee's own fault handlers will not get invoked!
//
if( buff->Value == DBG_X_FIRST_CHANCE && !ExpectingAFault ) {
if( stopOnSecond && !got_second_notification ) {
value = XCPT_CONTINUE_SEARCH;
break;
}
}
notify = DBG_N_Exception;
value = XCPT_CONTINUE_STOP;
/*
* Buffer contains the ptr to the exception block
*/
buff->Cmd = DBG_C_ReadMemBuf;
buff->Addr = buff->Buffer;
buff->Buffer = (ULONG)&ex;
buff->Len = sizeof( ex );
CallDosDebug( buff );
ExceptNum = ex.ExceptionNum;
if( ExceptNum == XCPT_PROCESS_TERMINATE ||
ExceptNum == XCPT_ASYNC_PROCESS_TERMINATE ||
ExceptNum == XCPT_GUARD_PAGE_VIOLATION ||
( ExceptNum & XCPT_CUSTOMER_CODE ) ) {
value = XCPT_CONTINUE_SEARCH;
break;
}
/*
* get the context record
*/
buff->Cmd = DBG_C_ReadMemBuf;
buff->Addr = fcp;
buff->Buffer = (ULONG)&fcr;
buff->Len = sizeof( fcr );
CallDosDebug( buff );
buff->EAX = fcr.ctx_RegEax;
buff->EBX = fcr.ctx_RegEbx;
buff->ECX = fcr.ctx_RegEcx;
buff->EDX = fcr.ctx_RegEdx;
buff->ESI = fcr.ctx_RegEsi;
buff->EDI = fcr.ctx_RegEdi;
buff->ESP = fcr.ctx_RegEsp;
buff->EBP = fcr.ctx_RegEbp;
buff->DS = fcr.ctx_SegDs;
buff->CS = fcr.ctx_SegCs;
buff->ES = fcr.ctx_SegEs;
buff->FS = fcr.ctx_SegFs;
buff->GS = fcr.ctx_SegGs;
buff->SS = fcr.ctx_SegSs;
buff->EIP = fcr.ctx_RegEip;
buff->EFlags = fcr.ctx_EFlags;
WriteRegs(buff);
if( ExpectingAFault || got_second_notification ) {
break;
}
if( stopOnSecond ) {
value = XCPT_CONTINUE_EXECUTION;
got_second_notification = TRUE;
}
break;
default:
if( notify != 0 ) {
buff->Cmd = notify;
// Check if we hit our splice DLL breakpoint
if( (notify == DBG_N_Breakpoint) && splice_bp_set && (splice_bp_lin_addr == ExceptLinear) ) {
uDB_t save;
// Remove breakpoint
WriteLinear( &saved_splice_bp, splice_bp_lin_addr, sizeof( byte ) );
splice_bp_set = FALSE;
splice_bp_lin_addr = 0;
// Attempt to load helper DLL
save.Pid = Pid;
save.Tid = 1;
ReadRegs( &save );
ExpectingAFault = TRUE;
// NB - the following will recursively call DebugExecute!
if( !CausePgmToLoadHelperDLL( ExceptLinear ) ) {
CanExecTask = FALSE;
} else {
CanExecTask = TRUE;
}
WriteRegs( &save );
break;
}
}
return( FALSE );
}
}
// return( FALSE );
}