136 lines
4.4 KiB
C
136 lines
4.4 KiB
C
/*
|
||
main_control Task
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "task\user_task.h"
|
||
#include "main.h"
|
||
#include "module/xm_quadctrl.h"
|
||
#include "device/buzzer.h"
|
||
#include "bsp/time.h"
|
||
#include "bsp/pwm.h"
|
||
#include "component/ahrs.h"
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
//uint32_t main_control_cnt_last;
|
||
//float main_control_dt;
|
||
|
||
//SensorData sensors = {0};
|
||
// main_control_rc;
|
||
|
||
//uint8_t main_control_running_states;
|
||
/* Private function --------------------------------------------------------- */
|
||
|
||
/* Exported functions ------------------------------------------------------- */
|
||
/**
|
||
* \brief main_control Task
|
||
*
|
||
* \param argument 未使用
|
||
*/
|
||
|
||
// Control_t ctrl;
|
||
void Task_main_control(void *argument) {
|
||
(void)argument; /* 未使用argument，消除警告 */
|
||
|
||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||
const uint32_t delay_tick = osKernelGetTickFreq() / MAIN_CTRL_INIT_DELAY;
|
||
|
||
osDelay(MAIN_CTRL_FREQ); /* 延时一段时间再开启任务 */
|
||
|
||
|
||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||
|
||
// FlightController_Init(MAIN_CTRL_INIT_DELAY);
|
||
// BSP_PWM_Start(BSP_PWM_MOTOR_OUT1);
|
||
// BSP_PWM_Start(BSP_PWM_MOTOR_OUT2);
|
||
// BSP_PWM_Start(BSP_PWM_MOTOR_OUT3);
|
||
// BSP_PWM_Start(BSP_PWM_MOTOR_OUT4);
|
||
while (1) {
|
||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||
|
||
/*User code begin*/
|
||
// AHRS_Accl_t accl_data;
|
||
// AHRS_Gyro_t gyro_data;
|
||
// float fixed_height_buffer;
|
||
// float speed_z_buffer;
|
||
// osMessageQueueGet(task_runtime.msgq.accl, &accl_data, NULL, 0);
|
||
// osMessageQueueGet(task_runtime.msgq.gyro, &gyro_data, NULL, 0);
|
||
// osMessageQueueGet(task_runtime.msgq.fixed_height, &fixed_height_buffer, NULL, 0);
|
||
// osMessageQueueGet(task_runtime.msgq.speed_z, &speed_z_buffer, NULL, 0);
|
||
// osMessageQueueGet(task_runtime.msgq.rc, &main_control_rc, NULL, 0);
|
||
// sensors.accel[0] = accl_data.x;
|
||
// sensors.accel[1] = accl_data.y;
|
||
// sensors.accel[2] = accl_data.z;
|
||
// sensors.gyro[0] = gyro_data.x;
|
||
// sensors.gyro[1] = gyro_data.y;
|
||
// sensors.gyro[2] = gyro_data.z;
|
||
// sensors.baro_alt = fixed_height_buffer;
|
||
// sensors.speed_z = speed_z_buffer;
|
||
// // 获取遥控器输入
|
||
|
||
// main_control_dt = DWT_GetDeltaT(&main_control_cnt_last);
|
||
|
||
// // 执行控制计算
|
||
//// Control_t ctrl;
|
||
// FlightController_Update(sensors, &ctrl, main_control_dt, main_control_rc);
|
||
// float output_check=0;
|
||
// switch(main_control_running_states){
|
||
// case 0:
|
||
// output_check = 0.275+0.21*0.8*((main_control_rc.Rocker_LY-0.5)*2);
|
||
// BSP_PWM_Set(BSP_PWM_MOTOR_OUT1, output_check);
|
||
// BSP_PWM_Set(BSP_PWM_MOTOR_OUT2, output_check);
|
||
// BSP_PWM_Set(BSP_PWM_MOTOR_OUT3, output_check);
|
||
// BSP_PWM_Set(BSP_PWM_MOTOR_OUT4, output_check);
|
||
// if(main_control_rc.Key_R_Up == 1){
|
||
// main_control_running_states=1;
|
||
// }else if(main_control_rc.Key_R_Right == 1){
|
||
// main_control_running_states=4;
|
||
// }
|
||
// break;
|
||
// case 1:
|
||
// BSP_BUZZER_Set(BSP_BUZZER_ON, BSP_BUZZER_C5, 1);
|
||
// osDelay(500);
|
||
// BSP_BUZZER_Stop();
|
||
// osDelay(2000);
|
||
// main_control_running_states=2;
|
||
// if(main_control_rc.Key_R_Right == 1){
|
||
// main_control_running_states=4;
|
||
// }
|
||
// break;
|
||
// case 2:
|
||
// MotorMixer_Init();
|
||
// Apply_Motor_Outputs(ctrl);
|
||
// if(main_control_rc.Key_R_Down == 1){
|
||
// main_control_running_states=3;
|
||
// }else if(main_control_rc.Key_R_Right == 1){
|
||
// main_control_running_states=4;
|
||
// }
|
||
// break;
|
||
// case 3:
|
||
// BSP_PWM_Set(BSP_PWM_MOTOR_OUT1, 0.275);
|
||
// BSP_PWM_Set(BSP_PWM_MOTOR_OUT2, 0.275);
|
||
// BSP_PWM_Set(BSP_PWM_MOTOR_OUT3, 0.275);
|
||
// BSP_PWM_Set(BSP_PWM_MOTOR_OUT4, 0.275);
|
||
// main_control_running_states=0;
|
||
// if(main_control_rc.Key_R_Right == 1){
|
||
// main_control_running_states=4;
|
||
// }
|
||
// break;
|
||
// case 4:
|
||
// MotorMixer_OutStop();
|
||
// break;
|
||
// default:
|
||
// break;
|
||
// }
|
||
// BSP_PWM_Set(BSP_PWM_MOTOR_OUT1, 0.275);
|
||
// BSP_PWM_Set(BSP_PWM_MOTOR_OUT2, 0.275);
|
||
// BSP_PWM_Set(BSP_PWM_MOTOR_OUT3, 0.275);
|
||
// BSP_PWM_Set(BSP_PWM_MOTOR_OUT4, 0.275);
|
||
/*User code end*/
|
||
|
||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||
}
|
||
}
|