void mbb_stat_feed(struct mbb_stat_pool *pool, struct mbb_stat_entry *entry) { struct stat_rec rec; union key key; rec.nbyte_in = entry->nbyte_in; rec.nbyte_out = entry->nbyte_out; key.point = mbb_stat_extract_hour(entry->point); key.skey.id = entry->unit_id; rec_update(pool->ustat, &key, sizeof(struct single_key), &rec); key.skey.id = entry->link_id; rec_update(pool->lstat, &key, sizeof(struct single_key), &rec); key.dkey.aid = entry->unit_id; key.dkey.bid = entry->link_id; rec_update(pool->ulstat, &key, sizeof(struct double_key), &rec); }
void controller_stable( void *ptr ) { unsigned long c = 0; float m_fl,m_bl,m_fr,m_br; //motor FL, BL, FR, BR float loop_ms = 1000.0f/GYRO_RATE; float loop_s = loop_ms/1000.0f; while (ms_update()!=0);//empty MPU for (int i=0;i<3;i++) { pid_setmode(&config.pid_r[i],1); pid_setmode(&config.pid_s[i],1); } flush(); t=rt_timer_read(); while(1) { ms_err = ms_update(); if (ms_err==0) continue; //dont do anything if gyro has no new data; depends on gyro RATE if (ms_err<0) { //something wrong with gyro: i2c issue or fifo full! ms.ypr[0]=ms.ypr[1]=ms.ypr[2] = 0.0f; ms.gyro[0]=ms.gyro[1]=ms.gyro[2] = 0.0f; } c++; //our counter so we know when to read barometer (c increases every 10ms) //bs_err = bs_update(c*loop_ms); rec_err = rec_update(); pre_flight(); handle_issues(); autoflight(); if (armed && rec.yprt[3]>(config.esc_min+10)) { inflight = 1; } if (rec.yprt[3]<=(config.esc_min+10)) { inflight = 0; sc_update(MOTOR_FL,config.esc_min); sc_update(MOTOR_BL,config.esc_min); sc_update(MOTOR_FR,config.esc_min); sc_update(MOTOR_BR,config.esc_min); yaw_target = ms.ypr[0]; bs.p0 = bs.p; } if (rec.yprt[3]<(config.esc_min+50)) { //use integral part only if there is some throttle for (int i=0;i<3;i++) { config.pid_r[i]._KiTerm = 0.0f; config.pid_s[i]._KiTerm = 0.0f; } } do_adjustments(); //our quad can rotate 360 degree if commanded, it is ok!; learned it the hard way! if (yaw_target-ms.ypr[0]<-180.0f) yaw_target*=-1; if (yaw_target-ms.ypr[0]>180.0f) yaw_target*=-1; //do STAB PID for (int i=0;i<3;i++) { if (i==0) //keep yaw_target pid_update(&config.pid_s[i],yaw_target,ms.ypr[i],loop_s); else pid_update(&config.pid_s[i],rec.yprt[i]+config.trim[i],ms.ypr[i],loop_s); } //yaw requests will be fed directly to rate pid if (abs(rec.yprt[0])>7.5f) { config.pid_s[0].value = rec.yprt[0]; yaw_target = ms.ypr[0]; } if (mode == 1) { //yaw setup config.pid_s[0].value = 0.0f; config.pid_s[1].value = ms.gyro[1]; config.pid_s[2].value = ms.gyro[2]; } else if (mode == 2) { //pitch setup config.pid_s[0].value = ms.gyro[0]; config.pid_s[1].value = 0.0f; config.pid_s[2].value = ms.gyro[2]; } else if (mode == 3) { //roll setup config.pid_s[0].value = ms.gyro[0]; config.pid_s[1].value = ms.gyro[1]; config.pid_s[2].value = 0.0f; } //do RATE PID for (int i=0;i<3;i++) { pid_update(&config.pid_r[i],config.pid_s[i].value,ms.gyro[i],loop_s); } //calculate motor speeds m_fl = rec.yprt[3]-config.pid_r[2].value-config.pid_r[1].value+config.pid_r[0].value; m_bl = rec.yprt[3]-config.pid_r[2].value+config.pid_r[1].value-config.pid_r[0].value; m_fr = rec.yprt[3]+config.pid_r[2].value-config.pid_r[1].value-config.pid_r[0].value; m_br = rec.yprt[3]+config.pid_r[2].value+config.pid_r[1].value+config.pid_r[0].value; log(); if (inflight) { sc_update(MOTOR_FL,m_fl); sc_update(MOTOR_BL,m_bl); sc_update(MOTOR_FR,m_fr); sc_update(MOTOR_BR,m_br); } } }