int main(int argc, char *argv[])
{
printf("Start Client\n");
char *ip = "127.0.0.1";
if (argc > 1)
ip = argv[1];
int do_sig = 0;
if (argc > 2)
do_sig = atoi(argv[2]);
// Init modules
udpInit(ip, do_sig);
signalerInit();
controllerInit();
// Tasks with corresponding functions
pthread_t tasks[NUM_THREADS];
taskFunc_t taskFunctions[NUM_THREADS] = { udpTaskFunction, controllerTaskFunction, signalerTaskFunction };
for (int i = 0; i < NUM_THREADS - (1 ^ do_sig); ++i)
pthread_create(&tasks[i], NULL, taskFunctions[i], NULL);
// Only wait for controller, NB task id
pthread_join(tasks[1], NULL);
// Cleanup modules
controllerCleanup();
signalerCleanup();
udpCleanup();
return 0;
}
void stabilizerInit(void) {
if (isInit)
return;
motorsInit();
imu6Init();
sensfusion6Init();
controllerInit();
rollRateDesired = 0;
pitchRateDesired = 0;
yawRateDesired = 0;
xTaskCreate(stabilizerTask, (const signed char * const )"STABILIZER",
2*configMINIMAL_STACK_SIZE, NULL, /*Piority*/2, NULL);
isInit = TRUE;
}
void stabilizerInit(void)
{
if(isInit)
return;
motorsInit();
imu6Init();
sensfusion6Init();
controllerInit();
rollRateDesired = 0;
pitchRateDesired = 0;
yawRateDesired = 0;
xTaskCreate(stabilizerTask, (const signed char * const)STABILIZER_TASK_NAME,
STABILIZER_TASK_STACKSIZE, NULL, STABILIZER_TASK_PRI, NULL);
isInit = true;
}
void stabilizerInit(void)
{
if(isInit)
return;
motorsInit(motorMapDefaultBrushed);
imu6Init();
sensfusion6Init();
controllerInit();
rollDesired = 0;
pitchDesired = 0;
yawDesired = 0;
xTaskCreate(stabilizerTask, STABILIZER_TASK_NAME,
STABILIZER_TASK_STACKSIZE, NULL, STABILIZER_TASK_PRI, NULL);
isInit = true;
}
void trainTaskInit(void) {
clockInitTask();
mioInit();
tioInit();
logInit();
Create(31, idlerTask);
Delay(50);
vtInit();
turnoutInit();
sensorInit();
parserInit();
clockDrawerInit();
controllerInit();
freightInit();
initTrackA(nodes, hashtbl);
}
void stabilizerInit(void)
{
if(isInit)
return;
motorsInit(motorMapBrushed);
imu6Init();
sensfusion6Init();
controllerInit();
#if defined(SITAW_ENABLED)
sitAwInit();
#endif
rollRateDesired = 0;
pitchRateDesired = 0;
yawRateDesired = 0;
xTaskCreate(stabilizerTask, (const signed char * const)STABILIZER_TASK_NAME,
STABILIZER_TASK_STACKSIZE, NULL, STABILIZER_TASK_PRI, NULL);
isInit = true;
}
/**
* The main() creates tasks or "threads". See the documentation of scheduler_task class at cpp_task.hpp
* for details. There is a very simple example towards the beginning of this class's declaration.
*
* @warning SPI #1 bus usage notes (interfaced to SD & Flash):
* - You can read/write files from multiple tasks because it automatically goes through SPI semaphore.
* - If you are going to use the SPI Bus in a FreeRTOS task, you need to use the API at L4_IO/fat/spi_sem.h
*
* @warning SPI #0 usage notes (Nordic wireless)
* - This bus is more tricky to use because if FreeRTOS is not running, the RIT interrupt may use the bus.
* - If FreeRTOS is running, then wireless task may use it.
* In either case, you should avoid using this bus or interfacing to external components because
* there is no semaphore configured for this bus and it should be used exclusively by nordic wireless.
*/
int main(void)
{
controllerInit();
scheduler_add_task(new canRxBufferTask(PRIORITY_CRITICAL));
scheduler_add_task(new canRxProcessTask(PRIORITY_HIGH));
/**
* A few basic tasks for this bare-bone system :
* 1. Terminal task provides gateway to interact with the board through UART terminal.
* 2. Remote task allows you to use remote control to interact with the board.
* 3. Wireless task responsible to receive, retry, and handle mesh network.
*
* Disable remote task if you are not using it. Also, it needs SYS_CFG_ENABLE_TLM
* such that it can save remote control codes to non-volatile memory. IR remote
* control codes can be learned by typing "learn" command.
*/
scheduler_add_task(new terminalTask(PRIORITY_HIGH));
/* Consumes very little CPU, but need highest priority to handle mesh network ACKs */
scheduler_add_task(new wirelessTask(PRIORITY_CRITICAL));
/* The task for the IR receiver */
// scheduler_add_task(new remoteTask (PRIORITY_LOW));
/* Your tasks should probably used PRIORITY_MEDIUM or PRIORITY_LOW because you
* want the terminal task to always be responsive so you can poke around in
* case something goes wrong.
*/
/**
* This is a the board demonstration task that can be used to test the board.
* This also shows you how to send a wireless packets to other boards.
*/
#if 0
scheduler_add_task(new example_io_demo());
#endif
/**
* Change "#if 0" to "#if 1" to enable examples.
* Try these examples one at a time.
*/
#if 0
scheduler_add_task(new example_task());
scheduler_add_task(new example_alarm());
scheduler_add_task(new example_logger_qset());
scheduler_add_task(new example_nv_vars());
#endif
/**
* Try the rx / tx tasks together to see how they queue data to each other.
*/
#if 0
scheduler_add_task(new queue_tx());
scheduler_add_task(new queue_rx());
#endif
/**
* Another example of shared handles and producer/consumer using a queue.
* In this example, producer will produce as fast as the consumer can consume.
*/
#if 0
scheduler_add_task(new producer());
scheduler_add_task(new consumer());
#endif
/**
* If you have RN-XV on your board, you can connect to Wifi using this task.
* This does two things for us:
* 1. The task allows us to perform HTTP web requests (@see wifiTask)
* 2. Terminal task can accept commands from TCP/IP through Wifly module.
*
* To add terminal command channel, add this at terminal.cpp :: taskEntry() function:
* @code
* // Assuming Wifly is on Uart3
* addCommandChannel(Uart3::getInstance(), false);
* @endcode
*/
#if 0
Uart3 &u3 = Uart3::getInstance();
u3.init(WIFI_BAUD_RATE, WIFI_RXQ_SIZE, WIFI_TXQ_SIZE);
scheduler_add_task(new wifiTask(Uart3::getInstance(), PRIORITY_LOW));
#endif
scheduler_start(); ///< This shouldn't return
return -1;
}
int main(void)
{
u8 i;
u8 RecvBuf[32];
u8 SendBuf[32];
u8 offline = 0;
u8 recv_flag = 0;
u32 pd2ms=0,pd20ams=0,pd20bms=0,pd100ms=0;
SystemInit();
RCC_Configuration();
NVIC_Configuration();
GPIO_Configuration();
USART1_Configuration();
dbgPrintf(" Init Ticktack !\r\n");
cycleCounterInit();
SysTick_Config(SystemCoreClock / 1000);
for(i=0;i<2;i++)
{
OP_LED1;OP_LED2;OP_LED3;OP_LED4;
delay_ms(500);
OP_LED1;OP_LED2;OP_LED3;OP_LED4;
delay_ms(500);
}
FilterInit();
controllerInit();
dbgPrintf(" Init eeprom!\r\n");
FLASH_Unlock();
EE_Init();
EE_Read_ACC_GYRO_Offset();
/* 如果PID丢失或者错误，将下面两行注释去掉，重新编译烧写，运行一遍，可将PID还原，然后重新注释，再烧写一遍 */
//EE_Write_PID();
//EE_Write_Rate_PID();
EE_Read_PID();
EE_Read_Rate_PID();
dbgPrintf(" Init adc!\r\n");
ADC_DMA_Init();
dbgPrintf(" Init NRF24L01 !\r\n");
SPI_NRF_Init();
Nrf24l01_Init();
NRF24l01_SetRxMode();
while(Nrf24l01_Check())
{
dbgPrintf("NRF24L01 Fault !\r\n");
delay_ms(500);
}
dbgPrintf("NRF24L01 Is Detected !\r\n");
dbgPrintf("Init MPU6050...\r\n");
IIC_Init();
MPU6050_initialize();
dbgPrintf("Init Motor...\r\n");
Motor_Init();
Motor_SetPwm(0,0,0,0);
pd20bms = TIMIRQCNT + 10*ITS_PER_MS;
while(1)
{
if(TIMIRQCNT>pd2ms + 2*ITS_PER_MS-1) // every 4ms
{
GetEulerAngle();
if(lock_flag==UNLOCK)
AttitudeToMotors(angle.y,angle.x,angle.z);
else
{
MOTOR1=0;
MOTOR2=0;
MOTOR3=0;
MOTOR4=0;
}
Motor_SetPwm(MOTOR1,MOTOR2,MOTOR3,MOTOR4);
pd2ms = TIMIRQCNT;
}
if(TIMIRQCNT>pd20ams + 20*ITS_PER_MS-1) // every 20ms
{
if(NRF24l01_Recv(RecvBuf)>10)
{
if((RecvBuf[RecvBuf[2]+3]==CheckSum(RecvBuf, RecvBuf[2]+3))&&(RecvBuf[0]==0xAA))
{
if(RecvBuf[1]!=0xC0)
OP_LED1;
offline=0;
switch(RecvBuf[1])
{
case 0xC0: //control
Getdesireddata(RecvBuf);
OP_LED2;
break;
case 0x10: //W PID
SetPID(RecvBuf);
break;
case 0x11: //W Attitude
SetAccGyroOffset(RecvBuf);
break;
case 0x12: //W Control offset
break;
case 0x14: //W Rate PID
SetRatePID(RecvBuf);
break;
case 0x20: //R PID
recv_flag = RESEND;
GetPID(SendBuf);
break;
case 0x21: //R Attitude
recv_flag = RESEND;
GetAccGyroOffset(SendBuf);
break;
case 0x22: //R Control offset
break;
case 0x24: //R Rate PID
recv_flag = RESEND;
GetRatePID(SendBuf);
break;
case 0x40: //校准姿态
EnableCalibration();
break;
case 0x41: //校准遥控器零点
break;
default:
break;
}
}
}
pd20ams = TIMIRQCNT;
}
if(TIMIRQCNT>pd20bms + 20*ITS_PER_MS-1) // every 20ms
{
if(recv_flag==0)
GetState(SendBuf);
else
recv_flag--;
NRF_SendData(SendBuf);
OP_LED3;
pd20bms = TIMIRQCNT;
}
if(TIMIRQCNT>pd100ms + 100*ITS_PER_MS-1) // every 100ms
{
if(offline>20)
lock_flag = LOCK;
offline++;
// OP_LED4;
pd100ms = TIMIRQCNT;
}
}
}
