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

A temporary, gyroscope-based attitude estimator. More...

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

Go to the source code of this file.

Functions

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.
 

Detailed Description

A temporary, gyroscope-based attitude estimator.

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

This module provides a temporary attitude estimation function that propagates the rocket's orientation (quaternion) forward in time using gyroscope data. It serves as a placeholder for more advanced filters like MEKF or a full Kalman filter.

This implementation was provided by the MSA subteam and is intended for use on hardware configurations that do not have a functioning magnetometer. Because it relies solely on integrating gyroscope rates (a form of dead reckoning), it is susceptible to drift over time.

Definition in file attitude_estimation.h.

Function Documentation

◆ 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.

Parameters
[in]quatThe current attitude quaternion [w, x, y, z].
[in]tsThe time step (delta time) in seconds since the last update.
[in]omega_xAngular velocity around the body's X-axis in radians/second.
[in]omega_yAngular velocity around the body's Y-axis in radians/second.
[in]omega_zAngular velocity around the body's Z-axis in radians/second.
[out]alphaPointer to a double to store the calculated angle between the rocket's body axis and the vertical, in degrees.
[out]quat_newAn array to store the resulting, updated attitude quaternion.
Returns
None

Definition at line 27 of file attitude_estimation.c.

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}
eye_attitude
void eye_attitude(double b_I[16])
Creates a 4x4 identity matrix, typically for attitude calculations.
Definition eye.c:46
alpha
double alpha
Definition main.c:249

References alpha, and eye_attitude().

Referenced by StartKalman_mkf().

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