feat(example): 新增飞行控制 C 示例项目
- 提供传感器、滤波、控制律和执行器等模块化示例 - 包含约 35 个函数用于代码分析与文档生成流程测试 - 使用 Makefile 支持最小化编译验证
This commit is contained in:
34
src/control.c
Normal file
34
src/control.c
Normal file
@@ -0,0 +1,34 @@
|
||||
#include "flight_app.h"
|
||||
|
||||
void Control_Init(CommandState *command) {
|
||||
command->target_speed = 200.0f;
|
||||
command->target_altitude = 2500.0f;
|
||||
command->pitch_limit = 10.0f;
|
||||
command->auto_mode = 0U;
|
||||
}
|
||||
|
||||
float Control_ComputeSpeedError(const CommandState *command, const SensorData *data) {
|
||||
return command->target_speed - data->air_speed;
|
||||
}
|
||||
|
||||
float Control_ComputeAltitudeError(const CommandState *command, const SensorData *data) {
|
||||
return command->target_altitude - data->altitude;
|
||||
}
|
||||
|
||||
float Control_PidStep(float error, float gain, float limit) {
|
||||
float output = error * gain;
|
||||
if (output > limit) {
|
||||
return limit;
|
||||
}
|
||||
if (output < -limit) {
|
||||
return -limit;
|
||||
}
|
||||
return output;
|
||||
}
|
||||
|
||||
void Control_Update(const CommandState *command, const SensorData *data, ActuatorState *actuator) {
|
||||
float speed_error = Control_ComputeSpeedError(command, data);
|
||||
float altitude_error = Control_ComputeAltitudeError(command, data);
|
||||
actuator->throttle = Control_PidStep(speed_error, 0.01f, 1.0f);
|
||||
actuator->elevator = Control_PidStep(altitude_error, 0.002f, command->pitch_limit);
|
||||
}
|
||||
Reference in New Issue
Block a user