void ANO_DT::Send_Senser2(s32 alt_bar,u16 alt_csb)
{
u8 _cnt=0;
vs32 _temp;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x07;
data_to_send[_cnt++]=0;
_temp = alt_bar;
data_to_send[_cnt++]=BYTE3(_temp);
data_to_send[_cnt++]=BYTE2(_temp);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = alt_csb;
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;
Send_Data(data_to_send, _cnt);
}
void ANO_DT::Send_Status(void)
{
u8 _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
vs16 _temp;
_temp = (int)(imu.angle.x*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(imu.angle.y*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(imu.angle.z*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
vs32 _temp2 = 0;//UltraAlt * 100;
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[3] = _cnt-4;
u8 sum = 0;
for(u8 i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
Send_Data(data_to_send, _cnt);
}
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;
Send_Data(data_to_send, _cnt);
}
void OnSlave(void)
{
switch(Radio->Process())
{
case RF_RX_DONE:
Radio->GetRxPacket(TxBuf,TxBufSize);
LEDR1_INVERSE();
Send_Data();
//printf("Rsn: %d dB\r\n",SX1276LoRaGetPacketSnr());
//printf("Rsii: %f dBm\r\n",SX1276LoRaGetPacketRssi());
// for(uint8_t i=0;i<TxBufSize;i++)
// {
// printf("%x ",TxBuf[i]);
// }
// printf("\r\n");
break;
case RF_TX_DONE:
LEDG_INVERSE();
memset( TxBuf, 0, ( size_t )TxBufSize);
Radio->StartRx();
break;
default:
break;
}
}
void ANO_DT::Send_RCData(void)
{
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(rc.rawData[THROTTLE]);
data_to_send[_cnt++]=BYTE0(rc.rawData[THROTTLE]);
data_to_send[_cnt++]=BYTE1(rc.rawData[YAW]);
data_to_send[_cnt++]=BYTE0(rc.rawData[YAW]);
data_to_send[_cnt++]=BYTE1(rc.rawData[ROLL]);
data_to_send[_cnt++]=BYTE0(rc.rawData[ROLL]);
data_to_send[_cnt++]=BYTE1(rc.rawData[PITCH]);
data_to_send[_cnt++]=BYTE0(rc.rawData[PITCH]);
data_to_send[_cnt++]=BYTE1(rc.rawData[AUX1]);
data_to_send[_cnt++]=BYTE0(rc.rawData[AUX1]);
data_to_send[_cnt++]=BYTE1(rc.rawData[AUX2]);
data_to_send[_cnt++]=BYTE0(rc.rawData[AUX2]);
data_to_send[_cnt++]=BYTE1(rc.rawData[AUX3]);
data_to_send[_cnt++]=BYTE0(rc.rawData[AUX3]);
data_to_send[_cnt++]=BYTE1(rc.rawData[AUX4]);
data_to_send[_cnt++]=BYTE0(rc.rawData[AUX4]);
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;
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;
Send_Data(data_to_send, _cnt);
}
void Send_DEBUG(void)
{
u8 _cnt=0;
u8 i;
vs16 _temp;
u8 sum = 0;
float data = 0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xF1;
data_to_send[_cnt++]=0;
data = Acc.z;
data_to_send[_cnt++]=BYTE3(data);
data_to_send[_cnt++]=BYTE2(data);
data_to_send[_cnt++]=BYTE1(data);
data_to_send[_cnt++]=BYTE0(data);
data = IMU_QCF.Acc_LPF2nd.lastout_z;
data_to_send[_cnt++]=BYTE3(data);
data_to_send[_cnt++]=BYTE2(data);
data_to_send[_cnt++]=BYTE1(data);
data_to_send[_cnt++]=BYTE0(data);
data = IMU_QCF.yaw;
data_to_send[_cnt++]=BYTE3(data);
data_to_send[_cnt++]=BYTE2(data);
data_to_send[_cnt++]=BYTE1(data);
data_to_send[_cnt++]=BYTE0(data);
data = EarthAcc.x;
data_to_send[_cnt++]=BYTE3(data);
data_to_send[_cnt++]=BYTE2(data);
data_to_send[_cnt++]=BYTE1(data);
data_to_send[_cnt++]=BYTE0(data);
data = EarthAcc.y;
data_to_send[_cnt++]=BYTE3(data);
data_to_send[_cnt++]=BYTE2(data);
data_to_send[_cnt++]=BYTE1(data);
data_to_send[_cnt++]=BYTE0(data);
data = EarthAcc.z;
data_to_send[_cnt++]=BYTE3(data);
data_to_send[_cnt++]=BYTE2(data);
data_to_send[_cnt++]=BYTE1(data);
data_to_send[_cnt++]=BYTE0(data);
data_to_send[3] = _cnt-4;
for(i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
Send_Data(data_to_send, _cnt);
}
void LCD_Write(uint8 data_or_command, uint8 data_value)
{
CE_Write(LOW);
CyDelayUs(DELAY_1_US);
DC_Write(data_or_command);
Send_Data(data_value);
CE_Write(HIGH);
CyDelayUs(DELAY_1_US);
}
/*********************************************************************
** @fn :
**
** @brief :
**
** @param :
**
** @return :
*********************************************************************/
static u16_t Task_GPS(u8_t ucTaskID,u16_t usEvent)
{
if(usEvent & EVENT_GPS_ACTIVE)
{
GPS_Info_t gpsInfo;
Get_GPS_Info(&gpsInfo);
Send_Data(ROUTER_LOCAL,(u8_t *)&gpsInfo,sizeof(gpsInfo));
CLR_TASK_EVENT(g_ucTaskGPSID,usEvent)
}
__interrupt void CC1101_ISR(void)
{
switch(__even_in_range(RF1AIV,32)) // Prioritizing Radio Core Interrupt
{
case 0: break; // No RF core interrupt pending
case 2: break; // RFIFG0
case 4: break; // RFIFG1
case 6: break; // RFIFG2
case 8: break; // RFIFG3
case 10: break; // RFIFG4
case 12: break; // RFIFG5
case 14: break; // RFIFG6
case 16: break; // RFIFG7
case 18: break; // RFIFG8
case 20: // RFIFG9
if(receiving) // RX end of packet
{
// Check for ADDRESS:
// If ADDRESS = Correct:
// Set transmitting = 1
// If ADDRESS = False:
// Break
// Read the length byte from the FIFO
RxBufferLength = ReadSingleReg( RXBYTES );
ReadBurstReg(RF_RXFIFORD, RxBuffer, RxBufferLength);
// Stop here to see contents of RxBuffer
__no_operation();
// Check the CRC results
if(RxBuffer[CRC_LQI_IDX] & CRC_OK){
// Check to see if the ADDRESS is correct)
if(RxBuffer[0] && ADDRESS){
Send_Data(ADDRESS_MASTER,TO_BE_CHANGED);
}
else {
break;
}
}
}
else if(transmitting) // TX end of packet
{
RF1AIE &= ~BIT9; // Disable TX end-of-packet interrupt
transmitting = 0;
}
else while(1); // trap
break;
case 22: break; // RFIFG10
case 24: break; // RFIFG11
case 26: break; // RFIFG12
case 28: break; // RFIFG13
case 30: break; // RFIFG14
case 32: break; // RFIFG15
}
__bic_SR_register_on_exit(LPM3_bits);
}
void Send_PID1(void)
{
u8 _cnt=0;
u8 i;
vs16 _temp;
u8 sum = 0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x10;
data_to_send[_cnt++]=0;
_temp = fc.pid[PIDROLL].kP ;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDROLL].kI ;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDROLL].kD ;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDPITCH].kP ;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDPITCH].kI ;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDPITCH].kD ;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDYAW].kP;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDYAW].kI;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDYAW].kD;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
for(i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
Send_Data(data_to_send, _cnt);
}
int miu_send_thmsg(GAL_APIHANDLE hAPICust, CDR_DATA_STRUCT *cdr)
{
Arb_connection *dbcust = galGetCustDbConnection(hAPICust);
CDR_DATA_STRUCT *pCdr;
for ( pCdr = cdr; pCdr; pCdr = pCdr->next )
{
if (Add_CDR_Data( pCdr) == FAILURE)
return FAILURE;
}
if (Send_Data(dbcust) == FAILURE)
return FAILURE;
return SUCCESS;
}
void Send_PID2(void)
{
u8 _cnt=0;
u8 i;
vs16 _temp;
u8 sum = 0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x11;
data_to_send[_cnt++]=0;
_temp = fc.pid[PIDALT].kP;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDALT].kI;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDALT].kD;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDLEVEL].kP;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDLEVEL].kI;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDLEVEL].kD;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDMAG].kP;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDMAG].kI;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid[PIDMAG].kD;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[3] = _cnt-4;
for(i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
Send_Data(data_to_send, _cnt);
}
/*
*@<function name=> Control_Task() </function>
*@<summary>
控制任务
*@</summary>
*@<param name="pdata">no use</param>
*@<returns> null </returns>
*/
static void Control_Task(void *pdata)
{
float now_speed = 0,now_lenth = 0;//速度位移的反馈值
static u8 ii = 0,jj = 0;
(void)pdata; // 'pdata' 并没有用到，防止编译器提示警告
Set_PID_Param(&PID_Loction,0,1,0,0,250,-250);//初始化变量
Set_PID_Param(&PID_Speed,1,0.5,0,0,800,-800);
Set_PID_Param(&PID_Current,0,1,0,0,10000,-10000);
KF_Simple_Init(&KF_Speed,0,10,100,100);
while(1)
{
vTaskDelay(1*configTICK_RATE_HZ/1000); //延时函数
if(jj == 0)//外环
{
jj = 10;
now_lenth = Get_Encoder_Lenth();
PID_Cal(&PID_Loction,Iface_MyMotor.lenth,now_lenth,100);//PID计算
}
if(ii == 0)//内环
{
ii = 3;
now_speed = Get_Encoder_Speed();
now_speed = KF_Simple_Calculate(&KF_Speed,now_speed);//速度KF滤波
// Iface_MyMotor.speed = PID_Loction.ref;//位移环输出作为速度环输入
PID_Cal(&PID_Speed,Iface_MyMotor.speed,now_speed,100);//PID计算
PID_Speed.ref += PID_Speed.fd * Iface_MyMotor.speed; //加前馈
// PID_Speed.ref = Iface_MyMotor.speed;//开环测试
Set_MotorSpeed(PID_Speed.ref);
Send_Data(now_lenth,ID_Osc_CH2);
Send_Data(now_speed,ID_Osc_CH3);
}
ii--,jj--;
}
}
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)
{
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;
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;
Send_Data(data_to_send, _cnt);
}
/*--------------------------------------------------
ComMode_Data()
主机接收到编码1信号后，会反馈一个编码1信号给附机
以表示主机在附机附近。
---------------------------------------------------*/
void ComMode_Data(tByte ComMode, x)
{
receiver_EN = 1;
Delay(20);
transmiter_EN = 0;
myTxRxData[0] = CmdHead;
myTxRxData[1] = MyAddress;
myTxRxData[2] = ComMode;
initsignal();
Send_Data(x);
transmiter_EN = 1;
receiver_EN = 0;
}
void Send_Senser(void)
{
u8 _cnt=0;
u8 i;
u8 sum = 0;
vs16 _temp;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x02;
data_to_send[_cnt++]=0;
_temp = Acc.x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = Acc.y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = Acc.z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
//_temp = mpu6050.Get_Gyro().x;
_temp = Gyro_ADC.x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
//_temp = mpu6050.Get_Gyro().y;
_temp = Gyro_ADC.y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
//_temp = mpu6050.Get_Gyro().z;
_temp = Gyro_ADC.z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = 0;
data_to_send[3] = _cnt-4;
for(i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++] = sum;
Send_Data(data_to_send, _cnt);
}
int Send_Message(int* Data, int Length)
{
if ((Remaining_Space() == 0) || (Remaining_Space() > Length))
{
while (Length)
{
int Check = Enque(*Data);
if (Check == -2)
{
break;
}
Data++;
Length--;
}
if (Length != 0)
{
return -2;
}
else if (Length == 0)
{
/*If the Buffer is not empty and do not disturb is
false*/
if (RingBuffer.Head != -1 && RingBuffer.DND == false)
{
/*Chek whether Tail is greater than the head therefore there is no
rollover for the buffer*/
while (RingBuffer.Head == -1) /*until the buffer is empty call the senddata function*/
{
RingBuffer.Send_Done = false;
Send_Data((RingBuffer.Buffer + RingBuffer.Head), SENDSIZE);
while (RingBuffer.Send_Done == false)
{
/*Wait till the data has been properly sent*/
}
}
return 0;
}
return 0;
}
}
else
{
return -2;
}
}
void Commander::Send_PID2(void)
{
uint8_t _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x11;
data_to_send[_cnt++]=0;
uint16_t _temp;
_temp = fc.pid_group[PIDALT].kP;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid_group[PIDALT].kI;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid_group[PIDALT].kD;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid_group[PIDLEVEL].kP;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid_group[PIDLEVEL].kI;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid_group[PIDLEVEL].kD;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid_group[PIDMAG].kP;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid_group[PIDMAG].kI;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = fc.pid_group[PIDMAG].kD;
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;
Send_Data(data_to_send, _cnt);
}
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;
Send_Data(data_to_send, 8);
}
void Commander::Send_Senser(void)
{
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 = imu.Acc.x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = imu.Acc.y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = imu.Acc.z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = Sensor_GetGyro().x;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = Sensor_GetGyro().y;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = Sensor_GetGyro().z;
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = 0;
data_to_send[_cnt++] = 0;
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;
Send_Data(data_to_send, _cnt);
}
void Commander::Send_Check(u16 check)
{
data_to_send[0]=0xAA;
data_to_send[1]=0xAA;
data_to_send[2]=0xF0;
data_to_send[3]=3;
data_to_send[4]=0xBA;
data_to_send[5]=BYTE1(check);
data_to_send[6]=BYTE0(check);
u8 sum = 0;
for(u8 i=0;i<7;i++)
sum += data_to_send[i];
data_to_send[7]=sum;
Send_Data(data_to_send, 8);
}
void Config_Handler1(uint8_t *buf)
{
uint8_t command=buf[0];
switch(command)
{
case 0x01:
Config_Handler(buf);
break;
case 0x02:
Send_Config();
break;
case 0x03:
Send_Data();
break;
default:
break;
}
Print_Config();
}
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;
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;
Send_Data(data_to_send, _cnt);
}
void Commander::Send_RCData(void)
{
uint8_t _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(rc.rawData[THROTTLE]);
data_to_send[_cnt++]=BYTE0(rc.rawData[THROTTLE]);
data_to_send[_cnt++]=BYTE1(rc.rawData[YAW]);
data_to_send[_cnt++]=BYTE0(rc.rawData[YAW]);
data_to_send[_cnt++]=BYTE1(rc.rawData[ROLL]);
data_to_send[_cnt++]=BYTE0(rc.rawData[ROLL]);
data_to_send[_cnt++]=BYTE1(rc.rawData[PITCH]);
data_to_send[_cnt++]=BYTE0(rc.rawData[PITCH]);
data_to_send[_cnt++]=BYTE1(rc.rawData[AUX1]);
data_to_send[_cnt++]=BYTE0(rc.rawData[AUX1]);
data_to_send[_cnt++]=BYTE1(rc.rawData[AUX2]);
data_to_send[_cnt++]=BYTE0(rc.rawData[AUX2]);
data_to_send[_cnt++]=BYTE1(rc.rawData[AUX3]);
data_to_send[_cnt++]=BYTE0(rc.rawData[AUX3]);
data_to_send[_cnt++]=BYTE1(rc.rawData[AUX4]);
data_to_send[_cnt++]=BYTE0(rc.rawData[AUX4]);
data_to_send[_cnt++]=0;//BYTE1(rc.rawData[AUX5]);
data_to_send[_cnt++]=0;//BYTE0(rc.rawData[AUX5]);
data_to_send[_cnt++]=0;//BYTE1(rc.rawData[AUX6]);
data_to_send[_cnt++]=0;//BYTE0(rc.rawData[AUX6]);
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;
Send_Data(data_to_send, _cnt);
}
void Send_MotoPWM(void)
{
u8 _cnt=0;
u8 i;
uint16_t Moto_PWM[6];
u8 sum = 0;
getPWM(Moto_PWM);
for(i=0;i<6;i++)
Moto_PWM[i] -= 1000;
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(Moto_PWM[1]);
data_to_send[_cnt++]=BYTE0(Moto_PWM[1]);
data_to_send[_cnt++]=BYTE1(Moto_PWM[3]);
data_to_send[_cnt++]=BYTE0(Moto_PWM[3]);
data_to_send[_cnt++]=BYTE1(Moto_PWM[5]);
data_to_send[_cnt++]=BYTE0(Moto_PWM[5]);
data_to_send[_cnt++]=BYTE1(Moto_PWM[2]);
data_to_send[_cnt++]=BYTE0(Moto_PWM[2]);
data_to_send[_cnt++]=BYTE1(Moto_PWM[0]);
data_to_send[_cnt++]=BYTE0(Moto_PWM[0]);
data_to_send[_cnt++]=BYTE1(Moto_PWM[4]);
data_to_send[_cnt++]=BYTE0(Moto_PWM[4]);
data_to_send[3] = _cnt-4;
for(i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
Send_Data(data_to_send, _cnt);
}
void Commander::Send_Status(void)
{
uint8_t _cnt=0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
uint16_t _temp;
_temp = (int)(imu.angle.x*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(imu.angle.y*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(imu.angle.z*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(1000);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
uint32_t _temp2 = 100;//UltraAlt * 100;
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[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;
Send_Data(data_to_send, _cnt);
}
void Send_Status(void)
{
u8 _cnt=0;
u8 i;
vs16 _temp;
vs32 _temp2 = 0;//UltraAlt * 100;
u8 sum = 0;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
_temp = (int)(IMU_QCF.roll*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(IMU_QCF.pitch*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(IMU_QCF.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[3] = _cnt-4;
for(i=0;i<_cnt;i++)
sum += data_to_send[i];
data_to_send[_cnt++]=sum;
Send_Data(data_to_send, _cnt);
}
