void mpu6050Cal(int16_t *gOfSet, int16_t *aOfSet, uint8_t ofCount){
int16_t data[3];
int i = ofCount;
memset(gOfSet, 0, 3);
memset(aOfSet, 0, 3);
while(i--){
sleep(50);
mpu6050GetGYRO(data);
memadd(gOfSet, data, 3);
mpu6050GetACCEL(data);
memadd(aOfSet, data, 3);
}
memdev(gOfSet, ofCount, 3);
memdev(aOfSet, ofCount, 3);
}
void aa_make(BYTE *data, int w, int h, int bytes_per_line) {
int y;
BYTE *b, *_b, *__b;
BYTE *d = data;
b = g_new0(BYTE, w*h);
_b = g_new0(BYTE, (w+2)*(h+2));
__b = b;
for (y = 0; y < h; y++) {
memcpy(__b, d, w);
__b+=w; d+=bytes_per_line;
}
memadd(b, _b, w, h);
memmul(_b, 16, (w+2)*(h+2));
d = data;
__b = _b;
for (y = 0; y < h+2; y++) {
memcpy(d, __b, w+2);
__b += (w+2); d+=bytes_per_line;
}
g_free(b);
g_free(_b);
}
