void ANO_DT_Send_Power(u16 votage, u16 current)
{
u8 _cnt=0;
u16 temp;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x05;
data_to_send[_cnt++]=0;
temp = votage;
data_to_send[_cnt++]=BYTE1(temp);
data_to_send[_cnt++]=BYTE0(temp);
temp = current;
data_to_send[_cnt++]=BYTE1(temp);
data_to_send[_cnt++]=BYTE0(temp);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Version(u8 hardware_type, u16 hardware_ver,u16 software_ver,u16 protocol_ver,u16 bootloader_ver)
{
u8 _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x00;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=hardware_type;
data_to_send[_cnt++]=BYTE1(hardware_ver);
data_to_send[_cnt++]=BYTE0(hardware_ver);
data_to_send[_cnt++]=BYTE1(software_ver);
data_to_send[_cnt++]=BYTE0(software_ver);
data_to_send[_cnt++]=BYTE1(protocol_ver);
data_to_send[_cnt++]=BYTE0(protocol_ver);
data_to_send[_cnt++]=BYTE1(bootloader_ver);
data_to_send[_cnt++]=BYTE0(bootloader_ver);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_PID(u8 group,float p1_p,float p1_i,float p1_d,float p2_p,float p2_i,float p2_d,float p3_p,float p3_i,float p3_d)
{
u8 _cnt=0;
vs16 _temp;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x10+group-1;
data_to_send[_cnt++]=0;
_temp = p1_p * 1000;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = p1_i * 1000;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = p1_d * 1000;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = p2_p * 1000;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = p2_i * 1000;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = p2_d * 1000;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = p3_p * 1000;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = p3_i * 1000;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = p3_d * 1000;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Senser(s16 a_x,s16 a_y,s16 a_z,s16 g_x,s16 g_y,s16 g_z,s16 m_x,s16 m_y,s16 m_z,s32 bar)
{
u8 _cnt=0;
vs16 _temp;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x02;
data_to_send[_cnt++]=0;
_temp = a_x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = a_y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = a_z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = g_x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = g_y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = g_z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = m_x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = m_y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = m_z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}
/***** 发送飞机传感器数值 *****/
void ANO_DT_Send_Senser(float a_x,float a_y,float a_z,float g_x,float g_y,float g_z,float m_x,float m_y,float m_z,int32_t bar)
{
uint8_t _cnt=0;
volatile int16_t _temp;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x02;
data_to_send[_cnt++]=0;
_temp = (int16_t)a_x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int16_t)a_y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int16_t)a_z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int16_t)g_x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int16_t)g_y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int16_t)g_z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int16_t)m_x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int16_t)m_y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int16_t)m_z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
uint8_t sum = 0;
for(uint8_t i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}
static void ANO_DT_Send_Check(u8 head, u8 check_sum)
{
data_to_send[0]=0xAA;
data_to_send[1]=0xAA;
data_to_send[2]=0xEF;
data_to_send[3]=2;
data_to_send[4]=head;
data_to_send[5]=check_sum;
u8 sum = 0;
for(u8 i=0;i<6;i++)
sum += data_to_send[i];
data_to_send[6]=sum;
ANO_DT_Send_Data(data_to_send, 7);
}
void ANO_DT_Send_RCData(u16 thr,u16 yaw,u16 rol,u16 pit,u16 aux1,u16 aux2,u16 aux3,u16 aux4,u16 aux5,u16 aux6)
{
u8 _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x03;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=BYTE1(thr);
data_to_send[_cnt++]=BYTE0(thr);
data_to_send[_cnt++]=BYTE1(yaw);
data_to_send[_cnt++]=BYTE0(yaw);
data_to_send[_cnt++]=BYTE1(rol);
data_to_send[_cnt++]=BYTE0(rol);
data_to_send[_cnt++]=BYTE1(pit);
data_to_send[_cnt++]=BYTE0(pit);
data_to_send[_cnt++]=BYTE1(aux1);
data_to_send[_cnt++]=BYTE0(aux1);
data_to_send[_cnt++]=BYTE1(aux2);
data_to_send[_cnt++]=BYTE0(aux2);
data_to_send[_cnt++]=BYTE1(aux3);
data_to_send[_cnt++]=BYTE0(aux3);
data_to_send[_cnt++]=BYTE1(aux4);
data_to_send[_cnt++]=BYTE0(aux4);
data_to_send[_cnt++]=BYTE1(aux5);
data_to_send[_cnt++]=BYTE0(aux5);
data_to_send[_cnt++]=BYTE1(aux6);
data_to_send[_cnt++]=BYTE0(aux6);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_Status(float angle_rol, float angle_pit, float angle_yaw, s32 alt, u8 fly_model, u8 armed)
{
u8 _cnt=0;
vs16 _temp;
vs32 _temp2 = alt;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
_temp = (int)(angle_rol*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(angle_pit*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(angle_yaw*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[_cnt++]=BYTE3(_temp2);
data_to_send[_cnt++]=BYTE2(_temp2);
data_to_send[_cnt++]=BYTE1(_temp2);
data_to_send[_cnt++]=BYTE0(_temp2);
data_to_send[_cnt++] = fly_model;
data_to_send[_cnt++] = armed;
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}
/***** 发送飞机当前姿态*****/
void ANO_DT_Send_Status(float angle_rol, float angle_pit, float angle_yaw)
{
uint8_t _cnt=0;
int16_t _temp;
int32_t _temp2 = 0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
_temp = (int)(angle_rol*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(angle_pit*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(angle_yaw*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[_cnt++]=BYTE3(_temp2);
data_to_send[_cnt++]=BYTE2(_temp2);
data_to_send[_cnt++]=BYTE1(_temp2);
data_to_send[_cnt++]=BYTE0(_temp2);
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = 0;
data_to_send[3] = _cnt-4;
uint8_t sum = 0;
for(uint8_t i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}
void ANO_DT_Send_MotoPWM(u16 m_1,u16 m_2,u16 m_3,u16 m_4,u16 m_5,u16 m_6,u16 m_7,u16 m_8)
{
u8 _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x06;
data_to_send[_cnt++]=0;
data_to_send[_cnt++]=BYTE1(m_1);
data_to_send[_cnt++]=BYTE0(m_1);
data_to_send[_cnt++]=BYTE1(m_2);
data_to_send[_cnt++]=BYTE0(m_2);
data_to_send[_cnt++]=BYTE1(m_3);
data_to_send[_cnt++]=BYTE0(m_3);
data_to_send[_cnt++]=BYTE1(m_4);
data_to_send[_cnt++]=BYTE0(m_4);
data_to_send[_cnt++]=BYTE1(m_5);
data_to_send[_cnt++]=BYTE0(m_5);
data_to_send[_cnt++]=BYTE1(m_6);
data_to_send[_cnt++]=BYTE0(m_6);
data_to_send[_cnt++]=BYTE1(m_7);
data_to_send[_cnt++]=BYTE0(m_7);
data_to_send[_cnt++]=BYTE1(m_8);
data_to_send[_cnt++]=BYTE0(m_8);
data_to_send[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
ANO_DT_Send_Data(data_to_send, _cnt);
}
