Line data Source code
1 : // clang-format off
2 : // In the frame of RPW LFR Sofware ICD Issue1 Rev8 (05/07/2013) => R2 FSW
3 : // version 1.0: 31/07/2013
4 : // version 1.1: 02/04/2014
5 : // version 1.2: 30/04/2014
6 : // version 1.3: 02/05/2014
7 : // version 1.4: 16/05/2014
8 : // version 1.5: 20/05/2014
9 : // version 1.6: 19/12/2014
10 : // version 1.7: 15/01/2015 (modifs de Paul + correction erreurs qui se compensaient (LSB <=> MSB + indices [0,2] <=> [1,3])
11 : // version 1.8: 02/02/2015 (gestion des divisions par zéro)
12 : // In the frame of RPW LFR Sofware ICD Issue3 Rev6 (27/01/2015) => R3 FSW
13 : // version 2.0: 19/06/2015
14 : // version 2.1: 22/06/2015 (modifs de Paul)
15 : // version 2.2: 23/06/2015 (modifs de l'ordre de déclaration/définition de init_k_coefficients dans basic_parameters.c ... + maintien des declarations dans le .h)
16 : // version 2.3: 01/07/2015 (affectation initiale des octets 7 et 9 dans les BP1 corrigée ...)
17 : // version 2.4: 05/10/2018 (added GPL headers)
18 : // version 2.5: 09/10/2018 (dans main.c #include "basic_parameters_utilities.h" est changé par les déclarations extern correspondantes ...!
19 : // + delta mise en conformité LOGISCOPE)
20 : // clang-format on
21 : /*------------------------------------------------------------------------------
22 : -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
23 : -- This file is a part of the LFR FSW
24 : -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
25 : --
26 : -- This program is free software; you can redistribute it and/or modify
27 : -- it under the terms of the GNU General Public License as published by
28 : -- the Free Software Foundation; either version 2 of the License, or
29 : -- (at your option) any later version.
30 : --
31 : -- This program is distributed in the hope that it will be useful,
32 : -- but WITHOUT ANY WARRANTY; without even the implied warranty of
33 : -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
34 : -- GNU General Public License for more details.
35 : --
36 : -- You should have received a copy of the GNU General Public License
37 : -- along with this program; if not, write to the Free Software
38 : -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
39 : -------------------------------------------------------------------------------*/
40 : /*-- Author : Thomas Chust
41 : -- Contact : Thomas Chust
42 : -- Mail : thomas.chust@lpp.polytechnique.fr
43 : ----------------------------------------------------------------------------*/
44 :
45 : #include <math.h>
46 : #include <stdint.h>
47 : #include <stdio.h>
48 :
49 : #include "basic_parameters_params.h"
50 : #include "custom_floats.h"
51 : #include "fsw_debug.h"
52 : #include "processing/ASM/spectralmatrices.h"
53 :
54 :
55 : inline const float* next_matrix(const float* const spectral_matrix) __attribute__((always_inline));
56 0 : const float* next_matrix(const float* const spectral_matrix)
57 : {
58 : DEBUG_CHECK_PTR(spectral_matrix);
59 1348 : return spectral_matrix + NB_FLOATS_PER_SM;
60 : }
61 :
62 : inline float elec_power_spectrum_density(const float* const spectral_matrix)
63 : __attribute__((always_inline));
64 0 : float elec_power_spectrum_density(const float* const spectral_matrix)
65 : {
66 : DEBUG_CHECK_PTR(spectral_matrix);
67 1142 : return spectral_matrix[ASM_COMP_E1E1] + spectral_matrix[ASM_COMP_E2E2];
68 : }
69 :
70 : inline float mag_power_spectrum_density(const float* const spectral_matrix)
71 : __attribute__((always_inline));
72 : inline float mag_power_spectrum_density(const float* const spectral_matrix)
73 : {
74 : DEBUG_CHECK_PTR(spectral_matrix);
75 2284 : return spectral_matrix[ASM_COMP_B1B1] + spectral_matrix[ASM_COMP_B2B2]
76 1142 : + spectral_matrix[ASM_COMP_B3B3];
77 : }
78 :
79 : typedef struct
80 : {
81 : float x;
82 : float y;
83 : float z;
84 : float ab; // Ellipse a.b product
85 : } normal_wave_vector_t;
86 :
87 : inline float square(const float value) __attribute__((always_inline));
88 0 : float square(const float value)
89 : {
90 17520 : return value * value;
91 : }
92 :
93 : inline normal_wave_vector_t normal_wave_vector(const float* const spectral_matrix)
94 : __attribute__((always_inline));
95 0 : normal_wave_vector_t normal_wave_vector(const float* const spectral_matrix)
96 : {
97 : DEBUG_CHECK_PTR(spectral_matrix);
98 3426 : const float ab = sqrtf(square(spectral_matrix[ASM_COMP_B1B2_imag])
99 1142 : + square(spectral_matrix[ASM_COMP_B1B3_imag])
100 4568 : + square(spectral_matrix[ASM_COMP_B2B3_imag]));
101 1142 : if (ab != 0.)
102 : {
103 390 : normal_wave_vector_t nvec = { .x = spectral_matrix[ASM_COMP_B2B3_imag] / ab,
104 390 : .y = -spectral_matrix[ASM_COMP_B1B3_imag] / ab,
105 390 : .z = spectral_matrix[ASM_COMP_B1B2_imag] / ab,
106 390 : .ab = ab };
107 390 : return nvec;
108 : }
109 : else
110 : {
111 752 : normal_wave_vector_t nvec = { .x = 0., .y = 0., .z = 0., .ab = 0. };
112 752 : return nvec;
113 : }
114 : }
115 :
116 : inline float wave_ellipticity_estimator(const float mag_PSD, const float nvec_denom)
117 : __attribute__((always_inline));
118 0 : float wave_ellipticity_estimator(const float mag_PSD, const float nvec_denom)
119 : {
120 1142 : if (mag_PSD != 0.f)
121 : {
122 390 : return 2.f * nvec_denom / mag_PSD;
123 : }
124 : else
125 : {
126 752 : return 0.;
127 : }
128 : }
129 :
130 : inline float degree_of_polarization(const float mag_PSD, const float* const spectral_matrix)
131 : __attribute__((always_inline));
132 0 : float degree_of_polarization(const float mag_PSD, const float* const spectral_matrix)
133 : {
134 : DEBUG_CHECK_PTR(spectral_matrix);
135 : const float B_square_trace = square(spectral_matrix[ASM_COMP_B1B1])
136 3426 : + square(spectral_matrix[ASM_COMP_B2B2]) + square(spectral_matrix[ASM_COMP_B3B3])
137 2284 : + 2.f
138 4568 : * (square(spectral_matrix[ASM_COMP_B1B2]) + square(spectral_matrix[ASM_COMP_B1B2_imag])
139 3426 : + square(spectral_matrix[ASM_COMP_B1B3])
140 2284 : + square(spectral_matrix[ASM_COMP_B1B3_imag])
141 2284 : + square(spectral_matrix[ASM_COMP_B2B3])
142 3426 : + square(spectral_matrix[ASM_COMP_B2B3_imag]));
143 1142 : const float square_B_trace = square(mag_PSD);
144 1142 : if (square_B_trace != 0.)
145 : {
146 390 : return sqrtf((3.f * B_square_trace - square_B_trace) / (2.f * square_B_trace));
147 : }
148 : else
149 : {
150 752 : return 0.f;
151 : }
152 : }
153 :
154 : inline compressed_complex X_poynting_vector(const float* const spectral_matrix)
155 : __attribute__((always_inline));
156 0 : compressed_complex X_poynting_vector(const float* const spectral_matrix)
157 : {
158 : DEBUG_CHECK_PTR(spectral_matrix);
159 : // E1B3 - E2B2
160 : compressed_complex X_PV;
161 1142 : X_PV.real = spectral_matrix[ASM_COMP_B3E1] - spectral_matrix[ASM_COMP_B2E2];
162 1142 : const float imag = spectral_matrix[ASM_COMP_B2E2_imag] - spectral_matrix[ASM_COMP_B3E1_imag];
163 1142 : X_PV.arg = fabs(imag) > fabs(X_PV.real);
164 1142 : return X_PV;
165 : }
166 :
167 : inline float modulus(const float a, const float b) __attribute__((always_inline));
168 0 : float modulus(const float a, const float b)
169 : {
170 2674 : return sqrtf(square(a) + square(b));
171 : }
172 :
173 : inline float cplx_modulus(const _Complex float value) __attribute__((always_inline));
174 0 : float cplx_modulus(const _Complex float value)
175 : {
176 780 : return modulus(__real__ value, __imag__ value);
177 : }
178 :
179 :
180 : inline compressed_complex phase_velocity_estimator(const float* const spectral_matrix,
181 : const normal_wave_vector_t nvec) __attribute__((always_inline));
182 0 : compressed_complex phase_velocity_estimator(
183 : const float* const spectral_matrix, const normal_wave_vector_t nvec)
184 : {
185 : DEBUG_CHECK_PTR(spectral_matrix);
186 : /*
187 : VPHI = abs(NEBX) * sign( Re[NEBX] ) / BXBX
188 : with:
189 : NEBX = nY<EZBX*>/rho_EZBX - nZ<EYBX*>/rho_EYBX = n2<E2B1*>/rho_E2B1 - n3<E1B1*>/rho_E1B1
190 : rho_E2B1 = |<E2B1*>| / sqrt(<E2E2*><B1B1*>)
191 : rho_E1B1 = |<E1B1*>| / sqrt(<E1E1*><B1B1*>)
192 : BXBX = <BXBX*> = <B1B1*>
193 : */
194 1142 : compressed_complex vphi = { .real = 0, .arg = 0 };
195 : const float sqrt_E2E2B1B1
196 1142 : = sqrtf(spectral_matrix[ASM_COMP_E2E2] * spectral_matrix[ASM_COMP_B1B1]);
197 : const float sqrt_E1E1B1B1
198 1142 : = sqrtf(spectral_matrix[ASM_COMP_E1E1] * spectral_matrix[ASM_COMP_B1B1]);
199 : const float mod_E2B1
200 2284 : = modulus(spectral_matrix[ASM_COMP_B1E2], spectral_matrix[ASM_COMP_B1E2_imag]);
201 : const float mod_E1B1
202 2284 : = modulus(spectral_matrix[ASM_COMP_B1E1], spectral_matrix[ASM_COMP_B1E1_imag]);
203 : _Complex float NEBX;
204 1142 : if (mod_E2B1 != 0. && mod_E1B1 != 0.)
205 : {
206 780 : __real__ NEBX = nvec.y * spectral_matrix[ASM_COMP_B1E2] * sqrt_E2E2B1B1 / mod_E2B1
207 390 : - nvec.z * spectral_matrix[ASM_COMP_B1E1] * sqrt_E1E1B1B1 / mod_E1B1;
208 :
209 780 : __imag__ NEBX = nvec.z * spectral_matrix[ASM_COMP_B1E1_imag] * sqrt_E1E1B1B1 / mod_E1B1
210 390 : - nvec.y * spectral_matrix[ASM_COMP_B1E2_imag] * sqrt_E2E2B1B1 / mod_E2B1;
211 :
212 390 : const float BXBX = spectral_matrix[ASM_COMP_B1B1];
213 390 : if (BXBX != 0.f)
214 : {
215 390 : vphi.real = cplx_modulus(NEBX) / BXBX;
216 390 : if (__real__ NEBX < 0.)
217 : {
218 210 : vphi.real = -vphi.real;
219 : }
220 : }
221 : else
222 : {
223 0 : if (__real__ NEBX >= 0.)
224 : {
225 0 : vphi.real = 1.e+20f;
226 : }
227 : else
228 : {
229 0 : vphi.real = -1.e+20f;
230 : }
231 : }
232 390 : vphi.arg = fabs(__imag__ NEBX) > fabs(__real__ NEBX);
233 : }
234 1142 : return vphi;
235 : }
236 :
237 : inline uint8_t* encode_nvec_z_ellip_dop(const float nvec_z, const float ellipticity,
238 : const float DOP, uint8_t* const bp_buffer_frame) __attribute__((always_inline));
239 0 : uint8_t* encode_nvec_z_ellip_dop(
240 : const float nvec_z, const float ellipticity, const float DOP, uint8_t* const bp_buffer_frame)
241 : {
242 : DEBUG_CHECK_PTR(bp_buffer_frame);
243 1142 : const str_float_t z = { .value = nvec_z };
244 : #ifdef LFR_BIG_ENDIAN
245 : union __attribute__((__packed__))
246 : {
247 : uint8_t value;
248 : struct __attribute__((__packed__))
249 : {
250 : uint8_t nvec_z_sign : 1;
251 : uint8_t ellipticity : 4;
252 : uint8_t DOP : 3;
253 : } str;
254 : } result;
255 : #endif
256 : #ifdef LFR_LITTLE_ENDIAN
257 : union __attribute__((__packed__))
258 : {
259 : uint8_t value;
260 : struct __attribute__((__packed__))
261 : {
262 : uint8_t DOP : 3;
263 : uint8_t ellipticity : 4;
264 : uint8_t nvec_z_sign : 1;
265 : } str;
266 : } result;
267 : #endif
268 1142 : result.str.nvec_z_sign = z.str.sign;
269 1142 : result.str.ellipticity = (uint8_t)(ellipticity * 15.f + 0.5f);
270 1142 : result.str.DOP = (uint8_t)(DOP * 7.f + 0.5f);
271 1142 : bp_buffer_frame[0] = result.value;
272 1142 : return bp_buffer_frame + 1;
273 : }
274 :
275 : inline uint8_t* encode_uint16_t(const uint16_t value, uint8_t* const bp_buffer_frame)
276 : __attribute__((always_inline));
277 0 : uint8_t* encode_uint16_t(const uint16_t value, uint8_t* const bp_buffer_frame)
278 : {
279 : DEBUG_CHECK_PTR(bp_buffer_frame);
280 5598 : const str_uint16_t value_split = { .value = value };
281 5598 : bp_buffer_frame[0] = value_split.str.MSB;
282 5598 : bp_buffer_frame[1] = value_split.str.LSB;
283 5598 : return bp_buffer_frame + 2;
284 : }
285 :
286 :
287 : inline uint8_t* encode_float_uint8_t(float value, uint8_t* const bp_buffer_frame)
288 : __attribute__((always_inline));
289 0 : uint8_t* encode_float_uint8_t(float value, uint8_t* const bp_buffer_frame)
290 : {
291 : DEBUG_CHECK_PTR(bp_buffer_frame);
292 6404 : bp_buffer_frame[0] = (uint8_t)(value * 127.5 + 128);
293 6404 : return bp_buffer_frame + 1;
294 : }
295 :
296 : inline uint8_t* encode_BP1(const float mag_PSD, const float elec_PSD,
297 : const normal_wave_vector_t nvec, const float ellipticity, const float DOP,
298 : const compressed_complex X_PV, const compressed_complex VPHI, uint8_t* bp1_buffer_frame)
299 : __attribute__((always_inline));
300 0 : uint8_t* encode_BP1(const float mag_PSD, const float elec_PSD, const normal_wave_vector_t nvec,
301 : const float ellipticity, const float DOP, const compressed_complex X_PV,
302 : const compressed_complex VPHI, uint8_t* bp1_buffer_frame)
303 : {
304 : DEBUG_CHECK_PTR(bp_buffer_frame);
305 : {
306 2284 : bp1_buffer_frame = encode_uint16_t(to_custom_float_6_10(elec_PSD), bp1_buffer_frame);
307 : }
308 : {
309 2284 : bp1_buffer_frame = encode_uint16_t(to_custom_float_6_10(mag_PSD), bp1_buffer_frame);
310 : }
311 : {
312 2284 : bp1_buffer_frame = encode_float_uint8_t(nvec.x, bp1_buffer_frame);
313 2284 : bp1_buffer_frame = encode_float_uint8_t(nvec.y, bp1_buffer_frame);
314 2284 : bp1_buffer_frame = encode_nvec_z_ellip_dop(nvec.z, ellipticity, DOP, bp1_buffer_frame);
315 : }
316 : {
317 2284 : bp1_buffer_frame = encode_uint16_t(to_custom_float_1_1_6_8(X_PV), bp1_buffer_frame);
318 : }
319 : {
320 2284 : bp1_buffer_frame = encode_uint16_t(to_custom_float_1_1_6_8(VPHI), bp1_buffer_frame);
321 : }
322 1142 : return bp1_buffer_frame;
323 : }
324 :
325 54 : void compute_BP1(const float* const spectral_matrices, const uint8_t spectral_matrices_count,
326 : uint8_t* bp1_buffer)
327 : {
328 : DEBUG_CHECK_PTR(spectral_matrices);
329 : DEBUG_CHECK_PTR(bp1_buffer);
330 54 : const float* spectral_matrix_ptr = spectral_matrices;
331 54 : uint8_t* bp1_buffer_frame = bp1_buffer;
332 1196 : for (int i = 0; i < spectral_matrices_count; i++)
333 : {
334 1142 : const float mag_PSD = mag_power_spectrum_density(spectral_matrix_ptr);
335 1142 : const float elec_PSD = elec_power_spectrum_density(spectral_matrix_ptr);
336 : const normal_wave_vector_t nvec = normal_wave_vector(spectral_matrix_ptr);
337 2284 : const float ellipticity = wave_ellipticity_estimator(mag_PSD, nvec.ab);
338 1142 : const float DOP = degree_of_polarization(mag_PSD, spectral_matrix_ptr);
339 : const compressed_complex X_PV = X_poynting_vector(spectral_matrix_ptr);
340 : const compressed_complex VPHI = phase_velocity_estimator(spectral_matrix_ptr, nvec);
341 1142 : bp1_buffer_frame
342 : = encode_BP1(mag_PSD, elec_PSD, nvec, ellipticity, DOP, X_PV, VPHI, bp1_buffer_frame);
343 1142 : spectral_matrix_ptr = next_matrix(spectral_matrix_ptr);
344 : }
345 54 : }
346 :
347 :
348 : inline uint8_t* _compute_BP2_cross_component(uint8_t* const bp2_frame, float auto1, float auto2,
349 : float cross_re, float cross_imag) __attribute__((always_inline));
350 0 : uint8_t* _compute_BP2_cross_component(
351 : uint8_t* const bp2_frame, float auto1, float auto2, float cross_re, float cross_imag)
352 : {
353 : DEBUG_CHECK_PTR(bp2_frame);
354 2060 : const float aux = sqrtf(auto1 * auto2);
355 2060 : if (aux != 0.f)
356 : {
357 440 : encode_float_uint8_t(cross_re / aux, bp2_frame);
358 440 : encode_float_uint8_t(cross_imag / aux, bp2_frame + 10);
359 : }
360 : else
361 : {
362 : encode_float_uint8_t(0.f, bp2_frame);
363 1620 : encode_float_uint8_t(0.f, bp2_frame + 10);
364 : }
365 2060 : return bp2_frame + 1;
366 : }
367 :
368 9 : void compute_BP2(const float* const spectral_matrices, const uint8_t spectral_matrices_count,
369 : uint8_t* bp2_buffer)
370 : {
371 : DEBUG_CHECK_PTR(spectral_matrices);
372 : DEBUG_CHECK_PTR(bp2_buffer);
373 :
374 9 : const float* sm_ptr = spectral_matrices;
375 9 : uint8_t* bp2_frame = bp2_buffer;
376 215 : for (int i = 0; i < spectral_matrices_count; i++)
377 : {
378 618 : bp2_frame = encode_uint16_t(to_custom_float_6_10(sm_ptr[ASM_COMP_B1B1]), bp2_frame);
379 618 : bp2_frame = encode_uint16_t(to_custom_float_6_10(sm_ptr[ASM_COMP_B2B2]), bp2_frame);
380 618 : bp2_frame = encode_uint16_t(to_custom_float_6_10(sm_ptr[ASM_COMP_B3B3]), bp2_frame);
381 618 : bp2_frame = encode_uint16_t(to_custom_float_6_10(sm_ptr[ASM_COMP_E1E1]), bp2_frame);
382 618 : bp2_frame = encode_uint16_t(to_custom_float_6_10(sm_ptr[ASM_COMP_E2E2]), bp2_frame);
383 :
384 1030 : bp2_frame = _compute_BP2_cross_component(bp2_frame, sm_ptr[ASM_COMP_B1B1],
385 618 : sm_ptr[ASM_COMP_B2B2], sm_ptr[ASM_COMP_B1B2], sm_ptr[ASM_COMP_B1B2_imag]);
386 :
387 1030 : bp2_frame = _compute_BP2_cross_component(bp2_frame, sm_ptr[ASM_COMP_B1B1],
388 618 : sm_ptr[ASM_COMP_B3B3], sm_ptr[ASM_COMP_B1B3], sm_ptr[ASM_COMP_B1B3_imag]);
389 :
390 1030 : bp2_frame = _compute_BP2_cross_component(bp2_frame, sm_ptr[ASM_COMP_B1B1],
391 618 : sm_ptr[ASM_COMP_E1E1], sm_ptr[ASM_COMP_B1E1], sm_ptr[ASM_COMP_B1E1_imag]);
392 :
393 1030 : bp2_frame = _compute_BP2_cross_component(bp2_frame, sm_ptr[ASM_COMP_B1B1],
394 618 : sm_ptr[ASM_COMP_E2E2], sm_ptr[ASM_COMP_B1E2], sm_ptr[ASM_COMP_B1E2_imag]);
395 :
396 1030 : bp2_frame = _compute_BP2_cross_component(bp2_frame, sm_ptr[ASM_COMP_B2B2],
397 618 : sm_ptr[ASM_COMP_B3B3], sm_ptr[ASM_COMP_B2B3], sm_ptr[ASM_COMP_B2B3_imag]);
398 :
399 1030 : bp2_frame = _compute_BP2_cross_component(bp2_frame, sm_ptr[ASM_COMP_B2B2],
400 618 : sm_ptr[ASM_COMP_E1E1], sm_ptr[ASM_COMP_B2E1], sm_ptr[ASM_COMP_B2E1_imag]);
401 :
402 1030 : bp2_frame = _compute_BP2_cross_component(bp2_frame, sm_ptr[ASM_COMP_B2B2],
403 618 : sm_ptr[ASM_COMP_E2E2], sm_ptr[ASM_COMP_B2E2], sm_ptr[ASM_COMP_B2E2_imag]);
404 :
405 1030 : bp2_frame = _compute_BP2_cross_component(bp2_frame, sm_ptr[ASM_COMP_B3B3],
406 618 : sm_ptr[ASM_COMP_E1E1], sm_ptr[ASM_COMP_B3E1], sm_ptr[ASM_COMP_B3E1_imag]);
407 :
408 1030 : bp2_frame = _compute_BP2_cross_component(bp2_frame, sm_ptr[ASM_COMP_B3B3],
409 618 : sm_ptr[ASM_COMP_E2E2], sm_ptr[ASM_COMP_B3E2], sm_ptr[ASM_COMP_B3E2_imag]);
410 :
411 1030 : bp2_frame = _compute_BP2_cross_component(bp2_frame, sm_ptr[ASM_COMP_E1E1],
412 618 : sm_ptr[ASM_COMP_E2E2], sm_ptr[ASM_COMP_E1E2], sm_ptr[ASM_COMP_E1E2_imag]);
413 :
414 206 : bp2_frame += 10;
415 206 : sm_ptr = next_matrix(sm_ptr);
416 : }
417 9 : }
|