optimalAngle.c File Reference

#include "optimalAngle.h"
#include "cdEvaluation.h"
#include "rt_nonfinite.h"
#include <math.h>

Include dependency graph for optimalAngle.c:

Go to the source code of this file.

Functions
static double	rt_powd_snf (double u0, double u1)
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])
	Calculates the optimal constant airbrake angle to reach a target apogee.

Function Documentation

optimalAngle()

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]
	)

Calculates the optimal constant airbrake angle to reach a target apogee.

Parameters

[in]	sim_time_step	The duration of a single time step used inside the internal trajectory simulation, in seconds (s).
[in]	altitude	The rocket's current altitude above ground level in meters (m).
[in]	temperature	Current ambient air temperature in Kelvin (K).
[in]	velocity	The rocket's current total speed (magnitude of velocity vector) in m/s.
[in]	pressure	Current ambient air pressure in Pascals (Pa).
[in]	current_delta	The current, real-world deployment angle of the airbrakes in degrees. (Note: May not be used in this version).
[in]	tilt_angle	The angle of the rocket's velocity vector with respect to the vertical axis in radians.
[in]	target_apogee	The desired final apogee for the rocket in meters (m).
[in]	tolerance	The desired precision for the binary search, in degrees. The search stops when dep_max - dep_min <= tolerance.
[in]	mach_states	An array of 5 discrete Mach numbers that form the x-axis of the Cd lookup table.
[in]	deployment_states	An array of 6 discrete airbrake deployment angles that form the y-axis of the Cd lookup table.
[in]	cd_table	A 1D array of 30 Coefficient of Drag (Cd) values, representing the 6x5 lookup grid (row-major).

Returns

The calculated optimal airbrake deployment angle in degrees.

Definition at line 86 of file optimalAngle.c.

{
  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);
}

References altitude, cd_table, cdEvaluation(), current_delta, deployment_states, mach_states, rt_powd_snf(), rtIsNaN(), rtNaN, sim_time_step, target_apogee, tilt_angle, tolerance, and velocity.

Referenced by StartMPC_Task().

Here is the call graph for this function:

Here is the caller graph for this function:

rt_powd_snf()

static double rt_powd_snf ( double  u0, double  u1  )

static

Definition at line 27 of file optimalAngle.c.

{
  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;
}

References rtInf, rtIsInf(), rtIsNaN(), and rtNaN.

Referenced by optimalAngle().

Here is the call graph for this function:

Here is the caller graph for this function: