void Commander::Data_Exchange(void)
{
static u8 cnt = 0;
switch(cnt)
{
case 1:
f.Send_RCData = 1;
break;
case 2:
f.Send_MotoPwm = 1;
break;
case 30:
cnt = 0;
break;
default:
if(cnt%3)
f.Send_Senser = 1;
else
f.Send_Status = 1;
}
cnt++;
if(f.Send_Status){
f.Send_Status = 0;
Send_Status();
}
if(f.Send_Senser){
f.Send_Senser = 0;
Send_Senser();
}
if(f.Send_RCData){
f.Send_RCData = 0;
Send_RCData();
}
if(f.Send_MotoPwm){
f.Send_MotoPwm = 0;
Send_MotoPWM();
}
if(f.Send_PID1){
f.Send_PID1 = 0;
Send_PID1();
}
if(f.Send_PID2){
f.Send_PID2 = 0;
Send_PID2();
}
}
void Data_Exchange(void)
{
static int cnt = 0;
switch(cnt)
{
case SEND_STATES:
Send_Status();
cnt++;
break;
case SEND_SENSER:
Send_Senser();
cnt++;
break;
case SEND_PID1:
Send_PID1();
cnt++;
break;
case SEND_PID2:
Send_PID2();
cnt++;
break;
case SEND_RC:
Send_RCData();
cnt++;
break;
case SEND_MOTORPWM:
Send_MotoPWM();
cnt++;
break;
case SEND_DEBUG:
Send_DEBUG();
cnt++;
break;
default:
cnt=0;
break;
}
}
void ANO_DT::Data_Exchange(void)
{
static u8 cnt = 0;
static u8 senser_cnt = 10;
static u8 user_cnt = 10;
static u8 status_cnt = 15;
static u8 rcdata_cnt = 50;
static u8 motopwm_cnt = 50;
static u8 power_cnt = 50;
static u8 senser2_cnt = 50;
if((cnt % senser_cnt) == (senser_cnt-1))
f.send_senser = 1;
if((cnt % senser2_cnt) == (senser2_cnt-1))
f.send_senser2 = 1;
if((cnt % user_cnt) == (user_cnt-1))
f.send_user = 1;
if((cnt % status_cnt) == (status_cnt-1))
f.send_status = 1;
if((cnt % rcdata_cnt) == (rcdata_cnt-1))
f.send_rcdata = 1;
if((cnt % motopwm_cnt) == (motopwm_cnt-1))
f.send_motopwm = 1;
if((cnt % power_cnt) == (power_cnt-1))
f.send_power = 1;
cnt++;
/////////////////////////////////////////////////////////////////////////////////////
if(f.send_version)
{
f.send_version = 0;
Send_Version(1,300,110,400,0);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_status)
{
f.send_status = 0;
Send_Status(imu.angle.x,imu.angle.y,imu.angle.z,0,0,ano.f.ARMED);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_user)
{
f.send_user = 0;
//Send_User();
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_senser)
{
f.send_senser = 0;
Send_Senser(imu.Acc.x,imu.Acc.y,imu.Acc.z,
imu.Gyro.x,imu.Gyro.y,imu.Gyro.z,
0,0,0);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_senser2)
{
f.send_senser2 = 0;
Send_Senser2(0,0);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_rcdata)
{
f.send_rcdata = 0;
Send_RCData(rc.rawData[THROTTLE],rc.rawData[YAW],rc.rawData[ROLL],rc.rawData[PITCH],
rc.rawData[AUX1],rc.rawData[AUX2],rc.rawData[AUX3],
rc.rawData[AUX4],rc.rawData[AUX5],rc.rawData[AUX6]);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_motopwm)
{
f.send_motopwm = 0;
int16_t Moto_PWM[4];
motor.getPWM(Moto_PWM);
for(u8 i=0;i<4;i++)
Moto_PWM[i] -= 1000;
Send_MotoPWM(Moto_PWM[0],Moto_PWM[1],Moto_PWM[2],Moto_PWM[3],0,0,0,0);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_power)
{
f.send_power = 0;
Send_Power(123,456);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_pid1)
{
f.send_pid1 = 0;
Send_PID(1,fc.pid[PIDROLL].kP,fc.pid[PIDROLL].kI,fc.pid[PIDROLL].kD,
fc.pid[PIDPITCH].kP,fc.pid[PIDPITCH].kI,fc.pid[PIDPITCH].kD,
fc.pid[PIDYAW].kP,fc.pid[PIDYAW].kI,fc.pid[PIDYAW].kD);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_pid2)
{
f.send_pid2 = 0;
Send_PID(2,fc.pid[PIDANGLE].kP,fc.pid[PIDANGLE].kI,fc.pid[PIDANGLE].kD,
fc.pid[PIDMAG].kP,fc.pid[PIDMAG].kI,fc.pid[PIDMAG].kD,
fc.pid[PIDVELZ].kP,fc.pid[PIDVELZ].kI,fc.pid[PIDVELZ].kD);
}
/////////////////////////////////////////////////////////////////////////////////////
else if(f.send_pid3)
{
f.send_pid3 = 0;
Send_PID(3,fc.pid[PIDALT].kP,fc.pid[PIDALT].kI,fc.pid[PIDALT].kD,
0,0,0,
0,0,0);
}
}
