Da Vinci Firmware 1
Firmware for the DaVinci-M rocket avionics board.
Loading...
Searching...
No Matches
attitude_estimation.c
Go to the documentation of this file.
1/*
2 * Academic License - for use in teaching, academic research, and meeting
3 * course requirements at degree granting institutions only. Not for
4 * government, commercial, or other organizational use.
5 * File: attitude_estimation.c
6 *
7 * MATLAB Coder version : 24.2
8 * C/C++ source code generated on : 02-Jun-2025 10:18:55
9 */
10
11/* Include Files */
12#include "attitude_estimation.h"
13#include "eye.h"
14#include <math.h>
15
16/* Function Definitions */
17/*
18 * Arguments : const double quat[4]
19 * double ts
20 * double omega_x
21 * double omega_y
22 * double omega_z
23 * double *alpha
24 * double quat_new[4]
25 * Return Type : void
26 */
27void attitude_estimation(const double quat[4], double ts, double omega_x,
28 double omega_y, double omega_z, double *alpha,
29 double quat_new[4])
30{
31 double dv[16];
32 double a;
33 double a_idx_0_tmp;
34 double a_idx_1_tmp;
35 double a_idx_2_tmp;
36 double a_idx_3_tmp;
37 double b_a_idx_0_tmp;
38 double b_a_idx_2_tmp;
39 double c_a_idx_0_tmp;
40 int i;
41 a = 0.5 * ts;
42 eye_attitude(dv);
43 a_idx_0_tmp = a * 0.0;
44 a_idx_1_tmp = a * -omega_x;
45 a_idx_2_tmp = a * -omega_y;
46 a_idx_3_tmp = a * -omega_z;
47 b_a_idx_0_tmp = a * omega_x;
48 b_a_idx_2_tmp = a * omega_z;
49 c_a_idx_0_tmp = a * omega_y;
50 dv[0] += a_idx_0_tmp;
51 dv[1] += b_a_idx_0_tmp;
52 dv[2] += c_a_idx_0_tmp;
53 dv[3] += b_a_idx_2_tmp;
54 dv[4] += a_idx_1_tmp;
55 dv[5] += a_idx_0_tmp;
56 dv[6] += a_idx_3_tmp;
57 dv[7] += c_a_idx_0_tmp;
58 dv[8] += a_idx_2_tmp;
59 dv[9] += b_a_idx_2_tmp;
60 dv[10] += a_idx_0_tmp;
61 dv[11] += a_idx_1_tmp;
62 dv[12] += a_idx_3_tmp;
63 dv[13] += a_idx_2_tmp;
64 dv[14] += b_a_idx_0_tmp;
65 dv[15] += a_idx_0_tmp;
66 a = quat[0];
67 c_a_idx_0_tmp = quat[1];
68 a_idx_0_tmp = quat[2];
69 a_idx_1_tmp = quat[3];
70 for (i = 0; i < 4; i++) {
71 quat_new[i] =
72 ((dv[i] * a + dv[i + 4] * c_a_idx_0_tmp) + dv[i + 8] * a_idx_0_tmp) +
73 dv[i + 12] * a_idx_1_tmp;
74 }
75 *alpha = acos(2.0 * (quat[0] * quat[0] + quat[3] * quat[3]) - 1.0);
76}
77
78/*
79 * File trailer for attitude_estimation.c
80 *
81 * [EOF]
82 */
attitude_estimation
void attitude_estimation(const double quat[4], double ts, double omega_x, double omega_y, double omega_z, double *alpha, double quat_new[4])
Propagates the rocket's attitude quaternion and calculates the angle from vertical.
Definition attitude_estimation.c:27
attitude_estimation.h
A temporary, gyroscope-based attitude estimator.
eye_attitude
void eye_attitude(double b_I[16])
Creates a 4x4 identity matrix, typically for attitude calculations.
Definition eye.c:46
eye.h
Utility functions for creating identity matrices of various sizes.
alpha
double alpha
Definition main.c:249