527 lines
15 KiB
C
527 lines
15 KiB
C
#include "MAIN.H"
|
||
//can信息
|
||
UnInfCan UnInfCan_1 = {0};
|
||
UnSwSample UnSwSample_1 = {0};
|
||
UnSwOut UnSwOut_1 = {0};
|
||
UnRemoteControlOutput UnRemoteControlOutput_1 = {0,0,0,0,0,0,0,0};//遥控器数据发送
|
||
UnRemoteControlOutput UnRemoteControlOutput_2 = {0,0,0,0,0,0,0,0};//遥控器数据发送
|
||
|
||
SBusData sbus_data;
|
||
|
||
//CAN发送周期
|
||
uword CntCan_1 = 0;
|
||
bit FlgCan_1 = 0;
|
||
|
||
//第一次采集到有效数据指示
|
||
bit FlgOneTime = 0;
|
||
ubyte uart2RxFlag = 0;
|
||
uword RgCanPerid = 50;
|
||
|
||
//PWM 20181227
|
||
ubyte PwmH[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
|
||
//-----------------------------------------------------------------------
|
||
//RTC定时器
|
||
//-----------------------------------------------------------------------
|
||
void RTCProcess(void)
|
||
{
|
||
//************************************************************************
|
||
//////CAN发送周期
|
||
if(FlgCan_1){CntCan_1++;if(CntCan_1 >= RgCanPerid){FlgCan_1=0; CntCan_1=0;}}else{CntCan_1=0;}
|
||
|
||
//************************************************************************
|
||
}
|
||
|
||
// SBus 数据解析函数
|
||
SBusData* parseSBusData(const uint8_t *input_data)
|
||
{
|
||
// 解析16个通道的数据，每个通道为11位
|
||
sbus_data.channels[0] = (input_data[1] | input_data[2] << 8) & 0x07FF;
|
||
sbus_data.channels[1] = (input_data[2] >> 3 | input_data[3] << 5) & 0x07FF;
|
||
sbus_data.channels[2] = (input_data[3] >> 6 | input_data[4] << 2 | input_data[5] << 10) & 0x07FF;
|
||
sbus_data.channels[3] = (input_data[5] >> 1 | input_data[6] << 7) & 0x07FF;
|
||
sbus_data.channels[4] = (input_data[6] >> 4 | input_data[7] << 4) & 0x07FF;
|
||
sbus_data.channels[5] = (input_data[7] >> 7 | input_data[8] << 1 | input_data[9] << 9) & 0x07FF;
|
||
|
||
sbus_data.channels[6] = (input_data[9] >> 2 | input_data[10] << 6) & 0x07FF;
|
||
sbus_data.channels[7] = (input_data[10] >> 5 | input_data[11] << 3) & 0x07FF;
|
||
sbus_data.channels[8] = (input_data[12] | input_data[13] << 8) & 0x07FF;
|
||
sbus_data.channels[9] = (input_data[13] >> 3 | input_data[14] << 5) & 0x07FF;
|
||
sbus_data.channels[10] = (input_data[14] >> 6 | input_data[15] << 2 | input_data[15] << 10) & 0x07FF;
|
||
sbus_data.channels[11] = (input_data[16] >> 1 | input_data[17] << 7) & 0x07FF;
|
||
sbus_data.channels[12] = (input_data[17] >> 4 | input_data[18] << 4) & 0x07FF;
|
||
sbus_data.channels[13] = (input_data[18] >> 7 | input_data[19] << 1 | input_data[20] << 9) & 0x07FF;
|
||
sbus_data.channels[14] = (input_data[20] >> 2 | input_data[21] << 6) & 0x07FF;
|
||
sbus_data.channels[15] = (input_data[21] >> 5 | input_data[22] << 3) & 0x07FF;
|
||
|
||
// 解析标志字节
|
||
sbus_data.flags = input_data[23];
|
||
uart2RxFlag = 1;
|
||
|
||
// for(ubyte i=0;i<8;i++)
|
||
// {
|
||
// printf( "%d ",i);
|
||
// printf( "%04x\r\n",sbus_data.channels[i]);
|
||
// }
|
||
// printf("%d\r\n",sbus_data.channels[0]);
|
||
return &sbus_data;
|
||
}
|
||
|
||
//can错误处理
|
||
void CanErrorProcess(void)
|
||
{
|
||
ubyte ubNSRL;
|
||
ubyte ubNSRH;
|
||
ubyte ubResetLEC = 0x3F;
|
||
//-------------------------------------------
|
||
EA = 0;
|
||
|
||
CAN_pushAMRegs(); // push the CAN Access Mediator Registers
|
||
SFR_PAGE(_su0, SST0); // switch to page 0
|
||
|
||
CAN_vWriteCANAddress(CAN_NSR0);
|
||
CAN_vReadEN();
|
||
ubNSRL = CAN_DATA0;
|
||
ubNSRH = CAN_DATA1;
|
||
if (ubNSRL & 0x20) // if ALERT
|
||
{
|
||
CAN_vInit();
|
||
}
|
||
if (ubNSRL & 0x07) // if LEC
|
||
{
|
||
ubResetLEC = 0x38;
|
||
}
|
||
//// Reset LEC, TXOK, RXOK, ALERT, EWRN, BOFF, LLE, LOE (if set)
|
||
CAN_vWriteCANAddress(CAN_NSR0); // Addressing CAN_NSR0
|
||
CAN_DATA0 = ~(ubNSRL & ubResetLEC); // load CAN_NSR0 status register[7-0]
|
||
CAN_DATA1 = ~(ubNSRH); // load CAN_NSR0 status register[15-8]
|
||
CAN_vWriteEN(D0_VALID+D1_VALID); // Data0 and Data1 are Valid for
|
||
|
||
SFR_PAGE(_su0, RST0); // restore the old SCU page
|
||
CAN_popAMRegs(); // restore the CAN Access Mediator Registers
|
||
|
||
EA = 1;
|
||
}
|
||
|
||
|
||
void CanTransmit(ubyte index,ubyte *buf)
|
||
{
|
||
ubyte CanTransmitbuf[8] = {0,0,0,0,0,0,0,0};
|
||
//----------------- --------
|
||
CanTransmitbuf[2] = buf[0];
|
||
CanTransmitbuf[3] = buf[1];
|
||
CanTransmitbuf[0] = buf[2];
|
||
CanTransmitbuf[1] = buf[3];
|
||
CanTransmitbuf[6] = buf[4];
|
||
CanTransmitbuf[7] = buf[5];
|
||
CanTransmitbuf[4] = buf[6];
|
||
CanTransmitbuf[5] = buf[7];
|
||
|
||
CAN_waitTransmit(index);//发送前等待上一帧发送完毕，这样不会丢帧
|
||
CAN_vLoadData(index, (ulong *)CanTransmitbuf);// 装载数据
|
||
CAN_vTransmit(index);
|
||
}
|
||
|
||
typedef enum {
|
||
STT_INIT_LIGHT, // 必须保留的初始状态
|
||
STT_STABLE, // 稳定态
|
||
STT_BLINK_CHECK, // 闪烁检测
|
||
STT_BLINKING // 闪烁保持
|
||
} BlinkState;
|
||
|
||
typedef struct {
|
||
BlinkState current_state;
|
||
uint8_t last_input;
|
||
uint16_t unified_timer; // 统一计时器[7](@ref)
|
||
uint8_t trans_count; // 保留必要的事件计数器
|
||
uint8_t stable_type;
|
||
} BlinkDetector;
|
||
|
||
BlinkDetector blink_detector = {0};
|
||
|
||
/* LED状态检测核心状态机（统一计时器优化版）
|
||
* 参数：
|
||
* ctx - 状态机上下文指针（需保证非空）
|
||
* current_input- 当前输入电平（0:灭 1:亮）
|
||
* output - 输出状态指针（0:无效 1:稳定亮 2:稳定灭 3:闪烁）
|
||
* 设计原则：
|
||
* 1. 单一定时器复用机制[8](@ref)
|
||
* 2. 防御性清零策略[1](@ref)
|
||
* 3. 状态迁移原子操作[9](@ref)
|
||
*/
|
||
void detectLedState(BlinkDetector* ctx, uint8_t current_input, uint8_t* output)
|
||
{
|
||
*output = 0;
|
||
|
||
switch(ctx->current_state)
|
||
{
|
||
/* 关键初始状态：确保首次正确捕获输入 */
|
||
case STT_INIT_LIGHT:
|
||
{
|
||
ctx->last_input = current_input; // 强制记录初始值
|
||
ctx->stable_type = current_input ? 1 : 2;
|
||
ctx->current_state = STT_STABLE; // 立即进入稳定态
|
||
ctx->unified_timer = 0; // 初始化计时器
|
||
*output = 0;
|
||
break;
|
||
}
|
||
|
||
case STT_STABLE:
|
||
{
|
||
if(current_input != ctx->last_input)
|
||
{
|
||
ctx->current_state = STT_BLINK_CHECK;
|
||
ctx->trans_count = 1;
|
||
ctx->unified_timer = 0;
|
||
ctx->last_input = current_input;
|
||
ctx->stable_type = current_input ? 1 : 2; // 同步状态类型[4](@ref)
|
||
}
|
||
else if(ctx->unified_timer >= STABLE_CONFIRM_TIME)
|
||
{
|
||
*output = ctx->stable_type;
|
||
ctx->unified_timer = 0;
|
||
ctx->stable_type = current_input ? 1 : 2; // 二次确认[10](@ref)
|
||
}
|
||
else
|
||
{
|
||
ctx->unified_timer++;
|
||
*output = ctx->stable_type;
|
||
}
|
||
break;
|
||
}
|
||
/* 闪烁检测态：在时间窗口内统计电平跳变次数 */
|
||
case STT_BLINK_CHECK:
|
||
{
|
||
if(ctx->unified_timer >= BLINK_WINDOW) // 达到检测窗口时长[8](@ref)
|
||
{
|
||
if(ctx->trans_count >= MIN_BLINKS) // 满足最小闪烁次数
|
||
{
|
||
ctx->current_state = STT_BLINKING;// 进入闪烁保持态
|
||
ctx->unified_timer = 0; // 重置超时计时器
|
||
}
|
||
else // 未达闪烁阈值
|
||
{
|
||
ctx->current_state = STT_STABLE; // 返回稳定态
|
||
ctx->unified_timer = 0; // 跨状态复用计时器
|
||
}
|
||
}
|
||
else // 窗口期内检测
|
||
{
|
||
ctx->unified_timer++; // 窗口期计时累计
|
||
if(current_input != ctx->last_input)
|
||
{
|
||
ctx->trans_count++; // 跳变次数累计
|
||
ctx->last_input = current_input; // 更新最新输入值
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
|
||
/* 闪烁保持态：监测闪烁活动持续性 */
|
||
case STT_BLINKING:
|
||
{
|
||
// 新增输入变化检测
|
||
if(current_input != ctx->last_input)
|
||
{
|
||
ctx->unified_timer = 0; // 重置超时计时器
|
||
ctx->last_input = current_input;
|
||
*output = 3; // 持续输出闪烁状态
|
||
}
|
||
else if(ctx->unified_timer >= BLINK_TIMEOUT)
|
||
{
|
||
ctx->current_state = STT_STABLE;
|
||
ctx->stable_type = current_input ? 1 : 2; // 根据最终状态更新类型[4](@ref)
|
||
ctx->unified_timer = 0;
|
||
}
|
||
else
|
||
{
|
||
ctx->unified_timer ++; // 仅当输入稳定时累计计时
|
||
*output = 3; // 持续输出闪烁状态
|
||
}
|
||
break;
|
||
}
|
||
|
||
default:break;
|
||
}
|
||
}
|
||
|
||
//SBUS和CAN的数据转换
|
||
void sbusCanDecode(void)
|
||
{
|
||
float SBUS2CANdecodeTemp = 0.0;
|
||
|
||
//----------------------------------------------
|
||
if(uart2RxFlag == 1)
|
||
{
|
||
uart2RxFlag = 0;
|
||
|
||
//转弯
|
||
SBUS2CANdecodeTemp = (float)(sbus_data.channels[0]);
|
||
SBUS2CANdecodeTemp = (DF_ZeroValue - SBUS2CANdecodeTemp)*23;//需要变换一下符号，因为加油是正
|
||
DF_Curvature = (int16_t)SBUS2CANdecodeTemp;//曲率
|
||
if( (DF_Curvature > (-200)) && (DF_Curvature < 200) ) //增加回0的死区，解决会中问题。
|
||
{
|
||
DF_Curvature = 0;
|
||
}
|
||
|
||
//转速
|
||
SBUS2CANdecodeTemp = (float)(sbus_data.channels[1]);
|
||
SBUS2CANdecodeTemp = (DF_ZeroValue - SBUS2CANdecodeTemp)*2.3;//需要变换一下符号，因为加油是正
|
||
DF_Speed = (int16_t)SBUS2CANdecodeTemp;//转速
|
||
if( (DF_Speed > (-50)) && (DF_Speed < 50) ) //增加回0的死区，解决会中问题。
|
||
{
|
||
DF_Speed = 0;
|
||
}
|
||
|
||
//备用
|
||
SBUS2CANdecodeTemp = (float)(sbus_data.channels[11]);
|
||
SBUS2CANdecodeTemp = (DF_ZeroValue - SBUS2CANdecodeTemp);//需要变换一下符号，因为加油是正
|
||
DF_Reserve = (int16_t)SBUS2CANdecodeTemp;//转速
|
||
if( (DF_Reserve > (-50)) && (DF_Reserve < 50) ) //增加回0的死区，解决会中问题。
|
||
{
|
||
DF_Reserve = 0;
|
||
}
|
||
|
||
|
||
//SwA
|
||
if(sbus_data.channels[12] >= 1700)//按下
|
||
{
|
||
DF_SwA = 1;
|
||
}
|
||
else
|
||
{
|
||
DF_SwA = 0;
|
||
}
|
||
|
||
//SwB
|
||
if(sbus_data.channels[4] >= 1700)//第三档
|
||
{
|
||
DF_SwB = 2;
|
||
}
|
||
else if(sbus_data.channels[4] <= 500)//第二档
|
||
{
|
||
DF_SwB = 0;
|
||
}
|
||
else//第一档
|
||
{
|
||
DF_SwB = 1;
|
||
}
|
||
|
||
//SwC
|
||
if(sbus_data.channels[7] >= 1700)//第三档
|
||
{
|
||
DF_SwC = 2;
|
||
}
|
||
else if(sbus_data.channels[7] <= 500)//第二档
|
||
{
|
||
DF_SwC = 0;
|
||
}
|
||
else//第一档
|
||
{
|
||
DF_SwC = 1;
|
||
}
|
||
|
||
//SwD
|
||
if(sbus_data.channels[13] >= 1700)//按下
|
||
{
|
||
DF_SwD = 1;
|
||
}
|
||
else
|
||
{
|
||
DF_SwD = 0;
|
||
}
|
||
|
||
if(0 == sbus_data.flags)//23通道数据 为0标志手柄连接正常
|
||
{
|
||
DF_RCHSta = 1;//手柄在线
|
||
}
|
||
else
|
||
{
|
||
DF_RCHSta = 0;//手柄离线
|
||
}
|
||
|
||
SBUS2CANdecodeTemp = (float)(sbus_data.channels[2]);
|
||
UnRemoteControlOutput_2.bit_data.speed = (int16_t)SBUS2CANdecodeTemp;//保留轴
|
||
|
||
SBUS2CANdecodeTemp = (float)(sbus_data.channels[3]);
|
||
UnRemoteControlOutput_2.bit_data.curvature = (int16_t)SBUS2CANdecodeTemp;//保留轴
|
||
|
||
UnRemoteControlOutput_2.bit_data.reserve1 = (uint16_t)(sbus_data.channels[10]);
|
||
|
||
|
||
//SwB
|
||
if(sbus_data.channels[5] >= 1700)//第三档
|
||
{
|
||
UnRemoteControlOutput_2.bit_data.switch_b = 2;
|
||
}
|
||
else if(sbus_data.channels[5] <= 500)//第二档
|
||
{
|
||
UnRemoteControlOutput_2.bit_data.switch_b = 0;
|
||
}
|
||
else//第一档
|
||
{
|
||
UnRemoteControlOutput_2.bit_data.switch_b = 1;
|
||
}
|
||
|
||
//SwC
|
||
if(sbus_data.channels[6] >= 1700)//第三档
|
||
{
|
||
UnRemoteControlOutput_2.bit_data.switch_c = 2;
|
||
}
|
||
else if(sbus_data.channels[6] <= 500)//第二档
|
||
{
|
||
UnRemoteControlOutput_2.bit_data.switch_c = 0;
|
||
}
|
||
else//第一档
|
||
{
|
||
UnRemoteControlOutput_2.bit_data.switch_c = 1;
|
||
}
|
||
|
||
}
|
||
|
||
}
|
||
|
||
//CAN发送接收
|
||
void CanRXTX(void)
|
||
{
|
||
stCAN_SWObj StrCanRx = {0};
|
||
uword tmp = 0;
|
||
ubyte i = 0,j = 0;
|
||
ubyte sbus_temp[2] = {0,0};
|
||
|
||
static ubyte match_freq_sta = 0;
|
||
static uword match_freq_cnt = 0;
|
||
static uword match_freq_time = 0;
|
||
//------------------------------------
|
||
if(CAN_ubNewData(DF_SwRx))
|
||
{
|
||
CAN_vGetMsgObj(DF_SwRx, &StrCanRx);
|
||
|
||
UnInfCan_1.ArrData.ArrRX[0][3] = StrCanRx.ulDATAL.ubDB[0];
|
||
UnInfCan_1.ArrData.ArrRX[0][2] = StrCanRx.ulDATAL.ubDB[1];
|
||
UnInfCan_1.ArrData.ArrRX[0][1] = StrCanRx.ulDATAL.ubDB[2];
|
||
UnInfCan_1.ArrData.ArrRX[0][0] = StrCanRx.ulDATAL.ubDB[3];
|
||
UnInfCan_1.ArrData.ArrRX[0][7] = StrCanRx.ulDATAH.ubDB[0];
|
||
UnInfCan_1.ArrData.ArrRX[0][6] = StrCanRx.ulDATAH.ubDB[1];
|
||
UnInfCan_1.ArrData.ArrRX[0][5] = StrCanRx.ulDATAH.ubDB[2];
|
||
UnInfCan_1.ArrData.ArrRX[0][4] = StrCanRx.ulDATAH.ubDB[3];
|
||
|
||
CAN_vReleaseObj(DF_SwRx);
|
||
}
|
||
|
||
if(CAN_ubNewData(DF_SwRxPwm))
|
||
{
|
||
// CAN_vGetMsgObj(DF_SwRxPwm, &StrCanRx);
|
||
|
||
// UnInfCan_1.ArrData.ArrRX[1][2] = StrCanRx.ulDATAL.ubDB[0];
|
||
// UnInfCan_1.ArrData.ArrRX[1][3] = StrCanRx.ulDATAL.ubDB[1];
|
||
// UnInfCan_1.ArrData.ArrRX[1][0] = StrCanRx.ulDATAL.ubDB[2];
|
||
// UnInfCan_1.ArrData.ArrRX[1][1] = StrCanRx.ulDATAL.ubDB[3];
|
||
// UnInfCan_1.ArrData.ArrRX[1][6] = StrCanRx.ulDATAH.ubDB[0];
|
||
// UnInfCan_1.ArrData.ArrRX[1][7] = StrCanRx.ulDATAH.ubDB[1];
|
||
// UnInfCan_1.ArrData.ArrRX[1][4] = StrCanRx.ulDATAH.ubDB[2];
|
||
// UnInfCan_1.ArrData.ArrRX[1][5] = StrCanRx.ulDATAH.ubDB[3];
|
||
|
||
// CAN_vReleaseObj(DF_SwRxPwm);
|
||
//// //接收的数据保存到PWM数据结构中20181227
|
||
//// PwmH[0 ] = (ubyte)UnInfCan_1.BitData.Pwm_01;
|
||
//// PwmH[1 ] = (ubyte)UnInfCan_1.BitData.Pwm_02;
|
||
//// PwmH[2 ] = (ubyte)UnInfCan_1.BitData.Pwm_03;
|
||
//// PwmH[3 ] = (ubyte)UnInfCan_1.BitData.Pwm_04;
|
||
//// PwmH[4 ] = (ubyte)UnInfCan_1.BitData.Pwm_05;
|
||
//// PwmH[5 ] = (ubyte)UnInfCan_1.BitData.Pwm_06;
|
||
//// PwmH[6 ] = (ubyte)UnInfCan_1.BitData.Pwm_07;
|
||
//// PwmH[7 ] = (ubyte)UnInfCan_1.BitData.Pwm_08;
|
||
//// PwmH[8 ] = (ubyte)UnInfCan_1.BitData.Pwm_09;
|
||
//// PwmH[9 ] = (ubyte)UnInfCan_1.BitData.Pwm_10;
|
||
//// PwmH[10] = (ubyte)UnInfCan_1.BitData.Pwm_11;
|
||
//// PwmH[11] = (ubyte)UnInfCan_1.BitData.Pwm_12;
|
||
//// PwmH[12] = (ubyte)UnInfCan_1.BitData.Pwm_13;
|
||
//// PwmH[13] = (ubyte)UnInfCan_1.BitData.Pwm_14;
|
||
//// PwmH[14] = (ubyte)UnInfCan_1.BitData.Pwm_15;
|
||
//// PwmH[15] = (ubyte)UnInfCan_1.BitData.Pwm_16;
|
||
|
||
}
|
||
|
||
//50ms发送一帧
|
||
if(!FlgCan_1)
|
||
{
|
||
FlgCan_1 = 1;
|
||
|
||
sbus_temp[0] = P1_3;
|
||
|
||
// sbus_temp[1] = 3;
|
||
detectLedState(&blink_detector, sbus_temp[0], &sbus_temp[1]);
|
||
// UnRemoteControlOutput_2.bit_data.enable = blink_detector.current_state;//
|
||
// UnRemoteControlOutput_2.bit_data.reserve1 = sbus_temp[1];
|
||
|
||
|
||
if( (MATCH_CMD == UnInfCan_1.ArrData.ArrRX[0][0]) || (CHANGE_CMD == UnInfCan_1.ArrData.ArrRX[0][0]) )//对频或者切换
|
||
{
|
||
if(MATCH_CMD == UnInfCan_1.ArrData.ArrRX[0][0])//对频
|
||
{
|
||
match_freq_time = MATCH_TIME;
|
||
}
|
||
else
|
||
{
|
||
match_freq_time = CHANGE_TIME;
|
||
}
|
||
|
||
switch(match_freq_sta)
|
||
{
|
||
case 0:
|
||
if(match_freq_cnt >= match_freq_time)//时间根据命令确定
|
||
{
|
||
SwOut01 = 0;//取消对频
|
||
match_freq_sta = 1;
|
||
match_freq_cnt = 0;
|
||
}
|
||
else
|
||
{
|
||
SwOut01 = 1;//对频指令
|
||
match_freq_sta = 0;
|
||
match_freq_cnt ++;
|
||
}
|
||
break;
|
||
|
||
case 1:
|
||
if(match_freq_cnt >= MATCH_WAIT_TIME)//
|
||
{
|
||
SwOut01 = 0;//取消对频
|
||
match_freq_sta = 0;
|
||
match_freq_cnt = 0;
|
||
UnInfCan_1.ArrData.ArrRX[0][0] = 0;
|
||
}
|
||
else
|
||
{
|
||
SwOut01 = 0;//对频指令
|
||
match_freq_sta = 1;
|
||
match_freq_cnt ++;
|
||
}
|
||
break;
|
||
default:;
|
||
}
|
||
}
|
||
|
||
CanTransmit(DF_SwTx,(ubyte *)&UnRemoteControlOutput_1.arr[0]);
|
||
CanTransmit(DF_SwTx1,(ubyte *)&UnRemoteControlOutput_2.arr[0]);
|
||
|
||
|
||
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|