Da Vinci Firmware 1
Firmware for the DaVinci-M rocket avionics board.
Loading...
Searching...
No Matches
Functions
cdEvaluation.h File Reference
Core Flight Logic » Control Algorithms

Performs 2D bilinear interpolation to estimate the coefficient of drag (Cd). More...

#include "rtwtypes.h"
#include <stddef.h>
#include <stdlib.h>
Include dependency graph for cdEvaluation.h:
This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Functions

double cdEvaluation (const double dep_states[6], const double mach_states[5], const double cd_table[30], double dep, double mach)
 Estimates the coefficient of drag (Cd) via 2D bilinear interpolation.
 

Detailed Description

Performs 2D bilinear interpolation to estimate the coefficient of drag (Cd).

Author
Francesco Abate, MSA (Originally generated by MATLAB Coder)
Version
24.2 (MATLAB Coder version)
Date
2025-05-06

This header declares the cdEvaluation function, which calculates the rocket's coefficient of drag (Cd) based on its current Mach number and airbrake deployment angle.

The function implements a 2D bilinear interpolation algorithm using a pre-defined lookup table of Cd values. It finds the four nearest points in the table surrounding the current state and interpolates between them to produce a smooth Cd estimate.

This functionality is a core component of the MPC controller, called by the optimalAngle function to predict the rocket's aerodynamic behavior. The implementation was provided by the MSA subteam.

Definition in file cdEvaluation.h.

Function Documentation

◆ cdEvaluation()

double cdEvaluation ( const double  dep_states[6],
const double  mach_states[5],
const double  cd_table[30],
double  dep,
double  mach 
)

Estimates the coefficient of drag (Cd) via 2D bilinear interpolation.

Parameters
[in]dep_statesAn array of 6 discrete airbrake deployment angles (the "y-axis" of the lookup table).
[in]mach_statesAn array of 5 discrete Mach numbers (the "x-axis" of the lookup table).
[in]cd_tableA 1D array of 30 Cd values, representing the 6x5 lookup grid. The data must be ordered row-major (all Mach values for the first dep_state, etc.).
[in]depThe current, continuous airbrake deployment angle to interpolate at.
[in]machThe current, continuous speed expressed as a Mach number to interpolate at.
Returns
The interpolated coefficient of drag (Cd), a dimensionless value.

Definition at line 30 of file cdEvaluation.c.

32{
33 double b_b_tmp_data;
34 double b_tmp_data;
35 double c_b_tmp_data;
36 double cd;
37 double cd_table_data;
38 double d_b_tmp_data;
39 double tmp_data;
40 double x1_data;
41 double x2_data;
42 double y1_data;
43 double y2_data;
44 double y2_idx_data;
45 int cd_table_size[2];
46 int tmp_size[2];
47 int i;
48 int i1;
49 int loop_ub;
50 int x1_idx_data;
51 int y1_idx_data;
52 int y1_size;
53 int y2_size;
54 bool b_dep_states[6];
55 bool b_mach_states[5];
56 for (i = 0; i < 6; i++) {
57 b_dep_states[i] = (dep_states[i] <= dep);
58 }
59 i1 = eml_find(b_dep_states, (int *)&y1_idx_data);
60 loop_ub = i1;
61 for (i = 0; i < i1; i++) {
62 x1_idx_data = y1_idx_data;
63 }
64 for (i = 0; i < i1; i++) {
65 cd = (double)x1_idx_data + 1.0;
66 }
67 /* Find indices of the surrounding y-coordinates */
68 for (i = 0; i < 5; i++) {
69 b_mach_states[i] = (mach_states[i] <= mach);
70 }
71 i1 = b_eml_find(b_mach_states, (int *)&y1_idx_data);
72 for (i = 0; i < i1; i++) {
73 y2_idx_data = (double)y1_idx_data + 1.0;
74 }
75 /* Extract the surrounding x and y coordinates */
76 for (i = 0; i < loop_ub; i++) {
77 x1_data = dep_states[x1_idx_data - 1];
78 x2_data = dep_states[(int)cd - 1];
79 }
80 for (i = 0; i < i1; i++) {
81 y1_data = mach_states[y1_idx_data - 1];
82 y2_data = mach_states[(int)y2_idx_data - 1];
83 }
84 /* Extract the corresponding z-values */
85 /* Perform bilinear interpolation */
86 for (i = 0; i < loop_ub; i++) {
87 b_tmp_data = x2_data - dep;
88 }
89 for (i = 0; i < i1; i++) {
90 b_b_tmp_data = y2_data - mach;
91 }
92 for (i = 0; i < loop_ub; i++) {
93 c_b_tmp_data = dep - x1_data;
94 }
95 for (i = 0; i < i1; i++) {
96 d_b_tmp_data = mach - y1_data;
97 y2_data -= y1_data;
98 }
99 cd_table_size[0] = i1;
100 cd_table_size[1] = loop_ub;
101 if (loop_ub - 1 >= 0) {
102 for (i = 0; i < i1; i++) {
103 cd_table_data = cd_table[(y1_idx_data + 5 * (x1_idx_data - 1)) - 1];
104 }
105 }
106 for (i = 0; i < loop_ub; i++) {
107 x2_data = (x2_data - x1_data) * y2_data;
108 }
109 mrdiv_2((double *)&x2_data, loop_ub, (double *)&tmp_data, tmp_size);
110 y2_size = mtimes((double *)&cd_table_data, cd_table_size,
111 (double *)&b_tmp_data, (double *)&y2_data);
112 if (loop_ub - 1 >= 0) {
113 for (i = 0; i < i1; i++) {
114 cd_table_data = cd_table[(y1_idx_data + 5 * ((int)cd - 1)) - 1];
115 }
116 }
117 y1_size = mtimes((double *)&cd_table_data, cd_table_size,
118 (double *)&c_b_tmp_data, (double *)&y1_data);
119 if (loop_ub - 1 >= 0) {
120 for (i = 0; i < i1; i++) {
121 cd_table_data = cd_table[((int)y2_idx_data + 5 * (x1_idx_data - 1)) - 1];
122 }
123 }
124 x1_idx_data = mtimes((double *)&cd_table_data, cd_table_size,
125 (double *)&b_tmp_data, (double *)&x2_data);
126 if (loop_ub - 1 >= 0) {
127 for (i = 0; i < i1; i++) {
128 cd_table_data = cd_table[((int)y2_idx_data + 5 * ((int)cd - 1)) - 1];
129 }
130 }
131 y1_idx_data = mtimes((double *)&cd_table_data, cd_table_size,
132 (double *)&c_b_tmp_data, (double *)&x1_data);
133 if (y2_size == 1) {
134 i = y1_size;
135 } else {
136 i = y2_size;
137 }
138 if (i == 1) {
139 i1 = x1_idx_data;
140 } else {
141 i1 = i;
142 }
143 if ((y2_size == y1_size) && (i == x1_idx_data) && (i1 == y1_idx_data)) {
144 cd = 0.0;
145 loop_ub = tmp_size[1];
146 for (i = 0; i < loop_ub; i++) {
147 cd += tmp_data * (((y2_data * b_b_tmp_data + y1_data * b_b_tmp_data) +
148 x2_data * d_b_tmp_data) +
149 x1_data * d_b_tmp_data);
150 }
151 } else {
152 cd = binary_expand_op((double *)&tmp_data, tmp_size, (double *)&y2_data,
153 &y2_size, (double *)&b_b_tmp_data, (double *)&y1_data,
154 &y1_size, (double *)&x2_data, &x1_idx_data,
155 (double *)&d_b_tmp_data, (double *)&x1_data,
156 &y1_idx_data);
157 }
158 return cd;
159}
binary_expand_op
double binary_expand_op(const double in1_data[], const int in1_size[2], const double in2_data[], const int *in2_size, const double in3_data[], const double in4_data[], const int *in4_size, const double in5_data[], const int *in5_size, const double in6_data[], const double in7_data[], const int *in7_size)
Performs a fused set of element-wise operations for 2D interpolation.
Definition eml_mtimes_helper.c:31
b_eml_find
int b_eml_find(const boolean_T x[5], int i_data[])
Finds the index of the last true element in a 5-element boolean array.
Definition find.c:21
eml_find
int eml_find(const boolean_T x[6], int i_data[])
Finds the index of the last true element in a 6-element boolean array.
Definition find.c:51
mach_states
const double mach_states[]
Definition main.c:261
cd_table
static double cd_table[]
Definition main.c:263
mrdiv_2
void mrdiv_2(const double B_data[], int B_size, double Y_data[], int Y_size[2])
Performs element-wise division of a scalar by each element of a matrix.
Definition mrdivide_helper.c:59
mtimes
int mtimes(const double A_data[], const int A_size[2], const double B_data[], double C_data[])
Performs matrix multiplication: C = A * B.
Definition mtimes.c:23

References b_eml_find(), binary_expand_op(), cd_table, eml_find(), mach_states, mrdiv_2(), and mtimes().

Referenced by optimalAngle().

Here is the call graph for this function:
Here is the caller graph for this function: