File content as of revision 4:bf278ddb8504:

#ifndef _IDLE_CHECK_H_
#define _IDLE_CHECK_H_

#define IDLE_CHECK_CNT  1400 //loop(2.5ms) * IDLE_CHECK_CNT = idle check time

unsigned int ex_cmd_roll=0;
unsigned int ex_cmd_pitch=0; 
unsigned int ex_cmd_heave=0; 
unsigned int ex_cmd_sway=0; 
unsigned int ex_cmd_surge=0; 
unsigned int ex_cmd_yaw=0;

bool idle_state = false;
int idle_cnt = 0;

void idle_check(unsigned int roll, unsigned int pitch, unsigned int heave, unsigned int sway, unsigned int surge, unsigned int yaw)
{
    
    if( roll    ==  ex_cmd_roll     &&
        pitch   ==  ex_cmd_pitch    &&
        heave   ==  ex_cmd_heave    &&
        sway    ==  ex_cmd_sway     &&
        surge   ==  ex_cmd_surge    &&
        yaw     ==  ex_cmd_yaw )
        {
            idle_cnt++;
        }else
        {
            idle_cnt = idle_cnt - 50;
        }
        
        if(idle_cnt > IDLE_CHECK_CNT)
            idle_cnt = IDLE_CHECK_CNT;
        if(idle_cnt < 0)
            idle_cnt = 0;
        
        if(idle_cnt > 1100)
            idle_state = true;
        else
            idle_state = false;
        
        
        ex_cmd_roll = roll;
        ex_cmd_pitch = pitch;
        ex_cmd_heave = heave;
        ex_cmd_sway = sway;
        ex_cmd_surge = surge;
        ex_cmd_yaw = yaw;
}

#endif