/** Main loop. */
static void
main_loop (void)
{
main_timer[3] = timer_read ();
/* Compute absolute position. */
aux_pos_update ();
main_timer[4] = timer_read ();
/* Compute trajectory. */
aux_traj_update ();
/* Prepare control system. */
cs_update_prepare ();
main_timer[5] = timer_read ();
/* Wait for next cycle. */
timer_wait ();
/* Encoder update. */
encoder_update ();
main_timer[0] = timer_read ();
/* Control system update. */
cs_update ();
main_timer[1] = timer_read ();
/* Pwm setup. */
output_update ();
main_timer[2] = timer_read ();
/* Sequences. */
seq_update (&seq_aux[0], &cs_aux[0].state);
seq_update (&seq_aux[1], &cs_aux[1].state);
/* Stats. */
if (main_sequence_ack
&& (seq_aux[0].ack != seq_aux[0].finish
|| seq_aux[1].ack != seq_aux[1].finish)
&& !--main_sequence_ack_cpt)
{
//XXX here
proto_send2b ('A', seq_aux[0].finish, seq_aux[1].finish);
main_sequence_ack_cpt = main_sequence_ack;
}
if (main_stat_counter && !--main_stat_counter_cpt)
{
proto_send2w ('C', encoder_aux[0].cur, encoder_aux[1].cur);
main_stat_counter_cpt = main_stat_counter;
}
if (main_stat_aux_pos && !--main_stat_aux_pos_cpt)
{
proto_send2w ('Y', aux[0].pos, aux[1].pos);
main_stat_aux_pos_cpt = main_stat_aux_pos;
}
if (main_stat_speed && !--main_stat_speed_cpt)
{
proto_send2w ('S', cs_aux[0].speed.cur_f >> 8,
cs_aux[1].speed.cur_f >> 8);
main_stat_speed_cpt = main_stat_speed;
}
void c_seq::remove(wisdom_IOStream& io)
{
SEQ_INIT();
if (!is_clean())
return;
c_wlock lock(&m_lock);
uint32 del_count = 0;
uint64_t ms = get_ms();
uint32 last = m_seq_head.g_cleantime();
uint32 count = m_seq_head.g_count();
uint32 ibegin = seq_begin_index();
leveldb::WriteOptions op;
leveldb::WriteBatch bh;
while (true)
{
string value;
_zmsg_head* head = NULL;
if (c_base::get_value(__tos(m_key << "@" << ibegin), value, &head) != 0)
{
break;
}
if (head->type != T_SEQ_VALUE)
{
LOG4_ERROR("SEQ KEY:" << __tos(m_key << "@" << ibegin) << " error.");
break;
}
uint32 idelay = m_seq_head.g_delay() == 0 ? c_server::get_instance()->seq_valid_time() : m_seq_head.g_delay();
if (head->g_effective() + idelay > time(0))
{
break;
}
seq_del(ibegin, bh);
del_count++;
ibegin++;
}
m_seq_head.s_cleantime(time(0));
seq_update(bh);
if (!c_base::write(op, bh))
{
m_seq_head.s_count(count);
ZERROR_RESULT("seq write error.");
}
io->push(ZRESULT_OK);
LOG4_INFO("SEQ " << m_key << " clean record:" << del_count
<< " last min " << ((time(0) - last) / 60)
<< " cost " << (uint32)(get_ms() - ms) << " ms");
}
void c_seq::delay(uint32 idelay, wisdom_IOStream& io)
{
SEQ_INIT();
c_wlock lock(&m_lock);
uint32 delay_ = m_seq_head.g_delay();
m_seq_head.s_delay(idelay);
leveldb::WriteOptions op;
leveldb::WriteBatch bh;
seq_update(bh);
if (!c_base::write(op, bh))
{
m_seq_head.s_delay(delay_);
ZERROR_RESULT("seq write error.");
}
io->push(ZRESULT_OK);
}
void c_seq::push(char* data, int n, wisdom_IOStream& io, uint32 idelay)
{
SEQ_INIT();
c_wlock lock(&m_lock);
leveldb::WriteOptions op;
leveldb::WriteBatch bh;
uint32 count = m_seq_head.g_count();
uint32 index = m_seq_head.g_index();
uint32 uptime = m_seq_head.g_uptime();
uint32 delay = m_seq_head.g_delay();
leveldb::Slice value(data, n);
seq_set(bh, value);
if (idelay > 0)
m_seq_head.s_delay(idelay);
seq_update(bh);
if (c_base::write(op, bh))
{
io->push(ZRESULT_OK);
io->push(itostr(m_seq_head.g_index()));
}
else
{
m_seq_head.s_count(count);
m_seq_head.s_index(index);
m_seq_head.s_uptime(uptime);
m_seq_head.s_delay(delay);
io->push(ZRESULT_ERROR);
io->push("write error.");
}
}