// Read the gyroscope data
void FXAS21002C::readGyroData()
{
uint8_t rawData[6]; // x/y/z gyro register data stored here
readRegs(FXAS21002C_H_OUT_X_MSB, 6, &rawData[0]); // Read the six raw data registers into data array
gyroData.x = ((int16_t)( rawData[0] << 8 | rawData[1])) >> 2;
gyroData.y = ((int16_t)( rawData[2] << 8 | rawData[3])) >> 2;
gyroData.z = ((int16_t)( rawData[4] << 8 | rawData[5])) >> 2;
}
void FXOS8700Q::disable(void) const
{
uint8_t data[2];
readRegs(FXOS8700Q_CTRL_REG1, &data[1], 1);
data[1] &= 0xFE;
data[0] = FXOS8700Q_CTRL_REG1;
writeRegs(data, sizeof(data));
}
// Read the magnometer data
void FXOS8700CQ::readMagData()
{
uint8_t rawData[6]; // x/y/z accel register data stored here
readRegs(FXOS8700CQ_M_OUT_X_MSB, 6, &rawData[0]); // Read the six raw data registers into data array
magData.x = ((int16_t) rawData[0] << 8 | rawData[1]) >> 2;
magData.y = ((int16_t) rawData[2] << 8 | rawData[3]) >> 2;
magData.z = ((int16_t) rawData[4] << 8 | rawData[5]) >> 2;
}
void MCP79410_getDate (MCP79410_t *rtc, RTC_DateTime_t *dateTime)
{
readRegs(rtc->twi, MCP79410_TIME_ADDR, 7);
uint8_t *ptr = regsBuffer+MCP79410_TIME_ADDR;
dateTime->seconds_bcd = (*ptr++) & 0x7F;
dateTime->minutes_bcd = (*ptr++) & 0x7F;
dateTime->hours_bcd = (*ptr++) & 0x3F;
dateTime->weekday = (*ptr++) & 0x07;
dateTime->day_bcd = (*ptr++) & 0x3F;
dateTime->month_bcd = (*ptr++) & 0x1F;
dateTime->year_bcd = (*ptr);
}
static void MCP79410_setAlarm (MCP79410_t *rtc, uint8_t baseAddr, RTC_DateTime_t *dateTime, uint8_t alarmMask)
{
readRegs(rtc->twi, baseAddr, 6);
uint8_t *ptr = regsBuffer+baseAddr;
*ptr = dateTime->seconds_bcd & 0x7F;
ptr ++;
*ptr++ = dateTime->minutes_bcd & 0x7F;
*ptr = (*ptr & 0x40) | (dateTime->hours_bcd & 0x3F);
ptr ++;
*ptr++ = (alarmMask & 0xF4) | (dateTime->weekday & 0x07);
*ptr++ = dateTime->day_bcd & 0x3F;
*ptr++ = dateTime->month_bcd & 0x1F;
writeRegs(rtc->twi, baseAddr, 6);
}
CompassVec Compass::getMeasurements()
{
byte buf[6];
//writeReg(2, 1);
//delay(10);
readRegs(COMPASS_REG_DATA_OUT_X, 6, buf);
float x, y, z;
x = (float)makeI16(buf[0], buf[1]);
z = (float)makeI16(buf[2], buf[3]);
y = (float)makeI16(buf[4], buf[5]);
CompassVec v(x, y, z);
return applyCalibration(v);
}
void MCP79410_setDate (MCP79410_t *rtc, RTC_DateTime_t *dateTime)
{
readRegs(rtc->twi, MCP79410_TIME_ADDR, 7);
uint8_t *ptr = regsBuffer+MCP79410_TIME_ADDR;
*ptr = (*ptr & 0x80) | (dateTime->seconds_bcd & 0x7F);
ptr ++;
*ptr++ = dateTime->minutes_bcd & 0x7F;
*ptr = (*ptr & 0x40) | (dateTime->hours_bcd & 0x3F);
ptr ++;
*ptr = (*ptr & 0xF4) | (dateTime->weekday & 0x07);
ptr ++;
*ptr++ = dateTime->day_bcd & 0x3F;
*ptr = (*ptr & 0xE0) | (dateTime->month_bcd & 0x1F);
ptr ++;
*ptr = dateTime->year_bcd;
writeRegs(rtc->twi, MCP79410_TIME_ADDR, 7);
}
void FXAS21002C::calibrate(float * gBias)
{
int32_t gyro_bias[3] = {0, 0, 0};
uint16_t ii, fcount;
int16_t temp[3];
// Clear all interrupts by reading the data output and STATUS registers
//readGyroData(temp);
readReg(FXAS21002C_H_STATUS);
standby(); // Must be in standby to change registers
writeReg(FXAS21002C_H_CTRL_REG1, 0x08); // select 50 Hz ODR
fcount = 50; // sample for 1 second
writeReg(FXAS21002C_H_CTRL_REG0, 0x03); // select 200 deg/s full scale
uint16_t gyrosensitivity = 41; // 40.96 LSB/deg/s
active(); // Set to active to start collecting data
uint8_t rawData[6]; // x/y/z FIFO accel data stored here
for(ii = 0; ii < fcount; ii++) // construct count sums for each axis
{
readRegs(FXAS21002C_H_OUT_X_MSB, 6, &rawData[0]); // Read the FIFO data registers into data array
temp[0] = ((int16_t)( rawData[0] << 8 | rawData[1])) >> 2;
temp[1] = ((int16_t)( rawData[2] << 8 | rawData[3])) >> 2;
temp[2] = ((int16_t)( rawData[4] << 8 | rawData[5])) >> 2;
gyro_bias[0] += (int32_t) temp[0];
gyro_bias[1] += (int32_t) temp[1];
gyro_bias[2] += (int32_t) temp[2];
delay(25); // wait for next data sample at 50 Hz rate
}
gyro_bias[0] /= (int32_t) fcount; // get average values
gyro_bias[1] /= (int32_t) fcount;
gyro_bias[2] /= (int32_t) fcount;
gBias[0] = (float)gyro_bias[0]/(float) gyrosensitivity; // get average values
gBias[1] = (float)gyro_bias[1]/(float) gyrosensitivity; // get average values
gBias[2] = (float)gyro_bias[2]/(float) gyrosensitivity; // get average values
ready(); // Set to ready
}
int run_debugger(pid_t pid)
{
pid_t r_pid;
int status;
REGS regs;
procmsg("[+] debugger start...\n");
printf("[+] start to attach process pid %d\n",pid);
/* attach process */
if( -1 == attach_pid(pid))
{
printf("[-] fail to attach process pid %d\n",pid);
return -1;
}
printf("[+] success to attach process pid %d\n",pid);
if(-1 == (r_pid = waitpid(pid,&status,__WALL))){
perror("waitpid error");
return -1;
}
if(WIFSTOPPED(status))
{
printf("[+] ****** attaching to process pid %d\n",r_pid);
/* read tracee regs info */
readRegs(pid, ®s);
printf("press any key to detach:");
getchar();
printf("[+] start to detach process pid %d\n",pid);
/* detach process */
if( -1 == detach_pid(pid))
{
printf("[-] fail to detach process pid %d\n",pid);
return -1;
}
printf("[+] success to detach process pid %d\n",pid);
}
return 0;
}
int16_t FXOS8700Q::getSensorAxis(uint8_t addr) const
{
uint8_t res[2];
readRegs(addr, res, sizeof(res));
return static_cast<int16_t>((res[0] << 8) | res[1]);
}
uint8_t FXOS8700Q::whoAmI() const
{
uint8_t who_am_i = 0;
readRegs(FXOS8700Q_WHOAMI, &who_am_i, sizeof(who_am_i));
return who_am_i;
}
uint32_t FXOS8700Q::dataReady(void) const
{
uint8_t stat = 0;
readRegs(FXOS8700Q_STATUS, &stat, 1);
return (uint32_t)stat;
}
