第一次提交
This commit is contained in:
159
app/app_base.c
Normal file
159
app/app_base.c
Normal file
@@ -0,0 +1,159 @@
|
||||
#include "app_config.h"
|
||||
#include "app_dependence.h"
|
||||
#include "interface.h"
|
||||
|
||||
#include "app_base.h"
|
||||
|
||||
|
||||
// 使用内联函数
|
||||
static inline uint8_t setBaseOn(void) { return 1; }
|
||||
static inline uint8_t setBaseOff(void) { return 0; }
|
||||
|
||||
BaseSystem base_data;
|
||||
|
||||
// 输出处理函数
|
||||
static void baseOutput(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
// 根据电机状态，填充发送数据结构，发送信号
|
||||
switch (base_data.base_command)
|
||||
{
|
||||
case 1: // 上升状态
|
||||
un_lifter_output.bit_data.cmd = BASE_UP;
|
||||
un_h_bridge_output1.bit_data.channel_01 = setBaseOn();
|
||||
un_h_bridge_output1.bit_data.channel_02 = setBaseOff();
|
||||
un_h_bridge_output2.bit_data.channel_01 = setBaseOn();
|
||||
un_h_bridge_output2.bit_data.channel_02 = setBaseOff();
|
||||
// printf("base: up\n");
|
||||
break;
|
||||
|
||||
case 2: // 下降状态
|
||||
un_lifter_output.bit_data.cmd = BASE_DOWN;
|
||||
un_h_bridge_output1.bit_data.channel_01 = setBaseOff();
|
||||
un_h_bridge_output1.bit_data.channel_02 = setBaseOn();
|
||||
un_h_bridge_output2.bit_data.channel_01 = setBaseOff();
|
||||
un_h_bridge_output2.bit_data.channel_02 = setBaseOn();
|
||||
// printf("base: down\n");
|
||||
break;
|
||||
|
||||
default://停止
|
||||
un_lifter_output.bit_data.cmd = BASE_STOP;
|
||||
un_h_bridge_output1.bit_data.channel_01 = setBaseOff();
|
||||
un_h_bridge_output1.bit_data.channel_02 = setBaseOff();
|
||||
un_h_bridge_output2.bit_data.channel_01 = setBaseOff();
|
||||
un_h_bridge_output2.bit_data.channel_02 = setBaseOff();
|
||||
// printf("base: stop\n");
|
||||
break;
|
||||
}
|
||||
publishMessage(&un_h_bridge_output1, 1);
|
||||
publishMessage(&un_h_bridge_output2, 1);
|
||||
publishMessage(&un_lifter_output, 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//处理开关以及模式状态。还需要保存状态防止重启的问题。
|
||||
static void baseTimerProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
if( (base_data.emergency_stop_switch) || (base_data.remote_emergency_stop) )//急停只要有一个有效就停止
|
||||
{
|
||||
base_data.base_command = BASE_STOP;
|
||||
}
|
||||
else if(base_data.mode)//自动模式 一直上升就自动
|
||||
{
|
||||
if(base_data.input_data > REMOTE_DEADBAND)//上升
|
||||
{
|
||||
base_data.base_command = BASE_UP;
|
||||
}
|
||||
else if(base_data.input_data < -REMOTE_DEADBAND)//下降
|
||||
{
|
||||
base_data.base_command = BASE_DOWN;
|
||||
}
|
||||
else//不停止
|
||||
{
|
||||
}
|
||||
|
||||
}
|
||||
else//手动模式-
|
||||
{
|
||||
if(base_data.input_data > REMOTE_DEADBAND)//上升
|
||||
{
|
||||
base_data.base_command = BASE_UP;
|
||||
}
|
||||
else if(base_data.input_data < -REMOTE_DEADBAND)//下降
|
||||
{
|
||||
base_data.base_command = BASE_DOWN;
|
||||
}
|
||||
else
|
||||
{
|
||||
base_data.base_command = BASE_STOP;
|
||||
}
|
||||
}
|
||||
baseOutput(signal_id);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 处理所有输入信号的函数
|
||||
static void baseInput(void *signal_id)
|
||||
{
|
||||
// BaseSystem old_data = base_data; // 定义并初始化old_data
|
||||
// 填充数据
|
||||
if (signal_id == &un_sw_sample)
|
||||
{
|
||||
base_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;
|
||||
base_data.Left_switch = (uint8_t)un_sw_sample.bit_data.Left_switch;
|
||||
base_data.right_switch = (uint8_t)un_sw_sample.bit_data.right_switch;
|
||||
}
|
||||
else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据
|
||||
{
|
||||
base_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;
|
||||
base_data.mode = ((uint8_t)un_remote_control_input.bit_data.switch_c == 1) ? 1 : 0;//
|
||||
|
||||
}
|
||||
else if( (signal_id == &un_remote_control_input1) && (1 == un_remote_control_input.bit_data.enable) )
|
||||
{
|
||||
base_data.input_data = (int16_t)(un_remote_control_input1.bit_data.speed) - REMOTE_ZERO;
|
||||
// printf("un_remote_control_input1: %d\n", base_data.input_data);
|
||||
}
|
||||
else if (signal_id == &can_fault_info)
|
||||
{
|
||||
// base_data.remote_fault = !can_fault_info.bit_data.remote_state;
|
||||
// base_data.can_bus_fault = !can_fault_info.bit_data.motor1_state || !can_fault_info.bit_data.motor2_state || !can_fault_info.bit_data.navigator_state;
|
||||
}
|
||||
else if (signal_id == ðernet_fault_Info)
|
||||
{
|
||||
// base_data.ethernet_fault = !ethernet_fault_Info.bit_data.auto_state && !ethernet_fault_Info.bit_data.manual_state;
|
||||
}
|
||||
baseTimerProcess(signal_id);
|
||||
}
|
||||
|
||||
// 修改APP模块的初始化函数
|
||||
void baseAppInit(void)
|
||||
{
|
||||
// 初始化
|
||||
memset(&base_data, 0, sizeof(BaseSystem));
|
||||
|
||||
// 初始化定时器
|
||||
timerInit(&base_data.base_timer);
|
||||
|
||||
// 订阅输入信号,处理刹车逻辑
|
||||
subscribe(&un_sw_sample, baseInput);
|
||||
subscribe(&un_remote_control_input, baseInput);
|
||||
subscribe(&un_remote_control_input1, baseInput);
|
||||
|
||||
printf("app_base: initial OK \n");
|
||||
}
|
||||
55
app/app_base.h
Normal file
55
app/app_base.h
Normal file
@@ -0,0 +1,55 @@
|
||||
#ifndef BASE_H
|
||||
#define BASE_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
// <20><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>״̬<D7B4><CCAC>״̬
|
||||
typedef enum {
|
||||
BASE_STOP,
|
||||
BASE_UP,
|
||||
BASE_DOWN,
|
||||
} baseState;
|
||||
|
||||
|
||||
|
||||
|
||||
typedef struct BaseSystem
|
||||
{
|
||||
uint32_t start_time;
|
||||
Timer base_timer; // <20><>ʱ<EFBFBD><CAB1><EFBFBD>ṹ<EFBFBD><E1B9B9>
|
||||
uint8_t base_command; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>վ<EFBFBD><D5BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0ֹͣ,1<><31><EFBFBD><EFBFBD>,2<>½<EFBFBD>
|
||||
uint8_t base_state; // <20><>վ״̬<D7B4><CCAC>1<EFBFBD><31>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,2<><32>ʾ<EFBFBD>½<EFBFBD><C2BD>У<EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD>λ<EFBFBD>ã<EFBFBD>4<EFBFBD><34><EFBFBD>½<EFBFBD><C2BD>̶<EFBFBD>λ<EFBFBD><CEBB>
|
||||
uint8_t mode; // ң<><D2A3>ģʽ<C4A3><CABD>0<EFBFBD><30>2<EFBFBD>ֶ<EFBFBD><D6B6><EFBFBD>1<EFBFBD>Զ<EFBFBD>
|
||||
uint8_t Left_switch; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
|
||||
uint8_t right_switch; // <20>Ҳ<EFBFBD><D2B2><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
|
||||
uint8_t emergency_stop_switch; // <20><>ͣ<EFBFBD><CDA3><EFBFBD>أ<EFBFBD><D8A3><EFBFBD><EFBFBD>¼<EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><C2BD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
|
||||
uint8_t remote_emergency_stop; // Զ<>̼<EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD>أ<EFBFBD><D8A3><EFBFBD><EFBFBD>¼<EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><C2BD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
|
||||
int16_t input_data; // <20><><EFBFBD><EFBFBD>ң<EFBFBD><D2A3><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD>
|
||||
} BaseSystem;
|
||||
|
||||
|
||||
|
||||
#define REMOTE_ZERO 1022
|
||||
|
||||
|
||||
#define REMOTE_DEADBAND 50
|
||||
|
||||
|
||||
|
||||
|
||||
void baseAppInit(void);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // BASE_H
|
||||
236
app/app_brake.c
Normal file
236
app/app_brake.c
Normal file
@@ -0,0 +1,236 @@
|
||||
#include "app_config.h"
|
||||
#include "interface.h"
|
||||
#include "app_frm_monitor.h"
|
||||
#include "app_frm_signal.h"
|
||||
#include "app_frm_timer.h"
|
||||
#include "app_param_manage.h"
|
||||
|
||||
#include "app_power.h"
|
||||
|
||||
#include "app_brake.h"
|
||||
#include "app_differential_drive.h"
|
||||
|
||||
// 使用内联函数
|
||||
static inline uint8_t setBrakeOn(void) { return 1; }
|
||||
static inline uint8_t setBrakeOff(void){ return 0; }
|
||||
|
||||
BrakeSystem brake_data;
|
||||
|
||||
// 输出处理函数
|
||||
static void brakeOutput(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
// 根据电机状态，填充发送数据结构，发送信号
|
||||
switch (brake_data.brake_motor_state)
|
||||
{
|
||||
case 1: // 电机前进状态
|
||||
un_h_bridge_output.bit_data.channel_01 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.channel_04 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_03 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.sleep_01 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.sleep_02 = setBrakeOn(); // 正转
|
||||
printf("Brake: Motor forward\n");
|
||||
break;
|
||||
|
||||
case 2: // 电机后退状态
|
||||
un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_04 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_02 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.channel_03 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.sleep_01 = setBrakeOn();
|
||||
un_h_bridge_output.bit_data.sleep_02 = setBrakeOn(); // 反转
|
||||
printf("Brake: Motor reverse\n");
|
||||
break;
|
||||
|
||||
default:
|
||||
un_h_bridge_output.bit_data.channel_01 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_04 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_02 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.channel_03 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.sleep_01 = setBrakeOff();
|
||||
un_h_bridge_output.bit_data.sleep_02 = setBrakeOff(); // 关闭
|
||||
printf("Brake: Motor off\n");
|
||||
break;
|
||||
}
|
||||
publishMessage(&un_h_bridge_output, 1);
|
||||
}
|
||||
|
||||
// 判断是否需要刹车
|
||||
static uint8_t shouldApplyBrake()
|
||||
{
|
||||
return (brake_data.emergency_stop_switch ||
|
||||
brake_data.remote_emergency_stop ||
|
||||
(brake_data.mode_signal == 0 && brake_data.remote_fault) ||
|
||||
(brake_data.mode_signal == 1 && brake_data.can_bus_fault));//20241021 修改不计算以太网故障
|
||||
// (brake_data.mode_signal == 1 && (brake_data.can_bus_fault || brake_data.ethernet_fault)));
|
||||
}
|
||||
|
||||
// 判断是否需要释放刹车
|
||||
static uint8_t shouldReleaseBrake()
|
||||
{
|
||||
return (!brake_data.emergency_stop_switch &&
|
||||
!brake_data.remote_emergency_stop &&
|
||||
((brake_data.mode_signal == 0 && !brake_data.remote_fault) ||
|
||||
(brake_data.mode_signal == 1 && !brake_data.can_bus_fault)));//20241021 修改不计算以太网故障
|
||||
// (brake_data.mode_signal == 1 && (!brake_data.can_bus_fault && !brake_data.ethernet_fault))));
|
||||
}
|
||||
|
||||
// 修改刹车定时器处理函数
|
||||
static void brakeTimerProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
switch (brake_data.state)
|
||||
{
|
||||
case BRAKE_STATE_IDLE:
|
||||
if (shouldApplyBrake())
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_APPLYING_BRAKE;
|
||||
if( 0 == brake_data.brake_direction)
|
||||
{
|
||||
brake_data.brake_motor_state = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
brake_data.brake_motor_state = 2;
|
||||
}
|
||||
|
||||
brakeOutput(NULL);
|
||||
timerStart(&brake_data.brake_apply_timer, (uint32_t)(getParam("brk_on")), 0);
|
||||
}
|
||||
break;
|
||||
|
||||
case BRAKE_STATE_BRAKE_ON:
|
||||
if (shouldReleaseBrake() && power_data.current_state == POWER_WORKING)
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_RELEASING_BRAKE;
|
||||
|
||||
if( 0 == brake_data.brake_direction)
|
||||
{
|
||||
brake_data.brake_motor_state = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
brake_data.brake_motor_state = 1;
|
||||
}
|
||||
brakeOutput(NULL);
|
||||
timerStart(&brake_data.brake_release_timer, (uint32_t)(getParam("brk_off")), 0);
|
||||
}
|
||||
break;
|
||||
|
||||
case BRAKE_STATE_APPLYING_BRAKE:
|
||||
if (!brake_data.brake_apply_timer.active)
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_BRAKE_ON;
|
||||
brake_data.brake_motor_state = 0;
|
||||
brakeOutput(NULL);
|
||||
brake_data.brake_position = 1; // 刹车位置：1表示刹车
|
||||
}
|
||||
break;
|
||||
|
||||
case BRAKE_STATE_RELEASING_BRAKE:
|
||||
if (!brake_data.brake_release_timer.active)
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_IDLE;
|
||||
brake_data.brake_motor_state = 0;
|
||||
brakeOutput(NULL);
|
||||
brake_data.brake_position = 0; // 刹车位置：0表示未刹车
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("ERROR: Unknown state\n");
|
||||
brake_data.state = BRAKE_STATE_IDLE;
|
||||
break;
|
||||
}
|
||||
|
||||
// 如果刹车位置有变化，存入EEPROM
|
||||
if (brake_data.brake_position != brake_data.old_brake_position)
|
||||
{
|
||||
setParam("brk_pos", (float)brake_data.brake_position);
|
||||
brake_data.old_brake_position = brake_data.brake_position;
|
||||
}
|
||||
|
||||
timerStart(&brake_data.brake_timer, 100, 1); // 周期调用
|
||||
}
|
||||
|
||||
|
||||
// 处理所有输入信号的函数
|
||||
static void brakeInput(void *signal_id)
|
||||
{
|
||||
// BrakeSystem old_data = brake_data; // 定义并初始化old_data
|
||||
|
||||
// 填充数据
|
||||
if (signal_id == &un_sw_sample)
|
||||
{
|
||||
brake_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;
|
||||
}
|
||||
else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据
|
||||
{
|
||||
brake_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;
|
||||
brake_data.mode_signal = ((uint8_t)un_remote_control_input.bit_data.switch_c == 1) ? 1 : 0;
|
||||
}
|
||||
else if (signal_id == &can_fault_info)
|
||||
{
|
||||
brake_data.remote_fault = !can_fault_info.bit_data.remote_state;
|
||||
brake_data.can_bus_fault = !can_fault_info.bit_data.motor1_state || !can_fault_info.bit_data.motor2_state || !can_fault_info.bit_data.navigator_state;
|
||||
}
|
||||
else if (signal_id == ðernet_fault_Info)
|
||||
{
|
||||
brake_data.ethernet_fault = !ethernet_fault_Info.bit_data.auto_state && !ethernet_fault_Info.bit_data.manual_state;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void paramUpdate(void *signal_id)
|
||||
{
|
||||
brake_data.brake_direction = (uint8_t)getParam("brk_rev");
|
||||
timerStart(&brake_data.brake_param_timer, 1000, 1);
|
||||
}
|
||||
|
||||
|
||||
// 修改APP模块的初始化函数
|
||||
void brakeAppInit(void)
|
||||
{
|
||||
// 初始化
|
||||
memset(&brake_data, 0, sizeof(BrakeSystem));
|
||||
brake_data.state = BRAKE_STATE_IDLE;
|
||||
// 初始化时恢复刹车位置
|
||||
brake_data.brake_position = (uint8_t)getParam("brk_pos");
|
||||
brake_data.old_brake_position = brake_data.brake_position;
|
||||
// 根据刹车位置恢复刹车状态
|
||||
if (brake_data.brake_position == 1)
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_BRAKE_ON;
|
||||
}
|
||||
else
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_IDLE;
|
||||
}
|
||||
|
||||
// 初始化定时器
|
||||
timerInit(&brake_data.brake_timer);
|
||||
timerInit(&brake_data.brake_apply_timer);
|
||||
timerInit(&brake_data.brake_release_timer);
|
||||
timerInit(&brake_data.brake_param_timer);
|
||||
|
||||
|
||||
// 订阅输入信号,处理刹车逻辑
|
||||
subscribe(&un_sw_sample, brakeInput);
|
||||
subscribe(&un_remote_control_input, brakeInput);
|
||||
subscribe(&can_fault_info, brakeInput);
|
||||
subscribe(ðernet_fault_Info, brakeInput);
|
||||
|
||||
|
||||
// 订阅定时器信号,用于状态机的定时处理
|
||||
subscribe(&brake_data.brake_timer, brakeTimerProcess);
|
||||
subscribe(&brake_data.brake_apply_timer, brakeTimerProcess);
|
||||
subscribe(&brake_data.brake_release_timer, brakeTimerProcess);
|
||||
subscribe(&brake_data.brake_param_timer, paramUpdate);
|
||||
// 启动定时器，定期调用 brakeTimerProcess
|
||||
timerStart(&brake_data.brake_timer, 500, 1);
|
||||
timerStart(&brake_data.brake_param_timer, 1000, 1);
|
||||
|
||||
printf("app_brake: initial OK \n");
|
||||
}
|
||||
57
app/app_brake.h
Normal file
57
app/app_brake.h
Normal file
@@ -0,0 +1,57 @@
|
||||
#ifndef APP_BRAKE_H
|
||||
#define APP_BRAKE_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "app_config.h"
|
||||
#include "app_frm_monitor.h"
|
||||
#include "app_frm_signal.h"
|
||||
#include "app_frm_timer.h"
|
||||
|
||||
|
||||
// 定义刹车状态机状态
|
||||
typedef enum {
|
||||
BRAKE_STATE_IDLE,
|
||||
BRAKE_STATE_APPLYING_BRAKE,
|
||||
BRAKE_STATE_RELEASING_BRAKE,
|
||||
BRAKE_STATE_BRAKE_ON,
|
||||
} BrakeState;
|
||||
|
||||
typedef struct BrakeSystem
|
||||
{
|
||||
uint32_t start_time;
|
||||
Timer brake_timer; // 定时器结构体
|
||||
uint8_t brake_command; // 刹车命令变量：1表示刹车,2表示释放
|
||||
uint8_t brake_motor_state; // 刹车电机状态变量：0停止,1前进,2后退
|
||||
uint8_t brake_command_in_progress; // 刹车命令执行状态：0表示空闲,1表示正在执行
|
||||
uint8_t brake_direction;
|
||||
|
||||
uint8_t emergency_stop_switch; // 急停开关
|
||||
uint8_t remote_emergency_stop; // 遥控器急停开关
|
||||
uint8_t remote_fault; // 遥控器故障
|
||||
uint8_t can_bus_fault; // CAN总线故障
|
||||
uint8_t ethernet_fault; // 以太网通信故障
|
||||
uint8_t mode_signal; // 模式信号：0表示手动模式,1表示自动模式
|
||||
BrakeState state; // 刹车状态机
|
||||
uint8_t brake_position; // 刹车位置：0表示未刹车,1表示刹车
|
||||
uint8_t old_brake_position; // 旧的刹车位置
|
||||
Timer brake_apply_timer; // 刹车定时器
|
||||
Timer brake_release_timer; // 释放刹车定时器
|
||||
Timer brake_param_timer; //参数更新时间
|
||||
|
||||
} BrakeSystem;
|
||||
|
||||
|
||||
// 声明外部变量
|
||||
extern BrakeSystem brake_data;
|
||||
|
||||
|
||||
void brakeAppInit(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // APP_BRAKE_H
|
||||
39
app/app_config.h
Normal file
39
app/app_config.h
Normal file
@@ -0,0 +1,39 @@
|
||||
#ifndef APP_CONFIG_H
|
||||
#define APP_CONFIG_H
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// 标准库，每个模块中也会包含
|
||||
#include <stdio.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
|
||||
//外部依赖放在这里，方便分层管理，实际代码中也有这个文件，但是没有依赖
|
||||
//移植的时候，存放依赖的文件不用移植
|
||||
#include "app_dependence.h"
|
||||
// 接口，全局变量都放在这里
|
||||
#include "interface.h"
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
static inline uint8_t app_close(void) { return 1; }
|
||||
static inline uint8_t app_open(void) { return 0; }
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
#endif // APP_CONFIG_H
|
||||
43
app/app_dependence.cpp
Normal file
43
app/app_dependence.cpp
Normal file
@@ -0,0 +1,43 @@
|
||||
#include "app_dependence.h"
|
||||
#include <QDateTime>
|
||||
|
||||
// 添加Qt相关头文件
|
||||
#include <QDateTime>
|
||||
|
||||
UnCanFault can_fault_info;
|
||||
UnEthernetFault ethernet_fault_Info;
|
||||
|
||||
unsigned int wrbyte_24c02(unsigned int addr, unsigned char data)
|
||||
{
|
||||
(void)addr;
|
||||
(void)data;
|
||||
return 0;
|
||||
}
|
||||
|
||||
unsigned char rdbyte_24c02(unsigned int addr)
|
||||
{
|
||||
(void)addr;
|
||||
return 0;
|
||||
}
|
||||
|
||||
void feedWatchdog(void)
|
||||
{
|
||||
}
|
||||
|
||||
void udelay(uint32_t t )
|
||||
{
|
||||
(void)t;
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
uint32_t getCurrentTime(void)
|
||||
{
|
||||
// 获取当前时间
|
||||
QDateTime current = QDateTime::currentDateTime();
|
||||
// 转换为毫秒级时间戳
|
||||
uint32_t timestamp = static_cast<uint32_t>(current.toMSecsSinceEpoch());
|
||||
|
||||
return timestamp;
|
||||
}
|
||||
}
|
||||
|
||||
90
app/app_dependence.h
Normal file
90
app/app_dependence.h
Normal file
@@ -0,0 +1,90 @@
|
||||
#ifndef TEST_H
|
||||
#define TEST_H
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stddef.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
#include "irq.h"
|
||||
#include "udelay\udelay.h"
|
||||
|
||||
#define PERIOD_TICK 1000000.0f // 1000000us = 1s
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
//unsigned int wrbyte_24c02(unsigned int addr, unsigned char data);
|
||||
//unsigned char rdbyte_24c02(unsigned int addr);
|
||||
//void feedWatchdog(void);
|
||||
//uint32_t getCurrentTime(void);
|
||||
//void udelay(uint32_t t );
|
||||
//
|
||||
//typedef unsigned int irq_state_t;
|
||||
|
||||
//static inline irq_state_t arch_irq_save(void)
|
||||
//{
|
||||
// unsigned int cpsr = 0;
|
||||
// return cpsr;
|
||||
//}
|
||||
//
|
||||
//static inline void arch_irq_restore(irq_state_t flags)
|
||||
//{
|
||||
// flags += 1;
|
||||
//}
|
||||
|
||||
//// 故障信息
|
||||
//typedef struct _StrCanFault
|
||||
//{
|
||||
// uint8_t navigator_count; // 导航仪计数器
|
||||
// uint8_t motor1_count; // 左电机计数器
|
||||
// uint8_t motor2_count; // 右电机计数器
|
||||
// uint8_t bms_count; // bms计数器
|
||||
// uint8_t temperature_count; // 温度计数器
|
||||
// uint8_t remote_count; // 遥控计数器
|
||||
//
|
||||
// uint8_t navigator_state; // 导航仪故障状态
|
||||
// uint8_t motor1_state; // 电机1故障状态
|
||||
// uint8_t motor2_state; // 电机2故障状态
|
||||
// uint8_t bms_state; // bms状态
|
||||
// uint8_t temperature_state; // 温度计数器
|
||||
// uint8_t remote_state; // 遥控状态
|
||||
//} StrCanFault;
|
||||
|
||||
//typedef union _UnCanFault
|
||||
//{
|
||||
// StrCanFault bit_data; // 使用定义的结构体类型
|
||||
// uint8_t arr[sizeof(StrCanFault)]; // 通过结构体确定数组大小
|
||||
//} UnCanFault;
|
||||
|
||||
//extern UnCanFault can_fault_info;
|
||||
|
||||
//typedef struct _StrEthernetFault
|
||||
//{
|
||||
// uint8_t auto_count; // 自动数据计数器
|
||||
// uint8_t manual_count; // 手动数据计数器
|
||||
// uint8_t auto_state; // 自动状态 | 0正常 1故障
|
||||
// uint8_t manual_state; // 手动状态 | 0正常 1故障
|
||||
//} StrEthernetFault;
|
||||
//
|
||||
//typedef union _UnEthernetFault
|
||||
//{
|
||||
// StrEthernetFault bit_data; // 使用定义的结构体类型
|
||||
// uint8_t arr[sizeof(StrEthernetFault)]; // 通过结构体大小确定数组大小
|
||||
//} UnEthernetFault;
|
||||
|
||||
//extern UnEthernetFault ethernet_fault_Info;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // TEST_H
|
||||
1103
app/app_differential_drive (conflicted copy 2025-07-03 101418).c
Normal file
1103
app/app_differential_drive (conflicted copy 2025-07-03 101418).c
Normal file
File diff suppressed because it is too large
Load Diff
1364
app/app_differential_drive.c
Normal file
1364
app/app_differential_drive.c
Normal file
File diff suppressed because it is too large
Load Diff
128
app/app_differential_drive.h
Normal file
128
app/app_differential_drive.h
Normal file
@@ -0,0 +1,128 @@
|
||||
#ifndef APP_DIFFERENTIAL_DRIVE_H
|
||||
#define APP_DIFFERENTIAL_DRIVE_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#include "app_config.h"
|
||||
|
||||
#define SPEED_MODE 0x0C
|
||||
#define THROTTLE_MODE 0x05
|
||||
#define TORQUE_MODE 0x0A
|
||||
|
||||
#define MOTOR_MODE THROTTLE_MODE
|
||||
|
||||
#define SPEED_PID_MODE 0
|
||||
#define THROTTLE_PID_MODE 1
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#define SWITCH_SPEED 20.0f
|
||||
|
||||
|
||||
#define DIFF_SPEED 20.0f
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
STATE_INIT, ///< 初始状态（转速为0且等待扭矩方向判定）
|
||||
STATE_FORWARD, ///< 正向旋转状态
|
||||
STATE_BACKWARD, ///< 反向旋转状态
|
||||
DEC_FORWARD, ///< 正向减速状态
|
||||
DEC_FORWARD2, ///< 正向溜坡状态
|
||||
DEC_BACKWARD, ///< 反向减速状态
|
||||
DEC_BACKWARD2 ///< 反向溜坡状态
|
||||
|
||||
} MotorState;
|
||||
|
||||
|
||||
// 状态机内部状态
|
||||
typedef enum
|
||||
{
|
||||
STATE_CALIB_INIT, // 初始状态：检测方向一致性
|
||||
STATE_CALIB_JUDGE // 判断状态：300ms验证期
|
||||
} DirState;
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
FORWARD, ///< 正向旋转状态
|
||||
BACKWARD ///< 反向旋转状态
|
||||
} MotorDir;
|
||||
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
MODE_MANUAL, // 手动模式
|
||||
MODE_AUTO // 自动模式
|
||||
} ControlMode;
|
||||
|
||||
typedef struct DiffData
|
||||
{
|
||||
ControlMode mode ; // 控制模式
|
||||
MotorState state; //当前状态机状态
|
||||
MotorDir motor_dir; // 当前车辆方向
|
||||
DirState dir_state;
|
||||
|
||||
float desired_speed; // 期望速度
|
||||
float desired_curvature; // 期望曲率
|
||||
float left_motor_speed; // 当前左电机速度
|
||||
float right_motor_speed; // 当前右电机速度
|
||||
|
||||
float left_front_motor_speed; // 当前左前电机速度
|
||||
float right_front_motor_speed; // 当前右前电机速度
|
||||
float left_rear_motor_speed; // 当前左后电机速度
|
||||
float right_rear_motor_speed; // 当前右后电机速度
|
||||
|
||||
uint8_t out_motor_dir; // 电机状态
|
||||
|
||||
float speed_last; // 速度过去值
|
||||
float torq_last; // 扭矩过去值
|
||||
uint8_t counter; // 方向判断计数器
|
||||
|
||||
float left_speed_diff; // 左侧转速差
|
||||
float right_speed_diff; // 右侧转速差
|
||||
|
||||
float left_diff_touue; // 左侧扭矩差
|
||||
float right_diff_touue; // 右侧扭矩差
|
||||
|
||||
float diff_dead_zone; // 差速速度死区
|
||||
|
||||
|
||||
float out_motor_speed[4];
|
||||
float speed; // 当前车速
|
||||
float curvature; // 当前曲率
|
||||
float yaw_rate; // 当前角速度
|
||||
float desired_yaw_rate; // 期望角速度
|
||||
float acceleration; // 当前加速度
|
||||
float deceleration; // 当前减速度
|
||||
float max_speed; // 最大速度
|
||||
float desired_acceleration; // 期望加速度
|
||||
float desired_deceleration; // 期望减速度
|
||||
uint8_t emergency_stop_switch; // 急停开关
|
||||
uint8_t remote_emergency_stop; // 遥控器急停开关
|
||||
uint8_t emergency_stop_state; // 急停状态
|
||||
float out_left_motor_speed; // 输出左电机速度
|
||||
float out_right_motor_speed; // 输出右电机速度
|
||||
uint16_t max_Torq; // 最大扭矩限制
|
||||
uint16_t speed_inter;
|
||||
|
||||
|
||||
} DiffData;
|
||||
|
||||
|
||||
// 声明外部变量
|
||||
extern DiffData diff_data;
|
||||
|
||||
void diffAppInit(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // APP_DIFFERENTIAL_DRIVE_H
|
||||
175
app/app_frm_monitor.c
Normal file
175
app/app_frm_monitor.c
Normal file
@@ -0,0 +1,175 @@
|
||||
#include "app_config.h"
|
||||
#include "interface.h"
|
||||
|
||||
#include "app_frm_monitor.h"
|
||||
#include "app_frm_signal.h"
|
||||
#include "app_frm_timer.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
|
||||
|
||||
// 全局日志缓冲区实例
|
||||
LogBuffer log_buffer;
|
||||
|
||||
// 打印当前信号队列状态
|
||||
void printSignalQueueStatus(void)
|
||||
{
|
||||
for (uint32_t i = 0; i < PRIORITY_LEVELS; i++)
|
||||
{
|
||||
printf("Priority %u Signal Count: %d\n", i,
|
||||
getSignalCount(i));
|
||||
}
|
||||
}
|
||||
|
||||
// 打印订阅者信息
|
||||
void printSubscriberInfo(void)
|
||||
{
|
||||
printf("Total Subscribers: %u\n", getSubscriberCount());
|
||||
}
|
||||
|
||||
// 打印定时器状态
|
||||
void printTimerStatus(void)
|
||||
{
|
||||
printf("Current Timer Count: %u / %u\n",
|
||||
getCurrentTimerCount(), MAX_TIMERS);
|
||||
}
|
||||
|
||||
// 监控函数
|
||||
void monitorSignalSystem(void)
|
||||
{
|
||||
static uint32_t last_print_time = 0;
|
||||
uint32_t current_print_time = getCurrentTime();
|
||||
uint32_t time_interval = current_print_time - last_print_time;
|
||||
|
||||
printf("---------------------------------------------------\n");
|
||||
printf("Time since last print: %u us\n", time_interval);
|
||||
printSignalQueueStatus();
|
||||
printSubscriberInfo();
|
||||
printTimerStatus();
|
||||
printf("---------------------------------------------------\n");
|
||||
|
||||
last_print_time = current_print_time;
|
||||
}
|
||||
|
||||
int logBufferWrite(LogBuffer *lb, const char *format, ...)
|
||||
{
|
||||
if (format == NULL || lb == NULL)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
char temp_buffer[256]; // 临时缓冲区，假设单条日志不超过256字节
|
||||
va_list args;
|
||||
va_start(args, format);
|
||||
int length = vsnprintf(temp_buffer, sizeof(temp_buffer), format, args);
|
||||
va_end(args);
|
||||
|
||||
if (length <= 0)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 进入关键区，保护缓冲区的写操作
|
||||
irq_state_t saved_state = enter_critical_section();
|
||||
|
||||
// 检查缓冲区是否有足够的空间
|
||||
if (lb->count + length > LOG_BUFFER_SIZE)
|
||||
{
|
||||
// 缓冲区满，无法写入
|
||||
exit_critical_section(saved_state);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 写入数据到缓冲区
|
||||
for (int i = 0; i < length; i++)
|
||||
{
|
||||
lb->buffer[lb->tail] = temp_buffer[i];
|
||||
lb->tail = (lb->tail + 1) % LOG_BUFFER_SIZE;
|
||||
}
|
||||
lb->count += length;
|
||||
|
||||
exit_critical_section(saved_state);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int logBufferRead(LogBuffer *lb, char *data, uint32_t max_length)
|
||||
{
|
||||
if (max_length == 0 || data == NULL || lb == NULL)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 进入关键区，保护缓冲区的读操作
|
||||
irq_state_t saved_state = enter_critical_section();
|
||||
|
||||
uint32_t length = lb->count < max_length ? lb->count : max_length;
|
||||
|
||||
// 从缓冲区读取数据
|
||||
for (uint32_t i = 0; i < length; i++)
|
||||
{
|
||||
data[i] = lb->buffer[lb->head];
|
||||
lb->head = (lb->head + 1) % LOG_BUFFER_SIZE;
|
||||
}
|
||||
lb->count -= length;
|
||||
|
||||
exit_critical_section(saved_state);
|
||||
return length;
|
||||
}
|
||||
|
||||
// 初始化日志缓冲区
|
||||
void initLogBuffer(LogBuffer *lb)
|
||||
{
|
||||
lb->head = 0;
|
||||
lb->tail = 0;
|
||||
lb->count = 0;
|
||||
memset(lb->buffer, 0, LOG_BUFFER_SIZE);
|
||||
}
|
||||
|
||||
// 日志信号处理函数
|
||||
void logSignalHandler(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
char log_data[256]; // 假设单次读取不超过256字节
|
||||
int read_length;
|
||||
|
||||
// 从缓冲区读取日志信息
|
||||
read_length = logBufferRead(&log_buffer, log_data, sizeof(log_data) - 1);
|
||||
|
||||
if (read_length > 0)
|
||||
{
|
||||
log_data[read_length] = '\0'; // 确保字符串以NULL结尾
|
||||
|
||||
// 调用printf进行打印
|
||||
printf("%s", log_data);
|
||||
}
|
||||
}
|
||||
|
||||
Timer log_timer;
|
||||
|
||||
void logTimerHandler(void *timer_id)
|
||||
{
|
||||
(void)timer_id;
|
||||
// monitorSignalSystem();
|
||||
timerStart(&log_timer, 1000, 1);
|
||||
}
|
||||
|
||||
void appMonitorInit(void)
|
||||
{
|
||||
// 初始化日志缓冲区
|
||||
initLogBuffer(&log_buffer);
|
||||
// 订阅日志信号
|
||||
subscribe(&log_buffer, logSignalHandler);
|
||||
timerInit(&log_timer);
|
||||
timerStart(&log_timer, 1000, 1);
|
||||
subscribe(&log_timer, logTimerHandler);
|
||||
}
|
||||
|
||||
// // 使用格式化字符串记录日志
|
||||
// if (logBufferWrite(&log_buffer, "ISR triggered: IRQ %d\n", irq_number) == 0) {
|
||||
// // 发送信号，通知主线程有新日志
|
||||
// publishMessage(&log_buffer, LOG_SIGNAL_PRIORITY);
|
||||
// }
|
||||
41
app/app_frm_monitor.h
Normal file
41
app/app_frm_monitor.h
Normal file
@@ -0,0 +1,41 @@
|
||||
#ifndef APP_FRM_MONITOR_H
|
||||
#define APP_FRM_MONITOR_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define LOG_BUFFER_SIZE 1024 // 日志缓冲区的大小
|
||||
#define LOG_SIGNAL_PRIORITY 1 // 日志信号的优先级
|
||||
|
||||
typedef struct
|
||||
{
|
||||
char buffer[LOG_BUFFER_SIZE];
|
||||
uint32_t head; // 读指针
|
||||
uint32_t tail; // 写指针
|
||||
uint32_t count; // 缓冲区中的数据量
|
||||
} LogBuffer;
|
||||
|
||||
// 全局日志缓冲区实例
|
||||
extern LogBuffer log_buffer;
|
||||
|
||||
// 函数声明
|
||||
void printSignalQueueStatus(void);
|
||||
void printSubscriberInfo(void);
|
||||
void printTimerStatus(void);
|
||||
void monitorSignalSystem(void);
|
||||
|
||||
// 新增函数接口
|
||||
int32_t logBufferWrite(LogBuffer *lb, const char *format, ...);
|
||||
int32_t logBufferRead(LogBuffer *lb, char *data, uint32_t max_length);
|
||||
void initLogBuffer(LogBuffer *lb);
|
||||
void logSignalHandler(void *signal_id);
|
||||
void appMonitorInit(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // APP_FRM_MONITOR_H
|
||||
333
app/app_frm_signal.c
Normal file
333
app/app_frm_signal.c
Normal file
@@ -0,0 +1,333 @@
|
||||
#include "app_config.h"
|
||||
#include "interface.h"
|
||||
|
||||
#include "app_frm_signal.h"
|
||||
|
||||
typedef struct
|
||||
{
|
||||
void *signals[MAX_SIGNALS];
|
||||
uint32_t head;
|
||||
uint32_t tail;
|
||||
uint32_t count;
|
||||
} SignalQueue;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
void *signal_id;
|
||||
CallbackFunc callbacks[MAX_CALLBACKS];
|
||||
uint32_t callback_count;
|
||||
} Subscriber;
|
||||
|
||||
// 优先级队列
|
||||
static SignalQueue priority_queues[PRIORITY_LEVELS];
|
||||
|
||||
// 订阅者表
|
||||
static Subscriber subscriber_table[MAX_SUBSCRIBERS] = {{NULL, {NULL}, 0}};
|
||||
|
||||
// 初始化队列
|
||||
static void initQueue(SignalQueue *q)
|
||||
{
|
||||
q->head = 0;
|
||||
q->tail = 0;
|
||||
q->count = 0;
|
||||
}
|
||||
|
||||
// 将信号请求添加到队列中（按优先级）
|
||||
static int32_t enqueue(SignalQueue *q, void *signal_id)
|
||||
{
|
||||
if (q == NULL)
|
||||
{
|
||||
printf("Error: enqueue received NULL queue pointer\n");
|
||||
return -1;
|
||||
}
|
||||
if (signal_id == NULL)
|
||||
{
|
||||
printf("Error: Cannot enqueue NULL signal_id\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
irq_state_t saved_state = enter_critical_section();
|
||||
|
||||
if (q->count >= MAX_SIGNALS)
|
||||
{
|
||||
// 队列已满，移除最前面的信号
|
||||
q->signals[q->head] = NULL;
|
||||
q->head = (q->head + 1) % MAX_SIGNALS;
|
||||
q->count--;
|
||||
printf("Error: Signal queue is full, remove the first signal\n");
|
||||
}
|
||||
|
||||
// 添加新的信号到队列尾部
|
||||
q->signals[q->tail] = signal_id;
|
||||
q->tail = (q->tail + 1) % MAX_SIGNALS;
|
||||
q->count++;
|
||||
|
||||
exit_critical_section(saved_state);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// 从队列中取出信号请求（按优先级）
|
||||
static int32_t dequeue(SignalQueue *q, void **signal_id)
|
||||
{
|
||||
if (q == NULL)
|
||||
{
|
||||
printf("Error: dequeue received NULL queue pointer\n");
|
||||
return -1;
|
||||
}
|
||||
if (signal_id == NULL)
|
||||
{
|
||||
printf("Error: dequeue received NULL signal pointer\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 仅在修改共享资源时进入临界区
|
||||
if (q->count > 0)
|
||||
{
|
||||
irq_state_t saved_state = enter_critical_section();
|
||||
|
||||
*signal_id = q->signals[q->head];
|
||||
q->signals[q->head] = NULL; // 清除已取出的信号
|
||||
q->head = (q->head + 1) % MAX_SIGNALS;
|
||||
q->count--;
|
||||
|
||||
exit_critical_section(saved_state);
|
||||
return 0;
|
||||
}
|
||||
// printf("Warning: dequeue attempted to remove signal from an empty queue\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 哈希函数
|
||||
static uint32_t hash(void *ptr)
|
||||
{
|
||||
uintptr_t value = (uintptr_t)ptr;
|
||||
uint32_t hash = 0;
|
||||
while (value != 0)
|
||||
{
|
||||
hash += value & 0xFF;
|
||||
hash += (hash << 10);
|
||||
hash ^= (hash >> 6);
|
||||
value >>= 8;
|
||||
}
|
||||
hash += (hash << 3);
|
||||
hash ^= (hash >> 11);
|
||||
hash += (hash << 15);
|
||||
return hash;
|
||||
}
|
||||
|
||||
// 订阅信号, 给每个信号指定回调函数
|
||||
int32_t subscribe(void *signal_id, CallbackFunc callback)
|
||||
{
|
||||
if (signal_id == NULL || callback == NULL)
|
||||
{
|
||||
printf("Error: Invalid signal_id or callback\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
irq_state_t saved_state = enter_critical_section();
|
||||
|
||||
uint32_t index = hash(signal_id) % MAX_SUBSCRIBERS;
|
||||
uint32_t original_index = index;
|
||||
|
||||
do
|
||||
{
|
||||
// if(signal_id == &un_remote_control_input)
|
||||
// {
|
||||
// printf("remote的地址：%p\n", (void*)&un_remote_control_input); // 输出示例：0x7ffd9a3c8b40[5](@ref)
|
||||
// printf("signal_id的地址：%p\n", (void*)(subscriber_table[index].signal_id)); // 输出示例：0x7ffd9a3c8b40[5](@ref)
|
||||
// }
|
||||
//
|
||||
if (subscriber_table[index].signal_id == NULL ||
|
||||
subscriber_table[index].signal_id == signal_id)
|
||||
{
|
||||
if (subscriber_table[index].signal_id == NULL)
|
||||
{
|
||||
subscriber_table[index].signal_id = signal_id;
|
||||
subscriber_table[index].callback_count = 0;
|
||||
}
|
||||
|
||||
if (subscriber_table[index].callback_count < MAX_CALLBACKS)
|
||||
{
|
||||
subscriber_table[index].callbacks[subscriber_table[index].callback_count++] = callback;
|
||||
|
||||
exit_critical_section(saved_state);
|
||||
return 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("Error: Maximum callbacks reached for this signal\n");
|
||||
exit_critical_section(saved_state);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
index = (index + 1) % MAX_SUBSCRIBERS;
|
||||
} while (index != original_index);
|
||||
|
||||
// printf("remote addrss：%p\n", (void*)signal_id); // 输出示例：0x7ffd9a3c8b40[5](@ref)
|
||||
// printf("signal_id addrss：%p\n", (void*)(subscriber_table[index].signal_id)); // 输出示例：0x7ffd9a3c8b40[5](@ref)
|
||||
|
||||
printf("Error: Subscriber table is full\n");
|
||||
exit_critical_section(saved_state);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 检查信号是否有订阅者
|
||||
static unsigned char hasSubscribers(void *signal_id)
|
||||
{
|
||||
irq_state_t saved_state = enter_critical_section();
|
||||
|
||||
uint32_t index = hash(signal_id) % MAX_SUBSCRIBERS;
|
||||
uint32_t original_index = index;
|
||||
|
||||
do
|
||||
{
|
||||
if (subscriber_table[index].signal_id == signal_id)
|
||||
{
|
||||
unsigned char result = subscriber_table[index].callback_count > 0;
|
||||
exit_critical_section(saved_state);
|
||||
return result;
|
||||
}
|
||||
if (subscriber_table[index].signal_id == NULL)
|
||||
{
|
||||
exit_critical_section(saved_state);
|
||||
return 0; // 没有订阅者
|
||||
}
|
||||
index = (index + 1) % MAX_SUBSCRIBERS;
|
||||
} while (index != original_index);
|
||||
|
||||
exit_critical_section(saved_state);
|
||||
return 0; // 没有订阅者
|
||||
}
|
||||
|
||||
// 内部一致性检查
|
||||
static void internalConsistencyCheck(void)
|
||||
{
|
||||
for (uint32_t i = 0; i < MAX_SUBSCRIBERS; i++)
|
||||
{
|
||||
assert(subscriber_table[i].callback_count <= MAX_CALLBACKS);
|
||||
}
|
||||
}
|
||||
|
||||
// 处理队列中的信号, 调用所有匹配的回调函数
|
||||
static void processSignals(void)
|
||||
{
|
||||
void *signal_id;
|
||||
|
||||
for (uint32_t priority = 0; priority < PRIORITY_LEVELS; priority++)
|
||||
{
|
||||
SignalQueue *q = &priority_queues[priority];
|
||||
|
||||
while (dequeue(q, &signal_id) == 0)
|
||||
{
|
||||
// 进入临界区保护 subscriber_table 的读取
|
||||
irq_state_t saved_state = enter_critical_section();
|
||||
|
||||
uint32_t index = hash(signal_id) % MAX_SUBSCRIBERS;
|
||||
uint32_t original_index = index;
|
||||
unsigned char found = 0;
|
||||
|
||||
do
|
||||
{
|
||||
if (subscriber_table[index].signal_id == signal_id)
|
||||
{
|
||||
CallbackFunc *callbacks = subscriber_table[index].callbacks;
|
||||
uint32_t callback_count = subscriber_table[index].callback_count;
|
||||
|
||||
// 复制回调函数指针, 避免在临界区外访问共享数据
|
||||
CallbackFunc local_callbacks[MAX_CALLBACKS];
|
||||
memcpy(local_callbacks, callbacks, sizeof(CallbackFunc) * callback_count);
|
||||
|
||||
exit_critical_section(saved_state);
|
||||
|
||||
// 在临界区外调用回调函数
|
||||
for (uint32_t i = 0; i < callback_count; i++)
|
||||
{
|
||||
local_callbacks[i](signal_id);
|
||||
}
|
||||
found = 1;
|
||||
break;
|
||||
}
|
||||
if (subscriber_table[index].signal_id == NULL)
|
||||
{
|
||||
exit_critical_section(saved_state);
|
||||
break; // 没有订阅者
|
||||
}
|
||||
index = (index + 1) % MAX_SUBSCRIBERS;
|
||||
} while (index != original_index);
|
||||
|
||||
if (!found)
|
||||
{
|
||||
printf("Warning: No subscribers found for signal %p\n", signal_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
internalConsistencyCheck(); // 处理完所有信号后进行一致性检查
|
||||
}
|
||||
|
||||
// 初始化框架
|
||||
void initFramework(void)
|
||||
{
|
||||
irq_state_t saved_state = enter_critical_section();
|
||||
|
||||
for (uint32_t i = 0; i < PRIORITY_LEVELS; i++)
|
||||
{
|
||||
initQueue(&priority_queues[i]);
|
||||
}
|
||||
memset(subscriber_table, 0, sizeof(subscriber_table));
|
||||
|
||||
exit_critical_section(saved_state);
|
||||
}
|
||||
|
||||
// 将信号请求添加到指定优先级的队列中
|
||||
int32_t publishMessage(void *signal_id, uint8_t priority)
|
||||
{
|
||||
if ((uint32_t)priority >= PRIORITY_LEVELS)
|
||||
{
|
||||
printf("Error: Invalid priority\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (hasSubscribers(signal_id))
|
||||
{
|
||||
return enqueue(&priority_queues[priority], signal_id);
|
||||
}
|
||||
else return 1;
|
||||
}
|
||||
|
||||
// 处理所有优先级的信号
|
||||
void processMessages(void)
|
||||
{
|
||||
processSignals();
|
||||
}
|
||||
|
||||
// 获取当前队列中的信号数量
|
||||
int32_t getSignalCount(uint32_t priority)
|
||||
{
|
||||
if (priority >= PRIORITY_LEVELS)
|
||||
{
|
||||
printf("Error: Invalid priority\n");
|
||||
return -1;
|
||||
}
|
||||
return priority_queues[priority].count;
|
||||
}
|
||||
|
||||
// 获取订阅者数量
|
||||
uint32_t getSubscriberCount(void)
|
||||
{
|
||||
uint32_t count = 0;
|
||||
|
||||
irq_state_t saved_state = enter_critical_section();
|
||||
|
||||
for (uint32_t i = 0; i < MAX_SUBSCRIBERS; i++)
|
||||
{
|
||||
if (subscriber_table[i].signal_id != NULL)
|
||||
{
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
||||
exit_critical_section(saved_state);
|
||||
return count;
|
||||
}
|
||||
47
app/app_frm_signal.h
Normal file
47
app/app_frm_signal.h
Normal file
@@ -0,0 +1,47 @@
|
||||
#ifndef APP_FRM_SIGNAL_H
|
||||
#define APP_FRM_SIGNAL_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
#include "app_dependence.h"
|
||||
|
||||
#define MAX_SIGNALS 500u // 每个优先级的最大信号数量
|
||||
#define MAX_SUBSCRIBERS 100u // 不同信号的订阅者数量 20250519 解决列表满的问题
|
||||
#define MAX_CALLBACKS 25u // 每个信号最多支持多少订阅者
|
||||
#define PRIORITY_LEVELS 2u // 优先级层次
|
||||
|
||||
|
||||
// 回调函数类型定义
|
||||
typedef void (*CallbackFunc)(void *signal_id);
|
||||
|
||||
// 函数声明
|
||||
void initFramework(void);
|
||||
int32_t subscribe(void *signal_id, CallbackFunc callback);
|
||||
int32_t publishMessage(void *signal_id, uint8_t priority);
|
||||
void processMessages(void);
|
||||
int32_t getSignalCount(uint32_t priority);
|
||||
uint32_t getSubscriberCount(void);
|
||||
|
||||
// 进入临界区，禁用中断，并返回之前的中断状态
|
||||
static inline irq_state_t enter_critical_section(void)
|
||||
{
|
||||
return arch_irq_save();
|
||||
}
|
||||
|
||||
// 退出临界区，恢复之前的中断状态
|
||||
static inline void exit_critical_section(irq_state_t saved_state)
|
||||
{
|
||||
arch_irq_restore(saved_state);
|
||||
}
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // APP_FRM_SIGNAL_H
|
||||
165
app/app_frm_timer.c
Normal file
165
app/app_frm_timer.c
Normal file
@@ -0,0 +1,165 @@
|
||||
#include "app_config.h"
|
||||
#include "interface.h"
|
||||
|
||||
#include "app_frm_timer.h"
|
||||
#include "app_frm_signal.h"
|
||||
|
||||
// 全局计时器列表头指针
|
||||
static Timer *timer_list = NULL;
|
||||
|
||||
// 添加一个静态变量来跟踪当前定时器数量
|
||||
static uint32_t current_timer_count = 0;
|
||||
|
||||
// 初始化定时器
|
||||
void timerInit(Timer *timer)
|
||||
{
|
||||
if (timer == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
timer->target_time = 0;
|
||||
timer->elapsed_time = 0;
|
||||
timer->start_time = 0;
|
||||
timer->active = 0;
|
||||
timer->priority = 1; // 默认优先级为1
|
||||
timer->next = NULL;
|
||||
}
|
||||
|
||||
// 修改 timerStart 函数
|
||||
void timerStart(Timer *timer, uint32_t target_time, unsigned char priority)
|
||||
{
|
||||
if (timer == NULL || target_time == 0)
|
||||
{
|
||||
printf("Error: Null timer pointer or invalid target time\n");
|
||||
return;
|
||||
}
|
||||
|
||||
// 检查定时器是否已经在列表中
|
||||
Timer *current = timer_list;
|
||||
while (current != NULL)
|
||||
{
|
||||
if (current == timer)
|
||||
{
|
||||
// 定时器已在列表中, 只需更新其参数
|
||||
irq_state_t saved_state = enter_critical_section();
|
||||
|
||||
timer->target_time = target_time;
|
||||
timer->start_time = getCurrentTime();
|
||||
timer->elapsed_time = 0;
|
||||
timer->active = 1;
|
||||
timer->priority = priority;
|
||||
|
||||
exit_critical_section(saved_state);
|
||||
return;
|
||||
}
|
||||
current = current->next;
|
||||
}
|
||||
|
||||
// 检查是否达到最大定时器数量
|
||||
if (current_timer_count >= MAX_TIMERS)
|
||||
{
|
||||
printf("Error: Maximum number of timers reached\n");
|
||||
return;
|
||||
}
|
||||
|
||||
// 初始化新定时器
|
||||
irq_state_t saved_state = enter_critical_section();
|
||||
|
||||
timer->target_time = target_time;
|
||||
timer->start_time = getCurrentTime();
|
||||
timer->elapsed_time = 0;
|
||||
timer->active = 1;
|
||||
timer->priority = priority;
|
||||
|
||||
// 将定时器插入到定时器列表
|
||||
timer->next = timer_list;
|
||||
timer_list = timer;
|
||||
current_timer_count++;
|
||||
|
||||
exit_critical_section(saved_state);
|
||||
}
|
||||
|
||||
// 停止定时器
|
||||
void timerStop(Timer *timer)
|
||||
{
|
||||
if (timer == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
irq_state_t saved_state = enter_critical_section();
|
||||
|
||||
Timer *current = timer_list;
|
||||
Timer *prev = NULL;
|
||||
while (current != NULL)
|
||||
{
|
||||
if (current == timer)
|
||||
{
|
||||
// 从链表中移除定时器
|
||||
if (prev == NULL)
|
||||
{
|
||||
timer_list = current->next;
|
||||
}
|
||||
else
|
||||
{
|
||||
prev->next = current->next;
|
||||
}
|
||||
current_timer_count--;
|
||||
timer->active = 0;
|
||||
timer->next = NULL; // 断开链接
|
||||
break;
|
||||
}
|
||||
prev = current;
|
||||
current = current->next;
|
||||
}
|
||||
|
||||
exit_critical_section(saved_state);
|
||||
}
|
||||
|
||||
// 更新所有定时器, 在定时器中断中调用
|
||||
void timerUpdateAll(void)
|
||||
{
|
||||
irq_state_t saved_state = enter_critical_section();
|
||||
|
||||
Timer *prev = NULL;
|
||||
Timer *current = timer_list;
|
||||
|
||||
while (current != NULL)
|
||||
{
|
||||
if (current->active)
|
||||
{
|
||||
current->elapsed_time++;
|
||||
if (current->elapsed_time >= current->target_time)
|
||||
{
|
||||
current->active = 0;
|
||||
// 发送定时器过期消息
|
||||
publishMessage((void *)(uintptr_t)current, current->priority);
|
||||
|
||||
// 从链表中移除已过期的定时器
|
||||
if (prev == NULL)
|
||||
{
|
||||
timer_list = current->next;
|
||||
}
|
||||
else
|
||||
{
|
||||
prev->next = current->next;
|
||||
}
|
||||
current_timer_count--;
|
||||
Timer *expired_timer = current;
|
||||
current = current->next;
|
||||
expired_timer->next = NULL; // 断开链接
|
||||
continue; // 跳过前进 prev 指针
|
||||
}
|
||||
}
|
||||
prev = current;
|
||||
current = current->next;
|
||||
}
|
||||
|
||||
exit_critical_section(saved_state);
|
||||
}
|
||||
|
||||
// 获取当前定时器数量
|
||||
uint32_t getCurrentTimerCount(void)
|
||||
{
|
||||
return current_timer_count;
|
||||
}
|
||||
35
app/app_frm_timer.h
Normal file
35
app/app_frm_timer.h
Normal file
@@ -0,0 +1,35 @@
|
||||
#ifndef APP_FRM_TIMER_H
|
||||
#define APP_FRM_TIMER_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define MAX_TIMERS 100 // 定义最大定时器数量
|
||||
|
||||
// 定时器结构体定义
|
||||
typedef struct Timer {
|
||||
uint32_t target_time;
|
||||
uint32_t elapsed_time;
|
||||
uint32_t start_time;
|
||||
unsigned char active;
|
||||
unsigned char priority;
|
||||
struct Timer *next;
|
||||
} Timer;
|
||||
|
||||
// 函数声明
|
||||
void timerInit(Timer *timer);
|
||||
void timerStart(Timer *timer, uint32_t target_time, unsigned char priority);
|
||||
void timerStop(Timer *timer);
|
||||
void timerUpdateAll(void);
|
||||
uint32_t getCurrentTimerCount(void);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // APP_FRM_TIMER_H
|
||||
227
app/app_light.c
Normal file
227
app/app_light.c
Normal file
@@ -0,0 +1,227 @@
|
||||
#include "app_config.h"
|
||||
#include "interface.h"
|
||||
#include "app_frm_monitor.h"
|
||||
#include "app_frm_signal.h"
|
||||
#include "app_frm_timer.h"
|
||||
#include "app_param_manage.h"
|
||||
|
||||
#include "app_light.h"
|
||||
#include "app_power.h"
|
||||
|
||||
// 声明 light_data 变量
|
||||
LightSystem light_data;
|
||||
|
||||
// ... existing code ...
|
||||
|
||||
// 定义灯光按钮状态枚举
|
||||
typedef enum {
|
||||
BUTTON_STATE_INITIAL,
|
||||
BUTTON_STATE_SHORT_PRESS,
|
||||
BUTTON_STATE_DOUBLE_PRESS_DETECTED,
|
||||
BUTTON_STATE_SECOND_PRESS_DETECTED
|
||||
} LightButtonState;
|
||||
|
||||
// 定义灯光按钮结构体
|
||||
typedef struct {
|
||||
LightButtonState state;
|
||||
uint32_t press_start_time;
|
||||
uint32_t release_start_time;
|
||||
uint8_t is_light_on;
|
||||
Timer timer;
|
||||
} LightButton;
|
||||
|
||||
// 全局变量
|
||||
static LightButton light_button = {BUTTON_STATE_INITIAL, 0, 0, 0, {0}};
|
||||
|
||||
static uint8_t power_state = 0;
|
||||
|
||||
// 灯光按钮处理函数
|
||||
static void handleLightButton(void)
|
||||
{
|
||||
switch (light_button.state)
|
||||
{
|
||||
case BUTTON_STATE_INITIAL:
|
||||
if (light_data.light_switch == app_close())
|
||||
{
|
||||
light_button.state = BUTTON_STATE_SHORT_PRESS;
|
||||
timerStart(&light_button.timer, 300, 0); // 启动短按定时器，300ms
|
||||
}
|
||||
break;
|
||||
|
||||
case BUTTON_STATE_SHORT_PRESS:
|
||||
if (light_data.light_switch == app_open())
|
||||
{
|
||||
if (light_button.timer.active) // 定时器未到期，短按完成，等待双击
|
||||
{
|
||||
light_button.state = BUTTON_STATE_DOUBLE_PRESS_DETECTED;
|
||||
timerStart(&light_button.timer, 300, 0); // 启动双击等待定时器，300ms
|
||||
}
|
||||
}
|
||||
else if (!light_button.timer.active) // 短按定时器到期，重置为初始状态
|
||||
{
|
||||
light_button.state = BUTTON_STATE_INITIAL;
|
||||
}
|
||||
break;
|
||||
|
||||
case BUTTON_STATE_DOUBLE_PRESS_DETECTED:
|
||||
if (light_data.light_switch == app_close())
|
||||
{
|
||||
light_button.state = BUTTON_STATE_SECOND_PRESS_DETECTED;
|
||||
timerStart(&light_button.timer, 300, 0); // 启动第二次按下定时器，300ms
|
||||
}
|
||||
else if (!light_button.timer.active) // 双击等待定时器到期，重置为初始状态
|
||||
{
|
||||
light_button.state = BUTTON_STATE_INITIAL;
|
||||
}
|
||||
break;
|
||||
|
||||
case BUTTON_STATE_SECOND_PRESS_DETECTED:
|
||||
if (light_data.light_switch == app_open())
|
||||
{
|
||||
if (!light_button.timer.active) // 第二次按下完成，切换灯光状态
|
||||
{
|
||||
light_button.is_light_on = !light_button.is_light_on;
|
||||
printf("LightButton: is_light_on = %d\n", light_button.is_light_on);
|
||||
light_button.state = BUTTON_STATE_INITIAL;
|
||||
}
|
||||
}
|
||||
else if (!light_button.timer.active) // 第二次按下定时器到期，重置为初始状态
|
||||
{
|
||||
light_button.state = BUTTON_STATE_INITIAL;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
light_button.state = BUTTON_STATE_INITIAL;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 灯光输出处理函数
|
||||
static void lightOutput(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
// 根据当前状态,控制各个灯光
|
||||
for (int32_t i = 0; i < LIGHT_COUNT; i++)
|
||||
{
|
||||
uint8_t state_value = (light_data.states[i] == LIGHT_ON) ? setLightOn() : setLightOff();
|
||||
// 先判断灯光类型并设置状态
|
||||
switch (i)
|
||||
{//正常所有灯光熄灭
|
||||
case LIGHT_HEAD://头灯，前面4个灯
|
||||
un_inf_can_kgf_output2.bit_data.KGF07 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF09 = state_value;
|
||||
break;
|
||||
case LIGHT_TAIL://尾灯，后面4个灯
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF13 = state_value;
|
||||
break;
|
||||
case LIGHT_LEFT_TURN://左转向，左边4个灯
|
||||
un_inf_can_kgf_output2.bit_data.KGF08 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF12 = state_value;
|
||||
break;
|
||||
case LIGHT_RIGHT_TURN://右转向灯，右边4个灯
|
||||
un_inf_can_kgf_output2.bit_data.KGF10 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF14 = state_value;
|
||||
break;
|
||||
case LIGHT_BRAKE://刹车灯，四个黄灯
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF13 = state_value;
|
||||
break;
|
||||
case LIGHT_ALARM://报警灯，四个红灯
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF13 = state_value;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 灯的状态有变化，就存入参数，一个灯对应一位，共6位
|
||||
for (int32_t i = 0; i < LIGHT_COUNT; i++)
|
||||
{
|
||||
light_data.light_state |= (light_data.states[i] << i);
|
||||
}
|
||||
if (light_data.light_state != light_data.old_light_state)
|
||||
{
|
||||
setParam("lightSt", (float)light_data.light_state);
|
||||
light_data.old_light_state = light_data.light_state;
|
||||
}
|
||||
|
||||
publishMessage(&un_inf_can_kgf_output2, 1);
|
||||
}
|
||||
|
||||
// 灯光状态处理函数
|
||||
static void lightProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
if(0 != power_state)//处于非掉电模式才能打开
|
||||
{
|
||||
// 调用按钮处理函数
|
||||
handleLightButton();
|
||||
}
|
||||
else
|
||||
{
|
||||
light_button.state = BUTTON_STATE_INITIAL;//一直保持初始化模式
|
||||
light_button.is_light_on = LIGHT_OFF;//一直关闭
|
||||
}
|
||||
|
||||
// 根据双击标志控制灯光输出，相当于灯光总开关
|
||||
for (int32_t i = 0; i < LIGHT_COUNT; i++)
|
||||
{
|
||||
light_data.states[i] = (LightState)(light_button.is_light_on);
|
||||
}
|
||||
lightOutput(NULL);
|
||||
|
||||
timerStart(&light_data.timer_function, 100, 1);
|
||||
}
|
||||
|
||||
// 处理输入信号的函数
|
||||
static void lightInput(void *signal_id)
|
||||
{
|
||||
// LightSystem old_data = light_data;
|
||||
|
||||
// 填充数据
|
||||
if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据
|
||||
{
|
||||
// 保存遥控器输入开关状态
|
||||
light_data.light_switch = un_remote_control_input.bit_data.switch_d ? LIGHT_ON : LIGHT_OFF;
|
||||
}
|
||||
else if(signal_id == &power_data)
|
||||
{
|
||||
power_state = power_data.current_state;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// APP模块的初始化
|
||||
void lightAppInit(void)
|
||||
{
|
||||
// 初始化时恢复灯光状态
|
||||
light_data.light_state = (uint8_t)getParam("lightSt");
|
||||
light_data.old_light_state = light_data.light_state;
|
||||
// 根据灯光状态恢复灯光
|
||||
for (int32_t i = 0; i < LIGHT_COUNT; i++)
|
||||
{
|
||||
light_data.states[i] = (LightState)( (light_data.light_state >> i) & 1 );
|
||||
}
|
||||
// 初始化
|
||||
memset(&light_data, 0, sizeof(LightSystem));
|
||||
for (int32_t i = 0; i < LIGHT_COUNT; i++)
|
||||
{
|
||||
light_data.states[i] = LIGHT_OFF;
|
||||
light_data.brightness[i] = 255; // 默认最大亮度
|
||||
light_data.blink_state[i] = 0; // 初始化闪烁状态
|
||||
light_data.blink_interval[i] = 500; // 默认闪烁间隔500ms
|
||||
}
|
||||
|
||||
// 订阅输入信号,处理灯光逻辑
|
||||
subscribe(&un_remote_control_input, lightInput);
|
||||
subscribe(&power_data, lightInput);
|
||||
|
||||
// 订阅定时器
|
||||
subscribe(&light_data.timer_function, lightProcess);
|
||||
timerStart(&light_data.timer_function, 500, 1);
|
||||
|
||||
printf("app_light: initial OK \n");
|
||||
}
|
||||
62
app/app_light.h
Normal file
62
app/app_light.h
Normal file
@@ -0,0 +1,62 @@
|
||||
#ifndef APP_LIGHT_H
|
||||
#define APP_LIGHT_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
// 定义灯光状态枚举
|
||||
typedef enum
|
||||
{
|
||||
LIGHT_OFF,
|
||||
LIGHT_ON,
|
||||
LIGHT_BLINK
|
||||
} LightState;
|
||||
|
||||
// 定义灯光类型枚举
|
||||
typedef enum
|
||||
{
|
||||
LIGHT_HEAD,
|
||||
LIGHT_TAIL,
|
||||
LIGHT_LEFT_TURN,
|
||||
LIGHT_RIGHT_TURN,
|
||||
LIGHT_BRAKE,
|
||||
LIGHT_ALARM,
|
||||
LIGHT_COUNT
|
||||
} LightType;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
LightState states[LIGHT_COUNT];
|
||||
uint8_t brightness[LIGHT_COUNT];
|
||||
uint16_t blink_interval[LIGHT_COUNT]; // 闪烁间隔(毫秒)
|
||||
uint8_t blink_state[LIGHT_COUNT]; // 闪烁状态
|
||||
Timer timer;// 设置定时器，定时调用灯光处理函数
|
||||
uint8_t old_light_state; // 旧的灯光状态
|
||||
uint8_t light_state; // 当前的灯光状态
|
||||
Timer timer_function; // 函数定时器
|
||||
uint8_t light_switch; // 当前的灯光开关状态
|
||||
uint8_t double_click_flag;//双击标志
|
||||
} LightSystem;
|
||||
|
||||
// 在头文件中声明外部变量
|
||||
extern LightSystem light_data;
|
||||
|
||||
// 使用内联函数
|
||||
static inline uint8_t setLightOn(void) { return 1; }
|
||||
static inline uint8_t setLightOff(void) { return 0; }
|
||||
|
||||
void lightAppInit(void);
|
||||
void setLightState(LightType light, LightState state);
|
||||
void setLightBrightness(LightType light, uint8_t brightness);
|
||||
void setBlinkInterval(uint16_t interval);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // APP_LIGHT_H
|
||||
464
app/app_param_manage.c
Normal file
464
app/app_param_manage.c
Normal file
@@ -0,0 +1,464 @@
|
||||
#include "app_config.h"
|
||||
#include "app_frm_monitor.h"
|
||||
#include "app_frm_signal.h"
|
||||
#include "app_frm_timer.h"
|
||||
#include "app_param_manage.h"
|
||||
|
||||
|
||||
#define E2_RESERVE_COUNT 0x20 //增加IP地址 修改为0x20 20250110
|
||||
|
||||
// 待发送的参数请求信号
|
||||
UnParamRequest un_param_request1 ;
|
||||
UnParamRequest un_param_request2;
|
||||
|
||||
RequestContext request_send ;
|
||||
RequestContext request_context ;
|
||||
|
||||
uint8_t read_write_e2_finished = 0;
|
||||
|
||||
// 定义全局信号实例，读写信号现在包括 offset 和 size
|
||||
ParamSignal param_signal = {
|
||||
.param_ptr = NULL, // 参数指针初始化为 NULL
|
||||
.type = READ_OPERATION, // 操作类型设置为读操作
|
||||
.offset = 0, // 整个数据块的偏移
|
||||
.size = sizeof(param_manager.arr) // 整个数据块的大小
|
||||
};
|
||||
|
||||
|
||||
// 全局变量：初始化参数名称结构体
|
||||
ParamNames param_names = {
|
||||
#define X(name) .name = #name,
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
};
|
||||
|
||||
|
||||
UnParamManager param_manager ;
|
||||
|
||||
static uint8_t is_param_initialized = 0;
|
||||
|
||||
// 打印所有参数的名称和值, 每行 4 个参数
|
||||
void printParams()
|
||||
{
|
||||
unsigned int param_count = 0;
|
||||
|
||||
#define X(name) \
|
||||
printf("%-8s: %-8.2f", param_names.name, param_manager.bit_data.name); \
|
||||
param_count++; \
|
||||
if (param_count % 4 == 0) { \
|
||||
printf("\n"); \
|
||||
} else { \
|
||||
printf(" "); \
|
||||
}
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
// 如果最后一行不足 4 个参数, 打印换行
|
||||
if (param_count % 4 != 0)
|
||||
{
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void writeByte24c02(uint16_t addr, uint8_t data)
|
||||
{
|
||||
if(0 != wrbyte_24c02(addr,data))
|
||||
{
|
||||
printf("E2PROM write error!\n");
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t readByte24c02(uint16_t addr)
|
||||
{
|
||||
return rdbyte_24c02(addr);
|
||||
}
|
||||
|
||||
// 定义一个通用的 EEPROM 访问函数
|
||||
uint8_t accessEeprom(size_t offset, void *data, size_t size, OperationType type)
|
||||
{
|
||||
if (data == NULL || size == 0)
|
||||
{
|
||||
return 1; // 返回错误状态，表示无效的参数
|
||||
}
|
||||
|
||||
uint8_t *byte_data = (uint8_t *)data; // 将 void* 转换为 uint8_t*，方便逐字节操作
|
||||
size_t index;
|
||||
|
||||
if (type == WRITE_OPERATION)
|
||||
{
|
||||
// 写入操作
|
||||
for (index = 0; index < size; index++)
|
||||
{
|
||||
writeByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT), byte_data[index]);
|
||||
udelay(4000);//写入一个字节延时4ms
|
||||
}
|
||||
// 校验
|
||||
for (index = 0; index < size; index++)
|
||||
{
|
||||
if (readByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT)) != byte_data[index])
|
||||
{
|
||||
return 2; // 返回错误状态，表示写入验证失败
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 读取操作
|
||||
for (index = 0; index < size; index++)
|
||||
{
|
||||
byte_data[index] = readByte24c02((uint16_t)(offset + index + E2_RESERVE_COUNT));
|
||||
}
|
||||
}
|
||||
return 0; // 返回状态，表示成功
|
||||
}
|
||||
|
||||
void handleParamOp(void *data)
|
||||
{
|
||||
ParamSignal *signal = (ParamSignal *)data;
|
||||
|
||||
if (signal->param_ptr == NULL)
|
||||
{
|
||||
// 操作整个参数管理器
|
||||
if (accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), signal->type) == 0)
|
||||
{
|
||||
read_write_e2_finished = 1;
|
||||
publishMessage(&read_write_e2_finished, 1); // 读写成功
|
||||
}
|
||||
else
|
||||
{
|
||||
read_write_e2_finished = 2;
|
||||
publishMessage(&read_write_e2_finished, 1); // 读写失败
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 根据信号中的偏移和大小操作单个参数
|
||||
if (accessEeprom(signal->offset, signal->param_ptr, signal->size, signal->type) == 0)
|
||||
{
|
||||
read_write_e2_finished = 1;
|
||||
publishMessage(&read_write_e2_finished, 1); // 读写成功
|
||||
}
|
||||
else
|
||||
{
|
||||
read_write_e2_finished = 2;
|
||||
publishMessage(&read_write_e2_finished, 1); // 读写失败
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
uint8_t calculateCRC(const uint8_t* data, uint32_t length) {
|
||||
uint8_t crc = 0;
|
||||
for (uint32_t i = 0; i < length; ++i) {
|
||||
crc += data[i]; // 简单的校验和，按字节累加
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
|
||||
float readParameter(const char *param_name) {
|
||||
float float_value = 0;
|
||||
unsigned int offset = 0;
|
||||
|
||||
#define X(name) \
|
||||
if (strcmp(param_name, param_names.name) == 0) { \
|
||||
accessEeprom(offset, ¶m_manager.bit_data.name,sizeof(param_manager.bit_data.name), READ_OPERATION);\
|
||||
memcpy(&float_value, ¶m_manager.bit_data.name, sizeof(param_manager.bit_data.name)); \
|
||||
return float_value; \
|
||||
} \
|
||||
offset += 4;
|
||||
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
printf("Parameter not found: %s\n", param_name);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void writeParameter(const char *param_name, const uint8_t *data) {
|
||||
unsigned int offset = 0;
|
||||
|
||||
#define X(name) \
|
||||
if (strcmp(param_name, param_names.name) == 0) { \
|
||||
memcpy(¶m_manager.bit_data.name, data, sizeof(param_manager.bit_data.name)); \
|
||||
accessEeprom(offset, ¶m_manager.bit_data.name,sizeof(param_manager.bit_data.name), WRITE_OPERATION);\
|
||||
return; \
|
||||
} \
|
||||
offset += 4;
|
||||
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
printf("Parameter not found: %s\n", param_name);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void sendParamRequestResponse(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port, uint8_t isWriteOperation) {
|
||||
// 准备响应帧
|
||||
paramRequest->bit_data.frame_header = 0xFF80;
|
||||
paramRequest->bit_data.frame_type = 0x002B;
|
||||
paramRequest->bit_data.frame_length = sizeof(StrParamRequest);
|
||||
paramRequest->bit_data.accumulated = 0;
|
||||
paramRequest->bit_data.request_id = isWriteOperation ? 98 : 99;
|
||||
|
||||
paramRequest->bit_data.crc = calculateCRC(paramRequest->arr, sizeof(paramRequest->arr) - 1);
|
||||
|
||||
request_send.param_request = paramRequest;
|
||||
request_send.sender_ip = sender_ip;
|
||||
request_send.sender_port = sender_port;
|
||||
// 发送信号，从UDP发送
|
||||
publishMessage(&request_send, 1);
|
||||
}
|
||||
|
||||
|
||||
void processReadAllParams(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) {
|
||||
uint8_t allParams[256][4]; // Size based on E2 size
|
||||
unsigned int i = 0;
|
||||
float param_value;
|
||||
uint8_t exceeded_max = 0; // 新增标志变量
|
||||
|
||||
// 清零 paramRequest
|
||||
memset(paramRequest, 0, sizeof(UnParamRequest));
|
||||
|
||||
accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION);
|
||||
|
||||
printf("Sending parameter data:\n");
|
||||
|
||||
#define X(name) \
|
||||
if (!exceeded_max) { \
|
||||
if (i < 256) { \
|
||||
strncpy((char *)paramRequest->bit_data.param_name[i], #name, sizeof(paramRequest->bit_data.param_name[i]) - 1); \
|
||||
paramRequest->bit_data.param_name[i][sizeof(paramRequest->bit_data.param_name[i]) - 1] = '\0'; \
|
||||
memcpy(allParams[i], ¶m_manager.bit_data.name, sizeof(param_manager.bit_data.name)); \
|
||||
memcpy(¶m_value, allParams[i], sizeof(float)); \
|
||||
printf("Parameter name: %-20s Value: %f\n", #name, param_value); \
|
||||
i++; \
|
||||
} else { \
|
||||
printf("Warning: Exceeded maximum number of parameters\n"); \
|
||||
exceeded_max = 1; \
|
||||
} \
|
||||
}
|
||||
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
// Pack all parameter data into paramRequest
|
||||
memcpy(paramRequest->bit_data.data, allParams, sizeof(allParams));
|
||||
|
||||
printf("Total parameters sent: %d\n", i);
|
||||
|
||||
// Send response
|
||||
sendParamRequestResponse(paramRequest, sender_ip, sender_port, 0);
|
||||
}
|
||||
|
||||
|
||||
void processWriteRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) {
|
||||
float value;
|
||||
printf("Processing write request.\n");
|
||||
|
||||
// 先发送信号，然后从结构体读数
|
||||
for (int i = 0; i < 256; ++i) {
|
||||
if (strlen((char *)paramRequest->bit_data.param_name[i]) > 0) {
|
||||
writeParameter(paramRequest->bit_data.param_name[i], paramRequest->bit_data.data[i]);
|
||||
printf("paramRequest->bit_data.param_name[i]:%s \n",paramRequest->bit_data.param_name[i]);
|
||||
memcpy(&value, paramRequest->bit_data.data[i], sizeof(float));
|
||||
printf("paramRequest->bit_data.data[i]:%f \n", value);
|
||||
}
|
||||
}
|
||||
|
||||
// 发送响应，发送所有参数
|
||||
processReadAllParams(paramRequest, sender_ip, sender_port);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void processReadRequestFrame(UnParamRequest *paramRequest, uint32_t sender_ip, uint16_t sender_port) {
|
||||
// 处理读请求的逻辑
|
||||
printf("Processing read request.\n");
|
||||
|
||||
// 清零 paramRequest
|
||||
memset(paramRequest, 0, sizeof(UnParamRequest));
|
||||
|
||||
// 先发送信号，然后从结构体读数
|
||||
for (int i = 0; i < 256; ++i) {
|
||||
if (strlen((char *)paramRequest->bit_data.param_name[i]) > 0) {
|
||||
float readData = readParameter(paramRequest->bit_data.param_name[i]);
|
||||
memcpy(paramRequest->bit_data.data[i], &readData, sizeof(paramRequest->bit_data.data[i]));
|
||||
}
|
||||
}
|
||||
|
||||
// 发送响应
|
||||
sendParamRequestResponse(paramRequest, sender_ip, sender_port, 0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void OnParamSignal(void *data)
|
||||
{
|
||||
RequestContext *signal = (RequestContext *)data;
|
||||
|
||||
uint8_t *datagram = (uint8_t *)signal->param_request->arr;
|
||||
uint16_t request_id = ((uint16_t)datagram[7] << 8) | (uint16_t)datagram[8];// 大端模式
|
||||
|
||||
// 调试输出
|
||||
printf("Received request ID: 0x%04X\n", request_id);
|
||||
|
||||
// 计算CRC
|
||||
uint8_t calculatedCrc = calculateCRC(datagram, sizeof(UnParamRequest) - 1);
|
||||
uint8_t receivedCrc = datagram[sizeof(UnParamRequest) - 1];
|
||||
|
||||
// 比较CRC
|
||||
if (calculatedCrc != receivedCrc)
|
||||
{
|
||||
printf("CRC check failed, discarding data\n");
|
||||
printf("Calculated CRC: 0x%02X, Received CRC: 0x%02X\n", calculatedCrc, receivedCrc);
|
||||
return;
|
||||
}
|
||||
|
||||
printf("CRC check passed\n");
|
||||
|
||||
if (request_id == 100)
|
||||
{ // 读请求
|
||||
processReadRequestFrame(signal->param_request, signal->sender_ip, signal->sender_port);
|
||||
}
|
||||
else if (request_id == 101)
|
||||
{ // 写请求
|
||||
processWriteRequestFrame(signal->param_request, signal->sender_ip, signal->sender_port);
|
||||
}
|
||||
else if (request_id == 102)
|
||||
{ // 读取所有参数
|
||||
processReadAllParams(signal->param_request, signal->sender_ip, signal->sender_port);
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("Unknown request ID.\n");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
float getParam(const char *param_name)
|
||||
{
|
||||
// 检查是否已初始化
|
||||
if (!is_param_initialized)
|
||||
{
|
||||
printf("Parameters not initialized, reinitializing\n");
|
||||
accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION);//Read all parameters from E2
|
||||
is_param_initialized = 1; // Mark as initialized
|
||||
printParams();
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// 检查参数名是否为空
|
||||
if (param_name == NULL)
|
||||
{
|
||||
printf("Error: Parameter name is empty\n");
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// 遍历所有参数
|
||||
#define X(name) \
|
||||
if (strcmp(param_name, #name) == 0) \
|
||||
{ \
|
||||
return param_manager.bit_data.name; \
|
||||
}
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
// 如果没有找到匹配的参数名
|
||||
printf("Error: Parameter %s not found\n", param_name);
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// setParam 函数
|
||||
uint8_t setParam(const char *param_name, float value)
|
||||
{
|
||||
// 检查参数名是否为空
|
||||
if (param_name == NULL)
|
||||
{
|
||||
printf("Error: Parameter name is empty\n");
|
||||
return 2; // 返回错误码
|
||||
}
|
||||
|
||||
// 参数名和值写入EEPROM，先转成字节数组
|
||||
uint8_t data[sizeof(float)];
|
||||
memcpy(data, &value, sizeof(float));
|
||||
writeParameter(param_name, data);
|
||||
|
||||
// 更新参数
|
||||
#define X(name) \
|
||||
if (strcmp(param_name, #name) == 0) \
|
||||
{ \
|
||||
memcpy(¶m_manager.bit_data.name, data, sizeof(param_manager.bit_data.name)); \
|
||||
}
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void paramAppInit(void)
|
||||
{
|
||||
// 初始化全局变量
|
||||
memset(&un_param_request1, 0, sizeof(UnParamRequest));
|
||||
memset(&un_param_request2, 0, sizeof(UnParamRequest));
|
||||
|
||||
// 正确初始化 RequestContext 结构体
|
||||
request_send.param_request = &un_param_request1;
|
||||
request_send.sender_ip = 0;
|
||||
request_send.sender_port = 0;
|
||||
|
||||
request_context.param_request = &un_param_request2;
|
||||
request_context.sender_ip = 0;
|
||||
request_context.sender_port = 0;
|
||||
|
||||
// 上电读取所有参数
|
||||
memset(param_manager.arr, 0, sizeof(param_manager.arr));
|
||||
accessEeprom(0, param_manager.arr, sizeof(param_manager.arr), READ_OPERATION);
|
||||
|
||||
// 初始化每个参数
|
||||
// param_manager.bit_data.whl_bas = 1.5f; // 初始化轮距
|
||||
// param_manager.bit_data.max_rpm = 5500.0f; // 初始化最大转速
|
||||
// param_manager.bit_data.whl_dia = 0.6f; // 初始化轮直径
|
||||
// param_manager.bit_data.max_acc = 1.0f; // 初始化最大加速度
|
||||
// param_manager.bit_data.spd_kp = 5.0f; // 初始化速度控制 KP
|
||||
// param_manager.bit_data.spd_ki = 1.0f; // 初始化速度控制 KI
|
||||
// param_manager.bit_data.spd_kd = 0.0f; // 初始化速度控制 KD
|
||||
// param_manager.bit_data.spd_il = 5.0f; // 初始化速度控制 IL
|
||||
// param_manager.bit_data.spd_ol = 5.0f; // 初始化速度控制 OL
|
||||
// param_manager.bit_data.crv_kp = 1.0f; // 初始化曲线控制 KP
|
||||
// param_manager.bit_data.crv_ki = 0.0f; // 初始化曲线控制 KI
|
||||
// param_manager.bit_data.crv_kd = 0.0f; // 初始化曲线控制 KD
|
||||
// param_manager.bit_data.crv_il = 2.0f; // 初始化曲线控制 IL
|
||||
// param_manager.bit_data.crv_ol = 2.0f; // 初始化曲线控制 OL
|
||||
// param_manager.bit_data.brk_on = 1500.0f; // 初始化制动开启参数
|
||||
// param_manager.bit_data.brk_off = 800.0f; // 初始化制动关闭参数
|
||||
// param_manager.bit_data.maxTorq = 60.0f; // 初始化最大扭矩
|
||||
// param_manager.bit_data.feedPwr = 10000.0f; // 初始化馈电功率
|
||||
// param_manager.bit_data.dispPwr = 10000.0f; // 初始化显示功率
|
||||
// param_manager.bit_data.VehMass = 700.0f; // 初始化车辆质量
|
||||
// param_manager.bit_data.gRatio = 28.0f; // 初始化减速比
|
||||
// param_manager.bit_data.prCTime = 5.0f; // 初始化预充时间
|
||||
// param_manager.bit_data.brk_pos = 0.0f; // 初始化刹车位置, 0表示未刹车
|
||||
// param_manager.bit_data.pwr_sta = 0.0f; // 初始化电源状态
|
||||
// param_manager.bit_data.lightSt = 0.0f; // 初始化灯光状态
|
||||
// param_manager.bit_data.pwr_btn = 0.0f; // 初始化电源按钮状态
|
||||
// param_manager.bit_data.test = 0.0f; // 初始化测试参数
|
||||
|
||||
|
||||
// 订阅信号
|
||||
subscribe(¶m_signal, handleParamOp);
|
||||
subscribe(&request_context, OnParamSignal);// 接收到上位机读写参数信号
|
||||
|
||||
printParams();//打印所有参数
|
||||
|
||||
is_param_initialized = 1; // 标记初始化完成
|
||||
|
||||
printf("paramAPP init OK! %d\n",getCurrentTime());
|
||||
}
|
||||
148
app/app_param_manage.h
Normal file
148
app/app_param_manage.h
Normal file
@@ -0,0 +1,148 @@
|
||||
#ifndef APP_PARAM_MANAGE_H
|
||||
#define APP_PARAM_MANAGE_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#include "app_config.h"
|
||||
|
||||
|
||||
// 定义参数列表宏
|
||||
#define PARAM_LIST \
|
||||
X(whl_bas) \
|
||||
X(max_rpm) \
|
||||
X(whl_dia) \
|
||||
X(max_acc) \
|
||||
X(spd_kp) \
|
||||
X(spd_ki) \
|
||||
X(spd_kd) \
|
||||
X(spd_il) \
|
||||
X(spd_ol) \
|
||||
X(crv_kp) \
|
||||
X(crv_ki) \
|
||||
X(crv_kd) \
|
||||
X(crv_il) \
|
||||
X(crv_ol) \
|
||||
X(brk_on) \
|
||||
X(brk_off) \
|
||||
X(maxTorq) \
|
||||
X(feedPwr) \
|
||||
X(dispPwr) \
|
||||
X(VehMass) \
|
||||
X(gRatio) \
|
||||
X(prCTime) \
|
||||
X(brk_pos) \
|
||||
X(pwr_sta) \
|
||||
X(lightSt) \
|
||||
X(pwr_btn) \
|
||||
X(sleepTm) \
|
||||
X(wakeTm) \
|
||||
X(R_maxP) \
|
||||
X(R_minP) \
|
||||
X(R_zeroP) \
|
||||
X(R_speed) \
|
||||
X(L_maxP) \
|
||||
X(L_minP) \
|
||||
X(L_zeroP) \
|
||||
X(L_speed) \
|
||||
X(brk_rev) \
|
||||
X(tra_wid) \
|
||||
X(mot_kp) \
|
||||
X(mot_ki) \
|
||||
X(mot_kd) \
|
||||
X(mot_il) \
|
||||
X(mot_ol) \
|
||||
X(diff_sp) \
|
||||
X(test)
|
||||
|
||||
// 定义一个包含所有参数名称的结构体
|
||||
typedef struct {
|
||||
#define X(name) const char* name;
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
} ParamNames;
|
||||
|
||||
// 参数结构体，不能使用位域，不要超过E2的大小, 1K byte
|
||||
typedef struct
|
||||
{
|
||||
#define X(name) float name;
|
||||
PARAM_LIST
|
||||
#undef X
|
||||
} ParamData;
|
||||
|
||||
typedef union
|
||||
{
|
||||
ParamData bit_data; // 使用定义的结构体变量名
|
||||
uint8_t arr[sizeof(ParamData)]; // 通过结构体类型确定大小
|
||||
} UnParamManager;
|
||||
|
||||
// 定义信号操作类型
|
||||
typedef enum
|
||||
{
|
||||
READ_OPERATION,
|
||||
WRITE_OPERATION
|
||||
} OperationType;
|
||||
|
||||
// 定义信号数据结构
|
||||
typedef struct
|
||||
{
|
||||
void *param_ptr; // 参数数据的指针
|
||||
OperationType type; // 操作类型
|
||||
size_t offset; // 参数在结构体中的偏移
|
||||
size_t size; // 参数大小
|
||||
} ParamSignal;
|
||||
|
||||
#pragma pack(push, 1)
|
||||
|
||||
typedef struct _StrParamRequest {
|
||||
//--------------------------------------------------
|
||||
uint16_t frame_header; // 帧头 固定值0xFF80 (16位)
|
||||
uint16_t frame_type; // 帧类型 固定值0x002A (16位)
|
||||
uint16_t frame_length; // 帧长 根据参数数据的长度动态设置 (16位)
|
||||
uint8_t accumulated; // 累加值 按帧累加 (8位)
|
||||
uint16_t request_id; // 请求帧ID 请求ID 100表示读，101 表示写参数 (16位)
|
||||
char param_name[256][8]; // 参数名称 标识要写入或读出的参数
|
||||
uint8_t data[256][4]; // 数据 用于写入或读出的参数值，一个参数最大4字节 (8位*4)
|
||||
uint8_t crc; // CRC 按字节累加之和 取低8位 (8位)
|
||||
} StrParamRequest;
|
||||
|
||||
typedef union _UnParamRequest {
|
||||
StrParamRequest bit_data; // 使用定义的结构体变量名
|
||||
unsigned char arr[sizeof(StrParamRequest)]; // 通过结构体类型确定大小
|
||||
} UnParamRequest;
|
||||
|
||||
typedef struct {
|
||||
UnParamRequest *param_request; // 指向 UnParamRequest 的指针
|
||||
uint32_t sender_ip; // 发送方的 IP 地址（使用标准的32位整数表示）
|
||||
uint16_t sender_port; // 发送方的端口号（使用标准的16位整数表示）
|
||||
} RequestContext;
|
||||
|
||||
#pragma pack(pop)
|
||||
|
||||
extern UnParamRequest un_param_request;// 声明用于参数响应的帧实例
|
||||
extern RequestContext request_context;
|
||||
|
||||
// 声明全局信号实例
|
||||
extern ParamNames param_names;
|
||||
extern UnParamManager param_manager; // 全局参数管理实例
|
||||
extern ParamSignal param_signal;
|
||||
extern uint8_t read_write_e2_finished;
|
||||
extern RequestContext request_send;// 待发送的参数请求信号
|
||||
|
||||
uint8_t access_eeprom(size_t offset, void *data, size_t size, OperationType type);
|
||||
void paramAppInit(void);
|
||||
|
||||
// 在适当的位置添加以下函数声明
|
||||
float getParam(const char *param_name);
|
||||
uint8_t setParam(const char *param_name, float value);
|
||||
void printParams(void);
|
||||
void handleParamOp(void *data);
|
||||
void OnParamSignal(void *data);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // APP_PARAM_MANAGE_H
|
||||
130
app/app_pid.c
Normal file
130
app/app_pid.c
Normal file
@@ -0,0 +1,130 @@
|
||||
#include "app_config.h"
|
||||
|
||||
#include "app_pid.h"
|
||||
|
||||
|
||||
|
||||
|
||||
// PID控制器初始化
|
||||
void initializePid(PID_t *pid, pid_mode_t mode, float dtMin)
|
||||
{
|
||||
pid->mode = mode;
|
||||
pid->dt_min = dtMin > SIGMA ? dtMin : SIGMA;
|
||||
pid->kp = 0.0f;
|
||||
pid->ki = 0.0f;
|
||||
pid->kd = 0.0f;
|
||||
pid->integral = 0.0f;
|
||||
pid->integral_limit = 0.0f;
|
||||
pid->output_limit = 0.0f;
|
||||
pid->error_previous = 0.0f;
|
||||
pid->last_output = 0.0f;
|
||||
}
|
||||
|
||||
// 设置PID参数
|
||||
int32_t setPidParameters(PID_t *pid, float kp, float ki, float kd, float integralLimit, float outputLimit)
|
||||
{
|
||||
int32_t ret = 0;
|
||||
|
||||
if (isfinite(kp)) {
|
||||
pid->kp = kp;
|
||||
} else {
|
||||
ret = -1;
|
||||
}
|
||||
|
||||
if (isfinite(ki)) {
|
||||
pid->ki = ki;
|
||||
} else {
|
||||
ret = -2;
|
||||
}
|
||||
|
||||
if (isfinite(kd)) {
|
||||
pid->kd = kd;
|
||||
} else {
|
||||
ret = -3;
|
||||
}
|
||||
|
||||
if (isfinite(integralLimit)) {
|
||||
pid->integral_limit = integralLimit;
|
||||
} else {
|
||||
ret = -4;
|
||||
}
|
||||
|
||||
if (isfinite(outputLimit)) {
|
||||
pid->output_limit = outputLimit;
|
||||
} else {
|
||||
ret = -5;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 计算PID控制器的输出
|
||||
*
|
||||
* @param pid 指向PID_t结构体的指针，包含PID控制器的状态和参数
|
||||
* @param sp 设定值(Setpoint)，期望系统达到的目标值
|
||||
* @param val 当前值(Current Value)，系统的实际测量值
|
||||
* @param val_dot 当前值的导数(Derivative of Current Value)，即测量值的变化率（用于微分项计算）
|
||||
* @param dt 时间增量(Time Increment)，两次调用之间的时间间隔，用于计算积分和微分
|
||||
*
|
||||
* @return float 返回PID控制器计算出的输出值
|
||||
*/
|
||||
float calculatePidOutput(PID_t *pid, float sp, float val, float val_dot, float dt)
|
||||
{
|
||||
// 检查输入参数的有效性
|
||||
if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt) || dt < pid->dt_min) {
|
||||
return pid->last_output;
|
||||
}
|
||||
|
||||
// 计算误差
|
||||
float error = sp - val;
|
||||
|
||||
// 根据模式计算微分项
|
||||
float derivative = 0.0f;
|
||||
switch (pid->mode) {
|
||||
case PID_MODE_DERIVATIVE_CALC:
|
||||
derivative = (error - pid->error_previous) / dt;
|
||||
pid->error_previous = error;
|
||||
break;
|
||||
case PID_MODE_DERIVATIVE_CALC_NO_SP:
|
||||
derivative = (-val - pid->error_previous) / dt;
|
||||
pid->error_previous = -val;
|
||||
break;
|
||||
case PID_MODE_DERIVATIVE_SET:
|
||||
derivative = -val_dot;
|
||||
break;
|
||||
default:
|
||||
derivative = 0.0f;
|
||||
break;
|
||||
}
|
||||
|
||||
// 计算比例和微分项的输出
|
||||
float output = (pid->kp * error) + (pid->kd * derivative);
|
||||
|
||||
// 计算积分项，并检查积分饱和
|
||||
if (pid->ki > SIGMA) {
|
||||
pid->integral += error * dt;
|
||||
if (pid->integral > pid->integral_limit) {
|
||||
pid->integral = pid->integral_limit;
|
||||
} else if (pid->integral < -pid->integral_limit) {
|
||||
pid->integral = -pid->integral_limit;
|
||||
}
|
||||
output += pid->ki * pid->integral;
|
||||
}
|
||||
|
||||
// 限制输出范围
|
||||
if (output > pid->output_limit) {
|
||||
output = pid->output_limit;
|
||||
} else if (output < -pid->output_limit) {
|
||||
output = -pid->output_limit;
|
||||
}
|
||||
|
||||
pid->last_output = output;
|
||||
return output;
|
||||
}
|
||||
|
||||
// 重置积分器
|
||||
void resetPidIntegral(PID_t *pid)
|
||||
{
|
||||
pid->integral = 0.0f;
|
||||
}
|
||||
46
app/app_pid.h
Normal file
46
app/app_pid.h
Normal file
@@ -0,0 +1,46 @@
|
||||
#ifndef PID_H
|
||||
#define PID_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
// 防止除零和其他误差的极小值
|
||||
#define EPSILON 1e-5f
|
||||
#define SIGMA EPSILON
|
||||
|
||||
#include "app_config.h"
|
||||
|
||||
// PID 控制模式
|
||||
typedef enum {
|
||||
PID_MODE_DERIVATIVE_NONE = 0, // PI 控制器模式，无微分项
|
||||
PID_MODE_DERIVATIVE_CALC, // 根据当前误差计算微分项
|
||||
PID_MODE_DERIVATIVE_CALC_NO_SP, // 根据当前值计算微分项，忽略设定值
|
||||
PID_MODE_DERIVATIVE_SET // 使用外部提供的微分项值
|
||||
} pid_mode_t;
|
||||
|
||||
// PID 控制器结构体
|
||||
typedef struct {
|
||||
pid_mode_t mode; // 控制模式
|
||||
float dt_min; // 最小时间间隔
|
||||
float kp; // 比例系数
|
||||
float ki; // 积分系数
|
||||
float kd; // 微分系数
|
||||
float integral; // 积分累积值
|
||||
float integral_limit; // 积分限幅
|
||||
float output_limit; // 输出限幅
|
||||
float error_previous; // 上一次的误差值
|
||||
float last_output; // 上一次的输出值
|
||||
} PID_t;
|
||||
|
||||
// 函数声明
|
||||
void initializePid(PID_t *pid, pid_mode_t mode, float dtMin);
|
||||
int32_t setPidParameters(PID_t *pid, float kp, float ki, float kd, float integralLimit, float outputLimit);
|
||||
float calculatePidOutput(PID_t *pid, float sp, float val, float val_dot, float dt);
|
||||
void resetPidIntegral(PID_t *pid);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // PID_H
|
||||
366
app/app_power.c
Normal file
366
app/app_power.c
Normal file
@@ -0,0 +1,366 @@
|
||||
#include "app_config.h"
|
||||
#include "interface.h"
|
||||
#include "app_frm_monitor.h"
|
||||
#include "app_frm_signal.h"
|
||||
#include "app_frm_timer.h"
|
||||
#include "app_param_manage.h"
|
||||
#include "app_power.h"
|
||||
|
||||
|
||||
// 定义按钮状态枚举
|
||||
typedef enum {
|
||||
BUTTON_STATE_INITIAL,
|
||||
BUTTON_STATE_SHORT_PRESS,
|
||||
BUTTON_STATE_SHORT_PRESS_DETECTED,
|
||||
BUTTON_STATE_WAIT_FOR_LONG_PRESS,
|
||||
BUTTON_STATE_LONG_PRESS,
|
||||
BUTTON_STATE_LONG_PRESS_WAIT
|
||||
} ButtonState;
|
||||
|
||||
// 定义按钮结构体
|
||||
typedef struct {
|
||||
ButtonState state;
|
||||
uint32_t press_start_time;
|
||||
uint32_t release_start_time;
|
||||
uint8_t is_power_on;
|
||||
Timer timer;
|
||||
uint8_t old_is_power_on;
|
||||
Timer timer1;
|
||||
} PowerButton;
|
||||
|
||||
// 全局变量
|
||||
PowerSystem power_data;
|
||||
static PowerButton power_button = {BUTTON_STATE_INITIAL, 0, 0, 0, {0},0};
|
||||
|
||||
// 电源按钮处理函数
|
||||
static void handlePowerButton(void)
|
||||
{
|
||||
switch (power_button.state)
|
||||
{
|
||||
case BUTTON_STATE_INITIAL:
|
||||
if (power_data.remote_power_switch == app_close())
|
||||
{
|
||||
power_button.state = BUTTON_STATE_SHORT_PRESS_DETECTED;
|
||||
timerStart(&power_button.timer, 500, 0); // 启动短按定时器，500ms
|
||||
}
|
||||
break;
|
||||
|
||||
case BUTTON_STATE_SHORT_PRESS_DETECTED:
|
||||
if (power_data.remote_power_switch == app_open())
|
||||
{
|
||||
if (power_button.timer.active) // 定时器未到期,短按完成，启动等待长按定时器
|
||||
{
|
||||
power_button.state = BUTTON_STATE_WAIT_FOR_LONG_PRESS;
|
||||
timerStart(&power_button.timer, 500, 0); // 启动等待长按定时器，500ms
|
||||
}
|
||||
}
|
||||
else if (!power_button.timer.active)// 短按定时器到期，按键仍被按下，视为无效，重置为初始状态
|
||||
{
|
||||
power_button.state = BUTTON_STATE_INITIAL;
|
||||
}
|
||||
break;
|
||||
|
||||
case BUTTON_STATE_WAIT_FOR_LONG_PRESS:
|
||||
if (power_data.remote_power_switch == app_close())// 检测是否在等待时间内进行长按
|
||||
{
|
||||
power_button.state = BUTTON_STATE_LONG_PRESS;
|
||||
timerStart(&power_button.timer, 1000, 0); // 启动长按定时器，1000ms
|
||||
}
|
||||
else if (!power_button.timer.active) // 等待长按超时，重置为初始状态
|
||||
{
|
||||
power_button.state = BUTTON_STATE_INITIAL;
|
||||
}
|
||||
break;
|
||||
|
||||
case BUTTON_STATE_LONG_PRESS:
|
||||
|
||||
if (!power_button.timer.active)// 长按完成，切换电源状态 20250423 修改不需要判断松开按键就打开控制器
|
||||
{
|
||||
power_button.is_power_on = !power_button.is_power_on;
|
||||
printf("PowerButton: is_power_on = %d\n", power_button.is_power_on);
|
||||
power_button.state = BUTTON_STATE_LONG_PRESS_WAIT;
|
||||
}
|
||||
else if(power_data.remote_power_switch == app_open())
|
||||
{
|
||||
power_button.state = BUTTON_STATE_INITIAL;
|
||||
printf("Long press for short duration");
|
||||
}
|
||||
else
|
||||
break;
|
||||
|
||||
case BUTTON_STATE_LONG_PRESS_WAIT:
|
||||
if (power_data.remote_power_switch == app_open())// 检测按键释放
|
||||
{
|
||||
power_button.state = BUTTON_STATE_INITIAL;
|
||||
printf("Release the button");
|
||||
}
|
||||
default:
|
||||
power_button.state = BUTTON_STATE_INITIAL;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// 输出处理函数
|
||||
static void powerOutput(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
// 根据当前状态,控制各个设备的电源
|
||||
switch (power_data.current_state)
|
||||
{
|
||||
case POWER_PRE_CHARGE:
|
||||
publishMessage(&power_data.pre_charge_finish, 1);//发布预充完成信号，100ms发送一次，直到预充完成
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOn(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电
|
||||
un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
break;
|
||||
|
||||
case POWER_STANDBY:
|
||||
// 初始状态,只开启基本设备
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电
|
||||
un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
break;
|
||||
|
||||
case POWER_WORKING:
|
||||
// 工作状态,除预充继电器外所有设备开启
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOn(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOn(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电
|
||||
un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
break;
|
||||
|
||||
case POWER_EMERGENCY:
|
||||
// 急停状态,断开高压
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电
|
||||
un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
break;
|
||||
|
||||
case POWER_SLEEP:
|
||||
// 休眠状态,关闭所有设备
|
||||
un_inf_can_kgf_output1.bit_data.KGF04 = setPowerOff(); // 预充继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setPowerOff(); // 高压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF12 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setPowerOn(); // 低压充电
|
||||
un_inf_can_kgf_output2.bit_data.KGF01 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF02 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF16 = setPowerOn(); // 计算机
|
||||
un_inf_can_kgf_output2.bit_data.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
publishMessage(&power_data, 1);
|
||||
publishMessage(&un_inf_can_kgf_output1, 1);
|
||||
publishMessage(&un_inf_can_kgf_output2, 1);
|
||||
}
|
||||
|
||||
|
||||
static void wakeupProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
un_gather_output.bit_data.sleep_duration = (uint16_t)getParam("sleepTm");
|
||||
un_gather_output.bit_data.wakeup_interval = (uint16_t)getParam("wakeTm");
|
||||
|
||||
if(un_gather_output.bit_data.sleep_duration < 5)//最小值限定
|
||||
{
|
||||
un_gather_output.bit_data.sleep_duration = 5;
|
||||
}
|
||||
|
||||
if(un_gather_output.bit_data.wakeup_interval < 5)//最小值限定
|
||||
{
|
||||
un_gather_output.bit_data.wakeup_interval = 5;
|
||||
}
|
||||
|
||||
un_gather_output.bit_data.vehicle_mode = power_data.current_state;
|
||||
|
||||
publishMessage(&un_gather_output, 1);
|
||||
timerStart(&power_data.timer1, 500, 1); //周期调用
|
||||
}
|
||||
|
||||
// 定时器处理函数
|
||||
static void powerTimerProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
// 调用电源按钮处理函数
|
||||
handlePowerButton();
|
||||
// 电源按钮状态有变化，保存到参数
|
||||
if (power_button.is_power_on != power_button.old_is_power_on)
|
||||
{
|
||||
setParam("pwr_btn", (float)power_button.is_power_on);
|
||||
power_button.old_is_power_on = power_button.is_power_on;
|
||||
}
|
||||
|
||||
// 状态转换逻辑
|
||||
switch (power_data.current_state)
|
||||
{
|
||||
case POWER_PRE_CHARGE:
|
||||
if (!power_data.timer_pre_charge.active) // 预充时间到
|
||||
{
|
||||
power_data.current_state = POWER_WORKING; // 工作
|
||||
power_data.pre_charge_finish = 1; // 预充完成
|
||||
printf("Power: Transitioning from PRE_CHARGE to WORKING state\n");
|
||||
}
|
||||
break;
|
||||
case POWER_STANDBY:
|
||||
if (power_button.is_power_on == app_close() && power_data.emergency_stop == app_close()) // 遥控器电源开关闭合且急停开关闭合
|
||||
{
|
||||
power_data.current_state = POWER_EMERGENCY; // 急停
|
||||
printf("Power: Transitioning from STANDBY to EMERGENCY state\n");
|
||||
}
|
||||
break;
|
||||
case POWER_WORKING:
|
||||
if (power_data.emergency_stop == app_close()) // 急停开关闭合
|
||||
{
|
||||
power_data.current_state = POWER_EMERGENCY; // 急停
|
||||
printf("Power: Transitioning from WORKING to EMERGENCY state\n");
|
||||
}
|
||||
break;
|
||||
case POWER_EMERGENCY:
|
||||
if (power_button.is_power_on == app_open()) // 遥控器电源开关断开
|
||||
{
|
||||
power_data.current_state = POWER_STANDBY; // 待机
|
||||
printf("Power: Transitioning from EMERGENCY to STANDBY state\n");
|
||||
}
|
||||
else if (power_data.emergency_stop == app_open()) // 急停断开
|
||||
{
|
||||
power_data.current_state = POWER_PRE_CHARGE; // 预充
|
||||
timerStart(&power_data.timer_pre_charge, (uint32_t)(getParam("prCTime") * 1000), 1); // 启动预充定时器
|
||||
printf("Power: Transitioning from EMERGENCY to PRE_CHARGE state\n");
|
||||
}
|
||||
break;
|
||||
// case POWER_SLEEP:
|
||||
// if (power_data.high_voltage_switch == app_close()) // 高压开关闭合
|
||||
// {
|
||||
// power_data.current_state = POWER_STANDBY; // 待机
|
||||
// printf("Power: Transitioning from SLEEP to STANDBY state\n");
|
||||
// }
|
||||
// break;
|
||||
default:
|
||||
power_data.current_state = POWER_STANDBY; // 待机
|
||||
break;
|
||||
}
|
||||
|
||||
powerOutput(NULL); // 输出
|
||||
|
||||
// 电源状态有变化，记录到参数
|
||||
if (power_data.old_state != power_data.current_state)
|
||||
{
|
||||
power_data.old_state = power_data.current_state;
|
||||
setParam("pwr_sta", (float)power_data.current_state);
|
||||
}
|
||||
|
||||
timerStart(&power_data.timer, 100, 1); //周期调用
|
||||
}
|
||||
|
||||
// 处理所有输入信号的函数
|
||||
static void powerInput(void *signal_id)
|
||||
{
|
||||
//不能直接赋值，用memcpy
|
||||
PowerSystem old_data;
|
||||
memcpy(&old_data, &power_data, sizeof(PowerSystem));
|
||||
|
||||
// 填充数据
|
||||
if (signal_id == &un_sw_sample)
|
||||
{
|
||||
power_data.emergency_stop_switch = (uint8_t)un_sw_sample.bit_data.emergency_stop_switch;//急停开关
|
||||
power_data.high_voltage_switch = (uint8_t)un_sw_sample.bit_data.High_voltage_switch;//高压开关
|
||||
|
||||
printf("emergency_stop_switch: = %d,high_voltage_switch = %d\n", power_data.emergency_stop_switch,power_data.high_voltage_switch);
|
||||
}
|
||||
else if ( (signal_id == &un_remote_control_input) && (1 == un_remote_control_input.bit_data.enable) )// 遥控器断线，不更新数据
|
||||
{
|
||||
power_data.remote_power_switch = (uint8_t)un_remote_control_input.bit_data.switch_d; // 遥控器电源开关
|
||||
power_data.remote_emergency_stop = ((uint8_t)un_remote_control_input.bit_data.switch_b == 1) ? 0 : 1;// 遥控器急停开关
|
||||
}
|
||||
|
||||
// 急停开关
|
||||
power_data.emergency_stop = (uint8_t)( (power_data.emergency_stop_switch == app_close()) || (power_data.remote_emergency_stop == app_close()) );
|
||||
|
||||
}
|
||||
|
||||
|
||||
// APP模块的初始化
|
||||
void powerAppInit(void)
|
||||
{
|
||||
// 初始化变量
|
||||
memset(&power_data, 0, sizeof(PowerSystem));
|
||||
power_data.current_state = POWER_STANDBY;
|
||||
// 初始化时恢复电源状态
|
||||
power_data.current_state = (PowerState)getParam("pwr_sta");
|
||||
power_data.old_state = power_data.current_state;
|
||||
// 恢复电源按钮状态
|
||||
power_button.is_power_on = (uint8_t)getParam("pwr_btn");
|
||||
power_button.old_is_power_on = power_button.is_power_on;
|
||||
|
||||
// 订阅输入信号
|
||||
subscribe(&un_sw_sample, powerInput); // 急停开关、高压开关
|
||||
subscribe(&un_remote_control_input, powerInput); // 遥控器电源开关
|
||||
// 定时器
|
||||
timerInit(&power_data.timer);
|
||||
subscribe(&power_data.timer, powerTimerProcess);
|
||||
timerStart(&power_data.timer, 500, 1); // 周期调用
|
||||
//定时器唤醒
|
||||
timerInit(&power_data.timer1);
|
||||
subscribe(&power_data.timer1, wakeupProcess);
|
||||
timerStart(&power_data.timer1, 500, 1); // 周期调用
|
||||
|
||||
//预充定时器
|
||||
timerInit(&power_data.timer_pre_charge);
|
||||
subscribe(&power_data.timer_pre_charge, powerTimerProcess);
|
||||
|
||||
printf("app_power: initial OK\n");
|
||||
}
|
||||
53
app/app_power.h
Normal file
53
app/app_power.h
Normal file
@@ -0,0 +1,53 @@
|
||||
#ifndef APP_POWER_H
|
||||
#define APP_POWER_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "app_config.h"
|
||||
#include "app_frm_monitor.h"
|
||||
#include "app_frm_signal.h"
|
||||
#include "app_frm_timer.h"
|
||||
|
||||
// 定义电源状态枚举
|
||||
typedef enum {
|
||||
POWER_STANDBY,
|
||||
POWER_WORKING,
|
||||
POWER_EMERGENCY,
|
||||
POWER_PRE_CHARGE,
|
||||
POWER_SLEEP
|
||||
} PowerState;
|
||||
|
||||
// 使用内联函数
|
||||
static inline uint8_t setPowerOn(void) { return 1; }
|
||||
static inline uint8_t setPowerOff(void) { return 0; }
|
||||
|
||||
|
||||
// 声明 power_data 变量结构体
|
||||
typedef struct {
|
||||
PowerState current_state; // 当前电源状态
|
||||
uint32_t start_time; // 定时器起始时间
|
||||
Timer timer; // 定时器
|
||||
Timer timer1;
|
||||
Timer timer_pre_charge; // 预充定时器
|
||||
PowerState last_state; // 上一次状态
|
||||
uint8_t emergency_stop_switch; // 急停开关
|
||||
uint8_t high_voltage_switch; // 高压开关
|
||||
uint8_t remote_power_switch; // 遥控器电源开关
|
||||
uint8_t remote_emergency_stop; // 遥控器急停开关
|
||||
uint8_t emergency_stop; // 急停状态
|
||||
uint8_t pre_charge_finish; // 预充完成标志位
|
||||
uint8_t old_state; // 上一次状态
|
||||
} PowerSystem;
|
||||
|
||||
// 声明外部变量
|
||||
extern PowerSystem power_data;
|
||||
|
||||
void powerAppInit(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // APP_POWER_H
|
||||
415
app/app_request.c
Normal file
415
app/app_request.c
Normal file
@@ -0,0 +1,415 @@
|
||||
#include "app_config.h"
|
||||
#include "interface.h"
|
||||
#include "app_request.h"
|
||||
#include "app_param_manage.h"
|
||||
|
||||
uint16_t request_id = 0;
|
||||
|
||||
|
||||
static void processRequestframe(uint16_t id)
|
||||
{
|
||||
uint32_t TempAcc = 0;
|
||||
uint8_t i = 0;
|
||||
static uint8_t VehicleStaACC0 = 0;
|
||||
static uint8_t VehicleStaACC1 = 0;
|
||||
static uint8_t VehicleStaACC2 = 0;
|
||||
static uint8_t VehicleStaACC3 = 0;
|
||||
|
||||
uint16_t RgExchangeTemp = 0;
|
||||
uint32_t Rg32ExchangeTemp = 0;
|
||||
//-----------------------------------------------------------
|
||||
// printf("request_read_id:%d\n",id);
|
||||
switch (id)//注意是高位在前，低位在后
|
||||
{
|
||||
case 0x2000://状态帧
|
||||
un_vehicle_Info_output.bit_data.frame_header = 0xCCAA;//帧头
|
||||
un_vehicle_Info_output.bit_data.frame_type = 0x2000;//帧类型
|
||||
un_vehicle_Info_output.bit_data.frame_length = 0x2900;//帧长
|
||||
un_vehicle_Info_output.bit_data.accumulated = VehicleStaACC0++;//累加值
|
||||
|
||||
TempAcc = 0;
|
||||
for (i = 0; i < 40; i++)//累加前40个字节
|
||||
{
|
||||
TempAcc = TempAcc + (uint32_t)(un_vehicle_Info_output.arr[i]);
|
||||
}
|
||||
un_vehicle_Info_output.bit_data.crc = (uint8_t)TempAcc;//累加值
|
||||
|
||||
publishMessage(&un_vehicle_Info_output, 1);
|
||||
|
||||
break;
|
||||
|
||||
case 0x2100://电机帧
|
||||
un_motor_status_output.bit_data.frame_header = 0xCCAA;//帧头
|
||||
un_motor_status_output.bit_data.frame_type = 0x2100;//帧类型
|
||||
un_motor_status_output.bit_data.frame_length = 0x1E00;//帧长
|
||||
un_motor_status_output.bit_data.accumulated = VehicleStaACC1++;//累加值
|
||||
RgExchangeTemp = ( (uint16_t)getParam("maxTorq") + 300 ) *100 ;
|
||||
// un_motor_status_output.bit_data.left_torque_limit = ((RgExchangeTemp << 8) | (RgExchangeTemp >> 8));//左侧扭矩限制
|
||||
// un_motor_status_output.bit_data.right_torque_limit = ((RgExchangeTemp << 8) | (RgExchangeTemp >> 8));//右侧扭矩限制
|
||||
// un_motor_status_output.bit_data.left_power_in = (((uint16_t)getParam("feedPwr") << 8) | ((uint16_t)getParam("feedPwr") >> 8));//左侧馈电功率
|
||||
// un_motor_status_output.bit_data.right_power_in = (((uint16_t)getParam("feedPwr") << 8) | ((uint16_t)getParam("feedPwr") >> 8));//右侧馈电功率
|
||||
// un_motor_status_output.bit_data.left_power_out = (((uint16_t)getParam("dispPwr") << 8) | ((uint16_t)getParam("dispPwr") >> 8));//左侧放电功率
|
||||
// un_motor_status_output.bit_data.right_power_out = (((uint16_t)getParam("dispPwr") << 8) | ((uint16_t)getParam("dispPwr") >> 8));//右侧放电功率
|
||||
|
||||
TempAcc = 0;
|
||||
for (i = 0; i < 31; i++)//累加前31个字节
|
||||
{
|
||||
TempAcc = TempAcc + (uint32_t)(un_motor_status_output.arr[i]);
|
||||
un_motor_status_output.bit_data.checksum = (uint8_t)TempAcc;//累加值
|
||||
}
|
||||
|
||||
publishMessage(&un_motor_status_output, 1);
|
||||
break;
|
||||
|
||||
case 0x2200:
|
||||
un_pid_output.bit_data.frame_header = 0xCCAA;//帧头
|
||||
un_pid_output.bit_data.frame_type = 0x2200;//帧类型
|
||||
un_pid_output.bit_data.frame_length = 0x3800;//帧长
|
||||
un_pid_output.bit_data.accumulated = VehicleStaACC2++;//累加值
|
||||
|
||||
|
||||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kp")*1000);
|
||||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||||
un_pid_output.bit_data.rc_straight_p = Rg32ExchangeTemp;//遥控直行P参数
|
||||
|
||||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_ki")*1000);
|
||||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||||
un_pid_output.bit_data.rc_straight_i = Rg32ExchangeTemp;//遥控直行I参数
|
||||
|
||||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000);
|
||||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||||
un_pid_output.bit_data.rc_straight_d = Rg32ExchangeTemp;//遥控直行D参数
|
||||
|
||||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_il")*1000);
|
||||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||||
un_pid_output.bit_data.auto_straight_p = Rg32ExchangeTemp;//自主直行P参数
|
||||
|
||||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_ol")*1000);
|
||||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||||
un_pid_output.bit_data.auto_straight_i = Rg32ExchangeTemp;//自主直行I参数
|
||||
|
||||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000);
|
||||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||||
un_pid_output.bit_data.auto_straight_d = Rg32ExchangeTemp;//自主直行D参数
|
||||
|
||||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_kp")*1000);
|
||||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||||
un_pid_output.bit_data.rc_turn_p = Rg32ExchangeTemp;//遥控转弯P参数
|
||||
|
||||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_ki")*1000);
|
||||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||||
un_pid_output.bit_data.rc_turn_i = Rg32ExchangeTemp;//遥控转弯I参数
|
||||
|
||||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_kd")*1000);
|
||||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||||
un_pid_output.bit_data.rc_turn_d = Rg32ExchangeTemp;//遥控转弯D参数
|
||||
|
||||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_il")*1000);
|
||||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||||
un_pid_output.bit_data.auto_turn_p = Rg32ExchangeTemp;//自主转弯P参数
|
||||
|
||||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("crv_ol")*1000);
|
||||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||||
un_pid_output.bit_data.auto_turn_i = Rg32ExchangeTemp;//自主转弯I参数
|
||||
|
||||
Rg32ExchangeTemp = (uint32_t)(int32_t)(getParam("spd_kd")*1000);
|
||||
Rg32ExchangeTemp = ( ((Rg32ExchangeTemp >> 24) &0xff ) | ((Rg32ExchangeTemp >> 8) & 0xFF00) | ((Rg32ExchangeTemp << 8) & 0xFF0000) | ((Rg32ExchangeTemp << 24)) );
|
||||
un_pid_output.bit_data.auto_turn_d = Rg32ExchangeTemp;//自主转弯I参数
|
||||
|
||||
TempAcc = 0;
|
||||
for (i = 0; i < 55; i++)//累加前40个字节
|
||||
{
|
||||
TempAcc = TempAcc + (uint32_t)(un_pid_output.arr[i]);
|
||||
}
|
||||
un_pid_output.bit_data.checksum = (uint8_t)TempAcc;//累加值
|
||||
|
||||
publishMessage(&un_pid_output, 1);
|
||||
|
||||
break;
|
||||
|
||||
case 0x2300:
|
||||
// ETHTxtempArr[0] = 0xAA;
|
||||
// ETHTxtempArr[1] = 0xCC;
|
||||
// ETHTxtempArr[2] = 0x00;
|
||||
// ETHTxtempArr[3] = 0x23;
|
||||
// ETHTxtempArr[4] = 0;
|
||||
// ETHTxtempArr[5] = 27;
|
||||
// ETHTxtempArr[6] = VehicleStaACC3++;
|
||||
// //AIAO
|
||||
// memcpy(ÐTxtempArr[7],(uint8_t *)&(UnAIAOSignal_1.ArrData.ArrRx[0]),17);//电池数据拷贝
|
||||
// ETHTxtempArr[24] = (u8)(RgCanToAiAOCnt>>8);
|
||||
// ETHTxtempArr[25] = (u8)RgCanToAiAOCnt;//累加值
|
||||
//
|
||||
// TempAcc = 0;
|
||||
// for (i = 0; i < 26; i++)//累加前40个字节
|
||||
// {
|
||||
// TempAcc = TempAcc + (u32)(ETHTxtempArr[i]);
|
||||
// }
|
||||
// ETHTxtempArr[26] = (uint8_t)TempAcc;//累加值
|
||||
//
|
||||
// TxLen = 27;
|
||||
// UdpSendToData(UDPCB_2,(uint8_t *)(&un_analog_signal_output), 27, (uint8_t *)ðernet_parameter.upper_ip[0], ethernet_parameter.target_upper_port);
|
||||
break;
|
||||
|
||||
case 0x2400:
|
||||
un_remote_control_output.bit_data.frame_header = 0xCCAA;//帧头
|
||||
un_remote_control_output.bit_data.frame_type = 0x2400;//帧类型
|
||||
un_remote_control_output.bit_data.frame_length = 0x1400;//帧长
|
||||
un_remote_control_output.bit_data.accumulated = VehicleStaACC3++;//累加值
|
||||
//RCH_3
|
||||
|
||||
TempAcc = 0;
|
||||
for (i = 0; i < 17; i++)//累加前40个字节
|
||||
{
|
||||
TempAcc = TempAcc + (uint32_t)(un_remote_control_output.arr[i]);
|
||||
}
|
||||
un_remote_control_output.bit_data.crc = (uint8_t)TempAcc;//累加值
|
||||
|
||||
publishMessage(&un_remote_control_output, 1);
|
||||
break;
|
||||
|
||||
case 0x2500:
|
||||
un_manual_control_output.bit_data.frame_type = 0x2500;
|
||||
|
||||
TempAcc = 0;
|
||||
for (i = 0; i < 14; i++)//累加前40个字节
|
||||
{
|
||||
TempAcc = TempAcc + (uint32_t)(un_manual_control_output.arr[i]);
|
||||
}
|
||||
un_manual_control_output.bit_data.crc_2 = (uint8_t)TempAcc;//累加值
|
||||
|
||||
publishMessage(&un_manual_control_output, 1);
|
||||
break;
|
||||
|
||||
case 0x2600:
|
||||
un_auto_control_output.bit_data.frame_type = 0x2600;
|
||||
|
||||
TempAcc = 0;
|
||||
for (i = 0; i < 27; i++)//累加前40个字节
|
||||
{
|
||||
TempAcc = TempAcc + (uint32_t)(un_auto_control_output.arr[i]);
|
||||
}
|
||||
un_auto_control_output.bit_data.crc = (uint8_t)TempAcc;//累加值
|
||||
|
||||
publishMessage(&un_auto_control_output, 1);
|
||||
break;
|
||||
|
||||
// case 0x2700:
|
||||
// memcpy(ÐTxtempArr[0],(uint8_t *)&RemoteControlRoomToVcuBuf,11);//手柄拷贝
|
||||
// ETHTxtempArr[2] = 0;//修改帧类型为0x0025
|
||||
// ETHTxtempArr[3] = 0x27;
|
||||
// ETHTxtempArr[11] = (u8)(RgETHToRemoteControlCnt>>8);
|
||||
// ETHTxtempArr[12] = (u8)RgETHToRemoteControlCnt;//累加值
|
||||
// TempAcc = 0;
|
||||
// for (i = 0; i < 13; i++)//累加前40个字节
|
||||
// {
|
||||
// TempAcc = TempAcc + (u32)(ETHTxtempArr[i]);
|
||||
// }
|
||||
// ETHTxtempArr[13] = (uint8_t)TempAcc;//累加值
|
||||
//
|
||||
// TxLen = 14;
|
||||
// UdpSendToData(SocketId2,ETHTxtempArr,&TxLen,ip_addr,port);//测试
|
||||
// break;
|
||||
|
||||
// case 0xFFFF:
|
||||
// memcpy(ÐTxtempArr[0],(uint8_t *)&RemoteControlRoomToVcuBuf,11);//手柄拷贝
|
||||
// ETHTxtempArr[2] = 0x27;//修改帧类型为0x0025
|
||||
// ETHTxtempArr[3] = 0;
|
||||
// ETHTxtempArr[11] = (u8)RgETHToRemoteControlCnt;
|
||||
// ETHTxtempArr[12] = (u8)(RgETHToRemoteControlCnt>>8);//累加值
|
||||
// TempAcc = 0;
|
||||
// for (i = 0; i < 13; i++)//累加前40个字节
|
||||
// {
|
||||
// TempAcc = TempAcc + (u32)(ETHTxtempArr[i]);
|
||||
// }
|
||||
// ETHTxtempArr[13] = (uint8_t)TempAcc;//累加值
|
||||
//
|
||||
// TxLen = 14;
|
||||
// UdpSendToData(SocketId2,ETHTxtempArr,&TxLen,ip_addr,port);//测试
|
||||
// break;
|
||||
default: break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 处理所有输入信号的函数
|
||||
static void requestInput(void *signal_id)
|
||||
{
|
||||
uint16_t request16_temp = 0;
|
||||
|
||||
//--------------------------------------------------------
|
||||
if (signal_id == &diff_data)
|
||||
{
|
||||
request16_temp = (uint16_t)(int16_t)(diff_data.desired_speed * 100.0);
|
||||
un_vehicle_Info_output.bit_data.desired_speed = ((request16_temp << 8) | (request16_temp >> 8));//当前速度
|
||||
|
||||
request16_temp = (uint16_t)(int16_t)(diff_data.left_diff_touue * 10000.0);
|
||||
un_vehicle_Info_output.bit_data.desired_curvature = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率
|
||||
|
||||
request16_temp = (uint16_t)(int16_t)(diff_data.speed * 100.0);
|
||||
un_vehicle_Info_output.bit_data.speed = ((request16_temp << 8) | (request16_temp >> 8));//当前速度
|
||||
|
||||
request16_temp = (uint16_t)(int16_t)(diff_data.right_diff_touue * 10000.0);
|
||||
un_vehicle_Info_output.bit_data.curvature = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率
|
||||
|
||||
request16_temp = (uint16_t)(int16_t)(diff_data.out_motor_speed[0]);//
|
||||
un_vehicle_Info_output.bit_data.set_left_speed = ((request16_temp << 8) | (request16_temp >> 8));//左前
|
||||
|
||||
request16_temp = (uint16_t)(int16_t)(diff_data.out_motor_speed[1]);//out_right_motor_speed
|
||||
un_vehicle_Info_output.bit_data.set_right_speed = ((request16_temp << 8) | (request16_temp >> 8));//右前
|
||||
|
||||
request16_temp = (uint16_t)(int16_t)(diff_data.out_motor_speed[2]);//
|
||||
un_motor_status_output.bit_data.left_power_in = ((request16_temp << 8) | (request16_temp >> 8));//左后
|
||||
|
||||
request16_temp = (uint16_t)(int16_t)(diff_data.out_motor_speed[3]);//out_right_motor_speed
|
||||
un_motor_status_output.bit_data.right_power_in = ((request16_temp << 8) | (request16_temp >> 8));//右后
|
||||
|
||||
un_vehicle_Info_output.bit_data.vehicle_fault_state = diff_data.state;
|
||||
un_vehicle_Info_output.bit_data.battery_soc = diff_data.motor_dir;
|
||||
|
||||
request16_temp = (uint16_t)(int16_t)(diff_data.left_rear_motor_speed + 30000);
|
||||
un_motor_status_output.bit_data.left_wheel_speed = ((request16_temp << 8) | (request16_temp >> 8));;//左侧后轮速
|
||||
|
||||
request16_temp = (uint16_t)(int16_t)(diff_data.right_rear_motor_speed + 30000);
|
||||
un_motor_status_output.bit_data.right_wheel_speed = ((request16_temp << 8) | (request16_temp >> 8));;//右侧后轮速
|
||||
|
||||
request16_temp = (uint16_t)(int16_t)( (diff_data.left_front_motor_speed + 300)*100);
|
||||
un_motor_status_output.bit_data.left_torque = ((request16_temp << 8) | (request16_temp >> 8));;//左侧前轮速
|
||||
|
||||
request16_temp = (uint16_t)(int16_t)( (diff_data.right_front_motor_speed + 300)*100);
|
||||
un_motor_status_output.bit_data.right_torque = ((request16_temp << 8) | (request16_temp >> 8));;//右侧前轮速
|
||||
}
|
||||
else if(signal_id == &un_auto_computer_input)
|
||||
{
|
||||
un_vehicle_Info_output.bit_data.longitude = un_auto_computer_input.bit_data.longitude;//经度
|
||||
|
||||
un_vehicle_Info_output.bit_data.latitude = un_auto_computer_input.bit_data.latitude;
|
||||
|
||||
un_vehicle_Info_output.bit_data.altitude = un_auto_computer_input.bit_data.altitude;
|
||||
|
||||
un_vehicle_Info_output.bit_data.heading_angle = un_auto_computer_input.bit_data.heading;//航向
|
||||
|
||||
memcpy(&un_auto_control_output.arr[0],&(un_auto_computer_input.arr[0]),25);//手柄拷贝
|
||||
}
|
||||
else if(signal_id == &un_bms_input)
|
||||
{
|
||||
un_vehicle_Info_output.bit_data.battery_voltage = un_bms_input.bit_data.bus_voltage/10;//电池电压读取得的10mV，所以输出的是100mV单位，所以缩小10倍
|
||||
un_vehicle_Info_output.bit_data.battery_soc = un_bms_input.bit_data.soc;//SOC
|
||||
un_vehicle_Info_output.bit_data.battery_current = un_bms_input.bit_data.bus_current;//电池电流
|
||||
}
|
||||
else if(signal_id == &un_motor_input1)
|
||||
{
|
||||
// un_motor_status_output.bit_data.left_wheel_speed = ((un_motor_input1.bit_data.speed << 8) | (un_motor_input1.bit_data.speed >> 8));//左侧轮速
|
||||
// un_motor_status_output.bit_data.left_torque = ((un_motor_input1.bit_data.torque << 8) | (un_motor_input1.bit_data.torque >> 8));//左侧扭矩
|
||||
// un_motor_status_output.bit_data.left_voltage = ((un_motor_input1.bit_data.bus_voltage << 8) | (un_motor_input1.bit_data.bus_voltage >> 8));//左侧电压
|
||||
// un_motor_status_output.bit_data.left_fault_code = un_motor_input1.bit_data.fault_code;//左侧故障码
|
||||
//
|
||||
}
|
||||
else if(signal_id == &un_motor_input2)
|
||||
{
|
||||
// un_motor_status_output.bit_data.right_wheel_speed = ((un_motor_input2.bit_data.speed << 8) | (un_motor_input2.bit_data.speed >> 8));//右侧轮速
|
||||
// un_motor_status_output.bit_data.right_torque = ((un_motor_input2.bit_data.torque << 8) | (un_motor_input2.bit_data.torque >> 8));//右侧扭矩
|
||||
// un_motor_status_output.bit_data.right_fault_code = un_motor_input2.bit_data.fault_code;//右侧故障码
|
||||
// un_motor_status_output.bit_data.right_voltage = ((un_motor_input2.bit_data.bus_voltage << 8) | (un_motor_input2.bit_data.bus_voltage >> 8));//右侧电压
|
||||
//
|
||||
}
|
||||
else if(signal_id == &un_remote_control_input)
|
||||
{
|
||||
//RCH_3
|
||||
un_remote_control_output.bit_data.speed = ((un_remote_control_input.bit_data.speed << 8) | (un_remote_control_input.bit_data.speed >> 8));//遥控器期望速度
|
||||
un_remote_control_output.bit_data.curvature = ((un_remote_control_input.bit_data.curvature << 8) | (un_remote_control_input.bit_data.curvature >> 8));//遥控器期望曲率
|
||||
memcpy(&un_remote_control_output.arr[11],&un_remote_control_input.arr[4],4);//遥控数据拷贝
|
||||
}
|
||||
else if(signal_id == &un_manual_computer_input)
|
||||
{
|
||||
memcpy(&un_manual_control_output.arr[0],&(un_manual_computer_input.arr[0]),12);//手柄拷贝
|
||||
}
|
||||
else if(signal_id == ðernet_fault_Info)
|
||||
{
|
||||
un_auto_control_output.arr[25] = (uint8_t)(ethernet_fault_Info.bit_data.auto_count>>8);
|
||||
un_auto_control_output.arr[26] = (uint8_t)(ethernet_fault_Info.bit_data.auto_count);//累加值
|
||||
un_manual_control_output.arr[12] = (uint8_t)(ethernet_fault_Info.bit_data.manual_count>>8);
|
||||
un_manual_control_output.arr[13] = (uint8_t)(ethernet_fault_Info.bit_data.manual_count);//累加值
|
||||
}
|
||||
|
||||
else if(signal_id == &can_fault_info)
|
||||
{
|
||||
un_remote_control_output.arr[15] = (uint8_t)(can_fault_info.bit_data.remote_count>>8);
|
||||
un_remote_control_output.arr[16] = (uint8_t)(can_fault_info.bit_data.remote_count);//累加值
|
||||
}
|
||||
else if(signal_id == &un_request_frame)
|
||||
{
|
||||
request_id = REQUEST_READ_ID;
|
||||
processRequestframe(request_id);
|
||||
}
|
||||
else{}
|
||||
}
|
||||
|
||||
// 修改APP模块的初始化函数
|
||||
void requestAppInit(void)
|
||||
{
|
||||
// 订阅消息，使用静态成员函数作为回调 参数回馈
|
||||
subscribe(&diff_data, requestInput);
|
||||
subscribe(&un_auto_computer_input, requestInput);
|
||||
subscribe(&un_bms_input, requestInput);
|
||||
subscribe(&un_motor_input1, requestInput);
|
||||
subscribe(&un_motor_input2, requestInput);
|
||||
subscribe(&un_remote_control_input, requestInput);
|
||||
subscribe(&can_fault_info, requestInput);
|
||||
subscribe(ðernet_fault_Info, requestInput);
|
||||
|
||||
// // 订阅消息，使用静态成员函数作为回调 参数回馈
|
||||
subscribe(&un_request_frame, requestInput);
|
||||
|
||||
printf("app_request: initial OK \n");
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
83
app/app_request.h
Normal file
83
app/app_request.h
Normal file
@@ -0,0 +1,83 @@
|
||||
#ifndef APP_REQUEST_H
|
||||
#define APP_REQUEST_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#include "app_config.h"
|
||||
#include "app_differential_drive.h"
|
||||
|
||||
|
||||
|
||||
//typedef enum
|
||||
//{
|
||||
// MODE_MANUAL, // 手动模式
|
||||
// MODE_AUTO // 自动模式
|
||||
//} ControlMode;
|
||||
//
|
||||
typedef struct RequestData
|
||||
{
|
||||
float desired_speed; // 期望速度
|
||||
float desired_curvature; // 期望曲率
|
||||
float left_motor_speed; // 当前左电机速度
|
||||
float right_motor_speed; // 当前右电机速度
|
||||
float speed; // 当前车速
|
||||
float curvature; // 当前曲率
|
||||
float yaw_rate; // 当前角速度
|
||||
float desired_yaw_rate; // 期望角速度
|
||||
float acceleration; // 当前加速度
|
||||
float deceleration; // 当前减速度
|
||||
float max_speed; // 最大速度
|
||||
float desired_acceleration; // 期望加速度
|
||||
float desired_deceleration; // 期望减速度
|
||||
float out_left_motor_speed; // 输出左电机速度
|
||||
float out_right_motor_speed; // 输出右电机速度
|
||||
} RequestData;
|
||||
|
||||
|
||||
|
||||
|
||||
// 声明外部变量
|
||||
extern DiffData diff_data;
|
||||
|
||||
|
||||
void requestAppInit(void);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // APP_DIFFERENTIAL_DRIVE_H
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
115
app/app_temp.c
Normal file
115
app/app_temp.c
Normal file
@@ -0,0 +1,115 @@
|
||||
#include "app_config.h"
|
||||
#include "interface.h"
|
||||
#include "app_frm_monitor.h"
|
||||
#include "app_frm_signal.h"
|
||||
#include "app_frm_timer.h"
|
||||
#include "app_param_manage.h"
|
||||
|
||||
#include "app_temp.h"
|
||||
|
||||
|
||||
// 声明 temp_data 变量
|
||||
TempSystem temp_data;
|
||||
|
||||
// 温度输出处理函数
|
||||
static void tempOutput(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
// 根据当前状态,控制风扇
|
||||
switch (temp_data.state)
|
||||
{
|
||||
case TEMP_NORMAL:
|
||||
// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇
|
||||
// un_inf_can_kgf_output1.bit_data.KGF03 = setFanOff();
|
||||
// un_inf_can_kgf_output1.bit_data.KGF14 = setFanOff();//整车风扇
|
||||
// un_inf_can_kgf_output2.bit_data.KGF03 = setFanOff();
|
||||
break;
|
||||
case TEMP_WARNING:
|
||||
case TEMP_CRITICAL:
|
||||
// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();
|
||||
// un_inf_can_kgf_output1.bit_data.KGF03 = setFanOn();
|
||||
// un_inf_can_kgf_output1.bit_data.KGF14 = setFanOn();
|
||||
// un_inf_can_kgf_output2.bit_data.KGF03 = setFanOn();
|
||||
break;
|
||||
}
|
||||
|
||||
// // 设置风扇速度
|
||||
// temp_data.fan_speed = 0;
|
||||
// un_inf_can_kgf_output1.bit_data.pwm_01 = temp_data.fan_speed;
|
||||
|
||||
publishMessage(&un_inf_can_kgf_output1, 1);
|
||||
}
|
||||
|
||||
// 温度状态处理函数
|
||||
static void tempProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
// 获取当前最高温度
|
||||
int16_t max_temp = temp_data.current_temp[0];
|
||||
for (uint8_t i = 1; i < 8; i++)
|
||||
{
|
||||
if (temp_data.current_temp[i] > max_temp)
|
||||
{
|
||||
max_temp = temp_data.current_temp[i];
|
||||
}
|
||||
}
|
||||
// 根据最高温度设置状态
|
||||
if (max_temp > 80) // 假设80度为危险温度
|
||||
{
|
||||
temp_data.state = TEMP_CRITICAL;
|
||||
temp_data.fan_speed = 255; // 全速运转
|
||||
}
|
||||
else if (max_temp > 60) // 假设60度为警告温度
|
||||
{
|
||||
temp_data.state = TEMP_WARNING;
|
||||
temp_data.fan_speed = 128; // 半速运转
|
||||
}
|
||||
else
|
||||
{
|
||||
temp_data.state = TEMP_NORMAL;
|
||||
temp_data.fan_speed = 0; // 停止运转
|
||||
}
|
||||
|
||||
tempOutput(NULL);
|
||||
}
|
||||
|
||||
// 处理输入信号的函数
|
||||
static void tempInput(void *signal_id)
|
||||
{
|
||||
// TempSystem old_data = temp_data;
|
||||
|
||||
// 填充数据
|
||||
if (signal_id == &un_temp_module_input)
|
||||
{
|
||||
temp_data.current_temp[0] = (int16_t)un_temp_module_input.bit_data.channel_1;
|
||||
temp_data.current_temp[1] = (int16_t)un_temp_module_input.bit_data.channel_2;
|
||||
temp_data.current_temp[2] = (int16_t)un_temp_module_input.bit_data.channel_3;
|
||||
temp_data.current_temp[3] = (int16_t)un_temp_module_input.bit_data.channel_4;
|
||||
temp_data.current_temp[4] = (int16_t)un_temp_module_input.bit_data.channel_5;
|
||||
temp_data.current_temp[5] = (int16_t)un_temp_module_input.bit_data.channel_6;
|
||||
temp_data.current_temp[6] = (int16_t)un_temp_module_input.bit_data.channel_7;
|
||||
temp_data.current_temp[7] = (int16_t)un_temp_module_input.bit_data.channel_8;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// APP模块的初始化
|
||||
void tempAppInit(void)
|
||||
{
|
||||
// 初始化
|
||||
memset(&temp_data, 0, sizeof(TempSystem));
|
||||
temp_data.state = TEMP_NORMAL;
|
||||
temp_data.mode = TEMP_MODE_AUTO;
|
||||
temp_data.target_temp = 25; // 默认目标温度25度
|
||||
|
||||
// 订阅输入信号,处理温度逻辑
|
||||
subscribe(&un_temp_module_input, tempInput);
|
||||
|
||||
// 启动定时器,每秒检查一次温度
|
||||
subscribe(&temp_data.timer, tempProcess);
|
||||
timerStart(&temp_data.timer, 1000, 1); //1s
|
||||
|
||||
printf("app_temp: initial OK \n");
|
||||
}
|
||||
53
app/app_temp.h
Normal file
53
app/app_temp.h
Normal file
@@ -0,0 +1,53 @@
|
||||
#ifndef APP_TEMP_H
|
||||
#define APP_TEMP_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
// 定义温度状态枚举
|
||||
typedef enum
|
||||
{
|
||||
TEMP_NORMAL,
|
||||
TEMP_WARNING,
|
||||
TEMP_CRITICAL
|
||||
} TempState;
|
||||
|
||||
// 定义温度控制模式枚举
|
||||
typedef enum
|
||||
{
|
||||
TEMP_MODE_AUTO,
|
||||
TEMP_MODE_MANUAL
|
||||
} TempMode;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
TempState state;
|
||||
TempMode mode;
|
||||
int16_t current_temp[8]; // 当前温度,对应8个通道
|
||||
int16_t target_temp; // 目标温度
|
||||
uint8_t fan_speed; // 风扇速度
|
||||
Timer timer; // 定时器
|
||||
} TempSystem;
|
||||
|
||||
// 在头文件中声明外部变量
|
||||
extern TempSystem temp_data;
|
||||
|
||||
// 使用内联函数
|
||||
static inline uint8_t setFanOn(void) { return 1; }
|
||||
static inline uint8_t setFanOff(void) { return 0; }
|
||||
|
||||
void tempAppInit(void);
|
||||
void setTempMode(TempMode mode);
|
||||
void setTargetTemp(int16_t temp);
|
||||
void setFanSpeed(uint8_t speed);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // APP_TEMP_H
|
||||
62
app/app_test.c
Normal file
62
app/app_test.c
Normal file
@@ -0,0 +1,62 @@
|
||||
#include "app_config.h"
|
||||
#include "app_dependence.h"
|
||||
#include "interface.h"
|
||||
|
||||
#include "app_frm_monitor.h"
|
||||
#include "app_frm_signal.h"
|
||||
#include "app_frm_timer.h"
|
||||
|
||||
#include "sdrv_vic.h"
|
||||
#include "app_test.h"
|
||||
#include "app_differential_drive.h"
|
||||
|
||||
|
||||
// 定时器结构体
|
||||
Timer test_timer;
|
||||
|
||||
// 定时器信号处理函数
|
||||
void testTimerProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||||
// static uint32_t start_time = 0;
|
||||
// uint32_t test_irq[6] = {0};
|
||||
// 再次启动定时器，实现周期定时
|
||||
|
||||
timerStart(&test_timer, 5000,1);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// printf("speed = %f ",diff_data.desired_speed);
|
||||
// printf("curvature = %f ",diff_data.desired_curvature);
|
||||
// printf("mode = %d\n",diff_data.mode);
|
||||
|
||||
|
||||
//
|
||||
// sdrv_vic_lld_read_all(test_irq);//打印所有中断使能位
|
||||
// printf("irq state = %#X ",test_irq[0]);
|
||||
// printf("irq state = %#X ",test_irq[1]);
|
||||
// printf("irq state = %#X ",test_irq[2]);
|
||||
// printf("irq state = %#X ",test_irq[3]);
|
||||
// printf("irq state = %#X ",test_irq[4]);
|
||||
// printf("irq state = %#X\n",test_irq[5]);
|
||||
|
||||
// printf("testAPP: %d us \n",getCurrentTime() - start_time);
|
||||
// start_time = getCurrentTime();
|
||||
|
||||
}
|
||||
|
||||
|
||||
// APP模块的初始化
|
||||
void testAppInit(void)
|
||||
{
|
||||
// 初始化定时器
|
||||
timerInit(&test_timer);
|
||||
// 订阅定时器信号，用于停止电机
|
||||
subscribe(&test_timer, testTimerProcess);
|
||||
|
||||
printf("testAPP: initial OK %d\n",getCurrentTime());
|
||||
|
||||
timerStart(&test_timer, 5000,1);
|
||||
}
|
||||
18
app/app_test.h
Normal file
18
app/app_test.h
Normal file
@@ -0,0 +1,18 @@
|
||||
#ifndef TEST_H
|
||||
#define TEST_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
extern void testAppInit(void);
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // TEST_H
|
||||
372
app/app_turn.c
Normal file
372
app/app_turn.c
Normal file
@@ -0,0 +1,372 @@
|
||||
#include "app_config.h"
|
||||
#include "app_dependence.h"
|
||||
#include "interface.h"
|
||||
|
||||
#include "app_turn.h"
|
||||
#include "app_differential_drive.h"
|
||||
#include "app_param_manage.h"
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
TurnData turn_data;
|
||||
Timer turn_timer2; // <20><>ʱ<EFBFBD><CAB1><EFBFBD>ṹ<EFBFBD><E1B9B9>
|
||||
|
||||
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>תuint32_t<5F><74><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD>洢<EFBFBD><E6B4A2>
|
||||
void floatToUint32(float num, uint32_t *result)
|
||||
{
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD><EFBFBD><EFBFBD>uint32_t<5F><74><EFBFBD><EFBFBD>
|
||||
memcpy(result, &num, sizeof(num));
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
static void turnOutput(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
uint32_t float_temp = 0;
|
||||
|
||||
switch(turn_data.direction)
|
||||
{
|
||||
case 0:
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
floatToUint32(turn_data.left_zero_position,&float_temp);
|
||||
un_sdo_output1.bit_data.data = float_temp;
|
||||
floatToUint32(turn_data.left_speed*2,&float_temp);
|
||||
un_sdo_output2.bit_data.data = float_temp;
|
||||
//<2F>ҵ<EFBFBD><D2B5><EFBFBD>
|
||||
floatToUint32(turn_data.right_zero_position,&float_temp);
|
||||
un_sdo_output5.bit_data.data = float_temp;
|
||||
floatToUint32(turn_data.right_speed*2,&float_temp);
|
||||
un_sdo_output6.bit_data.data = float_temp;
|
||||
break;
|
||||
case 1://<2F><>ת
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
floatToUint32(turn_data.inner_rad + turn_data.left_zero_position,&float_temp);
|
||||
un_sdo_output1.bit_data.data = float_temp;
|
||||
floatToUint32(turn_data.left_speed,&float_temp);
|
||||
un_sdo_output2.bit_data.data = float_temp;
|
||||
//<2F>ҵ<EFBFBD><D2B5><EFBFBD>
|
||||
floatToUint32(turn_data.outer_rad + turn_data.right_zero_position,&float_temp);
|
||||
un_sdo_output5.bit_data.data = float_temp;
|
||||
floatToUint32(turn_data.right_speed,&float_temp);
|
||||
un_sdo_output6.bit_data.data = float_temp;
|
||||
break;
|
||||
case 2://<2F><>ת
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
floatToUint32(turn_data.outer_rad + turn_data.left_zero_position,&float_temp);
|
||||
un_sdo_output1.bit_data.data = float_temp;
|
||||
floatToUint32(turn_data.left_speed,&float_temp);
|
||||
un_sdo_output2.bit_data.data = float_temp;
|
||||
//<2F>ҵ<EFBFBD><D2B5><EFBFBD>
|
||||
floatToUint32(turn_data.inner_rad + turn_data.right_zero_position,&float_temp);
|
||||
un_sdo_output5.bit_data.data = float_temp;
|
||||
floatToUint32(turn_data.right_speed,&float_temp);
|
||||
un_sdo_output6.bit_data.data = float_temp;
|
||||
break;default:;
|
||||
}
|
||||
|
||||
un_sdo_output1.bit_data.index = LOC_REF_INDEX;
|
||||
un_sdo_output1.bit_data.object_index = 0x0000;
|
||||
// publishMessage(&un_sdo_output1, 1);//<2F>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
|
||||
|
||||
un_sdo_output2.bit_data.index = LIMIT_SPEED_INDEX;
|
||||
un_sdo_output2.bit_data.object_index = 0x0000;
|
||||
// publishMessage(&un_sdo_output2, 1);//<2F>趨<EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>λ<EFBFBD><CEBB>
|
||||
|
||||
un_sdo_output5.bit_data.index = LOC_REF_INDEX;
|
||||
un_sdo_output5.bit_data.object_index = 0x0000;
|
||||
// publishMessage(&un_sdo_output5, 1);//<2F>趨<EFBFBD><E8B6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
|
||||
|
||||
un_sdo_output6.bit_data.index = LIMIT_SPEED_INDEX;
|
||||
un_sdo_output6.bit_data.object_index = 0x0000;
|
||||
// publishMessage(&un_sdo_output6, 1);//<2F>趨<EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>λ<EFBFBD><CEBB>
|
||||
|
||||
}
|
||||
|
||||
//// Ԥ<><D4A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>Ҫ<EFBFBD>ȷ<EFBFBD>һ<EFBFBD><D2BB>ʧ<EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD>ʹ<EFBFBD><CAB9>
|
||||
//void turnChargeFinish(uint8_t *stt, uint8_t *cnt)
|
||||
//{
|
||||
// switch(stt[0])//<2F>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD>л<EFBFBD>ģʽ<C4A3>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>ʹ<EFBFBD><CAB9>
|
||||
// {
|
||||
// case 0:
|
||||
// if(cnt[0] >= 20)//<2F><><EFBFBD><EFBFBD>50<35><30>
|
||||
// {
|
||||
// cnt[0] = 0;
|
||||
// stt[0] = 1;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// un_sdo_output5.bit_data.cmd = 0x2F;
|
||||
// un_sdo_output5.bit_data.object_index = 0x6060;
|
||||
// un_sdo_output5.bit_data.sub_index = 0;
|
||||
// un_sdo_output5.bit_data.data = 0x01;//<2F>趨λ<E8B6A8><CEBB>ģʽ
|
||||
// publishMessage(&un_sdo_output5, 1);
|
||||
//
|
||||
// un_sdo_output4.bit_data.cmd = 0x2B;
|
||||
// un_sdo_output4.bit_data.object_index = 0x6040;
|
||||
// un_sdo_output4.bit_data.sub_index = 0;
|
||||
// un_sdo_output4.bit_data.data = 0x06;//<2F><><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>
|
||||
// publishMessage(&un_sdo_output4, 1);
|
||||
// cnt[0] ++;
|
||||
// stt[0] = 0;
|
||||
// printf("position! \n");
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case 1:
|
||||
//
|
||||
// cnt[0] = 0;
|
||||
// stt[0] = 1;
|
||||
// printf("disable! \n");
|
||||
// break;default:;
|
||||
// }
|
||||
//}
|
||||
|
||||
/**
|
||||
* <20><><EFBFBD>㰢<EFBFBD><E3B0A2><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD><F2BCB8BA>е<EFBFBD><D0B5><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƣ<EFBFBD>
|
||||
* @param L <20><><EFBFBD>ࣨǰ<E0A3A8><C7B0><EFBFBD>־<EFBFBD><D6BE>룬<EFBFBD><EBA3AC>λ<EFBFBD><CEBB><EFBFBD>ף<EFBFBD>
|
||||
* @param W <20>־ࣨ<D6BE><E0A3A8><EFBFBD><EFBFBD><EFBFBD>־<EFBFBD><D6BE>룬<EFBFBD><EBA3AC>λ<EFBFBD><CEBB><EFBFBD>ף<EFBFBD>
|
||||
* @param inner_rad <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD>ȣ<EFBFBD>
|
||||
* @param[out] curvature <20><><EFBFBD><EFBFBD>·<EFBFBD><C2B7><EFBFBD><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD>λ<EFBFBD><CEBB>1/<2F>ף<EFBFBD>
|
||||
* @return <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ǣ<EFBFBD><C7A3><EFBFBD><EFBFBD>ȣ<EFBFBD>
|
||||
*/
|
||||
float calculate_outer_angle(float L, float W, float inner_rad, float* curvature)
|
||||
{
|
||||
// <20><><EFBFBD><EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD>
|
||||
int sign = (inner_rad < 0) ? -1 : 1;
|
||||
|
||||
// ʹ<>þ<EFBFBD><C3BE><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>м<EFBFBD><D0BC><EFBFBD>
|
||||
float inner_temp = fabsf(inner_rad);
|
||||
|
||||
// <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>뾶<EFBFBD><EBBEB6><EFBFBD>㣨<EFBFBD>ؼ<EFBFBD><D8BC>ĵ㣩
|
||||
float R_inner = L / tanf(inner_temp); // <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>뾶
|
||||
float R_outer = R_inner + W; // <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>뾶 = <20><><EFBFBD>ְ뾶 + <20>־<EFBFBD>
|
||||
|
||||
float R = R_inner + W / 2; // <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>뾶
|
||||
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʣ<EFBFBD>ע<EFBFBD><D7A2><EFBFBD><EFBFBD><EFBFBD>ţ<EFBFBD>
|
||||
*curvature = sign / R;
|
||||
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ǣ<EFBFBD>atan(L/R_outer)
|
||||
float delta_o = atanf(L / R_outer);
|
||||
|
||||
// <20>ָ<EFBFBD>ԭʼ<D4AD><CABC><EFBFBD><EFBFBD>
|
||||
return sign * delta_o;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
static void turnProcess(void *signal_id)
|
||||
{
|
||||
static uint8_t turn_process_state = 0;
|
||||
static uint8_t turn_process_cnt = 0;
|
||||
|
||||
(void)signal_id;
|
||||
if((turn_data.desired_curvature) > 0.01)//<2F><>ת
|
||||
{
|
||||
turn_data.direction = 1;
|
||||
}
|
||||
else if((turn_data.desired_curvature) < -0.01)//<2F><>ת
|
||||
{
|
||||
turn_data.direction = 2;
|
||||
}
|
||||
else//<2F><><EFBFBD><EFBFBD>
|
||||
{
|
||||
turn_data.direction = 0;
|
||||
}
|
||||
|
||||
turn_data.inner_rad = turn_data.desired_curvature*turn_data.left_max_position/2.0;//<2F><><EFBFBD><EFBFBD><EFBFBD>Ƕȣ<C7B6><C8A3><EFBFBD><EFBFBD>ֽǶ<D6BD> -2-2<><32><EFBFBD><EFBFBD><EFBFBD>ʶ<EFBFBD>Ӧ ת<><D7AA>Ϊ<EFBFBD><EFBFBD><DEB6>Ƕ<EFBFBD>
|
||||
turn_data.outer_rad = calculate_outer_angle(turn_data.wheel_base, turn_data.track_width, turn_data.inner_rad, &turn_data.curvature);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֽǶ<D6BD><C7B6>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// //<2F><><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>δ<EFBFBD><CEB4>can<61><6E><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD>Ե<EFBFBD><D4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>բδ<D5A2><EFBFBD><F2BFAAB5><EFBFBD><EFBFBD>⡣
|
||||
// if(power_data.current_state == POWER_WORKING)//<2F><>բ<EFBFBD><D5A2><EFBFBD><EFBFBD>
|
||||
// {
|
||||
// un_inf_can_kgf_output1.bit_data.KGF01 = 1;//<2F><EFBFBD>բ
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// un_inf_can_kgf_output1.bit_data.KGF01 = 0;//<2F><>բ
|
||||
// }
|
||||
|
||||
if ((turn_data.current_state == POWER_WORKING) && (1 == turn_data.motor_sleep))//work<72>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
{
|
||||
switch(turn_process_state)//<2F>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD>л<EFBFBD>ģʽ<C4A3>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD>ܣ<EFBFBD><DCA3><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1>ʹ<EFBFBD><CAB9> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
{
|
||||
case 0:
|
||||
if(turn_process_cnt >= 20)//<2F><><EFBFBD><EFBFBD>50<35><30>
|
||||
{
|
||||
turn_process_cnt = 0;
|
||||
turn_process_state = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
turn_process_cnt ++;
|
||||
|
||||
un_sdo_output4.bit_data.index = RUM_MODE;
|
||||
un_sdo_output4.bit_data.object_index = 0x0;
|
||||
un_sdo_output4.bit_data.data = POSITION_MODE_CSP;//<2F>趨λ<E8B6A8><CEBB>ģʽ
|
||||
publishMessage(&un_sdo_output4, 1);
|
||||
|
||||
un_sdo_output8.bit_data.index = RUM_MODE;
|
||||
un_sdo_output8.bit_data.object_index = 0x0;
|
||||
un_sdo_output8.bit_data.data = POSITION_MODE_CSP;//<2F>趨λ<E8B6A8><CEBB>ģʽ
|
||||
publishMessage(&un_sdo_output8, 1);
|
||||
}
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if(turn_process_cnt >= 10)//<2F><><EFBFBD><EFBFBD>50<35><30>
|
||||
{
|
||||
turn_process_cnt = 0;
|
||||
turn_process_state = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
turn_process_cnt ++;
|
||||
|
||||
un_sdo_output3.bit_data.index = 0x0;
|
||||
un_sdo_output3.bit_data.object_index = 0x0;
|
||||
un_sdo_output3.bit_data.data = 0x0;//
|
||||
publishMessage(&un_sdo_output3, 1);
|
||||
|
||||
un_sdo_output7.bit_data.index = 0x0;
|
||||
un_sdo_output7.bit_data.object_index = 0x0;
|
||||
un_sdo_output7.bit_data.data = 0x0;//
|
||||
publishMessage(&un_sdo_output7, 1);
|
||||
}
|
||||
break;
|
||||
|
||||
case 2:
|
||||
turnOutput(signal_id);
|
||||
turn_process_cnt = 0;
|
||||
turn_process_state = 2;
|
||||
break;default:;
|
||||
}
|
||||
}
|
||||
else//ÿ<>ε<EFBFBD><CEB5>綼<EFBFBD><E7B6BC><EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><C2B7><EFBFBD>ʹ<EFBFBD><CAB9>
|
||||
{
|
||||
turn_process_state = 0;
|
||||
turn_process_cnt = 0;
|
||||
turn_data.motor_sleep = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>źŵĺ<C5B5><C4BA><EFBFBD>
|
||||
static void turnInput(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
if (signal_id == &diff_data)
|
||||
{
|
||||
turn_data.mode = diff_data.mode;
|
||||
turn_data.desired_curvature = diff_data.desired_curvature;
|
||||
}
|
||||
else if(signal_id == &power_data)//<2F><><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>
|
||||
{
|
||||
turn_data.current_state = power_data.current_state;
|
||||
}
|
||||
else{}
|
||||
turn_data.motor_sleep = 1;
|
||||
turnProcess(signal_id);//<2F><><EFBFBD><EFBFBD>ӳ<EFBFBD><D3B3>
|
||||
}
|
||||
|
||||
|
||||
void turnParametersInit(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // <20><><EFBFBD>DZ<EFBFBD><C7B1><EFBFBD>Ϊ<EFBFBD><CEAA>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>
|
||||
turn_data.right_max_position = (float)getParam("R_maxP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
turn_data.right_zero_position = (float)getParam("R_zeroP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
turn_data.right_speed = (float)getParam("R_speed"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
|
||||
turn_data.left_max_position = (float)getParam("L_maxP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
turn_data.left_zero_position = (float)getParam("L_zeroP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
turn_data.left_speed = (float)getParam("L_speed"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
|
||||
|
||||
turn_data.wheel_base = (float)getParam("whl_bas"); //<2F><><EFBFBD><EFBFBD>
|
||||
turn_data.track_width = (float)getParam("tra_wid"); //<2F>־<EFBFBD>
|
||||
|
||||
timerStart(&turn_data.turn_timer,1000,1);//1s<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
|
||||
}
|
||||
|
||||
void motorInit(void *signal_id)
|
||||
{
|
||||
if(turn_data.current_state == POWER_WORKING)
|
||||
{
|
||||
un_sdo_output4.bit_data.index = RUM_MODE;
|
||||
un_sdo_output4.bit_data.object_index = 0x0;
|
||||
un_sdo_output4.bit_data.data = POSITION_MODE_CSP;//<2F>趨λ<E8B6A8><CEBB>ģʽ
|
||||
publishMessage(&un_sdo_output4, 1);
|
||||
|
||||
un_sdo_output8.bit_data.index = RUM_MODE;
|
||||
un_sdo_output8.bit_data.object_index = 0x0;
|
||||
un_sdo_output8.bit_data.data = POSITION_MODE_CSP;//<2F>趨λ<E8B6A8><CEBB>ģʽ
|
||||
publishMessage(&un_sdo_output8, 1);
|
||||
|
||||
un_sdo_output3.bit_data.index = 0x0;
|
||||
un_sdo_output3.bit_data.object_index = 0x0;
|
||||
un_sdo_output3.bit_data.data = 0x0;//
|
||||
publishMessage(&un_sdo_output3, 1);
|
||||
|
||||
un_sdo_output7.bit_data.index = 0x0;
|
||||
un_sdo_output7.bit_data.object_index = 0x0;
|
||||
un_sdo_output7.bit_data.data = 0x0;//
|
||||
publishMessage(&un_sdo_output7, 1);
|
||||
}
|
||||
// timerStart(&turn_timer2,1000,1);//1s<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// <20><EFBFBD>APPģ<50><C4A3><EFBFBD>ij<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
void turnAppInit(void)
|
||||
{
|
||||
// <20><>ʼ<EFBFBD><CABC>
|
||||
memset(&turn_data, 0, sizeof(TurnData));
|
||||
timerInit(&turn_data.turn_timer);
|
||||
timerInit(&turn_timer2);
|
||||
|
||||
|
||||
//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>
|
||||
turn_data.right_max_position = (float)getParam("R_maxP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
// turn_data.right_min_position = (float)getParam("R_minP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
turn_data.right_zero_position = (float)getParam("R_zeroP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
turn_data.right_speed = (float)getParam("R_speed"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
|
||||
turn_data.left_max_position = (float)getParam("L_maxP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
// turn_data.left_min_position = (float)getParam("L_minP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
turn_data.left_zero_position = (float)getParam("L_zeroP"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
turn_data.left_speed = (float)getParam("L_speed"); // <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>λ<EFBFBD>ã<EFBFBD>rad
|
||||
|
||||
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>,<2C><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD>
|
||||
subscribe(&diff_data, turnInput);
|
||||
subscribe(&power_data, turnInput);
|
||||
|
||||
subscribe(&turn_data.turn_timer, turnParametersInit);
|
||||
|
||||
subscribe(&turn_timer2, motorInit);
|
||||
|
||||
|
||||
timerStart(&turn_data.turn_timer,1000,1);//1s<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
|
||||
timerStart(&turn_timer2,1000,1);//1s<31><73><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>
|
||||
|
||||
printf("turnApp: initial OK \n");
|
||||
}
|
||||
|
||||
|
||||
95
app/app_turn.h
Normal file
95
app/app_turn.h
Normal file
@@ -0,0 +1,95 @@
|
||||
#ifndef TURN_H
|
||||
#define TURN_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#include "app_power.h"
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#define LIMIT_SPEED_INDEX 0x7017//CSP<53><50><EFBFBD>ٶ<EFBFBD>
|
||||
#define LOC_REF_INDEX 0x7016//CSP<53><50>λ<EFBFBD><CEBB>
|
||||
#define RUM_MODE 0x7005//modeģʽ
|
||||
|
||||
|
||||
|
||||
|
||||
#define OPERATION_MODE 0 // <20>˿<EFBFBD>ģʽ
|
||||
#define POSITION_MODE_PP 1 // λ<><CEBB>ģʽ (PP - Profile Position)
|
||||
#define VELOCITY_MODE 2 // <20>ٶ<EFBFBD>ģʽ
|
||||
#define CURRENT_MODE 3 // <20><><EFBFBD><EFBFBD>ģʽ
|
||||
#define POSITION_MODE_CSP 5 // λ<><CEBB>ģʽ (CSP - Cyclic Synchronous Position)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
typedef struct TurnData
|
||||
{
|
||||
uint8_t mode ; // <20><><EFBFBD><EFBFBD>ģʽ
|
||||
Timer turn_timer; // <20><>ʱ<EFBFBD><CAB1><EFBFBD>ṹ<EFBFBD><E1B9B9>
|
||||
float desired_curvature; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
float curvature; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
|
||||
uint32_t position; // ת<><D7AA>λ<EFBFBD><CEBB>
|
||||
uint8_t direction; // ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
uint8_t old_direction; // ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
float yaw_rate; // ת<><D7AA><EFBFBD><EFBFBD><EFBFBD>ٶȡ<D9B6>
|
||||
float inner_rad; //<2F><><EFBFBD>ֽǶ<D6BD>
|
||||
float outer_rad; //<2F><><EFBFBD>ֽǶ<D6BD>
|
||||
uint8_t motor_sleep; // <20><><EFBFBD><EFBFBD>״̬ 0δ<30>ϵ磬1<E7A3AC><31><EFBFBD>ϵ<EFBFBD>
|
||||
uint8_t emergency_stop_switch; // <20><>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD>
|
||||
uint8_t remote_emergency_stop; // ң<><D2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD>
|
||||
uint8_t emergency_stop_state; // <20><>ͣ״̬
|
||||
PowerState current_state; // <20><>ǰ<EFBFBD><C7B0>Դ״̬
|
||||
|
||||
float right_max_position; // <20>ұ<EFBFBD><D2B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD> rad
|
||||
float right_zero_position; // <20>ұ߹<D2B1><DFB9><EFBFBD><EFBFBD>Ƕ<EFBFBD> rad
|
||||
float right_speed; // <20>ұ<EFBFBD><D2B1>ٶ<EFBFBD> rad/s
|
||||
float left_max_position; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD> rad
|
||||
float left_zero_position; // <20><><EFBFBD>߹<EFBFBD><DFB9><EFBFBD><EFBFBD>Ƕ<EFBFBD> rad
|
||||
float left_speed; // <20><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> rad/s
|
||||
|
||||
float wheel_base; // <20><><EFBFBD><EFBFBD> m
|
||||
float track_width; // <20>־<EFBFBD> m
|
||||
|
||||
} TurnData;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//<2F>ⲿ<EFBFBD><E2B2BF><EFBFBD><EFBFBD>
|
||||
void turnAppInit(void);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // TURN_H
|
||||
Reference in New Issue
Block a user