35 lines
1.1 KiB
C
35 lines
1.1 KiB
C
#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);
|
|
}
|