optimalAngle.c

Go to the documentation of this file.

/*
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 * File: optimalAngle.c
 *
 * MATLAB Coder version            : 24.2
 * C/C++ source code generated on  : 06-May-2025 18:24:48
 */
 
/* Include Files */
#include "optimalAngle.h"
#include "cdEvaluation.h"
#include "rt_nonfinite.h"
#include "rt_nonfinite.h"
#include <math.h>
 
/* Function Declarations */
static double rt_powd_snf(double u0, double u1);
 
/* Function Definitions */
/*
 * Arguments    : double u0
 *                double u1
 * Return Type  : double
 */
static double rt_powd_snf(double u0, double u1)
{
  double y;
  if (rtIsNaN(u0) || rtIsNaN(u1)) {
    y = rtNaN;
  } else {
    double d;
    y = fabs(u0);
    d = fabs(u1);
    if (rtIsInf(u1)) {
      if (y == 1.0) {
        y = 1.0;
      } else if (y > 1.0) {
        if (u1 > 0.0) {
          y = rtInf;
        } else {
          y = 0.0;
        }
      } else if (u1 > 0.0) {
        y = 0.0;
      } else {
        y = rtInf;
      }
    } else if (d == 0.0) {
      y = 1.0;
    } else if (d == 1.0) {
      if (u1 > 0.0) {
        y = u0;
      } else {
        y = 1.0 / u0;
      }
    } else if (u1 == 2.0) {
      y = u0 * u0;
    } else if ((u1 == 0.5) && (u0 >= 0.0)) {
      y = sqrt(u0);
    } else if ((u0 < 0.0) && (u1 > floor(u1))) {
      y = rtNaN;
    } else {
      y = pow(u0, u1);
    }
  }
  return y;
}
 
/*
 * Arguments    : double sim_time_step
 *                double altitude
 *                double temperature
 *                double velocity
 *                double pressure
 *                double current_delta
 *                double tilt_angle
 *                double target_apogee
 *                double tolerance
 *                const double mach_states[5]
 *                const double deployment_states[6]
 *                const double cd_table[30]
 * Return Type  : double
 */
double optimalAngle(double sim_time_step, double altitude, double temperature,
                    double velocity, double pressure, double current_delta,
                    double tilt_angle, double target_apogee, double tolerance,
                    const double mach_states[5],
                    const double deployment_states[6],
                    const double cd_table[30])
{
  double dep_max;
  double dep_min;
  double max_change;
  double opt_dep;
  /* placeholder */
  dep_min = 0.0;
  /* deg */
  dep_max = 173.0;
  /* deg */
  max_change = 182.6 * sim_time_step;
  while (dep_max - dep_min > tolerance) {
    double delta;
    double elapsed_time;
    double pressure_current;
    double temperature_current;
    double test_delta;
    double v_current;
    double x_dot_predicted;
    double y_current;
    double y_dot_current;
    test_delta = (dep_max + dep_min) / 2.0;
    delta = current_delta;
    elapsed_time = 0.0;
    temperature_current = temperature;
    pressure_current = pressure;
    y_current = altitude;
    v_current = velocity;
    x_dot_predicted = velocity * sin(tilt_angle);
    y_dot_current = velocity * cos(tilt_angle);
    while (y_dot_current > 0.0) {
      double speed_of_sound;
      double x_dotdot;
      double y_dotdot;
      if (elapsed_time > 0.1) {
        opt_dep = test_delta - delta;
        if (fabs(opt_dep) > max_change) {
          if (rtIsNaN(opt_dep)) {
            opt_dep = rtNaN;
          } else if (opt_dep < 0.0) {
            opt_dep = -1.0;
          } else {
            opt_dep = (opt_dep > 0.0);
          }
          delta += opt_dep * max_change;
        } else {
          delta = test_delta;
        }
      }
      speed_of_sound = sqrt(401.79999999999995 * temperature_current);
      pressure_current = 0.5 *
                         (pressure_current / (287.0 * temperature_current)) *
                         0.014102609421964583;
      opt_dep = pressure_current * cdEvaluation(deployment_states, mach_states,
                                                cd_table, delta,
                                                v_current / speed_of_sound);
      x_dotdot = -opt_dep * x_dot_predicted * v_current / 23.458000008756152;
      y_dotdot =
          -opt_dep * y_dot_current * v_current / 23.458000008756152 - 9.81;
      opt_dep = x_dot_predicted + x_dotdot * sim_time_step;
      temperature_current = y_dot_current + y_dotdot * sim_time_step;
      v_current =
          sqrt(opt_dep * opt_dep + temperature_current * temperature_current);
      opt_dep = pressure_current * cdEvaluation(deployment_states, mach_states,
                                                cd_table, delta,
                                                v_current / speed_of_sound);
      x_dot_predicted += 0.5 *
                         (x_dotdot + -opt_dep * x_dot_predicted * v_current /
                                         23.458000008756152) *
                         sim_time_step;
      opt_dep =
          y_dot_current + 0.5 *
                              (y_dotdot + (-opt_dep * y_dot_current *
                                               v_current / 23.458000008756152 -
                                           9.81)) *
                              sim_time_step;
      y_current += 0.5 * (y_dot_current + opt_dep) * sim_time_step;
      y_dot_current = opt_dep;
      v_current = sqrt(x_dot_predicted * x_dot_predicted + opt_dep * opt_dep);
      temperature_current = temperature - 0.00649 * (y_current - altitude);
      pressure_current =
          pressure * rt_powd_snf(temperature_current / temperature, 5.2561);
      elapsed_time += sim_time_step;
    }
    if (y_current > target_apogee) {
      dep_min = test_delta;
    } else {
      dep_max = test_delta;
    }
  }
  return 180 - ((dep_min + dep_max) / 2.0);
}
 
/*
 * File trailer for optimalAngle.c
 *
 * [EOF]
 */