public override void InitializeDependencies(Dictionary <Type, AutopilotModule> modules)
{
gui_list[0] = pc = modules[typeof(PitchAngularVelocityController)] as PitchAngularVelocityController;
gui_list[1] = rc = modules[typeof(RollAngularVelocityController)] as RollAngularVelocityController;
gui_list[2] = yvc = modules[typeof(YawAngularVelocityController)] as YawAngularVelocityController;
gui_list[3] = yc = modules[typeof(SideslipController)] as SideslipController;
gui_list[4] = tc = modules[typeof(ProgradeThrustController)] as ProgradeThrustController;
im = modules[typeof(FlightModel)] as FlightModel;
}
public override void InitializeDependencies(Dictionary <Type, AutopilotModule> modules)
{
imodel = modules[typeof(FlightModel)] as FlightModel;
aoa_c = modules[typeof(PitchAoAController)] as PitchAoAController;
side_c = modules[typeof(SideslipController)] as SideslipController;
pitch_c = modules[typeof(PitchAngularVelocityController)] as PitchAngularVelocityController;
roll_c = modules[typeof(RollAngularVelocityController)] as RollAngularVelocityController;
yaw_c = modules[typeof(YawAngularVelocityController)] as YawAngularVelocityController;
}
public override void InitializeDependencies(Dictionary <Type, AutopilotModule> modules)
{
imodel = modules[typeof(FlightModel)] as FlightModel;
if (axis == PITCH)
{
v_controller = modules[typeof(PitchAngularVelocityController)] as PitchYawAngularVelocityController;
lin_model_gen = imodel.pitch_rot_model_gen;
lin_model = imodel.pitch_rot_model;
}
else
if (axis == YAW)
{
v_controller = modules[typeof(YawAngularVelocityController)] as PitchYawAngularVelocityController;
lin_model_gen = imodel.yaw_rot_model_gen;
lin_model = imodel.yaw_rot_model;
}
}
public override void InitializeDependencies(Dictionary <Type, AutopilotModule> modules)
{
this.imodel = modules[typeof(FlightModel)] as FlightModel;
}
public override void InitializeDependencies(Dictionary <Type, AutopilotModule> modules)
{
imodel = modules[typeof(FlightModel)] as FlightModel;
dir_c = modules[typeof(DirectorController)] as DirectorController;
thrust_c = modules[typeof(ProgradeThrustController)] as ProgradeThrustController;
}
protected override float get_required_input(FlightCtrlState cntrl, float target_value)
{
float new_input = 0.0f;
if (AtmosphereAutopilot.AeroModel == AtmosphereAutopilot.AerodinamycsModel.FAR)
{
authority = imodel.roll_rot_model.B[0, 0];
// check if we have inadequate model authority
if (authority < 1e-4)
{
float user_input = cntrl.roll;
if (user_input != 0.0f)
{
return(user_input);
}
else
{
return(Common.Clampf(target_value, 1.0f));
}
}
// get model prediction for next frame
cur_state[0, 0] = imodel.AngularVel(ROLL);
cur_state[1, 0] = imodel.ControlSurfPos(ROLL);
cur_state[2, 0] = imodel.GimbalPos(ROLL);
input_mat[0, 0] = cur_state[1, 0];
input_mat[1, 0] = FlightModel.far_exponential_blend(imodel.ControlSurfPos(YAW), yc.output);
input_mat[2, 0] = imodel.AoA(YAW);
double cur_acc_prediction = imodel.roll_rot_model.eval_row(0, cur_state, input_mat);
double acc_error = target_value - cur_acc_prediction;
new_input = (float)(imodel.ControlSurfPos(ROLL) + acc_error / authority);
new_input = Common.Clampf(new_input, 1.0f);
// Exponential blend can mess with rotation model, let's check it
if (FlightModel.far_blend_collapse(imodel.ControlSurfPos(ROLL), new_input))
{
// we need to recalculate new_input according to undelayed model
authority = imodel.roll_rot_model_undelayed.B[0, 0];
new_input = (float)(imodel.ControlSurfPos(ROLL) + acc_error / authority);
new_input = Common.Clampf(new_input, 1.0f);
}
model_predicted_acc = cur_acc_prediction + authority * (new_input - cur_state[1, 0]);
}
else
{
authority = imodel.roll_rot_model_undelayed.B[0, 0];
// check if we have inadequate model authority
if (authority < 1e-4)
{
float user_input = cntrl.roll;
if (user_input != 0.0f)
{
return(user_input);
}
else
{
return(Common.Clampf(target_value, 1.0f));
}
}
// get model prediction for next frame
cur_state[0, 0] = imodel.AngularVel(ROLL);
cur_state[1, 0] = imodel.GimbalPos(ROLL);
input_mat[0, 0] = imodel.ControlSurfPos(ROLL);
input_mat[1, 0] = FlightModel.stock_actuator_blend(imodel.ControlSurfPos(YAW), yc.output);
input_mat[2, 0] = imodel.AoA(YAW);
double cur_acc_prediction = imodel.roll_rot_model_undelayed.eval_row(0, cur_state, input_mat);
double acc_error = target_value - cur_acc_prediction;
new_input = (float)(input_mat[0, 0] + acc_error / authority);
new_input = Common.Clampf(new_input, 1.0f);
if (Math.Abs(new_input - input_mat[0, 0]) / TimeWarp.fixedDeltaTime > SyncModuleControlSurface.CSURF_SPD)
{
// we're exceeding control surface speed
cur_state[2, 0] = cur_state[1, 0];
cur_state[1, 0] = input_mat[0, 0];
if (new_input > input_mat[0, 0])
{
cur_state[1, 0] += TimeWarp.fixedDeltaTime * SyncModuleControlSurface.CSURF_SPD;
}
else
{
cur_state[1, 0] -= TimeWarp.fixedDeltaTime * SyncModuleControlSurface.CSURF_SPD;
}
cur_state[1, 0] = Common.Clamp(cur_state[1, 0], 1.0);
input_mat[0, 0] = cur_state[1, 0];
cur_acc_prediction = imodel.roll_rot_model.eval_row(0, cur_state, input_mat);
acc_error = target_value - cur_acc_prediction;
authority = imodel.roll_rot_model.B[0, 0];
new_input = (float)(cur_state[1, 0] + acc_error / authority);
new_input = Common.Clampf(new_input, 1.0f);
}
model_predicted_acc = cur_acc_prediction + authority * (new_input - input_mat[0, 0]);
}
if (write_telemetry)
{
prediction_writer.Write(model_predicted_acc.ToString("G8") + ',');
}
return(new_input);
}
