string removeKdigits(string num, int k) {
while (k -- > 0) {
std::size_t first_peak_pos = FindFirstPeak(num);
num.erase(first_peak_pos, 1);
}
std::size_t first_non_zero = num.find_first_not_of("0");
if (first_non_zero == std::string::npos) {
return "0";
}
return num.substr(first_non_zero);
}
static void mma8451_getdata(void)
{
byte Data;
LDD_TError ret;
byte Color[192];
signed short accel_x, accel_y, accel_z=0;
int i,j=0;
uint_8 sign_bit=0;
uint_8 angle=0;
MQX_TICK_STRUCT ttt;
// Determine source of interrupt by reading the system interrupt
ret = ReadAccRegs(g_I2C_DeviceData,
&g_DataState,
INT_SOURCE_REG,
ACC_REG_SIZE,
&Data);
if (ret)
{
APP_TRACE("interrupt source: 0x%X\r\n", Data);
// Set up Case statement here to service all of the possible interrupts
if ((Data &0x04) == 0x04) //detection interrupt
{
APP_TRACE("get detect interrupt\r\n");
//Perform an Action since Motion Flag has been set
//Read the Motion/Freefall Function to clear the interrupt
ret = ReadAccRegs(g_I2C_DeviceData,
&g_DataState,
FF_MT_SRC_REG,
ACC_REG_SIZE,
&Data);
enablemma8451running();
}
if ((Data &SRC_FIFO_MASK) == 0x40) // FIFO Data interrupt
{
ret = ReadAccRegs(g_I2C_DeviceData, &g_DataState, OUT_X_MSB, 192 * ACC_REG_SIZE, Color); // Read x,y,z acceleration data.
for(i=0;i<32;i++)
{
j=6*i;
accel_x = Color[j+1] | (Color[j+0]<<8);
accel_x >>= 2;
accel_y = Color[j+3] | (Color[j+2]<<8);
accel_y >>= 2;
accel_z = Color[j+5] | (Color[j+4]<<8);
accel_z >>= 2;
if(accel_z>=0)
sign_bit=1;
else
sign_bit=0;
angle=angle_calculation(accel_z,sign_bit);
//put status in still mode or not?
if(angle<15&&(abs(angle-z_angle_tmp))<1)
{
SetAccStatus(ACC_STILL);
}
z_angle_tmp=angle;
///////////end sti
Angle_1s[buffer_count]=angle;
buffer_count++;
if(buffer_count==buffer_size)
{
buffer_count=0;
_time_get_elapsed_ticks(&ttt);
// APP_TRACE("high tick %d, low first st%d\r\n", ttt.TICKS[1],ttt.TICKS[0]);
FindFirstPeak();
_time_get_elapsed_ticks(&ttt);
// APP_TRACE("high tick %d, low last end%d\r\n", ttt.TICKS[1],ttt.TICKS[0]);
FindLastPeak();
_time_get_elapsed_ticks(&ttt);
// APP_TRACE("high tick %d, low last end%d\r\n", ttt.TICKS[1],ttt.TICKS[0]);
}
// APP_TRACE("tilt %d,y=%05d,z=%05d\r\n", angle, accel_y, accel_z);
}
}
}
