#include "soft_rc_input.h" #include "auto_pilot.h" #include "common.h" #include "control_throttle.h" #include "hard_rc_sbus.h" #include "soft_flash.h" #include "soft_time.h" #include "ver_config.h" /* sbus 信号状态 */ typedef enum { SBUS_SIGNAL_OK = 0x00, SBUS_SIGNAL_LOST = 0x01, SBUS_SIGNAL_FAILSAFE = 0x03, } RCSbus_SignalStatus; //****************************************** //摇杆输入 short rock_in[4] = {1500, 1500, 1500, 1500}; //摇杆按键 char rock_key = KEY_UP; //****************************************** // PWM输入的值 单位 us //不上电时是默认值 short rc_in[RC_INPUT_CH_NUM] = {1500, 1500, 1500, 1500, 1200, 1800, 1800}; // pwm输入的原始值 //不上电时是默认值 short raw_rc_in[RC_INPUT_CH_NUM] = {1500, 1500, 1500, 1500, 1200, 1800, 1800}; /**************************实现函数******************************************** *函数原型: void rc_input_initial(void) *功　　能: 输入初始化 *******************************************************************************/ void rc_input_initial(void) { rc_sbus_init(SBUS_BPS); } /**************************实现函数******************************************** *函数原型: void get_rc_value(void) *功　　能: 获取get_rc的输入值 *返 回 值： 1 正常 0 异常 *******************************************************************************/ unsigned char subs_failsafe_status; //遥控器是否有效 comp_status comp_rc_status = COMP_NOEXIST; // sbus 失联记时 static uint32_t sbus_data_link_lost_time_us = 0; //遥控器健康标志位 RcSignalHealthType rc_signal_health = RC_SIGNAL_HEALTH; //遥控器输入中间量 short tmp_rc_in[RC_INPUT_CH_NUM] = {0}; void get_rc_value(void) { uint8_t i = 0; static short health_counts = 0; static short fail_counts = 0; static short bad_counts = 0; //如果遥控器有效 if (comp_rc_status == COMP_NORMAL) { for (i = 0; i < RC_CALIB_CH_NUM; i++) { if (raw_rc_in[i] >= rc_cal_offset[i]) { tmp_rc_in[i] = (raw_rc_in[i] - rc_cal_offset[i]) * rc_cal_factor_up[i] / 1000 + 1500; } else if (raw_rc_in[i] < rc_cal_offset[i]) { tmp_rc_in[i] = (raw_rc_in[i] - rc_cal_offset[i]) * rc_cal_factor_down[i] / 1000 + 1500; } } for (uint8_t i = RC_CH5; i < RC_INPUT_CH_NUM; ++i) { tmp_rc_in[i] = raw_rc_in[i]; } //所有通道都在最大最小值得范围内,所有数据正常 if ((tmp_rc_in[RC_CH1] >= RC_IN_MIN && tmp_rc_in[RC_CH1] <= RC_IN_MAX) && (tmp_rc_in[RC_CH2] >= RC_IN_MIN && tmp_rc_in[RC_CH2] <= RC_IN_MAX) && (tmp_rc_in[RC_CH3] >= RC_IN_MIN && tmp_rc_in[RC_CH3] <= RC_IN_MAX) && (tmp_rc_in[RC_CH4] >= RC_IN_MIN && tmp_rc_in[RC_CH4] <= RC_IN_MAX) && (tmp_rc_in[RC_CH5] >= RC_IN_MIN && tmp_rc_in[RC_CH5] <= RC_IN_MAX) && (tmp_rc_in[RC_CH6] >= RC_IN_MIN && tmp_rc_in[RC_CH6] <= RC_IN_MAX) && (tmp_rc_in[RC_CH7] >= RC_IN_MIN && tmp_rc_in[RC_CH7] <= RC_IN_MAX) && (subs_failsafe_status == SBUS_SIGNAL_OK)) { //正常数据，赋值给rc_in for (i = 0; i < RC_INPUT_CH_NUM; i++) { rc_in[i] = tmp_rc_in[i]; } //出错或失控后如果400ms都正常，就恢复遥控器的值 if ((health_counts++) >= 25) { rc_signal_health = RC_SIGNAL_HEALTH; health_counts = 25; } fail_counts = 0; bad_counts = 0; } //失控保护,futaba的遥控器的拨码开关不是直接跳变，会有中间值出现，所以要加缓冲 else if ((tmp_rc_in[RC_CH1] >= RC_IN_MIN && tmp_rc_in[RC_CH1] <= RC_IN_MAX) && (tmp_rc_in[RC_CH2] >= RC_IN_MIN && tmp_rc_in[RC_CH2] <= RC_IN_MAX) && (tmp_rc_in[RC_CH3] >= RC_IN_MIN && tmp_rc_in[RC_CH3] <= RC_IN_MAX) && (tmp_rc_in[RC_CH4] >= RC_IN_MIN && tmp_rc_in[RC_CH4] <= RC_IN_MAX) && (tmp_rc_in[RC_CH5] >= RC_IN_MIN && tmp_rc_in[RC_CH5] <= RC_IN_MAX) && (tmp_rc_in[RC_CH6] >= RC_IN_MIN && tmp_rc_in[RC_CH6] <= RC_IN_MAX) && ((tmp_rc_in[RC_CH7] >= RC_FS_MIN && tmp_rc_in[RC_CH7] <= RC_FS_MAX) || (subs_failsafe_status != SBUS_SIGNAL_OK))) { //失控保护数据，赋值给rc_in for (i = 0; i < RC_INPUT_CH_NUM; i++) { rc_in[i] = tmp_rc_in[i]; } if ((fail_counts++) >= 50) { rc_signal_health = RC_SIGNAL_FAIL; fail_counts = 50; } health_counts = 0; bad_counts = 0; } //跳出最大最小值的范围就认为遥控器出错,遥控器出错不更新rc_in的值，持续出错执行错误保护 else { //数据错误后都赋值为默认值，赋值给rc_in for (i = 0; i < 4; i++) { rc_in[i] = 1500; } // GPS模式 rc_in[RC_CH5] = 1200; //机头锁定 rc_in[RC_CH6] = 1800; //待命 rc_in[RC_CH7] = 1800; if ((bad_counts++) >= 25) { rc_signal_health = RC_SIGNAL_BAD; bad_counts = 25; } health_counts = 0; fail_counts = 0; } } } /** * @brief sbus 信号转 pwm */ void sbus2pwm(int16_t *pwm, int16_t *sbus_ch) { uint8_t i = 0; for (i = 0; i < RC_INPUT_CH_NUM; i++) { pwm[i] = (sbus_ch[i] - 1024) * 420 / 672 + 1500; } } /** * @brief sbus 信号解析 */ void sbus_channel_anslysis(uint8_t *sbus_data) { static int16_t channels[SBUS_CH] = {1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 0, 0}; // buf1的8位作为ch1的低8位，buf2的低3位作为ch1的高3位，组成ch1的11位。 channels[0] = ((sbus_data[1] | sbus_data[2] << 8) & 0x07FF); // buf2的高5位作为ch2的低5位，buf3的低6位作为ch2的高6位，组成ch2的11位。 channels[1] = ((sbus_data[2] >> 3 | sbus_data[3] << 5) & 0x07FF); //以此类推，每个通道占11位 channels[2] = ((sbus_data[3] >> 6 | sbus_data[4] << 2 | sbus_data[5] << 10) & 0x07FF); channels[3] = ((sbus_data[5] >> 1 | sbus_data[6] << 7) & 0x07FF); channels[4] = ((sbus_data[6] >> 4 | sbus_data[7] << 4) & 0x07FF); channels[5] = ((sbus_data[7] >> 7 | sbus_data[8] << 1 | sbus_data[9] << 9) & 0x07FF); channels[6] = ((sbus_data[9] >> 2 | sbus_data[10] << 6) & 0x07FF); channels[7] = ((sbus_data[10] >> 5 | sbus_data[11] << 3) & 0x07FF); #ifdef ALL_CHANNELS // & the other 8 + 2 channels if you need them channels[8] = ((sbus_data[12] | sbus_data[13] << 8) & 0x07FF); channels[9] = ((sbus_data[13] >> 3 | sbus_data[14] << 5) & 0x07FF); channels[10] = ((sbus_data[14] >> 6 | sbus_data[15] << 2 | sbus_data[16] << 10) & 0x07FF); channels[11] = ((sbus_data[16] >> 1 | sbus_data[17] << 7) & 0x07FF); channels[12] = ((sbus_data[17] >> 4 | sbus_data[18] << 4) & 0x07FF); channels[13] = ((sbus_data[18] >> 7 | sbus_data[19] << 1 | sbus_data[20] << 9) & 0x07FF); channels[14] = ((sbus_data[20] >> 2 | sbus_data[21] << 6) & 0x07FF); channels[15] = ((sbus_data[21] >> 5 | sbus_data[22] << 3) & 0x07FF); // DigiChannel 1 if (sbus_data[23] & (1 << 0)) { channels[16] = 1; } else { channels[16] = 0; } // DigiChannel 2 if (sbus_data[23] & (1 << 1)) { channels[17] = 1; } else { channels[17] = 0; } #endif // Failsafe subs_failsafe_status = SBUS_SIGNAL_OK; //信号丢失 if (sbus_data[23] & (1 << 2)) { subs_failsafe_status = SBUS_SIGNAL_LOST; } //失控保护激活 if (sbus_data[23] & (1 << 3)) { subs_failsafe_status = SBUS_SIGNAL_FAILSAFE; } sbus2pwm(raw_rc_in, channels); } /** * @brief 串口接收到sbus信号的钩子函数 */ void recv_rcsbus_data_hookfunction(unsigned int len) { //中断处理函数 if (len == 25 && uart2_dma_rx_buf[0] == 0x0f && (uart2_dma_rx_buf[24] == 0x00 || uart2_dma_rx_buf[24] == 0x04 || uart2_dma_rx_buf[24] == 0x14 || uart2_dma_rx_buf[24] == 0x24 || uart2_dma_rx_buf[24] == 0x34)) { comp_rc_status = COMP_NORMAL; sbus_data_link_lost_time_us = 0; sbus_channel_anslysis(uart2_dma_rx_buf); } } /** * @brief sbus 输入信号失联判断 * @param dt_s 运行间隔 s */ void check_sbus_link_status(float dt_s) { if (COMP_NORMAL == comp_rc_status) { sbus_data_link_lost_time_us += dt_s * 1000000; if (sbus_data_link_lost_time_us > 500 * 1000) { comp_rc_status = COMP_LOST; } } } unsigned int rc_cal_time = 0; //用来记录遥控器输入的最大最小值,全部初始化为1500.。。。 int rc_cal_max[RC_CALIB_CH_NUM] = {1500, 1500, 1500, 1500}, rc_cal_min[RC_CALIB_CH_NUM] = {1500, 1500, 1500, 1500}; //开始遥控器校准的参数 RCCalibStatusType rc_cal_flag = RC_CALIB_NO; bool rc_offset_capture_flag = false; // 遥控器各通道的中立位 short rc_cal_offset[RC_CALIB_CH_NUM] = {1500, 1500, 1500, 1500}; void rc_input_calib_start(void) { if (rc_cal_flag == RC_CALIB_NO) { //赋值进入遥控器校准模式 pilot_mode = PILOT_RC_CLB; rc_cal_flag = RC_CALIB_START; rc_offset_capture_flag = true; //点击遥控器校准后需要延时一段时间，，不然会记录一个原先的值。填充滤波新数据额需要时间，，， rc_cal_time = micros(); } } void rc_input_calib_end(void) { //开始校准并且上锁状态下 if ((rc_cal_flag == RC_CALIB_START) && (thr_lock_status == LOCKED)) { rc_cal_flag = RC_CALIB_NO; write_rcfactor_flag = true; //点击校准结束后，绿灯灭，只剩红灯 } } void rc_input_calibration(void) { uint8_t i = 0; //捕获中立位同时清除以前记录的最大最小值 if (rc_offset_capture_flag == true) { for (i = 0; i < RC_CALIB_CH_NUM; i++) { //记录中立位 rc_cal_offset[i] = raw_rc_in[i]; //初始化最大最小值 rc_cal_min[i] = 1500; rc_cal_max[i] = 1500; } //如果校准时油门没放在中间，就不记录油门的中立位 if (rc_cal_offset[RC_CH3] < 1350 || rc_cal_offset[RC_CH3] > 1650) { rc_cal_offset[RC_CH3] = 1500; } rc_offset_capture_flag = false; } for (i = 0; i < RC_CALIB_CH_NUM; i++) { //记录CH的最大最小值 if (rc_cal_max[i] < raw_rc_in[i]) { rc_cal_max[i] = raw_rc_in[i]; } if (rc_cal_min[i] > raw_rc_in[i]) { rc_cal_min[i] = raw_rc_in[i]; } } } //比例系数,,实际的比例系数放大1000倍后的值，便于flash存储 short rc_cal_factor_up[RC_CALIB_CH_NUM] = {1000, 1000, 1000, 1000}; short rc_cal_factor_down[RC_CALIB_CH_NUM] = {1000, 1000, 1000, 1000}; /** * @brief 计算遥控器校准系数 */ void calc_rcfactor_calibration(void) { unsigned char i = 0; for (i = 0; i < RC_CALIB_CH_NUM; i++) { rc_cal_factor_up[i] = (short)((1000 * 500) / (rc_cal_max[i] - rc_cal_offset[i])); rc_cal_factor_down[i] = (short)((1000 * 500) / (rc_cal_offset[i] - rc_cal_min[i])); } }