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