77 lines
2.3 KiB
C
77 lines
2.3 KiB
C
/**
|
|
* @file speed_planner.h
|
|
* @brief 斜坡速度规划器
|
|
* @details 提供最大加速度和最大速度限制,防止目标位移起飞
|
|
*/
|
|
|
|
#ifndef SPEED_PLANNER_H
|
|
#define SPEED_PLANNER_H
|
|
|
|
#include <stdint.h>
|
|
|
|
/* 速度规划器参数结构体 */
|
|
typedef struct {
|
|
float max_velocity; /* 最大速度 (m/s) */
|
|
float max_acceleration; /* 最大加速度 (m/s²) */
|
|
float max_position_error; /* 最大位置误差 (m), 防止位移起飞 */
|
|
} SpeedPlanner_Params_t;
|
|
|
|
/* 速度规划器状态结构体 */
|
|
typedef struct {
|
|
float current_velocity; /* 当前速度 (m/s) */
|
|
float target_velocity; /* 目标速度 (m/s) */
|
|
float planned_velocity; /* 规划后的速度 (m/s) */
|
|
float current_position; /* 当前位置 (m) */
|
|
float target_position; /* 目标位置 (m) */
|
|
float planned_position; /* 规划后的位置 (m) */
|
|
|
|
SpeedPlanner_Params_t param; /* 参数 */
|
|
} SpeedPlanner_t;
|
|
|
|
/**
|
|
* @brief 初始化速度规划器
|
|
* @param planner 规划器结构体指针
|
|
* @param params 参数结构体指针
|
|
* @return 0:成功, -1:失败
|
|
*/
|
|
int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, const SpeedPlanner_Params_t *params);
|
|
|
|
/**
|
|
* @brief 重置速度规划器
|
|
* @param planner 规划器结构体指针
|
|
* @param current_position 当前位置
|
|
* @param current_velocity 当前速度
|
|
*/
|
|
void SpeedPlanner_Reset(SpeedPlanner_t *planner, float current_position, float current_velocity);
|
|
|
|
/**
|
|
* @brief 更新速度规划器
|
|
* @param planner 规划器结构体指针
|
|
* @param target_velocity 目标速度 (m/s)
|
|
* @param current_position 当前位置 (m)
|
|
* @param current_velocity 当前速度 (m/s)
|
|
* @param dt 时间间隔 (s)
|
|
* @return 规划后的速度 (m/s)
|
|
*/
|
|
float SpeedPlanner_Update(SpeedPlanner_t *planner,
|
|
float target_velocity,
|
|
float current_position,
|
|
float current_velocity,
|
|
float dt);
|
|
|
|
/**
|
|
* @brief 获取规划后的目标位置
|
|
* @param planner 规划器结构体指针
|
|
* @return 规划后的目标位置 (m)
|
|
*/
|
|
float SpeedPlanner_GetPlannedPosition(const SpeedPlanner_t *planner);
|
|
|
|
/**
|
|
* @brief 获取规划后的速度
|
|
* @param planner 规划器结构体指针
|
|
* @return 规划后的速度 (m/s)
|
|
*/
|
|
float SpeedPlanner_GetPlannedVelocity(const SpeedPlanner_t *planner);
|
|
|
|
#endif // SPEED_PLANNER_H
|