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); } } }