uint8_t _temp_read(const temp_def *def)
{
uint8_t val = 0;
_temp_pin_out(def);
for (uint8_t i = 0; i < 8; i++)
{
_temp_pin_down(def);
sleep_us(2); // Pull low for minimum 1 us
val >>= 1;
_temp_pin_up(def);
_temp_pin_in(def);
sleep_us(2); // Sample within 15 us
if (_temp_check(def))
{
val |= 0x80;
}
sleep_us(60); // Cycle lasts at least 60 us
_temp_pin_out(def);
}
return val;
}
/**
* \brief Forks a separate simple milisecond-sleep() -&- sync periodic timer
*
* Forks a very basic periodic timer process, that just ms-sleep()s for
* the specified interval and then calls the timer function.
* The new "sync timer" process execution start immediately, the ms-sleep()
* is called first (so the first call to the timer function will happen
* \<interval\> seconds after the call to fork_basic_utimer)
* @param child_id @see fork_process()
* @param desc @see fork_process()
* @param make_sock @see fork_process()
* @param f timer function/callback
* @param param parameter passed to the timer function
* @param uinterval interval in mili-seconds.
* @return pid of the new process on success, -1 on error
* (doesn't return anything in the child process)
*/
int fork_sync_utimer(int child_id, char* desc, int make_sock,
utimer_function* f, void* param, int uinterval)
{
int pid;
ticks_t ts1 = 0;
ticks_t ts2 = 0;
pid=fork_process(child_id, desc, make_sock);
if (pid<0) return -1;
if (pid==0){
/* child */
ts2 = uinterval;
if (cfg_child_init()) return -1;
for(;;){
if(ts2>0) sleep_us(uinterval);
else sleep_us(1);
ts1 = get_ticks_raw();
cfg_update();
f(TICKS_TO_MS(ts1), param); /* ticks in mili-seconds */
ts2 = uinterval - get_ticks_raw() + ts1;
}
}
/* parent */
return pid;
}
void _temp_write(const temp_def *def, uint8_t data)
{
for (uint8_t i = 0; i < 8; i++)
{
_temp_pin_down(def);
sleep_us(2); // Pull low for minimum 1 us
if (data & 0x01)
{
_temp_pin_up(def);
}
else
{
_temp_pin_down(def);
}
data >>= 1;
sleep_us(60); // Cycle lasts at least 60 us
_temp_pin_up(def);
}
_temp_pin_up(def);
}
static inline int reload_permanent_list(struct bl_rule *first,
struct bl_rule *last,
struct bl_head *head)
{
struct bl_rule *p, *q;
/* get list for write */
lock_get( head->lock);
while(head->count_write){
lock_release( head->lock );
sleep_us(5);
lock_get( head->lock );
}
head->count_write = 1;
while(head->count_read){
lock_release( head->lock );
sleep_us(5);
lock_get( head->lock );
}
lock_release( head->lock );
for(p = head->first ; p ; ){
q = p;
p = p->next;
shm_free(q);
}
head->first = first;
head->last = last;
head->count_write = 0;
return 0;
}
/**
* \brief Forks a separate simple sleep() -&- sync periodic timer
*
* Forks a very basic periodic timer process, that just sleep()s for
* the specified interval and then calls the timer function.
* The new "sync timer" process execution start immediately, the sleep()
* is called first (so the first call to the timer function will happen
* \<interval\> seconds after the call to fork_sync_timer)
* @param child_id @see fork_process()
* @param desc @see fork_process()
* @param make_sock @see fork_process()
* @param f timer function/callback
* @param param parameter passed to the timer function
* @param interval interval in seconds.
* @return pid of the new process on success, -1 on error
* (doesn't return anything in the child process)
*/
int fork_sync_timer(int child_id, char* desc, int make_sock,
timer_function* f, void* param, int interval)
{
int pid;
ticks_t ts1 = 0;
ticks_t ts2 = 0;
pid=fork_process(child_id, desc, make_sock);
if (pid<0) return -1;
if (pid==0){
/* child */
interval *= 1000; /* miliseconds */
ts2 = interval;
if (cfg_child_init()) return -1;
for(;;){
if (ts2>interval)
sleep_us(1000); /* 1 milisecond sleep to catch up */
else
sleep_us(ts2*1000); /* microseconds sleep */
ts1 = get_ticks_raw();
cfg_update();
f(TICKS_TO_S(ts1), param); /* ticks in sec for compatibility with old
timers */
/* adjust the next sleep duration */
ts2 = interval - TICKS_TO_MS(get_ticks_raw()) + TICKS_TO_MS(ts1);
}
}
/* parent */
return pid;
}
static inline void delete_expired(struct bl_head *elem, unsigned int ticks)
{
struct bl_rule *p, *q;
p = q = 0;
/* get list for write */
lock_get(elem->lock);
while(elem->count_write){
lock_release(elem->lock);
sleep_us(5);
lock_get(elem->lock);
}
elem->count_write = 1;
while(elem->count_read){
lock_release(elem->lock);
sleep_us(5);
lock_get(elem->lock);
}
lock_release(elem->lock);
if(elem->first==NULL)
goto done;
for( q=0,p = elem->first ; p ; q=p,p=p->next) {
if(p->expire_end > ticks)
break;
}
if (q==NULL)
/* nothing to remove */
goto done;
if (p==NULL) {
/* remove everything */
q = elem->first;
elem->first = elem->last = NULL;
} else {
/* remove up to p */
q->next = 0;
q = elem->first;
elem->first = p;
}
done:
elem->count_write = 0;
for( ; q ; ){
p = q;
q = q->next;
shm_free(p);
}
return;
}
void c_client::client_run_state()
{
uchar message[UDP1_BUFFER_LONG];
ushort *num_cmd = (ushort *)&message[2];
int num_cmd_sent = 0, num_cmd_in = 0, num_cmd_err = 0;
for (ushort i = 0; i < CMD_SENT; i++)
{
if (i % 8 == 0)
{
if ((i / 8) % 2 == 1)
memcpy(message, message1_long, UDP1_BUFFER_LONG);
else
memcpy(message, message2_long, UDP1_BUFFER_LONG);
*num_cmd = i;
send_block((char *)message, UDP1_BUFFER_LONG);
num_cmd_sent++;
sleep_us(10000);
}
else
{
if (i % 2 == 1)
memcpy(message, message1_short, 100);
else
memcpy(message, message2_short, 100);
*num_cmd = i;
send_block((char *)message, 100);
num_cmd_sent++;
sleep_us(2000);
}
cout_mtx.lock();
cout << "client out cmd:" << (int) message[0] << " cmd_num=" << *num_cmd << endl;
cout_mtx.unlock();
int i1 = receive_block((char *)message);
if (i1 > 0)
{
num_cmd_in++;
cout_mtx.lock();
cout << "client in cmd:" << (int)message[0] << " cmd_num=" << *num_cmd << endl;
if ((i1 != 100) && (i1 != UDP1_BUFFER_LONG))
{
cout << "====== ERROR c_client received command length=" << i1 << endl;
num_cmd_err++;
}
cout_mtx.unlock();
}
}
cout_mtx.lock();
cout << "===== END c_client::server_run_state() =====" << endl;
cout << "Client sent commands=" << num_cmd_sent << " received=" << num_cmd_in << " wrong commands=" << num_cmd_err << endl;
cout_mtx.unlock();
}
int async_task_run(int idx)
{
async_task_t *ptask;
int received;
LM_DBG("async task worker %d ready\n", idx);
for( ; ; ) {
if(unlikely(_async_task_usleep)) sleep_us(_async_task_usleep);
if ((received = recvfrom(_async_task_sockets[0],
&ptask, sizeof(async_task_t*),
0, NULL, 0)) < 0) {
LM_ERR("failed to received task (%d: %s)\n", errno, strerror(errno));
continue;
}
if(received != sizeof(async_task_t*)) {
LM_ERR("invalid task size %d\n", received);
continue;
}
if(ptask->exec!=NULL) {
LM_DBG("task executed [%p] (%p/%p)\n", ptask,
ptask->exec, ptask->param);
ptask->exec(ptask->param);
}
shm_free(ptask);
}
return 0;
}
static inline struct mi_root* wait_async_reply(struct mi_handler *hdl)
{
struct mi_root *mi_rpl;
int i;
int x;
for( i=0 ; i<MAX_XMLRPC_WAIT ; i++ ) {
if (hdl->param)
break;
sleep_us(1000*500);
}
if (i==MAX_XMLRPC_WAIT) {
/* no more waiting ....*/
lock_get(xr_lock);
if (hdl->param==NULL) {
hdl->param = XMLRPC_ASYNC_EXPIRED;
x = 0;
} else {
x = 1;
}
lock_release(xr_lock);
if (x==0) {
LM_INFO("exiting before receiving reply\n");
return NULL;
}
}
mi_rpl = (struct mi_root *)hdl->param;
if (mi_rpl==XMLRPC_ASYNC_FAILED)
mi_rpl = NULL;
free_async_handler(hdl);
return mi_rpl;
}
/*
* Initialize all loaded modules, the initialization
* is done *AFTER* the configuration file is parsed
*/
int init_modules(void)
{
struct sr_module* t;
if(async_task_init()<0)
return -1;
for(t = modules; t; t = t->next) {
if (t->exports.init_f) {
if (t->exports.init_f() != 0) {
LM_ERR("Error while initializing module %s\n", t->exports.name);
return -1;
}
/* delay next module init, if configured */
if(unlikely(modinit_delay>0))
sleep_us(modinit_delay);
}
if (t->exports.response_f)
mod_response_cbk_no++;
}
mod_response_cbks=pkg_malloc(mod_response_cbk_no *
sizeof(response_function));
if (mod_response_cbks==0){
LM_ERR("memory allocation failure for %d response_f callbacks\n",
mod_response_cbk_no);
return -1;
}
for (t=modules, i=0; t && (i<mod_response_cbk_no); t=t->next) {
if (t->exports.response_f) {
mod_response_cbks[i]=t->exports.response_f;
i++;
}
}
return 0;
}
int write_to_fifo(const string& fifo, const char * buf, unsigned int len)
{
int fd_fifo;
int retry = SER_WRITE_TIMEOUT / SER_WRITE_INTERVAL;
for(; retry>0; retry--) {
if((fd_fifo = open(fifo.c_str(),
O_WRONLY | O_NONBLOCK)) == -1) {
ERROR("while opening %s: %s\n",
fifo.c_str(),strerror(errno));
if(retry)
sleep_us(50000);
}
else {
break;
}
}
if(!retry)
return -1;
DBG("write_to_fifo: <%s>\n",buf);
int l = write(fd_fifo,buf,len);
close(fd_fifo);
if(l==-1)
ERROR("while writing: %s\n",strerror(errno));
else
DBG("Write to fifo: completed\n");
return l;
}
static void calib(int max_cnt, s16 *avg_data) {
int cnt = 0;
int ax = 0, ay = 0, az = 0, gx = 0, gy = 0, gz = 0;
while(1){
sleep_us(10*1000);
static s16 acc_x, acc_y, acc_z, gy_x, gy_y, gy_z;
if(mouse_sensor_getdata_no_fifo(&acc_x, &acc_y, &acc_z, &gy_x, &gy_y, &gy_z)){
ax += acc_x;
ay += acc_y;
az += acc_z;
gx += gy_x;
gy += gy_y;
gz += gy_z;
++cnt;
if(cnt >= max_cnt){
break;
}
}
}
avg_data[0] = gx / max_cnt;
avg_data[1] = gy / max_cnt;
avg_data[2] = gz / max_cnt;
avg_data[3] = ax / max_cnt;
avg_data[4] = ay / max_cnt;
avg_data[5] = az / max_cnt;
}
void evrexec_process(evrexec_task_t *it, int idx)
{
sip_msg_t *fmsg;
sr_kemi_eng_t *keng = NULL;
str sidx = STR_NULL;
if(it!=NULL) {
fmsg = faked_msg_next();
set_route_type(LOCAL_ROUTE);
if(it->wait>0) sleep_us(it->wait);
keng = sr_kemi_eng_get();
if(keng==NULL) {
if(it->rtid>=0 && event_rt.rlist[it->rtid]!=NULL) {
run_top_route(event_rt.rlist[it->rtid], fmsg, 0);
} else {
LM_WARN("empty event route block [%.*s]\n",
it->ename.len, it->ename.s);
}
} else {
sidx.s = int2str(idx, &sidx.len);
if(sr_kemi_route(keng, fmsg, EVENT_ROUTE,
&it->ename, &sidx)<0) {
LM_ERR("error running event route kemi callback\n");
}
}
}
/* avoid exiting the process */
while(1) { sleep(3600); }
}
void
sink_udp(int sockfd) /* TODO: use recvfrom ?? */
{
int n, flags;
if (pauseinit)
sleep_us(pauseinit*1000);
for ( ; ; ) { /* read until peer closes connection; -n opt ignored */
/* msgpeek = 0 or MSG_PEEK */
flags = msgpeek;
oncemore:
if ( (n = recv(sockfd, rbuf, readlen, flags)) < 0) {
err_sys("recv error");
} else if (n == 0) {
if (verbose)
fprintf(stderr, "connection closed by peer\n");
break;
#ifdef notdef /* following not possible with TCP */
} else if (n != readlen)
err_quit("read returned %d, expected %d", n, readlen);
#else
}
#endif
if (verbose) {
fprintf(stderr, "received %d bytes%s\n", n,
(flags == MSG_PEEK) ? " (MSG_PEEK)" : "");
if (verbose > 1) {
fprintf(stderr, "printing %d bytes\n", n);
rbuf[n] = 0; /* make certain it's null terminated */
fprintf(stderr, "SDAP header: %lx\n", *((long *) rbuf));
fprintf(stderr, "next long: %lx\n", *((long *) rbuf+4));
fputs(&rbuf[8], stderr);
}
}
if (pauserw)
sleep_us(pauserw*1000);
if (flags != 0) {
flags = 0; /* avoid infinite loop */
goto oncemore; /* read the message again */
}
}
static int m_usleep(struct sip_msg *msg, int *useconds)
{
LM_DBG("sleep %d\n", *(unsigned int*)useconds);
sleep_us(*(unsigned int*)useconds);
return 1;
}
void lpc_eep_init(CORE* core)
{
LPC_EEPROM->PWRDWN |= LPC_EEPROM_PWRDWN_Msk;
//EEPROM operates on M3 clock
LPC_EEPROM->CLKDIV = lpc_power_get_core_clock_inside() / EEP_CLK - 1;
sleep_us(100);
LPC_EEPROM->PWRDWN &= ~LPC_EEPROM_PWRDWN_Msk;
}
void _temp_init(const temp_def *def)
{
// Send initialization sequence
_temp_pin_out(def);
_temp_pin_down(def);
sleep_us(500); // Pull low for minimum 480 us
_temp_pin_in(def);
sleep_us(20); // Wait 15 to 60 us
uint32_t presense_start = timer_now();
while (_temp_check(def))
{
// Presense pulse 60 to 240 us
if (timer_now() - presense_start > 3) break;
}
_temp_pin_out(def);
_temp_pin_up(def);
sleep_us(480); // Receive sequence is minimum 480 us
}
void rpc_mtree_match(rpc_t* rpc, void* ctx)
{
str tname = STR_NULL;
str tomatch = STR_NULL;
int mode = -1;
m_tree_t *tr;
if(!mt_defined_trees())
{
rpc->fault(ctx, 500, "Empty tree list.");
return;
}
if (rpc->scan(ctx, ".SSd", &tname, &tomatch, &mode) < 3) {
rpc->fault(ctx, 500, "Invalid Parameters");
return;
}
if (mode !=0 && mode != 2) {
rpc->fault(ctx, 500, "Invalid parameter 'mode'");
return;
}
again:
lock_get( mt_lock );
if (mt_reload_flag) {
lock_release( mt_lock );
sleep_us(5);
goto again;
}
mt_tree_refcnt++;
lock_release( mt_lock );
tr = mt_get_tree(&tname);
if(tr==NULL)
{
/* no tree with such name*/
rpc->fault(ctx, 404, "Not found tree");
goto error;
}
if(mt_rpc_match_prefix(rpc, ctx, tr, &tomatch, mode)<0)
{
LM_DBG("no prefix found in [%.*s] for [%.*s]\n",
tname.len, tname.s,
tomatch.len, tomatch.s);
rpc->fault(ctx, 404, "Not found");
}
error:
lock_get( mt_lock );
mt_tree_refcnt--;
lock_release( mt_lock );
}
static int m_usleep(struct sip_msg *msg, char *time, char *str2)
{
int s;
if(fixup_get_ivalue(msg, (gparam_t*)time, &s)!=0)
{
LM_ERR("cannot get time interval value\n");
return -1;
}
sleep_us((unsigned int)s);
return 1;
}
uint16_t TTP229::ReadForEvent()
{
uint16_t buttonState = 0;
for(uint8_t i = 0; i < keys; ++i) // sleep for some kind of slow data reading
{
digitalWrite(SCLport, SCLpin, LOW);
sleep_us(1);
digitalWrite(SCLport, SCLpin, HIGH);
sleep_us(1);
if(!digitalRead(SDOportPin, SDOpin))
buttonState |= _BV(i);
}
return buttonState;
}
int
main(int argc, char **argv)
{
int fd, i, nloop, nusec;
pid_t pid;
char mesg[MESGSIZE];
long offset;
struct shmstruct *ptr;
if (argc != 4) {
fprintf(stderr, "usage: client2 <name> <#loops> <#usec>");
exit(1);
}
nloop = atoi(argv[2]);
nusec = atoi(argv[3]);
/* 4open and map shared memory that server must create */
fd = shm_open((argv[1]), O_RDWR, FILE_MODE);
ptr = mmap(NULL, sizeof(struct shmstruct), PROT_READ | PROT_WRITE,
MAP_SHARED, fd, 0);
close(fd);
pid = getpid();
for (i = 0; i < nloop; i++) {
sleep_us(nusec);
snprintf(mesg, MESGSIZE, "pid %ld: message %d", (long) pid, i);
/*
Our client follows the basic algorithm for the consumer but instead of calling
sem-wait (nempty),which is where the consumer blocks if there is no room in the
buffer for its message, we call sem-trywait, which will not block. If the value of the
semaphore is 0, an error of EAGAIN is returned. We detect this error and increment the
overflow counter.
*/
if (sem_trywait(&ptr->nempty) == -1) {
if (errno == EAGAIN) {
sem_wait(&ptr->noverflowmutex);
ptr->noverflow++;
sem_post(&ptr->noverflowmutex);
continue;
} else {
fprintf(stderr, "sem_trywait error");
exit(1);
}
}
sem_wait(&ptr->mutex);
offset = ptr->msgoff[ptr->nput];
if (++(ptr->nput) >= NMESG)
ptr->nput = 0; /* circular buffer */
sem_post(&ptr->mutex);
strcpy(&ptr->msgdata[offset], mesg);
sem_post(&ptr->nstored);
}
exit(0);
}
static int m_usleep(struct sip_msg *msg, char *time, char *str2)
{
int s;
if(fixup_get_ivalue(msg, (gparam_t*)time, &s)!=0)
{
LM_ERR("cannot get time interval value\n");
return -1;
}
LM_DBG("sleep %lu microseconds\n", (unsigned long)time);
sleep_us((unsigned int)s);
return 1;
}
void _enc28j60_phyWrite(ENC28J60 *enc28j60, uint8_t address, uint16_t data) {
// set the PHY register address
_enc28j60_writeReg(enc28j60, MIREGADR, address);
// write the PHY data
_enc28j60_writeRegPair(enc28j60, MIWRL, data);
// wait until the PHY write completes
while (_enc28j60_readReg(enc28j60, MISTAT) & MISTAT_BUSY) {
sleep_us(15);
}
}
void
sink_tcp(int sockfd)
{
int n, flags;
if (pauseinit)
sleep_us(pauseinit*1000);
for ( ; ; ) { /* read until peer closes connection; -n opt ignored */
/* msgpeek = 0 or MSG_PEEK */
flags = msgpeek;
oncemore:
if ( (n = recv(sockfd, rbuf, readlen, flags)) < 0) {
err_sys("recv error");
} else if (n == 0) {
if (verbose)
fprintf(stderr, "connection closed by peer\n");
break;
#ifdef notdef /* following not possible with TCP */
} else if (n != readlen)
err_quit("read returned %d, expected %d", n, readlen);
#else
}
#endif
if (verbose)
fprintf(stderr, "received %d bytes%s\n", n,
(flags == MSG_PEEK) ? " (MSG_PEEK)" : "");
if (pauserw)
sleep_us(pauserw*1000);
if (flags != 0) {
flags = 0; /* no infinite loop */
goto oncemore; /* read the message again */
}
}
int main()
{
c_client *client = new c_client(TCPID_SERVER);
c_server *server = new c_server();
//sending the server thread!!!
cout << "Running threads" << endl;
time_t ini, fin;
ini = clock();
thread th_server(&c_server::server_run_state, server);
sleep_us(10000);
thread th_client(&c_client::client_run_state, client);
th_client.join();
sleep_us(2000);//2ms
server->set_end_state(true);
th_server.join();
fin = clock();
cout << "\n============= END =============" << endl;
cout << "Total time=" << 1.0*(fin - ini) / CLOCKS_PER_SEC << " seconds" << endl;
cout << "Expected>=" << CMD_SENT / 8 * 10e-3 + (CMD_SENT - CMD_SENT / 8)*2e-3 << " seconds" << endl;
getchar();
return 1;
}
int resume_async_sleep(int fd, struct sip_msg *msg, void *param)
{
unsigned long now = (unsigned long)
(((unsigned long)-1) & get_uticks());
/* apply a sync correction if (for whatever reasons) the sleep
* did not cover the whole interval so far */
if ( ((unsigned long)param) > (now+UTIMER_TICK) )
sleep_us((unsigned int)((unsigned long)param - now));
close (fd);
async_status = ASYNC_DONE;
return 1;
}
int main (void){
cpu_wakeup_init();
clock_init();
gpio_init();
gpio_write(GPIO_PD7, 0);
gpio_set_output_en(GPIO_PD7, 1);
i2c_init();
wd_stop();
sleep_us(50*1000);
mouse_sensor_no_fifo_init();
sleep_us(1000*1000); // for L3G, must delay enough time
s16 avg_data[6];
calib(64, avg_data);
flash_erase_sector(AIRMOUSE_CALIBRATION_ADDR);
flash_write_page(AIRMOUSE_CALIBRATION_ADDR, 12, avg_data);
while (1);
return 0;
}
int write_to_socket(int sd, const char* to_addr, const char * buf, unsigned int len)
{
int retry = SER_WRITE_TIMEOUT / SER_WRITE_INTERVAL;
int ret=-1;
if(AmConfig::SerSocketName.empty()){
ERROR("config parameter 'ser_socket_name' has not been configured !!!\n");
goto error;
}
struct sockaddr_un ser_addr;
memset (&ser_addr, 0, sizeof (ser_addr));
ser_addr.sun_family = AF_UNIX;
strncpy(ser_addr.sun_path,to_addr,UNIX_PATH_MAX);
DBG("sending: <%.*s>\n",len,buf);
for(;retry>0;retry--){
if( (sendto(sd,buf,len,MSG_DONTWAIT,
(struct sockaddr*)&ser_addr,
sizeof(struct sockaddr_un)) == -1) ) {
if(errno == EAGAIN){
if(retry)
sleep_us(SER_WRITE_INTERVAL);
continue;
}
ERROR("while sending request to %s: %s\n",
ser_addr.sun_path,strerror(errno));
goto error;
}
break;
}
if(!retry){
ERROR("timeout while sending request to %s\n",ser_addr.sun_path);
goto error;
}
DBG("write to unix socket: completed\n");
ret = 0;
error:
// close(sd);
// return (ret == -1 ? ret : len);
return ret;
}
/* use tree tn, match var, by mode, output in avp params */
static int mt_match(sip_msg_t *msg, str *tname, str *tomatch,
int mval)
{
m_tree_t *tr = NULL;
if(msg==NULL) {
LM_ERR("received null msg\n");
return -1;
}
again:
lock_get( mt_lock );
if (mt_reload_flag) {
lock_release( mt_lock );
sleep_us(5);
goto again;
}
mt_tree_refcnt++;
lock_release( mt_lock );
tr = mt_get_tree(tname);
if(tr==NULL) {
/* no tree with such name*/
goto error;
}
if(mt_match_prefix(msg, tr, tomatch, mval)<0)
{
LM_DBG("no prefix found in [%.*s] for [%.*s]\n",
tname->len, tname->s,
tomatch->len, tomatch->s);
goto error;
}
lock_get( mt_lock );
mt_tree_refcnt--;
lock_release( mt_lock );
return 1;
error:
lock_get( mt_lock );
mt_tree_refcnt--;
lock_release( mt_lock );
return -1;
}
u32 mouse_sensor_no_fifo_init(void){
// wait until MPU power ready
static int ok = 0;
for (int i = 0; i < 500; ++i) {
if (i2c_read(MPU6050_I2C_ID, 0x75) == 0x68) {
ok++;
if (ok++ > 3) {
break;
}
}
sleep_us (1000);
}
int len = sizeof (tbl_sensor_no_fifo_init);
for (int j=0; j<len; j+=2) {
i2c_write(MPU6050_I2C_ID, tbl_sensor_no_fifo_init[j], tbl_sensor_no_fifo_init[j+1]);
}
return 1;
}
