/*====================================================================================================*/
void System_Init( void )
{
uint8_t state = ERROR;
MPU_InitTypeDef MPU_InitStruct;
HAL_Init();
SystemClock_Config();
GPIO_Config();
Serial_Config();
LED_R_Reset;
Delay_100ms(1);
MPU_InitStruct.MPU_Gyr_FullScale = MPU_GyrFS_2000dps;
MPU_InitStruct.MPU_Gyr_LowPassFilter = MPU_GyrLPS_41Hz;
MPU_InitStruct.MPU_Acc_FullScale = MPU_AccFS_4g;
MPU_InitStruct.MPU_Acc_LowPassFilter = MPU_AccLPS_41Hz;
state = IMU_Init(&MPU_InitStruct);
while(state != SUCCESS) {
LED_G_Toggle;
Delay_100ms(1);
}
LED_G_Reset;
Delay_100ms(1);
}
int main(void)
{
LED_GPIO_Config(); //GPIO管脚初始化
I2Cx_Init();
IMU_Init();
while (1)
{
LED1_ON;
Delay(0XFFFFFF);
LED1_OFF;
Delay(0XFFFFFF);
IMU_GetYawPitchRoll(angles);
}
}
int apps_copter_init(void)
{
rt_thread_t attitude_thread,data_transmit_thread,receiver_thread,attitude_outter_thread;
//初始化IMU（惯性测量单元）
IMU_Init();
//初始化飞行控制
fc.Init();
//初始化参数
Params_Init();
//初始化Transmiter
cmd.Init("tcpserv");
attitude_thread = rt_thread_create("attitude",attitude_thread_entry,RT_NULL,1024,12,5);
if(attitude_thread != RT_NULL)
{
rt_thread_startup(attitude_thread);
}
attitude_outter_thread = rt_thread_create("att_outter",attitude_outter_thread_entry,RT_NULL,1024,11,5);
if(attitude_thread != RT_NULL)
{
rt_thread_startup(attitude_outter_thread);
}
data_transmit_thread = rt_thread_create("transmit",data_transmit_thread_entry,RT_NULL,1024,13,5);
if(data_transmit_thread != RT_NULL)
{
rt_thread_startup(data_transmit_thread);
}
receiver_thread = rt_thread_create("receiver",receiver_thread_entry,RT_NULL,1024,14,5);
if(receiver_thread != RT_NULL)
{
rt_thread_startup(receiver_thread);
}
return 0;
}
void InitAll()
{
__disable_irq();
TimInit();
PWMStart();
InputDecoder();
UartInit();
FlashInit();
SDFatFSInit();
printf("mpu6050 id:0x%x\r\n",MPU6050_Init());
IMU_Init();
UIInit();
sys.status = READY;
HAL_ADC_Start(&hadc1);
printf("init finish!\r\n");
__enable_irq();
}
/**
* @brief 板级初始化
*
* @param none
* @retval none
*/
void BSP_Init (void)
{
LED_Init();
Flash_Read(); //读取参数
Flash_CheckFirstTime(false); //检查FLASH里是否存在有效数据，如果没有则写入默认数据
// cfg.mixerConfiguration = MIXERCONFIG;统一在调参软件里设置机型
Mixer_Init(); //动力初始化
Serial_Init(cfg.serial_baudrate);//上位机调参初始化
I2C2_Init(); //I2C2
Sensor_DetectAndInit();
IMU_Init();
Altitude_Init();
Actuator_Init();
RC_Init();
Daemon_Init();
GPS_Init(cfg.gps_baudrate);
Telemetry_Init();
#ifdef OPTICALFLOW
OpticalFlow_Init();
#endif
EasyCtrl_Init();
VisionLanding_Init();
Avoid_Init();
}
int
main(int argc, char** argv)
{
uint32_t led_colors = 0;
uint8_t at_parm_test[10];
unsigned once;
uint32_t i = 0;
uint8_t canPrescaler = 0;
uint8_t* uart_tx_packet = 0;
uint8_t* uart_rx_packet;
uint32_t old_loadcell_data;
uint16_t timeStep = 1;
clock_init();
pin_init();
led_init();
timers_init();
state_init();
uart_init();
// Set up UART2 for 115200 baud. There's no round() on the dsPICs, so we implement our own.
double brg = (double) 140000000 / 2.0 / 16.0 / 115200.0 - 1.0;
if (brg - floor(brg) >= 0.5) {
brg = ceil(brg);
}
else {
brg = floor(brg);
}
// Uart2Init (brg); // Init UART 2 as 115200 baud/s
loadcell_init();
loadcell_start();
led_rgb_off();
led_rgb_set(50, 0, 100);
can_state.init_return = RET_UNKNOWN;
if (can_init()) {
while (1);
}
timer_state.prev_systime = 0;
timer_state.systime = 0;
#ifdef CONF71
// Start Reading the int pin on IMU
mpuData.startData = 0;
if (IMU_Init(400000, 70000000) == 0) {
// imu_state.init_return = RET_OK;
mpuData.startData = 1;
}
else {
//imu_state.init_return = RET_ERROR;
}
#endif
for (;;) {
if (timer_state.systime != timer_state.prev_systime) {
timer_state.prev_systime = timer_state.systime;
//everything in here will be executed once every ms
//make sure that everything in here takes less than 1ms
//useful for checking state consistency, synchronization, watchdog...
if(timer_state.systime % 1000 == 1) {
LED_4 = 1;
}
led_update();
if (timer_state.systime % 10 == 1) {
IMU_GetQuaternion(quaterion);
QuaternionToYawPitchRoll(quaterion, ypr);
}
if (timer_state.systime % 5 == 1) {
IMU_normalizeData(&mpuData, &magData, &imuData);
// Run AHRS algorithm
IMU_UpdateAHRS (&imuData);
// Run IMU algorithm (does not use MAG data)
// IMU_UpdateIMU(&imuData);
//copy state to CAN dictionary
IMU_CopyOutput(&imuData, &mpuData, &magData);
}
/**
* CANFestival Loop
*/
if (can_state.init_return == RET_OK) {
can_process();
/**
* Sets CANFestival shared variables
* specific to Sensor Board
*/
can_push_state();
}
/**
* Blinking LED Loop
*/
if (timer_state.systime % 25 == 0) {
LED_1 = !LED_1;
}
}
else {
//untimed processes in main loop:
//executed as fast as possible
//these processes should NOT block the main loop
// LED_4 = mpuData.accelX > 0;
// LED_3 = mpuData.accelY > 0;
// LED_1 = mpuData.accelZ > 0;
// IMU_GetData();
IMU_CopyI2CData(&mpuData, &magData);
if (!T1CONbits.TON) {
RGB_RED = 0;
RGB_GREEN = RGB_BLUE = 1;
while (1);
}
if(can_flag){
TimeDispatch();
can_flag = 0;
}
uart_rx_packet = uart_rx_cur_packet();
if (uart_rx_packet != 0) {
uart_rx_packet_consumed();
}
/**
* Handles CAN transmission buffers
*/
if ((txreq_bitarray & 0b00000001) && !C1TR01CONbits.TXREQ0) {
C1TR01CONbits.TXREQ0 = 1;
txreq_bitarray = txreq_bitarray & 0b11111110;
}
if ((txreq_bitarray & 0b00000010) && !C1TR01CONbits.TXREQ1) {
C1TR01CONbits.TXREQ1 = 1;
txreq_bitarray = txreq_bitarray & 0b11111101;
}
if ((txreq_bitarray & 0b00000100) && !C1TR23CONbits.TXREQ2) {
C1TR23CONbits.TXREQ2 = 1;
txreq_bitarray = txreq_bitarray & 0b11111011;
}
if ((txreq_bitarray & 0b00001000) && !C1TR23CONbits.TXREQ3) {
C1TR23CONbits.TXREQ3 = 1;
txreq_bitarray = txreq_bitarray & 0b11110111;
}
if ((txreq_bitarray & 0b00010000) && !C1TR45CONbits.TXREQ4) {
C1TR45CONbits.TXREQ4 = 1;
txreq_bitarray = txreq_bitarray & 0b11101111;
}
if ((txreq_bitarray & 0b00100000) && !C1TR45CONbits.TXREQ5) {
C1TR45CONbits.TXREQ5 = 1;
txreq_bitarray = txreq_bitarray & 0b11011111;
}
if ((txreq_bitarray & 0b01000000) && !C1TR67CONbits.TXREQ6) {
C1TR67CONbits.TXREQ6 = 1;
txreq_bitarray = txreq_bitarray & 0b10111111;
}
}
}
return (EXIT_SUCCESS);
}
