-
Notifications
You must be signed in to change notification settings - Fork 16
/
master_header.h
213 lines (186 loc) · 10.5 KB
/
master_header.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
/**
* @file master_header.h
* @author Danylo Malyuta <[email protected]>
* @version 1.0
*
* @brief Master header file.
*
* This is the header to master_funcs.c containing necessary definitions and
* initializations.
*/
#ifndef MASTER_HEADER_H_
#define MASTER_HEADER_H_
# include <stdint.h>
# include <stdio.h>
# include <sys/time.h>
# include <pthread.h>
# include "la_header.h"
# define VALVE_CHARAC_RESOLUTION 8 ///< The number of points there are in the calibrated valve thrust curve (flow rate vs. PWM)
char ERROR_MESSAGE[200]; ///< Allocate buffer for an error message to be printed into #error_log if errors occur
/**
* @name IMU timing
* Contains the timing structures and variables necessary for setting the IMU filtering thread loop frequency (see get_filtered_attitude_parallel()).
*/
/** @{ */
struct timeval now_imu;
struct timeval before_imu;
struct timeval elapsed_imu;
unsigned long long int time_imu;
/** @} */
/**
* @name Pressure sensor timing
* Contains the timing structures and variables necessary to set the the pressure/temperature logging thread loop frequency (see get_readings_SPI_parallel())
*/
/** @{ */
struct timeval now_pressure;
struct timeval before_pressure;
struct timeval elapsed_pressure;
unsigned long long int time_pressure;
/** @} */
/**
* @name General loop timing
* Contains the timing structures and variables necessary to make sure a loop executes a given amount of time
*/
/** @{ */
struct timeval now_loop;
struct timeval before_loop;
struct timeval elapsed_loop;
unsigned long long int time_loop;
/** @} */
/**
* @name Control loop timing
* Contains the timing structures and variables necessary for timing necessary to set the control loop frequency (see main())
*/
/** @{ */
struct timeval now_control;
struct timeval before_control;
struct timeval elapsed_control;
unsigned long long int time_control;
/** @} */
/**
* @name IMU filter get global time
* Contains the timing structures and variables necessary for getting the global time within the IMU data filtering loop (see get_filtered_attitude_parallel())
*/
/** @{ */
struct timeval now_imu_glob;
struct timeval elapsed_imu_glob;
unsigned long long int time_imu_glob;
/** @} */
/**
* @name Pressure read get global time
* Contains the timing structures and variables necessary for getting the global time within the pressure/tempearture sensor data logging loop (see get_readings_SPI_parallel())
*/
/** @{ */
struct timeval now_pressure_glob;
struct timeval elapsed_pressure_glob;
unsigned long long int time_pressure_glob;
/** @} */
/**
* @name Control loop get global time
* Contains the timing structures and variables necessary for getting the global time within the control loop (see main())
*/
/** @{ */
struct timeval now_control_glob;
struct timeval elapsed_control_glob;
unsigned long long int time_control_glob;
/** @} */
struct timeval GLOBAL__TIME_STARTPOINT; ///< Structure holding the time when the program started (very first line of main())
extern unsigned char IMU_SYNCHED;
extern unsigned long long int SPI__READ_TIMESTEP; ///< Time intervals [us] at which we read over SPI (for pressure/temperature Honeywell sensors).
extern unsigned char SPI_quit; ///< ==0 by default, ==1 signals the SPI reading thread (get_readings_SPI_parallel()) to exit.
extern unsigned long long int IMU__READ_TIMESTEP; ///< Time intervals [us] at which we read over UART the IMU data.
extern unsigned char IMU_quit; ///< ==0 by default, ==1 signals the IMU reading and filtering threads (read_IMU_parallel() and get_filtered_attitude_parallel()) to exit.
extern unsigned int PWM1; ///< PWM value for the R1 valve
extern unsigned int PWM2; ///< PWM value for the R2 valve
extern unsigned int PWM3; ///< PWM value for the R3 valve
extern unsigned int PWM4; ///< PWM value for the R4 valve
extern double R1; ///< Valve R1 thrust
extern double R2; ///< Valve R2 thrust
extern double R3; ///< Valve R3 thrust
extern double R4; ///< Valve R4 thrust
extern unsigned int PWM_valve_charac[VALVE_CHARAC_RESOLUTION];
extern double R_valve_charac[VALVE_CHARAC_RESOLUTION];
extern double d; ///< [m] offset distance of RCS valves from centerline (for roll control)
extern double Fpitch; ///< Pitch force (parallel to body -Z axis, so as to produce positive pitch rate when Fpitch>0 (right hand rule))
extern double Fyaw; ///< Yaw force (parallel to body +Y axis, so as to produce positive yaw rate when Fyaw>0 (right hand rule))
extern double Mroll; ///< Roll moment (positive about +X axis, so as to produce positive roll rate when Mroll>0 (right hand rule))
extern int N; ///< Number of variables in cost function. Our variables are R1, R2, R3, R4 so N=4
extern int M1; ///< No (<=) type constraints
extern int M2; ///< No (>=) type constraints
extern int M3; ///< 3 (=) type constraints (for Fpitch, Fyaw, Mroll)
extern int M; ///< Total number of constraints (M=M1+M2+M3)
extern double VALVE__MAX_THRUST; ///< Maximum thrust of RCS solenoid valves (i.e. when fully opened)
/**
* @name Log files group
* Contains the pointes to files we use for recording flight data.
* @{
*/
extern FILE *error_log; ///< Error log (stores errors)
extern FILE *pressure_log; ///< Pressure log (stores pressures and tempeartures collected by Honeywell HSC TruStability sensors)
extern FILE *imu_log; ///< IMU log (stores raw and filtered Euler angles and angular rates, the body rates and the accelerometer data)
extern FILE *control_log; ///< Control log (stores the control loop data such as computed #Fpitch, #Fyaw, #Mroll, the optimally distributed valves thrusts #R1,...,#R4 and the computed PWM signals #PWM1,...,#PWM4)
/** @} */
char MESSAGE[700]; ///< Message buffer string sometimes used for putting together a string, then writing it to a file
pthread_mutex_t error_log_write_lock; ///< Mutex to protect multiple threads from writing to the error log file at once
/*********** FILTERING MATRICES ************
* (Kalman filter)
*/
/**
* @name Yaw Kalman filtering matrices
* These matrices pertain to the real-time Kalman filtering of the yaw angle and angular rate
* @{
*/
struct MATRIX P_psi; ///< Predicted a priori and then updated a posteriori estimate covariance matrix of the #psi_filt estimate
struct MATRIX P_psidot; ///< Predicted a priori and then updated a posteriori estimate covariance matrix of the #psi_dot_filt estimate
struct MATRIX x_psi; ///< Predicted a priori and then updated a posteriori state estimate (the #MATRIX version of #psi_filt)
struct MATRIX x_psidot; ///< Predicted a priori and then updated a posteriori state estimate (the #MATRIX version of #psi_dot_filt)
struct MATRIX Q_psi; ///< Covariance matrix of process noise of #psi
struct MATRIX Q_psidot; ///< Covariance matrix of process noise of #psi_dot
struct MATRIX R_psi; ///< Covariance matrix of observation of #psi
struct MATRIX R_psidot; ///< Covariance matrix of observation of #psi_dot
/** @} */
/**
* @name Pitch Kalman filtering matrices
* These matrices pertain to the real-time Kalman filtering of the pitch angle and angular rate
* @{
*/
struct MATRIX P_theta; ///< Predicted a priori and then updated a posteriori estimate covariance matrix of the #theta_filt estimate
struct MATRIX x_theta; ///< Predicted a priori and then updated a posteriori state estimate (the #MATRIX version of #theta_filt)
struct MATRIX Q_theta; ///< Covariance matrix of process noise of #theta
struct MATRIX R_theta; ///< Covariance matrix of observation of #theta
struct MATRIX P_thetadot; ///< Predicted a priori and then updated a posteriori estimate covariance matrix of the #theta_dot_filt estimate
struct MATRIX x_thetadot; ///< Predicted a priori and then updated a posteriori state estimate (the #MATRIX version of #theta_dot_filt)
struct MATRIX Q_thetadot; ///< Covariance matrix of process noise of #theta_dot
struct MATRIX R_thetadot; ///< Covariance matrix of observation of #theta_dot
/** @} */
/**
* @name Roll Kalman filtering matrices
* These matrices pertain to the real-time Kalman filtering of the roll angle and angular rate
* @{
*/
struct MATRIX P_phi; ///< Predicted a priori and then updated a posteriori estimate covariance matrix of the #phi_filt estimate
struct MATRIX x_phi; ///< Predicted a priori and then updated a posteriori state estimate (the #MATRIX version of #phi_filt)
struct MATRIX Q_phi; ///< Covariance matrix of process noise of #phi
struct MATRIX R_phi; ///< Covariance matrix of observation of #phi
struct MATRIX P_phidot; ///< Predicted a priori and then updated a posteriori estimate covariance matrix of the #phi_dot_filt estimate
struct MATRIX x_phidot; ///< Predicted a priori and then updated a posteriori state estimate (the #MATRIX version of #phi_dot_filt)
struct MATRIX Q_phidot; ///< Covariance matrix of process noise of #phi_dot
struct MATRIX R_phidot; ///< Covariance matrix of observation of #phi_dot
/** @} */
struct MATRIX A_kalman; ///< Temporary matrix for Kalman filtering, dynamic equation x_dot=A_kalman*x ; y=C_kalman*x, but in discrete time!
struct MATRIX C_kalman; ///< Temporary matrix for Kalman filtering, dynamic equation x_dot=A_kalman*x ; y=C_kalman*x, but in discrete time!
struct MATRIX inn; ///< Temporary matrix "Innovation or measurement residual" (see <a href="http://en.wikipedia.org/wiki/Kalman_filter#Update">Wikipedia</a>)
struct MATRIX S; ///< Temporary matrix "Innovation (or residual) covariance" (see <a href="http://en.wikipedia.org/wiki/Kalman_filter#Update">Wikipedia</a>)
struct MATRIX K; ///< Temporary matrix "Optimal Kalman gain" (see <a href="http://en.wikipedia.org/wiki/Kalman_filter#Update">Wikipedia</a>)
struct MATRIX z_temp; ///< Temporary matrix used to convert the current scalar unfiltered signal value (e.g. #psi or #theta, etc.) into a [1x1] #MATRIX used in calculating #inn (see Kalman_filter()), needed due to how our linear algebra functions in la_header.h operate
//***************** Function declarations *******************
/** @cond INCLUDE_WITH_DOXYGEN */
void check_time(struct timeval *now, struct timeval before, struct timeval elapsed, unsigned long long int *time);
void write_to_file_custom(FILE *file_ptr, char *string,FILE *error_log);
void open_file(FILE **log, char *path, char *setting,FILE *error_log);
void open_error_file(FILE **error_log,char *path, char *setting);
void passive_wait(struct timeval *now,struct timeval *before,struct timeval *elapsed,unsigned long long int *time,unsigned long long int TIME__STEP);
void search_PWM(double R1_thrust,double R2_thrust,double R3_thrust,double R4_thrust,unsigned int *pwm1,unsigned int *pwm2,unsigned int *pwm3,unsigned int *pwm4);
void linear_search(double thrust, unsigned int *pwm);
/** @endcond */
#endif /* MASTER_HEADER_H_ */