第一次提交
This commit is contained in:
BIN
app/20250324植树车app.rar
Normal file
BIN
app/20250324植树车app.rar
Normal file
Binary file not shown.
224
app/app_brake.c
Normal file
224
app/app_brake.c
Normal file
@@ -0,0 +1,224 @@
|
||||
#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 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 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(); // 正转
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setBrakeOff(); // 抱闸继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF14 = setBrakeOff(); // 抱闸继电器
|
||||
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(); // 反转
|
||||
un_inf_can_kgf_output1.bit_data.KGF13 = setBrakeOn(); // 抱闸继电器
|
||||
un_inf_can_kgf_output1.bit_data.KGF14 = 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);
|
||||
publishMessage(&un_inf_can_kgf_output1, 1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 修改刹车定时器处理函数
|
||||
static void brakeTimerProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
//#ifdef OIL_BRAKE
|
||||
switch (brake_data.state)
|
||||
{
|
||||
case BRAKE_STATE_IDLE:
|
||||
if (shouldApplyBrake())
|
||||
{
|
||||
brake_data.state = BRAKE_STATE_APPLYING_BRAKE;
|
||||
brake_data.brake_motor_state = 1;
|
||||
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;
|
||||
brake_data.brake_motor_state = 2;
|
||||
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;
|
||||
|
||||
|
||||
|
||||
printf("writeE2 brake_position = %d\n",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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// 修改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);
|
||||
|
||||
// 订阅输入信号,处理刹车逻辑
|
||||
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);
|
||||
|
||||
// 启动定时器，定期调用 brakeTimerProcess
|
||||
timerStart(&brake_data.brake_timer, 500, 1);
|
||||
|
||||
printf("app_brake: initial OK \n");
|
||||
}
|
||||
58
app/app_brake.h
Normal file
58
app/app_brake.h
Normal file
@@ -0,0 +1,58 @@
|
||||
#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"
|
||||
|
||||
#define OIL_BRAKE 0
|
||||
#define ELECTROMAGNETIC_BRAKE 1
|
||||
|
||||
|
||||
// 定义刹车状态机状态
|
||||
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 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; // 释放刹车定时器
|
||||
|
||||
} 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
|
||||
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
|
||||
932
app/app_differential_drive.c
Normal file
932
app/app_differential_drive.c
Normal file
@@ -0,0 +1,932 @@
|
||||
#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_pid.h"
|
||||
|
||||
#include "app_differential_drive.h"
|
||||
#include "app_brake.h"
|
||||
#include "app_power.h"
|
||||
|
||||
Timer diff_app_timer;
|
||||
|
||||
// 定义全局变量
|
||||
DiffData diff_data;
|
||||
|
||||
PID_t speed_pid;
|
||||
PID_t yaw_rate_pid;
|
||||
|
||||
// 设置电机输出
|
||||
void setMotorOutput(float *out_torq, float max_torque, uint16_t feed_power, uint16_t discharge_power)
|
||||
{
|
||||
|
||||
float abs_left_front_speed = 0;
|
||||
float abs_right_front_speed = 0;
|
||||
float abs_left_rear_speed = 0;
|
||||
float abs_right_rear_speed = 0;
|
||||
|
||||
// 档位
|
||||
un_motor_output1.bit_data.gear = diff_data.state; // 1 表示前进，2 表示后退，0空挡
|
||||
un_motor_output2.bit_data.gear = diff_data.state;
|
||||
un_motor_output3.bit_data.gear = diff_data.state;
|
||||
un_motor_output4.bit_data.gear = diff_data.state;
|
||||
|
||||
|
||||
//增加系数以及偏移量
|
||||
if(diff_data.state == STATE_FORWARD)//根据挡位来判断，扭矩的正负
|
||||
{
|
||||
abs_left_front_speed = (out_torq[0] + 300.0f) *100.0f;
|
||||
abs_right_front_speed = (out_torq[1] + 300.0f) *100.0f;
|
||||
abs_left_rear_speed = (out_torq[2] + 300.0f) *100.0f;
|
||||
abs_right_rear_speed = (out_torq[3] + 300.0f) *100.0f;
|
||||
}
|
||||
else if(diff_data.state == STATE_BACKWARD)//倒挡直接修改为负扭矩发送出去
|
||||
{
|
||||
abs_left_front_speed = (-out_torq[0] + 300.0f) *100.0f;
|
||||
abs_right_front_speed = (-out_torq[1] + 300.0f) *100.0f;
|
||||
abs_left_rear_speed = (-out_torq[2] + 300.0f) *100.0f;
|
||||
abs_right_rear_speed = (-out_torq[3] + 300.0f) *100.0f;
|
||||
}
|
||||
else//空挡直接发0
|
||||
{
|
||||
abs_left_front_speed = 0;
|
||||
abs_right_front_speed = 0;
|
||||
abs_left_rear_speed = 0;
|
||||
abs_right_rear_speed = 0;
|
||||
}
|
||||
|
||||
|
||||
// 设置左右电机期望转速
|
||||
// un_motor_output1.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_left_speed) + 30000); // 20240921 增加偏移量 30000
|
||||
// un_motor_output2.bit_data.set_rotation_speed = ((uint16_t)roundf(abs_right_speed) + 30000); // 20240921 增加偏移量 30000
|
||||
|
||||
// 设置模式为扭矩模式
|
||||
un_motor_output1.bit_data.mode = MOTOR_MODE;
|
||||
un_motor_output2.bit_data.mode = MOTOR_MODE;
|
||||
un_motor_output3.bit_data.mode = MOTOR_MODE;
|
||||
un_motor_output4.bit_data.mode = MOTOR_MODE;
|
||||
|
||||
// 设置最大扭矩
|
||||
un_motor_output1.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_front_speed );
|
||||
un_motor_output2.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_front_speed );
|
||||
un_motor_output3.bit_data.set_torque = (uint16_t)( (int16_t)abs_left_rear_speed );
|
||||
un_motor_output4.bit_data.set_torque = (uint16_t)( (int16_t)abs_right_rear_speed );
|
||||
|
||||
|
||||
// 设置馈电功率
|
||||
un_motor_output1.bit_data.feed_power = feed_power;
|
||||
un_motor_output2.bit_data.feed_power = feed_power;
|
||||
un_motor_output3.bit_data.feed_power = feed_power;
|
||||
un_motor_output4.bit_data.feed_power = feed_power;
|
||||
|
||||
|
||||
// 设置放电功率
|
||||
un_motor_output1.bit_data.discharge_power = discharge_power;
|
||||
un_motor_output2.bit_data.discharge_power = discharge_power;
|
||||
un_motor_output3.bit_data.discharge_power = discharge_power;
|
||||
un_motor_output4.bit_data.discharge_power = discharge_power;
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
// 限制值在最小值和最大值之间
|
||||
float constrain(float value, float min_val, float max_val)
|
||||
{
|
||||
if (value < min_val)
|
||||
{
|
||||
return min_val;
|
||||
}
|
||||
else if (value > max_val)
|
||||
{
|
||||
return max_val;
|
||||
}
|
||||
else
|
||||
{
|
||||
return value;
|
||||
}
|
||||
}
|
||||
|
||||
// 计算当前速度、角速度
|
||||
uint8_t calculateCurrentSpeedYawRate(void)
|
||||
{
|
||||
// 获取轮子周长
|
||||
float wheel_circumference = (float)getParam("whl_dia") * M_PI;
|
||||
// 获取减速比
|
||||
float gear_ratio = (float)getParam("gRatio");
|
||||
if (fabsf(gear_ratio) < EPSILON)
|
||||
{
|
||||
return 0; // 避免除以0的情况
|
||||
}
|
||||
// 将电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比
|
||||
|
||||
|
||||
float left_speed_mps = (diff_data.left_motor_speed * wheel_circumference) / (60.0f * gear_ratio);
|
||||
float right_speed_mps = (diff_data.right_motor_speed * wheel_circumference) / (60.0f * gear_ratio);
|
||||
|
||||
// float left_speed_mps = 0;
|
||||
// float right_speed_mps = 0;
|
||||
|
||||
// float left_speed_mps = 0;
|
||||
// float right_speed_mps = 0;
|
||||
|
||||
// 计算当前速度
|
||||
diff_data.speed = (left_speed_mps + right_speed_mps) / 2.0f;
|
||||
// 计算速度差
|
||||
float speed_diff = left_speed_mps - right_speed_mps;
|
||||
// 计算角速度
|
||||
float wheel_base = (float)getParam("whl_bas");
|
||||
if (fabsf(wheel_base) < EPSILON)
|
||||
{
|
||||
return 0; // 避免除以0的情况
|
||||
}
|
||||
diff_data.yaw_rate = speed_diff / wheel_base;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// 计算加速度
|
||||
float calculateAcceleration(float speed, float previous_speed, float dt)
|
||||
{
|
||||
if (fabs(dt) < EPSILON)
|
||||
{
|
||||
return 0; // 避免除以0的情况
|
||||
}
|
||||
float acceleration = (speed - previous_speed) / dt;
|
||||
return acceleration;
|
||||
}
|
||||
|
||||
// 计算减速度
|
||||
float calculateDeceleration(float speed, float previous_speed, float dt)
|
||||
{
|
||||
if (fabs(dt) < EPSILON)
|
||||
{
|
||||
return 0; // 避免除以0的情况
|
||||
}
|
||||
float deceleration = calculateAcceleration(previous_speed, speed, dt); // 减速度就是负的加速度
|
||||
return deceleration;
|
||||
}
|
||||
|
||||
// 计算最大速度
|
||||
float calculateMaxSpeed()
|
||||
{
|
||||
// 获取最大电机转速 (RPM)
|
||||
float max_rpm = (float)getParam("max_rpm");
|
||||
// 获取轮子周长
|
||||
float wheel_circumference = (float)getParam("whl_dia") * M_PI;
|
||||
// 获取减速比
|
||||
float gear_ratio = (float)getParam("gRatio");
|
||||
if (fabsf(gear_ratio) < EPSILON)
|
||||
{
|
||||
return 0; // 避免除以0的情况
|
||||
}
|
||||
// 将最大电机转速 (RPM) 转换为线速度 (m/s)，考虑减速比
|
||||
float max_speed = (max_rpm * wheel_circumference) / (60.0f * gear_ratio);
|
||||
return max_speed;
|
||||
}
|
||||
|
||||
// 计算最大加速度
|
||||
float calculateMaxAcceleration(void)
|
||||
{
|
||||
// 获取车辆参数
|
||||
float max_motor_torque = (float)getParam("maxTorq"); // 最大电机扭矩
|
||||
float vehicle_mass = (float)getParam("VehMass"); // 车辆质量
|
||||
float wheel_radius = (float)getParam("whl_dia") / 2.0f; // 轮子半径
|
||||
float gear_ratio = (float)getParam("gRatio"); // 减速比
|
||||
if (fabsf(wheel_radius) < EPSILON || fabsf(vehicle_mass) < EPSILON )
|
||||
{
|
||||
return 0; // 避免除以0的情况
|
||||
}
|
||||
// 减速比计算扭矩
|
||||
float effective_torque = max_motor_torque * gear_ratio;
|
||||
// 计算最大加速度
|
||||
float max_acceleration = (effective_torque / wheel_radius) / vehicle_mass;
|
||||
return max_acceleration;
|
||||
}
|
||||
|
||||
// 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度
|
||||
void calculateCurrentState(float dt)
|
||||
{
|
||||
static float previous_speed = 0.0f;
|
||||
// 更新当前速度和当前角速度
|
||||
calculateCurrentSpeedYawRate();
|
||||
// 更新加速度、减速度等，根据需要计算
|
||||
diff_data.acceleration = calculateAcceleration(diff_data.speed, previous_speed, dt);
|
||||
diff_data.deceleration = calculateDeceleration(diff_data.speed, previous_speed, dt);
|
||||
diff_data.max_speed = calculateMaxSpeed();
|
||||
previous_speed = diff_data.speed;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief 基于转速反比的双电机扭矩分配函数
|
||||
* @param rpm1 电机1当前转速（单位：rpm）
|
||||
* @param rpm2 电机2当前转速（单位：rpm）
|
||||
* @param total_torque 系统总需求扭矩（单位：Nm）
|
||||
* @param torque1 [out] 电机1分配到的扭矩（单位：Nm）
|
||||
* @param torque2 [out] 电机2分配到的扭矩（单位：Nm）
|
||||
* @note 分配原则：转速越高的电机分配扭矩越小，确保负载均衡
|
||||
*/
|
||||
void distributeTorque(float rpm1, float rpm2, float total_torque, float* torque1, float* torque2, float max_torque, float min_torque)
|
||||
{
|
||||
|
||||
// 总扭矩为0时快速返回
|
||||
if (fabs(total_torque) < 0.001f) {
|
||||
*torque1 = 0.0f;
|
||||
*torque2 = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
// // 保护条件：当两电机均静止时采用平均分配策略
|
||||
// if (fabs(rpm1) < 0.001f && fabs(rpm2) < 0.001f) {
|
||||
// *torque1 = total_torque / 2.0f;
|
||||
// *torque2 = total_torque / 2.0f;
|
||||
// return;
|
||||
// }
|
||||
|
||||
// 计算权重因子（与转速成反比关系）
|
||||
// 注：添加0.001f防止零转速时除零错误，fabs确保负转速正确处理
|
||||
float weight1 = 1.0f / (fabs(rpm1) + 0.001f);
|
||||
float weight2 = 1.0f / (fabs(rpm2) + 0.001f);
|
||||
|
||||
// 归一化计算分配比例
|
||||
float total_weight = weight1 + weight2;
|
||||
*torque1 = total_torque * (weight1 / total_weight);
|
||||
*torque2 = total_torque * (weight2 / total_weight);
|
||||
|
||||
// 独立限制单侧扭矩（修改核心逻辑）
|
||||
if (fabs(*torque1) > max_torque) {
|
||||
*torque1 = copysignf(max_torque, *torque1);
|
||||
}
|
||||
if (fabs(*torque2) > max_torque) {
|
||||
*torque2 = copysignf(max_torque, *torque2);
|
||||
}
|
||||
|
||||
// 仅对非零扭矩应用下限限制
|
||||
if (fabs(*torque1) < min_torque) {
|
||||
*torque1 = copysignf(min_torque, *torque1);
|
||||
}
|
||||
if ( fabs(*torque2) < min_torque) {
|
||||
*torque2 = copysignf(min_torque, *torque2);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 根据轮速差动态调整电机扭矩（带非负限制）
|
||||
* @param speed_left 左轮速度（单位：rpm或自定义）
|
||||
* @param speed_right 右轮速度（单位：rpm或自定义）
|
||||
* @param torque_left 左轮扭矩指针（单位：Nm或自定义）
|
||||
* @param torque_right 右轮扭矩指针（单位：Nm或自定义）
|
||||
* @param threshold 触发调整的速差阈值（单位同轮速）
|
||||
* @param k 扭矩调整系数（无量纲，建议0<k<1）
|
||||
* @note 函数会直接修改传入的扭矩值，并确保扭矩不小于0
|
||||
*/
|
||||
void adjust_torque_by_speed_diff(float speed_left, float speed_right,
|
||||
float* torque_left, float* torque_right,
|
||||
float threshold, float k) {
|
||||
// 计算轮速差绝对值
|
||||
float speed_diff = fabsf(speed_left - speed_right);
|
||||
|
||||
if (speed_diff > threshold) {
|
||||
// 计算需要减少的扭矩量（速差超出阈值部分×系数）
|
||||
float torque_reduction = (speed_diff - threshold) * k;
|
||||
|
||||
if (speed_left > speed_right) {
|
||||
// 左轮过快时减少左扭矩，并限制最小值为0
|
||||
*torque_left = fmaxf(*torque_left - torque_reduction, 0.0f);
|
||||
} else {
|
||||
// 右轮过快时减少右扭矩，并限制最小值为0
|
||||
*torque_right = fmaxf(*torque_right - torque_reduction, 0.0f);
|
||||
}
|
||||
}
|
||||
}
|
||||
// 计算左右电机速度
|
||||
void computeInverseKinematics(float linear_velocity_x, float yaw_rate, float max_speed, float *motor_speed)
|
||||
{
|
||||
// 防止速度过低导致不必要的计算
|
||||
if (fabs(max_speed) < EPSILON)
|
||||
{
|
||||
motor_speed[0] = 0.0f;
|
||||
motor_speed[1] = 0.0f;
|
||||
motor_speed[2] = 0.0f;
|
||||
motor_speed[3] = 0.0f;
|
||||
return;
|
||||
}
|
||||
|
||||
#if THROTTLE_PID_MODE
|
||||
float max_torque = (float)getParam("maxTorq");
|
||||
|
||||
linear_velocity_x = constrain(linear_velocity_x, -max_torque, max_torque);
|
||||
yaw_rate = constrain(yaw_rate, -2*max_torque, 2*max_torque);
|
||||
|
||||
|
||||
float left_speed_mps = linear_velocity_x + yaw_rate;
|
||||
float right_speed_mps = linear_velocity_x - yaw_rate;
|
||||
|
||||
//扭矩分配
|
||||
if(max_torque < left_speed_mps)
|
||||
{
|
||||
right_speed_mps = right_speed_mps - (left_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差
|
||||
left_speed_mps = max_torque;
|
||||
}
|
||||
else if(-max_torque > left_speed_mps)
|
||||
{
|
||||
right_speed_mps = right_speed_mps - (left_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差
|
||||
left_speed_mps = -max_torque;
|
||||
}
|
||||
else if(max_torque < right_speed_mps)
|
||||
{
|
||||
left_speed_mps = left_speed_mps - (right_speed_mps - max_torque);//多减去超出限值得部分，保证转矩差
|
||||
right_speed_mps = max_torque;
|
||||
}
|
||||
else if(-max_torque > right_speed_mps)
|
||||
{
|
||||
left_speed_mps = left_speed_mps - (right_speed_mps + max_torque);//多减去超出限值得部分，保证转矩差
|
||||
right_speed_mps = -max_torque;
|
||||
}
|
||||
else{}
|
||||
|
||||
printf("input_torq: left=%.1f right=%.1f yaw_rate=%.1f\n", left_speed_mps, right_speed_mps, yaw_rate);
|
||||
|
||||
motor_speed[0] = left_speed_mps;
|
||||
motor_speed[2] = left_speed_mps;
|
||||
|
||||
motor_speed[1] = right_speed_mps;
|
||||
motor_speed[3] = right_speed_mps;
|
||||
|
||||
adjust_torque_by_speed_diff( diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed, &motor_speed[0], &motor_speed[2],100, 5);
|
||||
adjust_torque_by_speed_diff( diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed, &motor_speed[1], &motor_speed[3],100, 5);
|
||||
|
||||
|
||||
// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// distributeTorque(diff_data.left_front_motor_speed,diff_data.left_rear_motor_speed,2*left_speed_mps,&motor_speed[0],&motor_speed[2],diff_data.max_Torq,diff_data.min_Torq);
|
||||
// distributeTorque(diff_data.right_front_motor_speed,diff_data.right_rear_motor_speed,2*right_speed_mps,&motor_speed[1],&motor_speed[3],diff_data.max_Torq,diff_data.min_Torq);
|
||||
|
||||
// printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", motor_speed[0], motor_speed[1], motor_speed[2], motor_speed[3]);
|
||||
|
||||
// // 返回计算结果
|
||||
// *left_motor_speed = left_speed_mps;
|
||||
// *right_motor_speed = right_speed_mps;
|
||||
#else
|
||||
|
||||
// 限制线速度和偏航率
|
||||
linear_velocity_x = constrain(linear_velocity_x, -max_speed, max_speed);
|
||||
float max_yaw_rate = max_speed / ((float)getParam("whl_bas") / 2.0f);
|
||||
yaw_rate = constrain(yaw_rate, -max_yaw_rate, max_yaw_rate);
|
||||
|
||||
// 计算旋转速度
|
||||
float rotational_velocity = ((float)getParam("whl_bas") / 2.0f) * yaw_rate;
|
||||
|
||||
// 计算车辆左右线速度 (m/s)
|
||||
float left_speed_mps = linear_velocity_x - rotational_velocity; //20250316 为解决原地转向和直行转向相同，所以把左右输出的速度交换
|
||||
float right_speed_mps = linear_velocity_x + rotational_velocity;
|
||||
|
||||
// 计算轮子周长
|
||||
float wheel_circumference = (float)getParam("whl_dia") * M_PI;
|
||||
|
||||
// 将车辆左右线速度转换为轮子转速 (RPM)
|
||||
float left_wheel_rpm = (left_speed_mps * 60.0f) / wheel_circumference;
|
||||
float right_wheel_rpm = (right_speed_mps * 60.0f) / wheel_circumference;
|
||||
|
||||
// 获取减速比
|
||||
float gear_ratio = (float)getParam("gRatio");
|
||||
|
||||
// 将轮子转速转换为电机转速，考虑减速比
|
||||
float left_motor_rpm = left_wheel_rpm * gear_ratio;
|
||||
float right_motor_rpm = right_wheel_rpm * gear_ratio;
|
||||
|
||||
// 限制电机的最大和最小转速
|
||||
float max_motor_rpm = (float)getParam("max_rpm");
|
||||
left_motor_rpm = constrain(left_motor_rpm, -max_motor_rpm, max_motor_rpm);
|
||||
right_motor_rpm = constrain(right_motor_rpm, -max_motor_rpm, max_motor_rpm);
|
||||
// 当电机转速小于50转时，设置为0
|
||||
if (fabsf(left_motor_rpm) < 50)//速度慢所以设置位10转
|
||||
{
|
||||
left_motor_rpm = 0;
|
||||
}
|
||||
if (fabsf(right_motor_rpm) < 50)//速度慢所以设置位10转
|
||||
{
|
||||
right_motor_rpm = 0;
|
||||
}
|
||||
// 左边电机方向反一下，因为电机安装反了，返回来的数据也要反一下
|
||||
// left_motor_rpm = -left_motor_rpm;
|
||||
// 返回计算结果
|
||||
*left_motor_speed = left_motor_rpm;
|
||||
*right_motor_speed = right_motor_rpm;
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
// 映射遥控器速度，分为死区、低速区和高速区。
|
||||
float mapRemoteControlSpeed(
|
||||
float input_speed,
|
||||
float deadzone_limit,
|
||||
float input_max,
|
||||
float output_max,
|
||||
float input_slow,
|
||||
float output_slow
|
||||
)
|
||||
{
|
||||
float output_speed = 0.0f;
|
||||
// 获取输入速度的绝对值
|
||||
float abs_input = fabsf(input_speed);
|
||||
//diff_data.desired_speed, 0.1, 2, 10, 1, 5
|
||||
|
||||
// diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5);
|
||||
|
||||
if (abs_input < deadzone_limit + EPSILON)
|
||||
{
|
||||
output_speed = 0.0f;// 死区
|
||||
}
|
||||
else if (abs_input < input_slow + EPSILON)// 低速区
|
||||
{
|
||||
output_speed = (abs_input - deadzone_limit) * output_slow / (input_slow - deadzone_limit);//
|
||||
}
|
||||
else if (abs_input <= input_max + EPSILON)// 高速区
|
||||
{
|
||||
output_speed = output_slow + (abs_input - input_slow) * (output_max - output_slow) / (input_max - input_slow);// 0.2 + （3 - 0.5）* （15-0.2） / （17 - 0.5）
|
||||
}
|
||||
else // 超出范围
|
||||
{
|
||||
output_speed = output_max;
|
||||
}
|
||||
|
||||
// 根据原始输入速度的符号恢复方向
|
||||
if (input_speed < 0)
|
||||
{
|
||||
output_speed = -output_speed;
|
||||
}
|
||||
return output_speed;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 状态机处理函数（修改后版本）
|
||||
*/
|
||||
void handleVehicleState(DiffData *ctx)
|
||||
{
|
||||
switch (ctx->state)
|
||||
{
|
||||
//-------------------------------------------
|
||||
// 初始状态：根据期望速度方向跳转
|
||||
//-------------------------------------------
|
||||
case STATE_INIT:
|
||||
{
|
||||
if (ctx->desired_speed < 0.0f)
|
||||
{
|
||||
ctx->state = STATE_BACKWARD;
|
||||
}
|
||||
else
|
||||
{
|
||||
ctx->state = STATE_FORWARD;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
//-------------------------------------------
|
||||
// 前进状态：处理反向指令（新增else分支）
|
||||
//-------------------------------------------
|
||||
case STATE_FORWARD:
|
||||
{
|
||||
if ((ctx->desired_speed < 0.0f) && (ctx->speed == 0.0f))
|
||||
{
|
||||
ctx->state = STATE_BACKWARD; // 零速时允许切换方向
|
||||
}
|
||||
else if ((ctx->desired_speed < 0.0f) && (ctx->speed != 0.0f))
|
||||
{
|
||||
ctx->desired_speed = 0.0f; // 非零速时清空期望速度
|
||||
ctx->state = STATE_FORWARD; // 显式保持当前状态
|
||||
}
|
||||
else
|
||||
{
|
||||
ctx->state = STATE_FORWARD; // 新增：其他情况保持前进状态
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
//-------------------------------------------
|
||||
// 倒车状态：处理正向指令（新增else分支）
|
||||
//-------------------------------------------
|
||||
case STATE_BACKWARD:
|
||||
{
|
||||
if ((ctx->desired_speed > 0.0f) && (ctx->speed == 0.0f))
|
||||
{
|
||||
ctx->state = STATE_FORWARD; // 零速时允许切换方向
|
||||
}
|
||||
else if ((ctx->desired_speed > 0.0f) && (ctx->speed != 0.0f))
|
||||
{
|
||||
ctx->desired_speed = 0.0f; // 非零速时清空期望速度
|
||||
ctx->state = STATE_BACKWARD; // 显式保持当前状态
|
||||
}
|
||||
else
|
||||
{
|
||||
ctx->state = STATE_BACKWARD; // 新增：其他情况保持倒车状态
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// 差速处理函数
|
||||
static void diffProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
static float previous_time1 = 0.0f;
|
||||
|
||||
float time1 = (float)getCurrentTime();
|
||||
float dt = (time1 - previous_time1) / PERIOD_TICK;
|
||||
previous_time1 = time1;
|
||||
|
||||
// 计算当前状态，包括当前速度、角速度、加速度、减速度、最大速度
|
||||
calculateCurrentState(dt);
|
||||
|
||||
// 当速度小于1时，设定为原地转向 20250321 修改为考虑负号
|
||||
if( (diff_data.desired_speed >= 0) && (diff_data.desired_speed <= 1.0f) )
|
||||
{
|
||||
diff_data.desired_yaw_rate = diff_data.desired_curvature * 1.0f;
|
||||
}
|
||||
else if( (diff_data.desired_speed < 0) && (diff_data.desired_speed >= -1.0f) )
|
||||
{
|
||||
diff_data.desired_yaw_rate = diff_data.desired_curvature * -1.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.desired_yaw_rate = diff_data.desired_curvature * diff_data.desired_speed;
|
||||
}
|
||||
|
||||
handleVehicleState(&diff_data); //20250704 换挡函数 速度为0才能换挡
|
||||
|
||||
|
||||
// printf("desired_speed: %f, desired_yaw: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
|
||||
// 使用 PID 控制器计算输出速度和曲率
|
||||
float output_speed = calculatePidOutput(&speed_pid, diff_data.desired_speed, diff_data.speed, 0.0f, dt);
|
||||
float output_yaw_rate = calculatePidOutput(&yaw_rate_pid, diff_data.desired_yaw_rate, diff_data.yaw_rate, 0.0f, dt);
|
||||
|
||||
// 计算最大加速度，用函数计算
|
||||
float max_acceleration = calculateMaxAcceleration();
|
||||
// 限制输出速度在当前速度和最大加速度计算出来的速度之间
|
||||
// output_speed = constrain(output_speed, diff_data.speed - max_acceleration * dt, diff_data.speed + max_acceleration * dt);
|
||||
|
||||
|
||||
if( (0 == diff_data.desired_yaw_rate) && (0 == diff_data.desired_speed) )//手柄回中，速度小的时候清0
|
||||
{
|
||||
resetPidIntegral(&speed_pid);
|
||||
resetPidIntegral(&yaw_rate_pid);
|
||||
output_speed = 0;
|
||||
output_yaw_rate = 0;
|
||||
}
|
||||
|
||||
// printf("output_speed: %f, output_yaw: %f, integral: %f\n", output_speed, output_yaw_rate,speed_pid.integral);
|
||||
|
||||
|
||||
// if(diff_data.desired_yaw_rate != 0)//有转向的情况下下
|
||||
// {
|
||||
// if( (output_yaw_rate > -500) && (output_yaw_rate < 500) )//如果是转向输出在-500~500之间，那么开始原地转向扭矩太小，所以设定最小扭矩。
|
||||
// {
|
||||
// output_yaw_rate = 500;
|
||||
// }
|
||||
// }
|
||||
|
||||
|
||||
// 使用差速车辆动力学模型计算左右电机的期望速度
|
||||
computeInverseKinematics(output_speed, output_yaw_rate, diff_data.max_speed, &diff_data.out_torq[0]);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// if( (left_speed < 200) && (left_speed > -200) )
|
||||
// {
|
||||
// left_speed = 0;
|
||||
// }
|
||||
//
|
||||
// if( (right_speed < 200) && (right_speed > -200) )
|
||||
// {
|
||||
// right_speed = 0;
|
||||
// }
|
||||
|
||||
|
||||
// 设置电机输出
|
||||
setMotorOutput(&diff_data.out_torq[0],
|
||||
diff_data.max_Torq,//
|
||||
(uint16_t)getParam("feedPwr"),
|
||||
(uint16_t)getParam("dispPwr"));
|
||||
// 发布左右电机期望转速,电源在工作状态才能发送
|
||||
if (power_data.current_state == POWER_WORKING)
|
||||
{
|
||||
publishMessage(&un_motor_output1, 1);
|
||||
publishMessage(&un_motor_output2, 1);
|
||||
publishMessage(&un_motor_output3, 1);
|
||||
publishMessage(&un_motor_output4, 1);
|
||||
|
||||
}
|
||||
|
||||
|
||||
un_can_debug_output.bit_data.speed = (uint8_t)(int8_t)(diff_data.speed*10);
|
||||
un_can_debug_output.bit_data.desired_speed = (uint8_t)(int8_t)(diff_data.desired_speed*10);
|
||||
|
||||
un_can_debug_output.bit_data.curvature = (uint8_t)(int8_t)(diff_data.yaw_rate*10);
|
||||
un_can_debug_output.bit_data.desired_curvature = (uint8_t)(int8_t)(diff_data.desired_yaw_rate*10);
|
||||
|
||||
un_can_debug_output.bit_data.set_left_out = (uint16_t)(int16_t)(diff_data.left_motor_speed);
|
||||
un_can_debug_output.bit_data.set_right_out = (uint16_t)(int16_t)(diff_data.right_motor_speed);
|
||||
|
||||
publishMessage(&diff_data, 1);
|
||||
|
||||
}
|
||||
|
||||
/******************************************************************************
|
||||
Filter(); N个数中取两个
|
||||
******************************************************************************/
|
||||
int16_t Filter(int16_t *s,uint8_t Len)
|
||||
{
|
||||
uint8_t i,j;
|
||||
int16_t temp;
|
||||
//降序排序
|
||||
for(i=0;i<Len-1;i++)
|
||||
for(j=i+1;j<Len;j++)
|
||||
{
|
||||
if(*(s+i)>*(s+j))
|
||||
{
|
||||
*(s+i)=*(s+i)^*(s+j);
|
||||
*(s+j)=*(s+j)^*(s+i);
|
||||
*(s+i)=*(s+i)^*(s+j);
|
||||
}
|
||||
}
|
||||
temp=(*(s+Len/2)+*(s+(Len/2-1)))/2;//20210225修改为除以2，负数不能够右移
|
||||
return(temp);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 差速输入处理函数
|
||||
static void diffInput(void *signal_id)
|
||||
{
|
||||
float motor_speed_temp = 0.0f;
|
||||
|
||||
if (signal_id == &un_sw_sample)
|
||||
{
|
||||
diff_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) )// 遥控器断线，不更新数据
|
||||
{
|
||||
diff_data.remote_emergency_stop = !(uint8_t)un_remote_control_input.bit_data.switch_b;
|
||||
diff_data.mode = un_remote_control_input.bit_data.switch_c == 1 ? MODE_AUTO : MODE_MANUAL;
|
||||
|
||||
if (diff_data.mode == MODE_MANUAL)
|
||||
{
|
||||
diff_data.desired_speed = (float)((int16_t)(un_remote_control_input.bit_data.speed));
|
||||
diff_data.desired_curvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature));
|
||||
// 单位转换
|
||||
diff_data.desired_speed = diff_data.desired_speed * 0.01f;
|
||||
diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f;
|
||||
// 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
|
||||
diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.1, 20, 5, 5, 0.5);
|
||||
diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0.1, 2, 2, 1, 0.5);
|
||||
|
||||
if(diff_data.desired_speed >= 0)//20250320 增加根据速度大小来决定方向，解决后退时转弯反向的问题
|
||||
{
|
||||
diff_data.desired_curvature = diff_data.desired_curvature;
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.desired_curvature = -diff_data.desired_curvature;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if ( (signal_id == &un_manual_computer_input) && (diff_data.mode == MODE_AUTO) )
|
||||
{
|
||||
diff_data.desired_speed = (float)((int16_t)(un_manual_computer_input.bit_data.set_speed));
|
||||
diff_data.desired_curvature = (float)((int16_t)(un_manual_computer_input.bit_data.set_curvature));
|
||||
// 单位转换
|
||||
diff_data.desired_speed = diff_data.desired_speed * 0.01f;
|
||||
diff_data.desired_curvature = diff_data.desired_curvature * 0.0001f;
|
||||
// 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
|
||||
diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0.2, 2, 10, 1, 5);//20250320 修改死区为0.2解决停不住的问题
|
||||
diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1);
|
||||
}
|
||||
else if ( (signal_id == &un_auto_computer_input) && (diff_data.mode == MODE_AUTO) )
|
||||
{
|
||||
diff_data.desired_speed = (float)((int16_t)(un_auto_computer_input.bit_data.set_speed));
|
||||
diff_data.desired_curvature = (float)((int16_t)(un_auto_computer_input.bit_data.set_curvature));
|
||||
// 单位转换
|
||||
diff_data.desired_speed = diff_data.desired_speed * 0.01f;
|
||||
diff_data.desired_curvature = - diff_data.desired_curvature * 0.0001f;// 20241016 增加转弯反相
|
||||
// 遥控器速度映射，参数含义为：输入速度，死区，最大输入，最大输出，低速输入，低速输出
|
||||
diff_data.desired_speed = mapRemoteControlSpeed(diff_data.desired_speed, 0, 5, 10, 2.5, 5);
|
||||
diff_data.desired_curvature = mapRemoteControlSpeed(diff_data.desired_curvature, 0, 2, 2, 1, 1);
|
||||
}
|
||||
else if ( (signal_id == &un_motor_input1) || (signal_id == &un_motor_input3) )// 处理第一个电机速度信号（左电机）
|
||||
{
|
||||
diff_data.left_front_motor_speed = (float)((int16_t)(un_motor_input1.bit_data.speed - 30000));//20240921 增加偏移量
|
||||
diff_data.left_rear_motor_speed = (float)((int16_t)(un_motor_input3.bit_data.speed - 30000));//20240921 增加偏移量
|
||||
|
||||
if(fabs(diff_data.left_rear_motor_speed) > fabs(diff_data.left_front_motor_speed))//取速度较小的轮速
|
||||
{
|
||||
motor_speed_temp = diff_data.left_front_motor_speed;
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_speed_temp = diff_data.left_rear_motor_speed;
|
||||
}
|
||||
diff_data.left_motor_speed = motor_speed_temp;
|
||||
}
|
||||
else if( (signal_id == &un_motor_input2) || (signal_id == &un_motor_input4) )// 处理第二个电机速度信号（右电机）
|
||||
{
|
||||
diff_data.right_front_motor_speed = (float)((int16_t)(un_motor_input2.bit_data.speed - 30000)); // 20250502 1号控制器增加反相
|
||||
diff_data.right_rear_motor_speed = (float)((int16_t)(un_motor_input4.bit_data.speed - 30000));
|
||||
|
||||
if(fabs(diff_data.right_front_motor_speed) > fabs(diff_data.right_rear_motor_speed))//取速度较小的轮速
|
||||
{
|
||||
motor_speed_temp = diff_data.right_rear_motor_speed;
|
||||
}
|
||||
else
|
||||
{
|
||||
motor_speed_temp = diff_data.right_front_motor_speed;
|
||||
}
|
||||
|
||||
diff_data.right_motor_speed = motor_speed_temp;
|
||||
}
|
||||
|
||||
// 急停开关
|
||||
diff_data.emergency_stop_state = (uint8_t)(diff_data.emergency_stop_switch == app_close() || diff_data.remote_emergency_stop == app_close());
|
||||
|
||||
// 如果急停被激活,强制设定速度为0,急停包括车上急停开关和遥控器急停开关
|
||||
if (diff_data.emergency_stop_state == 1)
|
||||
{
|
||||
diff_data.desired_speed = 0.0;
|
||||
diff_data.desired_curvature = 0.0;
|
||||
}
|
||||
// 遥控器断线，而且是在手动模式，期望值清0
|
||||
if ( (diff_data.mode == MODE_MANUAL) && (0 == un_remote_control_input.bit_data.enable) )
|
||||
{
|
||||
diff_data.desired_speed = 0.0;
|
||||
diff_data.desired_curvature = 0.0;
|
||||
}
|
||||
|
||||
if (diff_data.emergency_stop_state == 1)//刹车 20241017 增加的扭矩限制
|
||||
{
|
||||
diff_data.max_Torq = 5;//20240403修改。刹车就是5N
|
||||
}
|
||||
else if ((0 == diff_data.desired_speed) && (0 == diff_data.desired_curvature) && (diff_data.left_motor_speed > -100) && (diff_data.left_motor_speed < 100)&& (((diff_data.right_motor_speed > -100) && (diff_data.right_motor_speed < 100))))//20240330只有当手柄回中，然后当前已经停止的状态才设置为最小停车扭矩
|
||||
{
|
||||
diff_data.max_Torq = 5;//停车 就为0 20250425 修改为5，解决手柄回中，震荡问题
|
||||
}
|
||||
else
|
||||
{
|
||||
diff_data.max_Torq = (uint16_t)getParam("maxTorq");//参数读取设定最大扭矩
|
||||
}
|
||||
|
||||
diffProcess(&diff_data);//计算左右电机期望转速
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 预充完成处理函数
|
||||
void preChargeFinish(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
float out_torq[4] = {0.0f,0.0f,0.0f,0.0f};
|
||||
|
||||
setMotorOutput(out_torq, (uint16_t)getParam("maxTorq"), (uint16_t)getParam("feedPwr"), (uint16_t)getParam("dispPwr"));
|
||||
// 档位
|
||||
// un_motor_output1.bit_data.gear = 0; // 0表示空挡
|
||||
// un_motor_output2.bit_data.gear = 0;
|
||||
publishMessage(&un_motor_output1, 1);
|
||||
publishMessage(&un_motor_output2, 1);
|
||||
}
|
||||
|
||||
|
||||
void diffParametersInit(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||||
|
||||
if(diff_data.mode == MODE_AUTO)//20250504 自动模式PID
|
||||
{
|
||||
setPidParameters(&speed_pid,
|
||||
getParam("Ospd_kp"),
|
||||
getParam("Ospd_ki"),
|
||||
getParam("Ospd_kd"),
|
||||
getParam("Ospd_il"),
|
||||
getParam("Ospd_ol")
|
||||
);
|
||||
|
||||
setPidParameters(&yaw_rate_pid,
|
||||
getParam("Ocrv_kp"),
|
||||
getParam("Ocrv_ki"),
|
||||
getParam("Ocrv_kd"),
|
||||
getParam("Ocrv_il"),
|
||||
getParam("Ocrv_ol")
|
||||
);
|
||||
}
|
||||
else//手动模式
|
||||
{
|
||||
setPidParameters(&speed_pid,
|
||||
getParam("spd_kp"),
|
||||
getParam("spd_ki"),
|
||||
getParam("spd_kd"),
|
||||
getParam("spd_il"),
|
||||
getParam("spd_ol")
|
||||
);
|
||||
|
||||
setPidParameters(&yaw_rate_pid,
|
||||
getParam("crv_kp"),
|
||||
getParam("crv_ki"),
|
||||
getParam("crv_kd"),
|
||||
getParam("crv_il"),
|
||||
getParam("crv_ol")
|
||||
);
|
||||
}
|
||||
diff_data.min_Torq = (uint16_t)getParam("minTorq");//参数读取设定最大扭矩
|
||||
|
||||
|
||||
printf("desired_speed: %f, desired_yaw_rate: %f\n", diff_data.desired_speed, diff_data.desired_yaw_rate);
|
||||
printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
|
||||
// printf("speed: %f, yaw_rate: %f\n", diff_data.speed, diff_data.yaw_rate);
|
||||
|
||||
printf("left_motor_speed = %f\n",diff_data.left_motor_speed);
|
||||
printf("right_motor_speed = %f\n",diff_data.right_motor_speed);
|
||||
|
||||
// printf("speed: FL=%.1f FR=%.1f RL=%.1f RR=%.1f\n", diff_data.left_front_motor_speed, diff_data.right_front_motor_speed, diff_data.left_rear_motor_speed, diff_data.right_rear_motor_speed);
|
||||
// printf("torq: FL=%.1fNm FR=%.1fNm RL=%.1fNm RR=%.1fNm\n", diff_data.out_torq[0], diff_data.out_torq[1], diff_data.out_torq[2], diff_data.out_torq[3]);
|
||||
|
||||
float deffspeed = (float)((int16_t)(un_remote_control_input.bit_data.speed));
|
||||
float deffcurvature = (float)((int16_t)(un_remote_control_input.bit_data.curvature));
|
||||
// 单位转换
|
||||
deffspeed = deffspeed * 0.01f;
|
||||
deffcurvature = deffcurvature * 0.0001f;
|
||||
|
||||
printf("remote speed = %f, remote curvature = %f\n", deffspeed, deffcurvature);
|
||||
|
||||
timerStart(&diff_app_timer,1000,1);//1s调用一次
|
||||
}
|
||||
|
||||
|
||||
// 差速初始化函数
|
||||
void diffAppInit(void)
|
||||
{
|
||||
// 初始化 diff_data
|
||||
memset(&diff_data, 0, sizeof(DiffData));
|
||||
|
||||
// 订阅相关信号
|
||||
subscribe(&un_sw_sample, diffInput); // 急停开关、高压开关
|
||||
subscribe(&un_motor_input1, diffInput);
|
||||
subscribe(&un_motor_input2, diffInput);
|
||||
|
||||
|
||||
subscribe(&un_auto_computer_input, diffInput);
|
||||
subscribe(&un_manual_computer_input, diffInput);
|
||||
subscribe(&un_remote_control_input, diffInput);
|
||||
subscribe(&power_data.pre_charge_finish, preChargeFinish);
|
||||
|
||||
// 初始化速度 PID 控制器
|
||||
initializePid(&speed_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
|
||||
// 设置速度 PID 控制器的参数
|
||||
setPidParameters(&speed_pid,
|
||||
getParam("spd_kp"),
|
||||
getParam("spd_ki"),
|
||||
getParam("spd_kd"),
|
||||
getParam("spd_il"),
|
||||
getParam("spd_ol")
|
||||
);
|
||||
|
||||
// 初始化曲率 PID 控制器
|
||||
initializePid(&yaw_rate_pid, PID_MODE_DERIVATIVE_CALC, 0.0001f);
|
||||
// 设置曲率 PID 控制器的参数
|
||||
setPidParameters(&yaw_rate_pid,
|
||||
getParam("crv_kp"),
|
||||
getParam("crv_ki"),
|
||||
getParam("crv_kd"),
|
||||
getParam("crv_il"),
|
||||
getParam("crv_ol")
|
||||
);
|
||||
|
||||
subscribe(&diff_app_timer, diffParametersInit);
|
||||
timerStart(&diff_app_timer,1000,1);//1s调用一次
|
||||
|
||||
printf("diffControl: diffAppInit OK \n");
|
||||
}
|
||||
84
app/app_differential_drive.h
Normal file
84
app/app_differential_drive.h
Normal file
@@ -0,0 +1,84 @@
|
||||
#ifndef APP_DIFFERENTIAL_DRIVE_H
|
||||
#define APP_DIFFERENTIAL_DRIVE_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#include "app_config.h"
|
||||
|
||||
#define SPEED_FITER_NUM 6
|
||||
|
||||
|
||||
#define SPEED_PID_MODE 0
|
||||
#define THROTTLE_PID_MODE 1
|
||||
|
||||
|
||||
|
||||
#define SPEED_MODE 0x01
|
||||
#define TORQUE_MODE 0x02
|
||||
|
||||
#define MOTOR_MODE TORQUE_MODE
|
||||
|
||||
|
||||
|
||||
#define ALPHA 0.1f // 滤波系数α∈[0.01,0.3]，0.2对应截止频率约10Hz（假设采样周期10ms）
|
||||
#define LOWPASS_FILTER(speed, prev) (ALPHA * (speed) + (1 - ALPHA) * (prev))
|
||||
|
||||
// 状态机内部状态
|
||||
typedef enum
|
||||
{
|
||||
STATE_INIT, ///< 初始状态（转速为0且等待扭矩方向判定）
|
||||
STATE_FORWARD, ///< 正向旋转状态（扭矩为正）
|
||||
STATE_BACKWARD, ///< 反向旋转状态（扭矩为负）
|
||||
} MotorState;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
MODE_MANUAL, // 手动模式
|
||||
MODE_AUTO // 自动模式
|
||||
} ControlMode;
|
||||
|
||||
typedef struct DiffData
|
||||
{
|
||||
ControlMode mode ; // 控制模式
|
||||
MotorState 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; // 当前右后电机速度
|
||||
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; // 输出右电机速度
|
||||
float out_torq[4]; //4个电机扭矩
|
||||
float max_Torq; // 最大扭矩限制
|
||||
float min_Torq; // 最小扭矩限制
|
||||
} 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
|
||||
324
app/app_frm_signal.c
Normal file
324
app/app_frm_signal.c
Normal file
@@ -0,0 +1,324 @@
|
||||
#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 (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("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 50u // 不同信号的订阅者数量
|
||||
#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
|
||||
240
app/app_light.c
Normal file
240
app/app_light.c
Normal file
@@ -0,0 +1,240 @@
|
||||
#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_output1.bit_data.KGF05 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF06 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF10 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = state_value;
|
||||
|
||||
break;
|
||||
case LIGHT_TAIL://尾灯，后面4个灯
|
||||
un_inf_can_kgf_output2.bit_data.KGF12 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF13 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF14 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = state_value;
|
||||
break;
|
||||
case LIGHT_LEFT_TURN://左转向，左边4个灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF06 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF12 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF13 = state_value;
|
||||
break;
|
||||
case LIGHT_RIGHT_TURN://右转向灯，右边4个灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF05 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF10 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF14 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = state_value;
|
||||
break;
|
||||
case LIGHT_BRAKE://刹车灯，四个黄灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF10 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF11 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF13 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF15 = state_value;
|
||||
break;
|
||||
case LIGHT_ALARM://报警灯，四个红灯
|
||||
un_inf_can_kgf_output1.bit_data.KGF05 = state_value;
|
||||
un_inf_can_kgf_output1.bit_data.KGF06 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF12 = state_value;
|
||||
un_inf_can_kgf_output2.bit_data.KGF14 = 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_output1, 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());
|
||||
}
|
||||
145
app/app_param_manage.h
Normal file
145
app/app_param_manage.h
Normal file
@@ -0,0 +1,145 @@
|
||||
#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(high_sw) \
|
||||
X(stop_sw) \
|
||||
X(lightSt) \
|
||||
X(pwr_btn) \
|
||||
X(sleepTm) \
|
||||
X(wakeTm) \
|
||||
X(Ospd_kp) \
|
||||
X(Ospd_ki) \
|
||||
X(Ospd_kd) \
|
||||
X(Ospd_il) \
|
||||
X(Ospd_ol) \
|
||||
X(Ocrv_kp) \
|
||||
X(Ocrv_ki) \
|
||||
X(Ocrv_kd) \
|
||||
X(Ocrv_il) \
|
||||
X(Ocrv_ol) \
|
||||
X(minTorq) \
|
||||
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
|
||||
429
app/app_power.c
Normal file
429
app/app_power.c
Normal file
@@ -0,0 +1,429 @@
|
||||
#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_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = 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.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
break;
|
||||
|
||||
case POWER_NEUTRAL:
|
||||
publishMessage(&power_data.pre_charge_finish, 1);//20250316增加，发送空挡信号，保证电机控制器高压上电后，发送空挡信号
|
||||
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_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = 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.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
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_output2.bit_data.KGF10 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器
|
||||
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.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
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_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = 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.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
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_output2.bit_data.KGF10 = setPowerOn(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = 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.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
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_output2.bit_data.KGF10 = setPowerOff(); // 低压继电器
|
||||
un_inf_can_kgf_output2.bit_data.KGF11 = setPowerOff(); // 低压继电器
|
||||
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.KGF03 = setPowerOn(); // 遥控器
|
||||
un_inf_can_kgf_output2.bit_data.KGF05 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF06 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output1.bit_data.KGF09 = setPowerOn(); // 网络交换机
|
||||
un_inf_can_kgf_output2.bit_data.KGF04 = setPowerOn(); // E3
|
||||
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_NEUTRAL; // 工作
|
||||
power_data.pre_charge_finish = 1; // 预充完成
|
||||
printf("Power: Transitioning from PRE_CHARGE to POWER_NEUTRAL state\n");
|
||||
}
|
||||
break;
|
||||
case POWER_NEUTRAL://20250316增加，发送空挡信号
|
||||
if (power_data.neutral_cnt >= 5) // 运行5次
|
||||
{
|
||||
power_data.neutral_cnt = 0;
|
||||
power_data.current_state = POWER_WORKING; // 工作
|
||||
power_data.pre_charge_finish = 1; // 预充完成
|
||||
printf("Power: Transitioning from POWER_NEUTRAL to WORKING state\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
power_data.neutral_cnt ++;
|
||||
power_data.current_state = POWER_NEUTRAL; // 空挡
|
||||
power_data.pre_charge_finish = 1; // 预充完成
|
||||
}
|
||||
break;
|
||||
|
||||
case POWER_STANDBY:
|
||||
if (power_data.high_voltage_switch == app_open()) // 高压开关断开
|
||||
{
|
||||
power_data.current_state = POWER_SLEEP; // 休眠
|
||||
printf("Power: Transitioning from STANDBY to SLEEP state\n");
|
||||
}
|
||||
else 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.high_voltage_switch == app_open()) // 高压开关断开
|
||||
{
|
||||
power_data.current_state = POWER_SLEEP; // 休眠
|
||||
printf("Power: Transitioning from STANDBY to SLEEP state\n");
|
||||
}
|
||||
else if (power_data.emergency_stop == app_close()) // 急停开关闭合
|
||||
{
|
||||
power_data.current_state = POWER_EMERGENCY; // 急停
|
||||
printf("Power: Transitioning from WORKING to EMERGENCY state\n");
|
||||
|
||||
printf("emergency_stop_switch: %d, remote_emergency_stop: %d\n", power_data.emergency_stop_switch, power_data.remote_emergency_stop); //打印状态
|
||||
printf("remote_stop: %d\n", un_remote_control_input.bit_data.switch_b);
|
||||
}
|
||||
break;
|
||||
case POWER_EMERGENCY:
|
||||
if (power_data.high_voltage_switch == app_open()) // 高压开关断开
|
||||
{
|
||||
power_data.current_state = POWER_SLEEP; // 休眠
|
||||
printf("Power: Transitioning from EMERGENCY to SLEEP state\n");
|
||||
}
|
||||
else 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;//高压开关
|
||||
}
|
||||
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()) );
|
||||
|
||||
// 急停开关有变化，记录到参数
|
||||
if (power_data.old_emergency_stop != power_data.emergency_stop)
|
||||
{
|
||||
power_data.old_emergency_stop = power_data.emergency_stop;
|
||||
setParam("stop_sw", (float)power_data.emergency_stop);
|
||||
}
|
||||
|
||||
// 高压开关状态有变化，记录到参数
|
||||
if (power_data.old_high_voltage_switch != power_data.high_voltage_switch)
|
||||
{
|
||||
power_data.old_high_voltage_switch = power_data.high_voltage_switch;
|
||||
setParam("high_sw", (float)power_data.high_voltage_switch);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 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;
|
||||
//恢复高压开关状态
|
||||
power_data.high_voltage_switch = (uint8_t)getParam("high_sw");
|
||||
power_data.old_high_voltage_switch = power_data.high_voltage_switch;
|
||||
//恢复急停开关状态
|
||||
power_data.emergency_stop = (uint8_t)getParam("stop_sw");
|
||||
power_data.old_emergency_stop = power_data.emergency_stop;
|
||||
|
||||
// 订阅输入信号
|
||||
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");
|
||||
}
|
||||
57
app/app_power.h
Normal file
57
app/app_power.h
Normal file
@@ -0,0 +1,57 @@
|
||||
#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_NEUTRAL,
|
||||
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 old_high_voltage_switch; // 上一次高压开关
|
||||
uint8_t remote_power_switch; // 遥控器电源开关
|
||||
uint8_t remote_emergency_stop; // 遥控器急停开关
|
||||
uint8_t emergency_stop; // 急停状态
|
||||
uint8_t old_emergency_stop; // 上一次急停开关
|
||||
uint8_t pre_charge_finish; // 预充完成标志位
|
||||
uint8_t old_state; // 上一次状态
|
||||
uint8_t neutral_cnt;
|
||||
} PowerSystem;
|
||||
|
||||
// 声明外部变量
|
||||
extern PowerSystem power_data;
|
||||
|
||||
void powerAppInit(void);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // APP_POWER_H
|
||||
396
app/app_request.c
Normal file
396
app/app_request.c
Normal file
@@ -0,0 +1,396 @@
|
||||
#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.desired_curvature * 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.curvature * 10000.0);
|
||||
un_vehicle_Info_output.bit_data.curvature = ((request16_temp << 8) | (request16_temp >> 8));//当前曲率
|
||||
|
||||
request16_temp = (uint16_t)(int16_t)(diff_data.left_motor_speed *6);
|
||||
un_vehicle_Info_output.bit_data.set_left_speed = ((request16_temp << 8) | (request16_temp >> 8));//当前速度
|
||||
|
||||
request16_temp = (uint16_t)(int16_t)(diff_data.right_motor_speed *6);
|
||||
un_vehicle_Info_output.bit_data.set_right_speed = ((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 = SWAP_ENDIAN_16( (uint16_t)((int16_t)(un_motor_input1.bit_data.MotCon_1Signal4) + 30000) );
|
||||
|
||||
// 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 = SWAP_ENDIAN_16 ( (uint16_t)((int16_t)(un_motor_input1.bit_data.MotCon_1Signal3) + 30000) );//左侧轮速
|
||||
// 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");
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
89
app/app_request.h
Normal file
89
app/app_request.h
Normal file
@@ -0,0 +1,89 @@
|
||||
#ifndef APP_REQUEST_H
|
||||
#define APP_REQUEST_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#include "app_config.h"
|
||||
#include "app_differential_drive.h"
|
||||
|
||||
|
||||
|
||||
#define SWAP_ENDIAN_16(x) ((((x) & 0xFF) << 8) | (((x) >> 8) & 0xFF))
|
||||
#define SWAP_ENDIAN_32(x) (((x) << 24) | (((x) & 0xFF00) << 8) | (((x) >> 8) & 0xFF00) | ((x) >> 24))
|
||||
|
||||
|
||||
|
||||
|
||||
//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
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
285
app/app_temp.c
Normal file
285
app/app_temp.c
Normal file
@@ -0,0 +1,285 @@
|
||||
#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 handleTemperatureAlarm(int16_t current_temp, float alarm_temp,
|
||||
float critical_temp, float threshold_temp,
|
||||
TempState *state)
|
||||
{
|
||||
switch (*state)
|
||||
{
|
||||
case TEMP_NORMAL:
|
||||
// 从正常状态进入警告状态的条件
|
||||
if (current_temp > (alarm_temp + threshold_temp))
|
||||
{
|
||||
*state = TEMP_WARNING;
|
||||
printf("Temperature Warning: Activated! Current temp: %d°C\n", current_temp);
|
||||
}
|
||||
// 从正常状态直接进入严重状态的条件
|
||||
else if (current_temp > (critical_temp + threshold_temp))
|
||||
{
|
||||
*state = TEMP_CRITICAL;
|
||||
printf("Temperature Critical: Activated! Current temp: %d°C\n", current_temp);
|
||||
}
|
||||
else
|
||||
{
|
||||
*state = TEMP_NORMAL;
|
||||
}
|
||||
break;
|
||||
|
||||
case TEMP_WARNING:
|
||||
// 从警告状态返回正常状态的条件
|
||||
if (current_temp < (alarm_temp - threshold_temp))
|
||||
{
|
||||
*state = TEMP_NORMAL;
|
||||
printf("Temperature Warning: Deactivated! Current temp: %d°C\n", current_temp);
|
||||
}
|
||||
// 从警告状态进入严重状态的条件
|
||||
else if (current_temp > (critical_temp + threshold_temp))
|
||||
{
|
||||
*state = TEMP_CRITICAL;
|
||||
printf("Temperature Critical: Activated! Current temp: %d°C\n", current_temp);
|
||||
}
|
||||
else
|
||||
{
|
||||
*state = TEMP_WARNING;
|
||||
}
|
||||
break;
|
||||
|
||||
case TEMP_CRITICAL:
|
||||
// 从严重状态返回警告状态的条件
|
||||
if (current_temp < (critical_temp - threshold_temp))
|
||||
{
|
||||
*state = TEMP_WARNING;
|
||||
printf("Temperature Critical: Deactivated! Current temp: %d°C\n", current_temp);
|
||||
}
|
||||
// 从严重状态直接返回正常状态的条件
|
||||
else if (current_temp < (alarm_temp - threshold_temp))
|
||||
{
|
||||
*state = TEMP_NORMAL;
|
||||
printf("Temperature Warning: Deactivated! Current temp: %d°C\n", current_temp);
|
||||
}
|
||||
else
|
||||
{
|
||||
*state = TEMP_CRITICAL;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
*state = TEMP_NORMAL;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// 温度输出处理函数
|
||||
static void tempOutput(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
|
||||
// 电机1风扇 左前
|
||||
switch (temp_data.state[0])
|
||||
{
|
||||
case TEMP_NORMAL:
|
||||
un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_01 = 0;
|
||||
break;
|
||||
case TEMP_WARNING:
|
||||
un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_01 = 5;
|
||||
break;
|
||||
case TEMP_CRITICAL:
|
||||
un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_01 = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
// 电机2风扇 右前
|
||||
switch (temp_data.state[1])
|
||||
{
|
||||
case TEMP_NORMAL:
|
||||
un_inf_can_kgf_output1.bit_data.KGF02 = setFanOff();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_02 = 0;
|
||||
break;
|
||||
case TEMP_WARNING:
|
||||
un_inf_can_kgf_output1.bit_data.KGF02 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_02 = 5;
|
||||
break;
|
||||
case TEMP_CRITICAL:
|
||||
un_inf_can_kgf_output1.bit_data.KGF02 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_02 = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
// 电机3风扇 左后
|
||||
switch (temp_data.state[2])
|
||||
{
|
||||
case TEMP_NORMAL:
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setFanOff();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_07 = 0;
|
||||
break;
|
||||
case TEMP_WARNING:
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_07 = 5;
|
||||
break;
|
||||
case TEMP_CRITICAL:
|
||||
un_inf_can_kgf_output1.bit_data.KGF07 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_07 = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
// 电机4风扇 右后
|
||||
switch (temp_data.state[3])
|
||||
{
|
||||
case TEMP_NORMAL:
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setFanOff();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_08 = 0;
|
||||
break;
|
||||
case TEMP_WARNING:
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_08 = 5;
|
||||
break;
|
||||
case TEMP_CRITICAL:
|
||||
un_inf_can_kgf_output1.bit_data.KGF08 = setFanOn();//电机控制器风扇
|
||||
un_inf_can_kgf_output1.bit_data.pwm_08 = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
// // 电机3风扇
|
||||
// switch (temp_data.state[2])
|
||||
// {
|
||||
// case TEMP_NORMAL:
|
||||
// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOff();//电机控制器风扇
|
||||
// un_inf_can_kgf_output1.bit_data.pwm_01 = 0;
|
||||
// break;
|
||||
// case TEMP_WARNING:
|
||||
// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇
|
||||
// un_inf_can_kgf_output1.bit_data.pwm_01 = 5;
|
||||
// break;
|
||||
// case TEMP_CRITICAL:
|
||||
// un_inf_can_kgf_output1.bit_data.KGF01 = setFanOn();//电机控制器风扇
|
||||
// un_inf_can_kgf_output1.bit_data.pwm_01 = 0;
|
||||
// break;
|
||||
// }
|
||||
|
||||
publishMessage(&un_inf_can_kgf_output1, 1);
|
||||
}
|
||||
|
||||
// 温度状态处理函数
|
||||
static void tempProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
int16_t max_temp[4] = {0,0};
|
||||
|
||||
max_temp[0] = temp_data.current_temp[0];
|
||||
max_temp[1] = temp_data.current_temp[1];
|
||||
|
||||
// printf("motor1 temp: %d, motor2 temp: %d\n", max_temp[0], max_temp[1]);
|
||||
handleTemperatureAlarm(max_temp[0], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[0]);
|
||||
|
||||
handleTemperatureAlarm(max_temp[1], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[1]);
|
||||
|
||||
handleTemperatureAlarm(max_temp[2], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[2]);
|
||||
|
||||
handleTemperatureAlarm(max_temp[3], MOTOR_WARNING_TEMP, MOTOR_CRITICAL_TEMP, MOTOR_THRESHOLD_TEMP, &temp_data.state[3]);
|
||||
|
||||
|
||||
|
||||
// if (max_temp[0] >= 60) // 假设60度为危险温度
|
||||
// {
|
||||
// temp_data.state[0] = TEMP_CRITICAL;
|
||||
// }
|
||||
// else if (max_temp[0] >= 40) // 假设40度为警告温度
|
||||
// {
|
||||
// temp_data.state[0] = TEMP_WARNING;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// temp_data.state[0] = TEMP_NORMAL;
|
||||
// }
|
||||
//
|
||||
//
|
||||
// max_temp[1] = temp_data.current_temp[1];
|
||||
// if (max_temp[1] >= 60) // 假设60度为危险温度
|
||||
// {
|
||||
// temp_data.state[1] = TEMP_CRITICAL;
|
||||
// }
|
||||
// else if (max_temp[1] >= 40) // 假设40度为警告温度
|
||||
// {
|
||||
// temp_data.state[1] = TEMP_WARNING;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// temp_data.state[1] = TEMP_NORMAL;
|
||||
// }
|
||||
//
|
||||
//// printf("motor1 temp: %d, motor2 temp: %d\n", max_temp[0], max_temp[1]);
|
||||
//// printf("motor1 state: %d, motor2 state: %d\n", temp_data.state[0], temp_data.state[1]);
|
||||
|
||||
tempOutput(NULL);
|
||||
|
||||
timerStart(&temp_data.timer, 1000, 1); //1s
|
||||
}
|
||||
|
||||
// 处理输入信号的函数
|
||||
static void tempInput(void *signal_id)
|
||||
{
|
||||
(void)signal_id;
|
||||
// 填充数据
|
||||
if (signal_id == &un_motor_temp1)
|
||||
{
|
||||
temp_data.current_temp[0] = ( (int16_t)(un_motor_temp1.bit_data.controller_temp) - 40);//40偏移量
|
||||
}
|
||||
else if(signal_id == &un_motor_temp2)
|
||||
{
|
||||
temp_data.current_temp[1] = ( (int16_t)(un_motor_temp2.bit_data.controller_temp) - 40);
|
||||
}
|
||||
else if(signal_id == &un_motor_temp3)
|
||||
{
|
||||
temp_data.current_temp[2] = ( (int16_t)(un_motor_temp3.bit_data.controller_temp) - 40);
|
||||
}
|
||||
else if(signal_id == &un_motor_temp4)
|
||||
{
|
||||
temp_data.current_temp[3] = ( (int16_t)(un_motor_temp4.bit_data.controller_temp) - 40);
|
||||
}
|
||||
else{}
|
||||
}
|
||||
|
||||
|
||||
// APP模块的初始化
|
||||
void tempAppInit(void)
|
||||
{
|
||||
// 初始化
|
||||
timerInit(&temp_data.timer);
|
||||
|
||||
memset(&temp_data, 0, sizeof(TempSystem));
|
||||
temp_data.state[0] = TEMP_NORMAL;
|
||||
temp_data.state[1] = TEMP_NORMAL;
|
||||
temp_data.mode = TEMP_MODE_AUTO;
|
||||
temp_data.target_temp = 25; // 默认目标温度25度
|
||||
|
||||
// 订阅输入信号,处理温度逻辑
|
||||
subscribe(&un_motor_temp1, tempInput);
|
||||
subscribe(&un_motor_temp2, tempInput);
|
||||
subscribe(&un_motor_temp3, tempInput);
|
||||
subscribe(&un_motor_temp4, tempInput);
|
||||
|
||||
// 启动定时器,每秒检查一次温度
|
||||
subscribe(&temp_data.timer, tempProcess);
|
||||
timerStart(&temp_data.timer, 1000, 1); //1s
|
||||
|
||||
printf("app_temp: initial OK \n");
|
||||
|
||||
|
||||
}
|
||||
58
app/app_temp.h
Normal file
58
app/app_temp.h
Normal file
@@ -0,0 +1,58 @@
|
||||
#ifndef APP_TEMP_H
|
||||
#define APP_TEMP_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
#define MOTOR_WARNING_TEMP 40
|
||||
#define MOTOR_CRITICAL_TEMP 60
|
||||
#define MOTOR_THRESHOLD_TEMP 5//回滞值
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 定义温度状态枚举
|
||||
typedef enum
|
||||
{
|
||||
TEMP_NORMAL,
|
||||
TEMP_WARNING,
|
||||
TEMP_CRITICAL
|
||||
} TempState;
|
||||
|
||||
// 定义温度控制模式枚举
|
||||
typedef enum
|
||||
{
|
||||
TEMP_MODE_AUTO,
|
||||
TEMP_MODE_MANUAL
|
||||
} TempMode;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
TempState state[8];
|
||||
TempMode mode;
|
||||
int16_t current_temp[8]; // 当前温度,8个不同的温度
|
||||
int16_t target_temp; // 目标温度
|
||||
uint8_t fan_speed[8]; // 风扇速度 8个通道
|
||||
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
|
||||
83
app/app_ultrasonic.c
Normal file
83
app/app_ultrasonic.c
Normal file
@@ -0,0 +1,83 @@
|
||||
#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_ultrasonic.h"
|
||||
|
||||
|
||||
// 定时器结构体
|
||||
Timer ultrasonic_timer;
|
||||
Timer ultrasonic_timer1;
|
||||
|
||||
uint16_t fornt_distance = 0;//mm
|
||||
|
||||
// 定时器信号处理函数
|
||||
void ultrasonicTimer1Process(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||||
|
||||
printf("fornt distance = %d\n",fornt_distance);
|
||||
|
||||
|
||||
timerStart(&ultrasonic_timer1, 1000,1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 定时器信号处理函数
|
||||
void ultrasonicTimerProcess(void *signal_id)
|
||||
{
|
||||
(void)signal_id; // 标记变量为已使用，避免编译器警告
|
||||
|
||||
un_ultrasonic_output1.bit_data.node = ULTRASONIC_FRONT_NODE;
|
||||
un_ultrasonic_output1.bit_data.function_code = ULTRASONIC_READ;
|
||||
un_ultrasonic_output1.bit_data.register_address = REALTIME_DISTANCE_REG;
|
||||
un_ultrasonic_output1.bit_data.data = 0x100;
|
||||
publishMessage(&un_ultrasonic_output1, 1);
|
||||
|
||||
timerStart(&ultrasonic_timer, 200,1);
|
||||
}
|
||||
|
||||
void ultrasonicInput(void *signal_id)
|
||||
{
|
||||
if(signal_id == &un_ultrasonic_input1)
|
||||
{
|
||||
fornt_distance = SWAP_ENDIAN_16(un_ultrasonic_input1.bit_data.data);//高低字节交换
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// APP模块的初始化
|
||||
void ultrasonicAppInit(void)
|
||||
{
|
||||
// 初始化定时器
|
||||
timerInit(&ultrasonic_timer);
|
||||
timerInit(&ultrasonic_timer1);
|
||||
// 订阅定时器信号
|
||||
subscribe(&ultrasonic_timer, ultrasonicTimerProcess);
|
||||
subscribe(&ultrasonic_timer1, ultrasonicTimer1Process);
|
||||
|
||||
subscribe(&un_ultrasonic_input1, ultrasonicInput);
|
||||
printf("ultrasonicAPP: initial OK %d\n",getCurrentTime());
|
||||
timerStart(&ultrasonic_timer, 200,1);
|
||||
timerStart(&ultrasonic_timer1, 1000,1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
75
app/app_ultrasonic.h
Normal file
75
app/app_ultrasonic.h
Normal file
@@ -0,0 +1,75 @@
|
||||
#ifndef ULTRASONIC_H
|
||||
#define ULTRASONIC_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
#define ULTRASONIC_FRONT_NODE 0x1
|
||||
#define ULTRASONIC_BACK_NODE 0x2
|
||||
|
||||
#define ULTRASONIC_ID_1 0x520 + ULTRASONIC_FRONT_NODE
|
||||
#define ULTRASONIC_ID_2 0x520 + ULTRASONIC_BACK_NODE
|
||||
|
||||
#define ULTRASONIC_READ 0x03
|
||||
#define ULTRASONIC_WRITE 0x06
|
||||
|
||||
|
||||
|
||||
|
||||
#define SOFTWARE_VERSION_REG 0x0000 // ģ<><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>汾<EFBFBD><E6B1BE>(ʮ<><CAAE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ), <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: uint16_t
|
||||
|
||||
|
||||
#define PROCESSED_DISTANCE_REG 0x0100 /*
|
||||
* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><C4BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ(mm), <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: uint16_t
|
||||
* <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>࣬<EFBFBD>㷨<EFBFBD><E3B7A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
* <20><>Ӧʱ<D3A6>䣺190-750ms(<28><><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD>)
|
||||
*/
|
||||
|
||||
#define REALTIME_DISTANCE_REG 0x0101 /*
|
||||
* ʵʱ<CAB5><CAB1><EFBFBD>β<EFBFBD><CEB2><EFBFBD>ֵ(mm), <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: uint16_t
|
||||
* <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><F3B4A5B7><EFBFBD><EFBFBD>β<EFBFBD><CEB2><EFBFBD>
|
||||
* <20><>Ӧʱ<D3A6>䣺15-140ms(<28><><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD>)
|
||||
*/
|
||||
|
||||
#define TEMPERATURE_REG 0x0102 /*
|
||||
* <20>¶ȴ<C2B6><C8B4><EFBFBD><EFBFBD><EFBFBD>ֵ, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: int16_t
|
||||
* <20><>λ<EFBFBD><CEBB>0.1<EFBFBD><EFBFBD> (ԭʼֵ<CABC><D6B5>/10), <20>ֱ<EFBFBD><D6B1>ʣ<EFBFBD>0.5<EFBFBD><EFBFBD>
|
||||
* <20><>Ӧʱ<D3A6>䣺15-140ms(<28><><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD>)
|
||||
*/
|
||||
|
||||
#define ECHO_TIME_REG 0x010A /*
|
||||
* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD>ʱ<EFBFBD><CAB1>ԭʼֵ(<28><>s), <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: uint16_t
|
||||
* <20><><EFBFBD>뻻<EFBFBD>㣺ֵ/5.75 = <20><><EFBFBD><EFBFBD><D7BE><EFBFBD>
|
||||
* <20><>Ӧʱ<D3A6>䣺15-140ms(<28><><EFBFBD><EFBFBD><EFBFBD>̶<EFBFBD><CCB6><EFBFBD>)
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#define SWAP_ENDIAN_16(x) ((((x) & 0xFF) << 8) | (((x) >> 8) & 0xFF))
|
||||
#define SWAP_ENDIAN_32(x) (((x) << 24) | (((x) & 0xFF00) << 8) | (((x) >> 8) & 0xFF00) | ((x) >> 24))
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void ultrasonicAppInit(void);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // ULTRASONIC_H
|
||||
Reference in New Issue
Block a user