/*====================================================================================================*/
int main( void )
{
u8 UART_BUF[32] = {0};
System_Init();
System_Corr(SampleRateFreg); // SampleRateFreg = 500 Hz
LED_G = LED_ON;
Delay_100ms(5);
LED_G = LED_OFF;
while(1) {
LED_B = !LED_B;
UART_BUF[0] = Byte8L((s16)(Acc.TrueX*1000)); // Acc.X 1 mg/LSB
UART_BUF[1] = Byte8H((s16)(Acc.TrueX*1000));
UART_BUF[2] = Byte8L((s16)(Acc.TrueY*1000)); // Acc.Y 1 mg/LSB
UART_BUF[3] = Byte8H((s16)(Acc.TrueY*1000));
UART_BUF[4] = Byte8L((s16)(Acc.TrueZ*1000)); // Acc.Z 1 mg/LSB
UART_BUF[5] = Byte8H((s16)(Acc.TrueZ*1000));
UART_BUF[6] = Byte8L((s16)(Gyr.TrueX*100)); // Gyr.X 10 mdps/LSB
UART_BUF[7] = Byte8H((s16)(Gyr.TrueX*100));
UART_BUF[8] = Byte8L((s16)(Gyr.TrueY*100)); // Gyr.Y 10 mdps/LSB
UART_BUF[9] = Byte8H((s16)(Gyr.TrueY*100));
UART_BUF[10] = Byte8L((s16)(Gyr.TrueZ*100)); // Gyr.Z 10 mdps/LSB
UART_BUF[11] = Byte8H((s16)(Gyr.TrueZ*100));
// UART_BUF[12] = Byte8L((s16)(Mag.TrueX)); // 100 nTesla/LSB
// UART_BUF[13] = Byte8L((s16)(Mag.TrueX));
// UART_BUF[14] = Byte8L((s16)(Mag.TrueY)); // 100 nTesla/LSB
// UART_BUF[15] = Byte8H((s16)(Mag.TrueY));
// UART_BUF[16] = Byte8L((s16)(Mag.TrueZ)); // 100 nTesla/LSB
// UART_BUF[17] = Byte8H((s16)(Mag.TrueZ));
UART_BUF[18] = Byte8L((s16)(Temp.TrueT*100)); // 0.01 degC/LSB
UART_BUF[19] = Byte8H((s16)(Temp.TrueT*100));
UART_BUF[20] = Byte8L((s16)(AngE.Pitch*100)); // 0.01 deg/LSB
UART_BUF[21] = Byte8H((s16)(AngE.Pitch*100)); // 0.01 deg/LSB
UART_BUF[22] = Byte8L((s16)(AngE.Roll*100)); // 0.01 deg/LSB
UART_BUF[23] = Byte8H((s16)(AngE.Roll*100)); // 0.01 deg/LSB
UART_BUF[24] = Byte8L((s16)(AngE.Yaw*10)); // 100 nTesla/LSB
UART_BUF[25] = Byte8H((s16)(AngE.Yaw*10)); // 0.1 deg/LSB
// UART_BUF[26] = Byte8L((s16)(Baro.Temp*100)); // 0.01 degC/LSB
// UART_BUF[27] = Byte8H((s16)(Baro.Temp*100));
// UART_BUF[28] = Byte8L((s16)(Baro.Press*10)); // 0.1 mbar/LSB
// UART_BUF[29] = Byte8H((s16)(Baro.Press*10));
// UART_BUF[30] = Byte8L((s16)(Baro.Height));
// UART_BUF[31] = Byte8H((s16)(Baro.Height));
if(KEY == 1)
RS232_VisualScope(UART_BUF); // Print Acc
else
RS232_VisualScope(UART_BUF+20); // Print AngE
}
}
/*=====================================================================================================*/
void VisualScope_Send( USART_TypeDef* USARTx )
{
u8 VisualScopeBuf[8] = {0};
RF_SendData.Ang.X = (s16)(AngE.Pitch*100); // 10 mdeg/LSB
RF_SendData.Ang.Y = (s16)(AngE.Roll*100); // 10 mdeg/LSB
RF_SendData.Ang.Z = (s16)(AngE.Yaw*10); // 100 mdeg/LSB
VisualScopeBuf[0] = (u8)Byte8L(RF_SendData.Ang.X);
VisualScopeBuf[1] = (u8)Byte8H(RF_SendData.Ang.X);
VisualScopeBuf[2] = (u8)Byte8L(RF_SendData.Ang.Y);
VisualScopeBuf[3] = (u8)Byte8H(RF_SendData.Ang.Y);
VisualScopeBuf[4] = (u8)Byte8L(RF_SendData.Ang.Z);
VisualScopeBuf[5] = (u8)Byte8H(RF_SendData.Ang.Z);
VisualScopeBuf[6] = (u8)(0);
VisualScopeBuf[7] = (u8)(0);
RS232_VisualScope(USARTx, VisualScopeBuf, 8);
}
/*=====================================================================================================*/
int main( void )
{
u8 TrData[8] = {0};
SystemInit();
GPIO_Config();
ADC_Config();
RS232_Config();
while(1) {
LED_G = ~LED_G;
ADC_Average(ADC_AveTr);
TrData[0] = (u8)(ADC_AveTr[0]);
TrData[1] = (u8)(ADC_AveTr[0]>>8);
TrData[2] = (u8)(ADC_AveTr[1]);
TrData[3] = (u8)(ADC_AveTr[1]>>8);
TrData[4] = (u8)(0);
TrData[5] = (u8)(0);
TrData[6] = (u8)(0);
TrData[7] = (u8)(0);
RS232_VisualScope(USART3, TrData, 8);
}
}
/*=====================================================================================================*/
int main(void)
{
u8 Sta = ERROR;
FSM_Mode FSM_State = FSM_Rx;
/* System Init */
System_Init();
test_printf();
/* Throttle Config */
if (KEY == 1) {
LED_B = 0;
Motor_Control(PWM_MOTOR_MAX, PWM_MOTOR_MAX, PWM_MOTOR_MAX, PWM_MOTOR_MAX);
}
while (KEY == 1);
LED_B = 1;
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
/* nRF Check */
while (Sta == ERROR)
Sta = nRF_Check();
/* Sensor Init */
if (Sensor_Init() == SUCCESS)
LED_G = 0;
Delay_10ms(10);
/* Systick Config */
if (SysTick_Config(SystemCoreClock / SampleRateFreg)) { // SampleRateFreg = 500 Hz
while (1);
}
/* Wait Correction */
while (SensorMode != Mode_Algorithm);
/* Lock */
LED_R = 1;
LED_G = 1;
LED_B = 1;
while (!KEY) {
LED_B = ~LED_B;
Delay_10ms(1);
Transport_Send(TxBuf[0]);
printf("Roll%d,Pitch%d,Yaw%d,CH1 %u(%d),CH2 %u(%d),CH3 %u(%d),CH4 %u(%d),CH5 %u()\r\n",
(int)AngE.Roll, (int)AngE.Pitch, (int)AngE.Yaw,
PWM1_InputCaptureValue, global_rc_roll,
PWM2_InputCaptureValue, global_rc_pitch,
PWM3_InputCaptureValue, global_rc_thr,
PWM4_InputCaptureValue, global_rc_yaw,
PWM5_InputCaptureValue);
}
LED_B = 1;
/* Final State Machine */
while (1) {
LED_G = ~LED_G;
switch (FSM_State) {
/************************** FSM Tx ****************************************/
case FSM_Tx:
// FSM_Tx
nRF_TX_Mode();
do {
Sta = nRF_Tx_Data(TxBuf[0]);
} while (Sta == MAX_RT);
// FSM_Tx End
FSM_State = FSM_Rx;
break;
/************************** FSM Rx ****************************************/
case FSM_Rx:
// FSM_Rx
nRF_RX_Mode();
Sta = nRF_Rx_Data(RxBuf[0]);
if (Sta == RX_DR) {
Transport_Recv(RxBuf[0]);
}
// FSM_Rx End
FSM_State = FSM_CTRL;
break;
/************************** FSM CTRL **************************************/
case FSM_CTRL:
// FSM_CTRL
CTRL_FlightControl();
// FSM_CTRL End
FSM_State = FSM_UART;
break;
/************************** FSM UART ***************************************/
case FSM_UART:
// FSM_USART
RS232_VisualScope(USART3, TxBuf[0] + 20, 8);
// FSM_USART End
FSM_State = FSM_DATA;
break;
/************************** FSM DATA **************************************/
case FSM_DATA:
// FSM_DATA
Transport_Send(TxBuf[0]);
// FSM_DATA End
FSM_State = FSM_Tx;
break;
}
}
}
/*=====================================================================================================*/
int main( void )
{
u8 Sta = ERROR;
FSM_Mode FSM_State = FSM_Rx;
/* System Init */
System_Init();
/* Throttle Config */
if(KEY == 1) {
LED_B = 0;
Motor_Control(PWM_MOTOR_MAX, PWM_MOTOR_MAX, PWM_MOTOR_MAX, PWM_MOTOR_MAX);
}
while(KEY == 1);
LED_B = 1;
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
/* nRF Check */
while(Sta == ERROR)
Sta = nRF_Check();
/* Sensor Init */
if(Sensor_Init() == SUCCESS)
LED_G = 0;
Delay_10ms(10);
/* Systick Config */
if(SysTick_Config(420000)) { // 168MHz / 420000 = 400Hz = 2.5ms
while(1);
}
/* Wait Correction */
while(SensorMode != Mode_Algorithm);
/* Lock */
LED_R = 1;
LED_G = 1;
LED_B = 1;
while(!KEY) {
LED_B = ~LED_B;
Delay_10ms(1);
Transport_Send(TxBuf[0]);
RS232_VisualScope(USART3, TxBuf[0]+2, 8);
}
LED_B = 1;
/* Final State Machine */
while(1) {
LED_G = ~LED_G;
switch(FSM_State) {
/************************** FSM Tx ****************************************/
case FSM_Tx:
// FSM_Tx
nRF_TX_Mode();
do {
Sta = nRF_Tx_Data(TxBuf[0]);
} while(Sta == MAX_RT);
// FSM_Tx End
FSM_State = FSM_Rx;
break;
/************************** FSM Rx ****************************************/
case FSM_Rx:
// FSM_Rx
nRF_RX_Mode();
Sta = nRF_Rx_Data(RxBuf[0]);
if(Sta == RX_DR) {
Transport_Recv(RxBuf[0]);
}
// FSM_Rx End
FSM_State = FSM_CTRL;
break;
/************************** FSM CTRL **************************************/
case FSM_CTRL:
// FSM_CTRL
CTRL_FlightControl();
// FSM_CTRL End
FSM_State = FSM_UART;
break;
/************************** FSM UART ***************************************/
case FSM_UART:
// FSM_USART
RS232_VisualScope(USART3, TxBuf[0]+2, 8);
// FSM_USART End
FSM_State = FSM_DATA;
break;
/************************** FSM DATA **************************************/
case FSM_DATA:
// FSM_DATA
Transport_Send(TxBuf[0]);
// FSM_DATA End
FSM_State = FSM_Tx;
break;
}
}
}
/*=====================================================================================================*/
int main( void )
{
u8 i = 0;
u8 Sta = 0;
FSM_Mode FSM_State = FSM_Rx;
/* System Init */
QCopterFC_Init();
/* Systick Config */
if(SysTick_Config(SystemCoreClock/SampleRateFreg)) { // SampleRateFreg = 500 Hz
while(1);
}
/* Wait Correction */
while(SensorMode != Mode_Algorithm);
/* Lock */
LED_R = 1;
LED_G = 1;
LED_B = 1;
while(KEY != KEY_ON) {
LED_B = ~LED_B;
Delay_10ms(1);
VisualScope_Send(USART3);
}
LED_B = 1;
/* Final State Machine */
while(1) {
LED_G = ~LED_G;
switch(FSM_State) {
/************************** FSM Tx ****************************************/
case FSM_Tx:
// FSM_Tx
nRF_TX_Mode();
for(i=0; i<3; i++) {
RF_SendData.Packet++;
Transport_Send(TxBuf);
do {
Sta = nRF_Tx_Data(TxBuf);
} while(Sta == MAX_RT);
}
RF_SendData.Packet = 0x00;
// FSM_Tx End
FSM_State = FSM_Rx;
break;
/************************** FSM Rx ****************************************/
case FSM_Rx:
// FSM_Rx
nRF_RX_Mode();
Sta = nRF_Rx_Data(RxBuf);
if(Sta == RX_DR) {
Transport_Recv(RxBuf);
}
// FSM_Rx End
FSM_State = FSM_CTRL;
break;
/************************** FSM CTRL **************************************/
case FSM_CTRL:
// FSM_CTRL
Ctrl_BasicThr();
// FSM_CTRL End
FSM_State = FSM_UART;
break;
/************************** FSM UART ***************************************/
case FSM_UART:
// FSM_USART
RS232_VisualScope(USART3, TxBuf+20, 8);
// FSM_USART End
FSM_State = FSM_DATA;
break;
/************************** FSM DATA **************************************/
case FSM_DATA:
// FSM_DATA
// FSM_DATA End
FSM_State = FSM_Tx;
break;
}
}
}
