##// END OF EJS Templates
HG_REPOSITORIES » LPP » INSTRUMENTATION » USERS » JEANDET » LFR_FSW
RSS

Fork of HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

Clone from https://hephaistos.lpp.polytechnique.fr/rhodecode/HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

Fusion avec No PWD scrub with FPU
jeandet -
r392:75028d90a6e7 merge R3++ draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -0,0 +1,4
1 3236b2a1a6a04bc11754d3f995873876b5046183 3.2.0.17
2 ad411bb94578a052d1b4aa6b4c8a769fe2711072 3.2.0.18
3 a9b894b0ab6a8fa48f50ce3dd7200406b83e2a62 3.2.0.19
4 bd1252670981361939ed2a1c3febc94247019956 3.2.0.20
Show file before | Show file after | Hide comments
@@ -0,0 +1,17
1 cmake_minimum_required(VERSION 3.6)
2 project(libgcov C)
3 include(sparc-rtems)
4 include(cppcheck)
5
6 set(LIB_GCOV_SOURCES
7 gcov-io.c
8 gcov-io.h
9 gcov-iov.h
10 libgcov.c
11 )
12
13 add_library(gcov STATIC ${LIB_GCOV_SOURCES})
14
15 add_custom_target(gcovr
16 COMMAND gcovr --exclude='.*gcov.*' --gcov-executable=${rtems_dir}/bin/sparc-rtems-gcov --object-directory ${CMAKE_BINARY_DIR} -r ${CMAKE_SOURCE_DIR} --html --html-details -o ${CMAKE_CURRENT_BINARY_DIR}/gcov.html && xdg-open ${CMAKE_CURRENT_BINARY_DIR}/gcov.html
17 )
Show file before | Show file after | Hide comments
@@ -0,0 +1,41
1 #!/usr/bin/env python3
2
3 __author__ = "Alexis Jeandet"
4 __copyright__ = "Copyright 2018, Laboratory of Plasma Physics"
5 __credits__ = []
6 __license__ = "GPLv2"
7 __version__ = "1.0.0"
8 __maintainer__ = "Alexis Jeandet"
9 __email__ = "alexis.jeandet@member.fsf.org"
10 __status__ = "Development"
11
12 import time
13 import sys
14 import os
15 import serial
16 import argparse
17 from datetime import datetime
18
19 parser = argparse.ArgumentParser()
20 parser.add_argument("-f", "--gcov-file", help="Gcov output file generated by record_lfr_console.py")
21 args = parser.parse_args()
22
23
24
25 def main():
26 with open(args.gcov_file,'r') as gcov:
27 files = []
28 for line in gcov.readlines():
29 head,dest_file,data = line.split(',')
30 if dest_file not in files:
31 files.append(dest_file)
32 if head == '_GCOV_':
33 print(f"Writing {dest_file}\n")
34 with open(dest_file,'wb') as gcda_file:
35 gcda_file.write(bytes([int(''.join(value),16) for value in zip(data[::2],data[1::2]) ]))
36 else:
37 raise
38
39
40 if __name__ == "__main__":
41 main()
Show file before | Show file after | Hide comments
@@ -0,0 +1,488
1 /* Test for GCC >= 3.4.4 && <= 4.4.6 */
2 //#if ( ( __GNUC__ > 3 ) || \
3 // ( __GNUC__ == 3 && __GNUC_MINOR__ > 4 )|| \
4 // ( __GNUC__ == 3 && __GNUC_MINOR__ == 4 && __GNUC_PATCHLEVEL__ >= 4 ) ) && \
5 // ( ( __GNUC__ < 4 ) || \
6 // ( __GNUC__ == 4 && __GNUC_MINOR__ < 4 )|| \
7 // ( __GNUC__ == 4 && __GNUC_MINOR__ == 4 && __GNUC_PATCHLEVEL__ <= 6 ) )
8 /*
9 * =====================================================================================
10 *
11 * Filename: gcov-io.c
12 *
13 * Description: This is the I/O file for embedded systems
14 *
15 * Version: 1.0
16 * Created: 03/04/08 09:51:59
17 * Revision: none
18 * Compiler: gcc
19 *
20 * Author: Aitor Viana Sanchez (avs), aitor.viana.sanchez@esa.int
21 * Company: European Space Agency (ESA-ESTEC)
22 *
23 * =====================================================================================
24 */
25
26 /* File format for coverage information
27 Copyright (C) 1996, 1997, 1998, 2000, 2002,
28 2003 Free Software Foundation, Inc.
29 Contributed by Bob Manson <manson@cygnus.com>.
30 Completely remangled by Nathan Sidwell <nathan@codesourcery.com>.
31
32 This file is part of GCC.
33
34 GCC is free software; you can redistribute it and/or modify it under
35 the terms of the GNU General Public License as published by the Free
36 Software Foundation; either version 2, or (at your option) any later
37 version.
38
39 GCC is distributed in the hope that it will be useful, but WITHOUT ANY
40 WARRANTY; without even the implied warranty of MERCHANTABILITY or
41 FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
42 for more details.
43
44 You should have received a copy of the GNU General Public License
45 along with GCC; see the file COPYING. If not, write to the Free
46 Software Foundation, 59 Temple Place - Suite 330, Boston, MA
47 02111-1307, USA. */
48
49 #include <stdio.h>
50 #include <stdlib.h> /* for atexit() */
51 #include <string.h>
52 #include "gcov-io.h"
53
54 /* Routines declared in gcov-io.h. This file should be #included by
55 another source file, after having #included gcov-io.h. */
56
57
58 /* This function shall be defined somewhere else */
59 //int send_data(unsigned char * buffer, unsigned int size);
60
61 /*-----------------------------------------------------------------------------
62 * PRIVATE INTERFACE
63 *-----------------------------------------------------------------------------*/
64
65 static void gcov_write_block (unsigned);
66 static gcov_unsigned_t *gcov_write_words (unsigned);
67 GCOV_LINKAGE int gcov_send (void);
68 GCOV_LINKAGE int gcov_close(void);
69
70 extern struct gcov_info * gcov_list;
71 extern gcov_unsigned_t gcov_crc32;
72
73 int dev_id = 0;
74
75 /*
76 * === FUNCTION ======================================================================
77 * Name: from_file
78 * Description: This function just return the given parameter
79 * =====================================================================================
80 */
81 static inline gcov_unsigned_t from_file (gcov_unsigned_t value)
82 {
83 return value;
84 }
85
86 /*
87 * === FUNCTION ======================================================================
88 * Name: gcov_version
89 * Description: This function returns TRUE (1) if the gcov version is the
90 * version expected. The function returns FALSE (0) in any other case.
91 * =====================================================================================
92 */
93 static int gcov_version (struct gcov_info *ptr, gcov_unsigned_t version)
94 {
95 if (version != GCOV_VERSION)
96 {
97 char v[4], e[4];
98
99 GCOV_UNSIGNED2STRING (v, version);
100 GCOV_UNSIGNED2STRING (e, GCOV_VERSION);
101
102 printf ("profiling:%s:Version mismatch - expected %.4s got %.4s\n",
103 ptr->filename, e, v);
104
105 return 0;
106 }
107 return 1;
108 }
109
110
111 /*-----------------------------------------------------------------------------
112 * PUBLIC INTERFACE
113 *-----------------------------------------------------------------------------*/
114
115 /* Dump the coverage counts. We merge with existing counts when
116 possible, to avoid growing the .da files ad infinitum. We use this
117 program's checksum to make sure we only accumulate whole program
118 statistics to the correct summary. An object file might be embedded
119 in two separate programs, and we must keep the two program
120 summaries separate. */
121
122 /*
123 * === FUNCTION ======================================================================
124 * Name: gcov_exit
125 * Description: This function dumps the coverage couns. The merging with
126 * existing counts is not done in embedded systems.
127 * =====================================================================================
128 */
129 void gcov_exit (void)
130 {
131 struct gcov_info *gi_ptr;
132 struct gcov_summary this_program;
133 struct gcov_summary all;
134 struct gcov_ctr_summary *cs_ptr;
135 const struct gcov_ctr_info *ci_ptr;
136 unsigned t_ix;
137 gcov_unsigned_t c_num;
138 unsigned long coreId = 0;
139
140 /* retrieve the id of the CPU the program is running on */
141 #ifdef LEON3
142 __asm__ __volatile__("rd %%asr17,%0\n\t"
143 "srl %0,28,%0" :
144 "=&r" (coreId) : );
145 #endif
146
147 printf("_GCOVEXIT_BEGIN_,core%d\n", coreId); /* see also _GCOVEXIT_END_ */
148
149 if(gcov_list == (void*)0x0)
150 printf("%s: gcov_list == NULL\n", __func__);
151
152 memset (&all, 0, sizeof (all));
153 /* Find the totals for this execution. */
154 memset (&this_program, 0, sizeof (this_program));
155 for (gi_ptr = gcov_list; gi_ptr; gi_ptr = gi_ptr->next)
156 {
157
158 ci_ptr = gi_ptr->counts;
159 for (t_ix = 0; t_ix < GCOV_COUNTERS_SUMMABLE; t_ix++)
160 {
161 if (!((1 << t_ix) & gi_ptr->ctr_mask))
162 continue;
163
164 cs_ptr = &this_program.ctrs[t_ix];
165 cs_ptr->num += ci_ptr->num;
166 for (c_num = 0; c_num < ci_ptr->num; c_num++)
167 {
168 cs_ptr->sum_all += ci_ptr->values[c_num];
169 if (cs_ptr->run_max < ci_ptr->values[c_num])
170 cs_ptr->run_max = ci_ptr->values[c_num];
171 }
172 ci_ptr++;
173 }
174 }
175 /* Now merge each file. */
176 for (gi_ptr = gcov_list; gi_ptr; gi_ptr = gi_ptr->next)
177 {
178
179 struct gcov_summary program;
180 gcov_type *values[GCOV_COUNTERS];
181 const struct gcov_fn_info *fi_ptr;
182 unsigned fi_stride;
183 unsigned c_ix, f_ix, n_counts;
184
185 c_ix = 0;
186 for (t_ix = 0; t_ix < GCOV_COUNTERS; t_ix++)
187 if ((1 << t_ix) & gi_ptr->ctr_mask)
188 {
189 values[c_ix] = gi_ptr->counts[c_ix].values;
190 c_ix++;
191 }
192
193 /* Calculate the function_info stride. This depends on the
194 number of counter types being measured. */
195 fi_stride = sizeof (struct gcov_fn_info) + c_ix * sizeof (unsigned);
196 if (__alignof__ (struct gcov_fn_info) > sizeof (unsigned))
197 {
198 fi_stride += __alignof__ (struct gcov_fn_info) - 1;
199 fi_stride &= ~(__alignof__ (struct gcov_fn_info) - 1);
200 }
201
202 if (!gcov_open (gi_ptr->filename))
203 {
204 printf ("profiling:%s:Cannot open\n", gi_ptr->filename);
205 continue;
206 }
207
208 program.checksum = gcov_crc32;
209
210 /* Write out the data. */
211 gcov_write_tag_length (GCOV_DATA_MAGIC, GCOV_VERSION);
212 gcov_write_unsigned (gi_ptr->stamp);
213
214 /* Write execution counts for each function. */
215 for (f_ix = 0; f_ix < gi_ptr->n_functions; f_ix++)
216 {
217 fi_ptr = (const struct gcov_fn_info *)
218 ((const char *) gi_ptr->functions + f_ix * fi_stride);
219
220 /* Announce function. */
221 gcov_write_tag_length (GCOV_TAG_FUNCTION, GCOV_TAG_FUNCTION_LENGTH);
222 gcov_write_unsigned (fi_ptr->ident);
223 gcov_write_unsigned (fi_ptr->checksum);
224
225 c_ix = 0;
226 for (t_ix = 0; t_ix < GCOV_COUNTERS; t_ix++)
227 {
228 gcov_type *c_ptr;
229
230 if (!((1 << t_ix) & gi_ptr->ctr_mask))
231 continue;
232
233 n_counts = fi_ptr->n_ctrs[c_ix];
234
235 gcov_write_tag_length (GCOV_TAG_FOR_COUNTER (t_ix),
236 GCOV_TAG_COUNTER_LENGTH (n_counts));
237 c_ptr = values[c_ix];
238 while (n_counts--)
239 gcov_write_counter (*c_ptr++);
240
241 values[c_ix] = c_ptr;
242 c_ix++;
243 }
244 }
245
246 gcov_send();
247 gcov_close();
248
249 }
250
251 printf("_GCOVEXIT_END_,core%d\n", coreId);
252 }
253
254
255 /* Called before fork or exec - write out profile information gathered so
256 far and reset it to zero. This avoids duplication or loss of the
257 profile information gathered so far. */
258
259 void
260 __gcov_flush (void)
261 {
262 const struct gcov_info *gi_ptr;
263
264 gcov_exit ();
265 for (gi_ptr = gcov_list; gi_ptr; gi_ptr = gi_ptr->next)
266 {
267 unsigned t_ix;
268 const struct gcov_ctr_info *ci_ptr;
269
270 for (t_ix = 0, ci_ptr = gi_ptr->counts; t_ix != GCOV_COUNTERS; t_ix++)
271 if ((1 << t_ix) & gi_ptr->ctr_mask)
272 {
273 memset (ci_ptr->values, 0, sizeof (gcov_type) * ci_ptr->num);
274 ci_ptr++;
275 }
276 }
277 }
278
279
280
281 /* Open a gcov file. NAME is the name of the file to open and MODE
282 indicates whether a new file should be created, or an existing file
283 opened for modification. If MODE is >= 0 an existing file will be
284 opened, if possible, and if MODE is <= 0, a new file will be
285 created. Use MODE=0 to attempt to reopen an existing file and then
286 fall back on creating a new one. Return zero on failure, >0 on
287 opening an existing file and <0 on creating a new one. */
288 GCOV_LINKAGE int gcov_open(const char *name)
289 {
290 // gcov_var.start is cleared in the gcov_close function.
291 // If this variable is not cleared...ERROR
292 if( gcov_var.start != 0 )
293 return 0;
294
295 // Clear everything
296 gcov_var.start = 0;
297 gcov_var.offset = gcov_var.length = 0;
298 gcov_var.overread = -1u;
299 gcov_var.error = 0;
300
301
302 // copy the filename in the gcov_var structure
303 strcpy(gcov_var.filename, name);
304
305
306 // return 1 means everything is OK
307 return 1;
308 }
309
310 /* Close the current gcov file. Flushes data to disk. Returns nonzero
311 on failure or error flag set. */
312
313 GCOV_LINKAGE int gcov_send (void)
314 {
315 /*printf("%s: file %s\n", __func__, gcov_var.filename);*/
316 if (gcov_var.offset)
317 gcov_write_block (gcov_var.offset);
318
319 gcov_var.length = 0;
320 return gcov_var.error;
321 }
322
323 GCOV_LINKAGE int gcov_close(void)
324 {
325 memset(gcov_var.filename, 0, strlen(gcov_var.filename));
326
327 // Clear the start variable because will be tested in the gcov_open
328 // function
329 gcov_var.start = 0;
330
331 // Return the error, not sure whether the error is modifed.
332 return gcov_var.error;
333 }
334
335
336 static void gcov_write_block (unsigned size) {
337 unsigned char *buffer = (unsigned char*) gcov_var.buffer;
338 unsigned int i;
339
340 printf("_GCOV_,%s,", gcov_var.filename);
341 /* to speed up the printing process, we display bytes 4 by 4 */
342 for(i = 0; i < size; i++) {
343 printf("%02X%02X%02X%02X", (unsigned int)(buffer[0]),
344 (unsigned int)(buffer[1]),
345 (unsigned int)(buffer[2]),
346 (unsigned int)(buffer[3]));
347
348 buffer += sizeof(gcov_unsigned_t);
349 }
350 printf("\n");
351
352 gcov_var.start += size;
353 gcov_var.offset -= size;
354 }
355
356 /* Allocate space to write BYTES bytes to the gcov file. Return a
357 pointer to those bytes, or NULL on failure. */
358
359 static gcov_unsigned_t *gcov_write_words (unsigned words) {
360 gcov_unsigned_t *result;
361
362 GCOV_CHECK_WRITING ();
363 if (gcov_var.offset >= GCOV_BLOCK_SIZE)
364 {
365 gcov_write_block (GCOV_BLOCK_SIZE);
366 if (gcov_var.offset)
367 {
368 GCOV_CHECK (gcov_var.offset == 1);
369 memcpy (gcov_var.buffer, gcov_var.buffer + GCOV_BLOCK_SIZE, 4);
370 }
371 }
372 result = &gcov_var.buffer[gcov_var.offset];
373 gcov_var.offset += words;
374
375 return result;
376 }
377
378 /* Write unsigned VALUE to coverage file. Sets error flag
379 appropriately. */
380
381 GCOV_LINKAGE void
382 gcov_write_unsigned (gcov_unsigned_t value)
383 {
384 gcov_unsigned_t *buffer = gcov_write_words (1);
385
386 buffer[0] = value;
387 }
388
389 /* Write counter VALUE to coverage file. Sets error flag
390 appropriately. */
391
392 GCOV_LINKAGE void
393 gcov_write_counter (gcov_type value)
394 {
395 gcov_unsigned_t *buffer = gcov_write_words (2);
396
397 buffer[0] = (gcov_unsigned_t) value;
398 if (sizeof (value) > sizeof (gcov_unsigned_t))
399 buffer[1] = (gcov_unsigned_t) (value >> 32);
400 else
401 buffer[1] = 0;
402
403 }
404
405 /* Write a tag TAG and length LENGTH. */
406
407 GCOV_LINKAGE void
408 gcov_write_tag_length (gcov_unsigned_t tag, gcov_unsigned_t length)
409 {
410 gcov_unsigned_t *buffer = gcov_write_words (2);
411
412 buffer[0] = tag;
413 buffer[1] = length;
414 }
415
416 /* Write a summary structure to the gcov file. Return nonzero on
417 overflow. */
418
419 GCOV_LINKAGE void
420 gcov_write_summary (gcov_unsigned_t tag, const struct gcov_summary *summary)
421 {
422 unsigned ix;
423 const struct gcov_ctr_summary *csum;
424
425 gcov_write_tag_length (tag, GCOV_TAG_SUMMARY_LENGTH);
426 gcov_write_unsigned (summary->checksum);
427 for (csum = summary->ctrs, ix = GCOV_COUNTERS_SUMMABLE; ix--; csum++)
428 {
429 gcov_write_unsigned (csum->num);
430 gcov_write_unsigned (csum->runs);
431 gcov_write_counter (csum->sum_all);
432 gcov_write_counter (csum->run_max);
433 gcov_write_counter (csum->sum_max);
434 }
435 }
436
437 GCOV_LINKAGE gcov_type
438 gcov_read_counter (void)
439 {
440 return 0;
441 }
442
443 /* Add a new object file onto the bb chain. Invoked automatically
444 when running an object file's global ctors. */
445
446 void
447 __gcov_init (struct gcov_info *info)
448 {
449 if (!info->version)
450 return;
451 if (gcov_version (info, info->version))
452 {
453 const char *ptr = info->filename;
454 gcov_unsigned_t crc32 = gcov_crc32;
455
456 /* Added by LESIA*/
457 printf("Covered file: %s\n", info->filename);
458 /* End of Added by LESIA*/
459
460 do
461 {
462 unsigned ix;
463 gcov_unsigned_t value = *ptr << 24;
464
465 for (ix = 8; ix--; value <<= 1)
466 {
467 gcov_unsigned_t feedback;
468
469 feedback = (value ^ crc32) & 0x80000000 ? 0x04c11db7 : 0;
470 crc32 <<= 1;
471 crc32 ^= feedback;
472 }
473 }
474 while (*ptr++);
475
476 gcov_crc32 = crc32;
477
478 if (!gcov_list)
479 atexit (gcov_exit);
480
481 info->next = gcov_list;
482 gcov_list = info;
483 }
484 else
485 printf("%s: Version mismatch\n", "WARNING");
486 info->version = 0;
487 }
488 //#endif /* __GNUC__ __GNUC_MINOR__ __GNUC_PATCHLEVEL__ */
Show file before | Show file after | Hide comments
@@ -0,0 +1,485
1 /* Test for GCC >= 3.4.4 && <= 4.4.6 */
2 //#if ( ( __GNUC__ > 3 ) || \
3 // ( __GNUC__ == 3 && __GNUC_MINOR__ > 4 )|| \
4 // ( __GNUC__ == 3 && __GNUC_MINOR__ == 4 && __GNUC_PATCHLEVEL__ >= 4 ) ) && \
5 // ( ( __GNUC__ < 4 ) || \
6 // ( __GNUC__ == 4 && __GNUC_MINOR__ < 4 )|| \
7 // ( __GNUC__ == 4 && __GNUC_MINOR__ == 4 && __GNUC_PATCHLEVEL__ <= 6 ) )
8 //#include <stdlib.h>
9 //#include <stdio.h>
10 /* File format for coverage information
11 Copyright (C) 1996, 1997, 1998, 2000, 2002,
12 2003, 2004 Free Software Foundation, Inc.
13 Contributed by Bob Manson <manson@cygnus.com>.
14 Completely remangled by Nathan Sidwell <nathan@codesourcery.com>.
15
16 This file is part of GCC.
17
18 GCC is free software; you can redistribute it and/or modify it under
19 the terms of the GNU General Public License as published by the Free
20 Software Foundation; either version 2, or (at your option) any later
21 version.
22
23 GCC is distributed in the hope that it will be useful, but WITHOUT ANY
24 WARRANTY; without even the implied warranty of MERCHANTABILITY or
25 FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
26 for more details.
27
28 You should have received a copy of the GNU General Public License
29 along with GCC; see the file COPYING. If not, write to the Free
30 Software Foundation, 59 Temple Place - Suite 330, Boston, MA
31 02111-1307, USA. */
32
33 /* As a special exception, if you link this library with other files,
34 some of which are compiled with GCC, to produce an executable,
35 this library does not by itself cause the resulting executable
36 to be covered by the GNU General Public License.
37 This exception does not however invalidate any other reasons why
38 the executable file might be covered by the GNU General Public License. */
39
40 /* Coverage information is held in two files. A notes file, which is
41 generated by the compiler, and a data file, which is generated
42 by the program under test. Both files use a similar structure. We
43 do not attempt to make these files backwards compatible with
44 previous versions, as you only need coverage information when
45 developing a program. We do hold version information, so that
46 mismatches can be detected, and we use a format that allows tools
47 to skip information they do not understand or are not interested
48 in.
49
50 Numbers are recorded in the 32 bit unsigned binary form of the
51 endianness of the machine generating the file. 64 bit numbers are
52 stored as two 32 bit numbers, the low part first. Strings are
53 padded with 1 to 4 NUL bytes, to bring the length up to a multiple
54 of 4. The number of 4 bytes is stored, followed by the padded
55 string. Zero length and NULL strings are simply stored as
56 a length of zero (they have no trailing NUL or padding).
57
58 int32: byte3 byte2 byte1 byte0 | byte0 byte1 byte2 byte3
59 int64: int32:low int32:high
60 string: int32:0 | int32:length char* char:0 padding
61 padding: | char:0 | char:0 char:0 | char:0 char:0 char:0
62 item: int32 | int64 | string
63
64 The basic format of the files is
65
66 file : int32:magic int32:version int32:stamp record*
67
68 The magic ident is different for the notes and the data files. The
69 magic ident is used to determine the endianness of the file, when
70 reading. The version is the same for both files and is derived
71 from gcc's version number. The stamp value is used to synchronize
72 note and data files and to synchronize merging within a data
73 file. It need not be an absolute time stamp, merely a ticker that
74 increments fast enough and cycles slow enough to distinguish
75 different compile/run/compile cycles.
76
77 Although the ident and version are formally 32 bit numbers, they
78 are derived from 4 character ASCII strings. The version number
79 consists of the single character major version number, a two
80 character minor version number (leading zero for versions less than
81 10), and a single character indicating the status of the release.
82 That will be 'e' experimental, 'p' prerelease and 'r' for release.
83 Because, by good fortune, these are in alphabetical order, string
84 collating can be used to compare version strings. Be aware that
85 the 'e' designation will (naturally) be unstable and might be
86 incompatible with itself. For gcc 3.4 experimental, it would be
87 '304e' (0x33303465). When the major version reaches 10, the
88 letters A-Z will be used. Assuming minor increments releases every
89 6 months, we have to make a major increment every 50 years.
90 Assuming major increments releases every 5 years, we're ok for the
91 next 155 years -- good enough for me.
92
93 A record has a tag, length and variable amount of data.
94
95 record: header data
96 header: int32:tag int32:length
97 data: item*
98
99 Records are not nested, but there is a record hierarchy. Tag
100 numbers reflect this hierarchy. Tags are unique across note and
101 data files. Some record types have a varying amount of data. The
102 LENGTH is the number of 4bytes that follow and is usually used to
103 determine how much data. The tag value is split into 4 8-bit
104 fields, one for each of four possible levels. The most significant
105 is allocated first. Unused levels are zero. Active levels are
106 odd-valued, so that the LSB of the level is one. A sub-level
107 incorporates the values of its superlevels. This formatting allows
108 you to determine the tag hierarchy, without understanding the tags
109 themselves, and is similar to the standard section numbering used
110 in technical documents. Level values [1..3f] are used for common
111 tags, values [41..9f] for the notes file and [a1..ff] for the data
112 file.
113
114 The basic block graph file contains the following records
115 note: unit function-graph*
116 unit: header int32:checksum string:source
117 function-graph: announce_function basic_blocks {arcs | lines}*
118 announce_function: header int32:ident int32:checksum
119 string:name string:source int32:lineno
120 basic_block: header int32:flags*
121 arcs: header int32:block_no arc*
122 arc: int32:dest_block int32:flags
123 lines: header int32:block_no line*
124 int32:0 string:NULL
125 line: int32:line_no | int32:0 string:filename
126
127 The BASIC_BLOCK record holds per-bb flags. The number of blocks
128 can be inferred from its data length. There is one ARCS record per
129 basic block. The number of arcs from a bb is implicit from the
130 data length. It enumerates the destination bb and per-arc flags.
131 There is one LINES record per basic block, it enumerates the source
132 lines which belong to that basic block. Source file names are
133 introduced by a line number of 0, following lines are from the new
134 source file. The initial source file for the function is NULL, but
135 the current source file should be remembered from one LINES record
136 to the next. The end of a block is indicated by an empty filename
137 - this does not reset the current source file. Note there is no
138 ordering of the ARCS and LINES records: they may be in any order,
139 interleaved in any manner. The current filename follows the order
140 the LINES records are stored in the file, *not* the ordering of the
141 blocks they are for.
142
143 The data file contains the following records.
144 data: {unit function-data* summary:object summary:program*}*
145 unit: header int32:checksum
146 function-data: announce_function arc_counts
147 announce_function: header int32:ident int32:checksum
148 arc_counts: header int64:count*
149 summary: int32:checksum {count-summary}GCOV_COUNTERS
150 count-summary: int32:num int32:runs int64:sum
151 int64:max int64:sum_max
152
153 The ANNOUNCE_FUNCTION record is the same as that in the note file,
154 but without the source location. The ARC_COUNTS gives the counter
155 values for those arcs that are instrumented. The SUMMARY records
156 give information about the whole object file and about the whole
157 program. The checksum is used for whole program summaries, and
158 disambiguates different programs which include the same
159 instrumented object file. There may be several program summaries,
160 each with a unique checksum. The object summary's checksum is zero.
161 Note that the data file might contain information from several runs
162 concatenated, or the data might be merged.
163
164 This file is included by both the compiler, gcov tools and the
165 runtime support library libgcov. IN_LIBGCOV and IN_GCOV are used to
166 distinguish which case is which. If IN_LIBGCOV is nonzero,
167 libgcov is being built. If IN_GCOV is nonzero, the gcov tools are
168 being built. Otherwise the compiler is being built. IN_GCOV may be
169 positive or negative. If positive, we are compiling a tool that
170 requires additional functions (see the code for knowledge of what
171 those functions are). */
172
173 #ifndef GCC_GCOV_IO_H
174 #define GCC_GCOV_IO_H
175
176 typedef unsigned int gcov_unsigned_t;
177 typedef unsigned int gcov_position_t;
178
179 typedef unsigned long long gcov_type;
180
181 /* No tengo ni idea de que es el SETLKW, asi que de momento el target
182 * no tiene de eso */
183
184 //#if defined (TARGET_HAS_F_SETLKW)
185 //#define GCOV_LOCKED 1
186 //#else
187 #define GCOV_LOCKED 0
188 //#endif
189 //#endif
190
191
192
193 /* In gcov we want function linkage to be static. In the compiler we want
194 it extern, so that they can be accessed from elsewhere. In libgcov we
195 need these functions to be extern, so prefix them with __gcov. In
196 libgcov they must also be hidden so that the instance in the executable
197 is not also used in a DSO. */
198
199 #define gcov_var __gcov_var
200 #define gcov_open __gcov_open
201 #define gcov_close __gcov_close
202 #define gcov_write_tag_length __gcov_write_tag_length
203 #define gcov_position __gcov_position
204 #define gcov_seek __gcov_seek
205 #define gcov_rewrite __gcov_rewrite
206 #define gcov_is_error __gcov_is_error
207 #define gcov_is_eof __gcov_is_eof
208 #define gcov_write_unsigned __gcov_write_unsigned
209 #define gcov_write_counter __gcov_write_counter
210 #define gcov_write_summary __gcov_write_summary
211 #define gcov_read_unsigned __gcov_read_unsigned
212 #define gcov_read_counter __gcov_read_counter
213 #define gcov_read_summary __gcov_read_summary
214
215 /* Esto no tengo ni repajolera idea de para que vale */
216
217 /* Poison these, so they don't accidentally slip in. */
218 //#pragma GCC poison gcov_write_string gcov_write_tag gcov_write_length
219 //#pragma GCC poison gcov_read_string gcov_sync gcov_time gcov_magic
220
221 #ifdef HAVE_GAS_HIDDEN
222 #define ATTRIBUTE_HIDDEN __attribute__ ((__visibility__ ("hidden")))
223 #else
224 #define ATTRIBUTE_HIDDEN
225 #endif
226
227 #ifndef GCOV_LINKAGE
228 #define GCOV_LINKAGE extern
229 //#define GCOV_LINKAGE
230 #endif
231
232 /* File suffixes. */
233 #define GCOV_DATA_SUFFIX ".gcda"
234 #define GCOV_NOTE_SUFFIX ".gcno"
235
236 /* File magic. Must not be palindromes. */
237 #define GCOV_DATA_MAGIC ((gcov_unsigned_t)0x67636461) /* "gcda" */
238 #define GCOV_NOTE_MAGIC ((gcov_unsigned_t)0x67636e6f) /* "gcno" */
239
240 /* gcov-iov.h is automatically generated by the makefile from
241 version.c, it looks like
242 #define GCOV_VERSION ((gcov_unsigned_t)0x89abcdef)
243 */
244 #include "gcov-iov.h"
245
246 /* Convert a magic or version number to a 4 character string. */
247 #define GCOV_UNSIGNED2STRING(ARRAY,VALUE) \
248 ((ARRAY)[0] = (char)((VALUE) >> 24), \
249 (ARRAY)[1] = (char)((VALUE) >> 16), \
250 (ARRAY)[2] = (char)((VALUE) >> 8), \
251 (ARRAY)[3] = (char)((VALUE) >> 0))
252
253 /* The record tags. Values [1..3f] are for tags which may be in either
254 file. Values [41..9f] for those in the note file and [a1..ff] for
255 the data file. */
256
257 #define GCOV_TAG_FUNCTION ((gcov_unsigned_t)0x01000000)
258 #define GCOV_TAG_FUNCTION_LENGTH (2)
259 #define GCOV_TAG_BLOCKS ((gcov_unsigned_t)0x01410000)
260 #define GCOV_TAG_BLOCKS_LENGTH(NUM) (NUM)
261 #define GCOV_TAG_BLOCKS_NUM(LENGTH) (LENGTH)
262 #define GCOV_TAG_ARCS ((gcov_unsigned_t)0x01430000)
263 #define GCOV_TAG_ARCS_LENGTH(NUM) (1 + (NUM) * 2)
264 #define GCOV_TAG_ARCS_NUM(LENGTH) (((LENGTH) - 1) / 2)
265 #define GCOV_TAG_LINES ((gcov_unsigned_t)0x01450000)
266 #define GCOV_TAG_COUNTER_BASE ((gcov_unsigned_t)0x01a10000)
267 #define GCOV_TAG_COUNTER_LENGTH(NUM) ((NUM) * 2)
268 #define GCOV_TAG_COUNTER_NUM(LENGTH) ((LENGTH) / 2)
269 #define GCOV_TAG_OBJECT_SUMMARY ((gcov_unsigned_t)0xa1000000)
270 #define GCOV_TAG_PROGRAM_SUMMARY ((gcov_unsigned_t)0xa3000000)
271 #define GCOV_TAG_SUMMARY_LENGTH \
272 (1 + GCOV_COUNTERS_SUMMABLE * (2 + 3 * 2))
273
274 /* Counters that are collected. */
275 #define GCOV_COUNTER_ARCS 0 /* Arc transitions. */
276 #define GCOV_COUNTERS_SUMMABLE 1 /* Counters which can be
277 summaried. */
278 #define GCOV_FIRST_VALUE_COUNTER 1 /* The first of counters used for value
279 profiling. They must form a consecutive
280 interval and their order must match
281 the order of HIST_TYPEs in
282 value-prof.h. */
283 #define GCOV_COUNTER_V_INTERVAL 1 /* Histogram of value inside an interval. */
284 #define GCOV_COUNTER_V_POW2 2 /* Histogram of exact power2 logarithm
285 of a value. */
286 #define GCOV_COUNTER_V_SINGLE 3 /* The most common value of expression. */
287 #define GCOV_COUNTER_V_DELTA 4 /* The most common difference between
288 consecutive values of expression. */
289 #define GCOV_LAST_VALUE_COUNTER 4 /* The last of counters used for value
290 profiling. */
291 #define GCOV_COUNTERS 5
292
293 /* Number of counters used for value profiling. */
294 #define GCOV_N_VALUE_COUNTERS \
295 (GCOV_LAST_VALUE_COUNTER - GCOV_FIRST_VALUE_COUNTER + 1)
296
297 /* A list of human readable names of the counters */
298 #define GCOV_COUNTER_NAMES {"arcs", "interval", "pow2", "single", "delta"}
299
300 /* Names of merge functions for counters. */
301 #define GCOV_MERGE_FUNCTIONS {"__gcov_merge_add", \
302 "__gcov_merge_add", \
303 "__gcov_merge_add", \
304 "__gcov_merge_single", \
305 "__gcov_merge_delta"}
306
307 /* Convert a counter index to a tag. */
308 #define GCOV_TAG_FOR_COUNTER(COUNT) \
309 (GCOV_TAG_COUNTER_BASE + ((gcov_unsigned_t)(COUNT) << 17))
310 /* Convert a tag to a counter. */
311 #define GCOV_COUNTER_FOR_TAG(TAG) \
312 ((unsigned)(((TAG) - GCOV_TAG_COUNTER_BASE) >> 17))
313 /* Check whether a tag is a counter tag. */
314 #define GCOV_TAG_IS_COUNTER(TAG) \
315 (!((TAG) & 0xFFFF) && GCOV_COUNTER_FOR_TAG (TAG) < GCOV_COUNTERS)
316
317 /* The tag level mask has 1's in the position of the inner levels, &
318 the lsb of the current level, and zero on the current and outer
319 levels. */
320 #define GCOV_TAG_MASK(TAG) (((TAG) - 1) ^ (TAG))
321
322 /* Return nonzero if SUB is an immediate subtag of TAG. */
323 #define GCOV_TAG_IS_SUBTAG(TAG,SUB) \
324 (GCOV_TAG_MASK (TAG) >> 8 == GCOV_TAG_MASK (SUB) \
325 && !(((SUB) ^ (TAG)) & ~GCOV_TAG_MASK(TAG)))
326
327 /* Return nonzero if SUB is at a sublevel to TAG. */
328 #define GCOV_TAG_IS_SUBLEVEL(TAG,SUB) \
329 (GCOV_TAG_MASK (TAG) > GCOV_TAG_MASK (SUB))
330
331 /* Basic block flags. */
332 #define GCOV_BLOCK_UNEXPECTED (1 << 1)
333
334 /* Arc flags. */
335 #define GCOV_ARC_ON_TREE (1 << 0)
336 #define GCOV_ARC_FAKE (1 << 1)
337 #define GCOV_ARC_FALLTHROUGH (1 << 2)
338
339 /* Structured records. */
340
341 /* Cumulative counter data. */
342 struct gcov_ctr_summary
343 {
344 gcov_unsigned_t num; /* number of counters. */
345 gcov_unsigned_t runs; /* number of program runs */
346 gcov_type sum_all; /* sum of all counters accumulated. */
347 gcov_type run_max; /* maximum value on a single run. */
348 gcov_type sum_max; /* sum of individual run max values. */
349 };
350
351
352 /* Object & program summary record. */
353 struct gcov_summary
354 {
355 gcov_unsigned_t checksum; /* checksum of program */
356 struct gcov_ctr_summary ctrs[GCOV_COUNTERS_SUMMABLE];
357 };
358
359 /* Structures embedded in coveraged program. The structures generated
360 by write_profile must match these. */
361
362 /* Information about a single function. This uses the trailing array
363 idiom. The number of counters is determined from the counter_mask
364 in gcov_info. We hold an array of function info, so have to
365 explicitly calculate the correct array stride. */
366 struct gcov_fn_info
367 {
368 gcov_unsigned_t ident; /* unique ident of function */
369 gcov_unsigned_t checksum; /* function checksum */
370 unsigned n_ctrs[0]; /* instrumented counters */
371 };
372
373 /* Type of function used to merge counters. */
374 typedef void (*gcov_merge_fn) (gcov_type *, gcov_unsigned_t);
375
376 /* Information about counters. */
377 struct gcov_ctr_info
378 {
379 gcov_unsigned_t num; /* number of counters. */
380 gcov_type *values; /* their values. */
381 gcov_merge_fn merge; /* The function used to merge them. */
382 };
383
384 /* Information about a single object file. */
385 struct gcov_info
386 {
387 gcov_unsigned_t version; /* expected version number */
388 struct gcov_info *next; /* link to next, used by libgcov */
389
390 gcov_unsigned_t stamp; /* uniquifying time stamp */
391 const char *filename; /* output file name */
392
393 unsigned n_functions; /* number of functions */
394 const struct gcov_fn_info *functions; /* table of functions */
395
396 unsigned ctr_mask; /* mask of counters instrumented. */
397 struct gcov_ctr_info counts[0]; /* count data. The number of bits
398 set in the ctr_mask field
399 determines how big this array
400 is. */
401 };
402
403 /* Register a new object file module. */
404 extern void __gcov_init (struct gcov_info *) ATTRIBUTE_HIDDEN;
405
406 /* Called before fork, to avoid double counting. */
407 extern void __gcov_flush (void) ATTRIBUTE_HIDDEN;
408
409 /* The merge function that just sums the counters. */
410 extern void __gcov_merge_add (gcov_type *, unsigned) ATTRIBUTE_HIDDEN;
411
412 /* The merge function to choose the most common value. */
413 extern void __gcov_merge_single (gcov_type *, unsigned) ATTRIBUTE_HIDDEN;
414
415 /* The merge function to choose the most common difference between
416 consecutive values. */
417 extern void __gcov_merge_delta (gcov_type *, unsigned) ATTRIBUTE_HIDDEN;
418
419 /* Optimum number of gcov_unsigned_t's read from or written to disk. */
420 // We limit GCOV_BLOCK_SIZE to 512 unsigned long because post processing with
421 // DOS batch cannot handle command lines bigger than 8191 characters, knowing
422 // that for each char, we print 4 characters (e.g "\x00")
423 #define GCOV_BLOCK_SIZE (1 << 11)
424 #define MAXFILENAME (1024)
425
426 GCOV_LINKAGE struct gcov_var
427 {
428 // FILE *file;
429 char filename[MAXFILENAME];
430 gcov_position_t start; /* Position of first byte of block */
431 unsigned offset; /* Read/write position within the block. */
432 unsigned length; /* Read limit in the block. */
433 unsigned overread; /* Number of words overread. */
434 int error; /* < 0 overflow, > 0 disk error. */
435 /* Holds one block plus 4 bytes, thus all coverage reads & writes
436 fit within this buffer and we always can transfer GCOV_BLOCK_SIZE
437 to and from the disk. libgcov never backtracks and only writes 4
438 or 8 byte objects. */
439 gcov_unsigned_t buffer[GCOV_BLOCK_SIZE + 1];
440 } gcov_var ATTRIBUTE_HIDDEN;
441
442 #if 1
443 /* Functions for reading and writing gcov files. In libgcov you can
444 open the file for reading then writing. Elsewhere you can open the
445 file either for reading or for writing. When reading a file you may
446 use the gcov_read_* functions, gcov_sync, gcov_position, &
447 gcov_error. When writing a file you may use the gcov_write
448 functions, gcov_seek & gcov_error. When a file is to be rewritten
449 you use the functions for reading, then gcov_rewrite then the
450 functions for writing. Your file may become corrupted if you break
451 these invariants. */
452 GCOV_LINKAGE int gcov_open (const char */*name*/) ATTRIBUTE_HIDDEN;
453 GCOV_LINKAGE int gcov_close (void) ATTRIBUTE_HIDDEN;
454
455 /* Available everywhere. */
456 /* static gcov_position_t gcov_position (void);
457 * static int gcov_is_error (void);
458 * static int gcov_is_eof (void);
459 */
460
461 GCOV_LINKAGE gcov_unsigned_t gcov_read_unsigned (void) ATTRIBUTE_HIDDEN;
462 GCOV_LINKAGE gcov_type gcov_read_counter (void) ATTRIBUTE_HIDDEN;
463 GCOV_LINKAGE void gcov_read_summary (struct gcov_summary *) ATTRIBUTE_HIDDEN;
464 #endif
465 /* Available only in libgcov */
466 GCOV_LINKAGE void gcov_write_counter (gcov_type) ATTRIBUTE_HIDDEN;
467 GCOV_LINKAGE void gcov_write_tag_length (gcov_unsigned_t,
468 gcov_unsigned_t) ATTRIBUTE_HIDDEN;
469 GCOV_LINKAGE void gcov_write_summary (gcov_unsigned_t /*tag*/,
470 const struct gcov_summary *) ATTRIBUTE_HIDDEN;
471
472 /* Available outside gcov */
473 GCOV_LINKAGE void gcov_write_unsigned (gcov_unsigned_t) ATTRIBUTE_HIDDEN;
474
475 /* Make sure the library is used correctly. */
476 #if ENABLE_CHECKING
477 #define GCOV_CHECK(expr) ((expr) ? (void)0 : (void)abort ())
478 #else
479 #define GCOV_CHECK(expr)
480 #endif
481 #define GCOV_CHECK_READING() GCOV_CHECK(gcov_var.mode > 0)
482 #define GCOV_CHECK_WRITING() GCOV_CHECK(gcov_var.mode < 0)
483
484 #endif /* GCC_GCOV_IO_H */
485 //#endif /* __GNUC__ __GNUC_MINOR__ __GNUC_PATCHLEVEL__ */
Show file before | Show file after | Hide comments
@@ -0,0 +1,13
1 /* Generated by the program `C:\home\obswteam\workspace\sparc-gcov-master\tools\version\gcov-iov.exe'
2 from `4.4 (4 4) and (*)'. */
3 #if ( __GNUC__ == 3 && __GNUC_MINOR__ == 4 )
4 #define GCOV_VERSION ((gcov_unsigned_t)0x3330342a) /* type 'gcov-iov 3.4 ""' */
5 #endif /* __GNUC__ __GNUC_MINOR__ */
6
7 #if ( __GNUC__ == 4 && __GNUC_MINOR__ == 4 )
8 #define GCOV_VERSION ((gcov_unsigned_t)0x3430342a) /* type 'gcov-iov 4.4 ""' */
9 #endif /* __GNUC__ __GNUC_MINOR__ */
10
11 #if ( __GNUC__ > 4 )
12 #define GCOV_VERSION ((gcov_unsigned_t)0x3430342a) /* type 'gcov-iov 4.4 ""' */
13 #endif /* __GNUC__ __GNUC_MINOR__ */
Show file before | Show file after | Hide comments
@@ -0,0 +1,134
1 /* Test for GCC >= 3.4.4 && <= 4.4.6 */
2 /*#if ( ( __GNUC__ > 3 ) || \
3 ( __GNUC__ == 3 && __GNUC_MINOR__ > 4 )|| \
4 ( __GNUC__ == 3 && __GNUC_MINOR__ == 4 && __GNUC_PATCHLEVEL__ >= 4 ) ) && \
5 ( ( __GNUC__ < 4 ) || \
6 ( __GNUC__ == 4 && __GNUC_MINOR__ < 4 )|| \
7 ( __GNUC__ == 4 && __GNUC_MINOR__ == 4 && __GNUC_PATCHLEVEL__ <= 6 ) )
8 */
9 /* Routines required for instrumenting a program. */
10 /* Compile this one with gcc. */
11 /* Copyright (C) 1989, 1992, 1993, 1994, 1995, 1996, 1997, 1998, 1999,
12 2000, 2001, 2002, 2003 Free Software Foundation, Inc.
13
14 This file is part of GCC.
15
16 GCC is free software; you can redistribute it and/or modify it under
17 the terms of the GNU General Public License as published by the Free
18 Software Foundation; either version 2, or (at your option) any later
19 version.
20
21 In addition to the permissions in the GNU General Public License, the
22 Free Software Foundation gives you unlimited permission to link the
23 compiled version of this file into combinations with other programs,
24 and to distribute those combinations without any restriction coming
25 from the use of this file. (The General Public License restrictions
26 do apply in other respects; for example, they cover modification of
27 the file, and distribution when not linked into a combine
28 executable.)
29
30 GCC is distributed in the hope that it will be useful, but WITHOUT ANY
31 WARRANTY; without even the implied warranty of MERCHANTABILITY or
32 FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
33 for more details.
34
35 You should have received a copy of the GNU General Public License
36 along with GCC; see the file COPYING. If not, write to the Free
37 Software Foundation, 59 Temple Place - Suite 330, Boston, MA
38 02111-1307, USA. */
39
40 #define GCOV_LINKAGE /* nothing */
41
42 #include "gcov-io.h"
43
44 /* Chain of per-object gcov structures. */
45 struct gcov_info *gcov_list;
46
47 /* A program checksum allows us to distinguish program data for an
48 object file included in multiple programs. */
49 gcov_unsigned_t gcov_crc32;
50
51 /* The profile merging function that just adds the counters. It is given
52 an array COUNTERS of N_COUNTERS old counters and it reads the same number
53 of counters from the gcov file. */
54 void
55 __gcov_merge_add (gcov_type *counters, unsigned n_counters)
56 {
57 for (; n_counters; counters++, n_counters--)
58 *counters += gcov_read_counter ();
59 }
60 /* The profile merging function for choosing the most common value.
61 It is given an array COUNTERS of N_COUNTERS old counters and it
62 reads the same number of counters from the gcov file. The counters
63 are split into 3-tuples where the members of the tuple have
64 meanings:
65
66 -- the stored candidate on the most common value of the measured entity
67 -- counter
68 -- total number of evaluations of the value */
69 void
70 __gcov_merge_single (gcov_type *counters, unsigned n_counters)
71 {
72 unsigned i, n_measures;
73 gcov_type value, counter, all;
74
75 GCOV_CHECK (!(n_counters % 3));
76 n_measures = n_counters / 3;
77 for (i = 0; i < n_measures; i++, counters += 3)
78 {
79 value = gcov_read_counter ();
80 counter = gcov_read_counter ();
81 all = gcov_read_counter ();
82
83 if (counters[0] == value)
84 counters[1] += counter;
85 else if (counter > counters[1])
86 {
87 counters[0] = value;
88 counters[1] = counter - counters[1];
89 }
90 else
91 counters[1] -= counter;
92 counters[2] += all;
93 }
94 }
95
96 /* The profile merging function for choosing the most common
97 difference between two consecutive evaluations of the value. It is
98 given an array COUNTERS of N_COUNTERS old counters and it reads the
99 same number of counters from the gcov file. The counters are split
100 into 4-tuples where the members of the tuple have meanings:
101
102 -- the last value of the measured entity
103 -- the stored candidate on the most common difference
104 -- counter
105 -- total number of evaluations of the value */
106
107 void
108 __gcov_merge_delta (gcov_type *counters, unsigned n_counters)
109 {
110 unsigned i, n_measures;
111 gcov_type last, value, counter, all;
112
113 GCOV_CHECK (!(n_counters % 4));
114 n_measures = n_counters / 4;
115 for (i = 0; i < n_measures; i++, counters += 4)
116 {
117 last = gcov_read_counter ();
118 value = gcov_read_counter ();
119 counter = gcov_read_counter ();
120 all = gcov_read_counter ();
121
122 if (counters[1] == value)
123 counters[2] += counter;
124 else if (counter > counters[2])
125 {
126 counters[1] = value;
127 counters[2] = counter - counters[2];
128 }
129 else
130 counters[2] -= counter;
131 counters[3] += all;
132 }
133 }
134 //#endif /* __GNUC__ __GNUC_MINOR__ __GNUC_PATCHLEVEL__ */
Show file before | Show file after | Hide comments
@@ -0,0 +1,40
1 #!/usr/bin/env python3
2
3 __author__ = "Alexis Jeandet"
4 __copyright__ = "Copyright 2018, Laboratory of Plasma Physics"
5 __credits__ = []
6 __license__ = "GPLv2"
7 __version__ = "1.0.0"
8 __maintainer__ = "Alexis Jeandet"
9 __email__ = "alexis.jeandet@member.fsf.org"
10 __status__ = "Development"
11
12 import time
13 import sys
14 import os
15 import serial
16 import argparse
17 from datetime import datetime
18
19 parser = argparse.ArgumentParser()
20 parser.add_argument("-p", "--port", help="Serial port")
21 parser.add_argument("-s", "--speed", help="Baud rate")
22 args = parser.parse_args()
23
24
25
26 def main():
27 with open('gcov_out_'+str(datetime.now())+'.txt','w') as gcov:
28 with open('console_'+str(datetime.now())+'.txt','w') as console:
29 with serial.Serial(args.port, args.speed, timeout=None) as ser:
30 line = ser.readline().decode()
31 while '_GCOVEXIT_BEGIN_' not in line:
32 console.write(line)
33 line = ser.readline().decode()
34 line = ser.readline().decode()
35 while '_GCOVEXIT_END_' not in line:
36 gcov.write(line)
37 line = ser.readline().decode()
38
39 if __name__ == "__main__":
40 main()
Show file before | Show file after | Hide comments
@@ -1,4 +1,4
1 header/lfr_common_headers = https://hephaistos.lpp.polytechnique.fr/rhodecode/HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/lfr_common_headers
1 header/lfr_common_headers = https://hephaistos.lpp.polytechnique.fr/rhodecode/HG_REPOSITORIES/LPP/INSTRUMENTATION/USERS/JEANDET/lfr_common_headers
2 2
3 3 LFR_basic-parameters = https://hephaistos.lpp.polytechnique.fr/rhodecode/HG_REPOSITORIES/LPP/INSTRUMENTATION/USERS/CHUST/LFR_basic-parameters
4 4
Show file before | Show file after | Hide comments
@@ -1,2 +1,2
1 1 3081d1f9bb20b2b64a192585337a292a9804e0c5 LFR_basic-parameters
2 e904b329ff977514bf36af92617afefd22fd06ab header/lfr_common_headers
2 f5b83fb540b1cfd5d87c68621fb53f238eb623ae header/lfr_common_headers
Show file before | Show file after | Hide comments
@@ -1,14 +1,14
1 1 cmake_minimum_required (VERSION 2.6)
2 2 project (LFR_FSW)
3 3
4 4 if(NOT CMAKE_BUILD_TYPE)
5 5 set(CMAKE_BUILD_TYPE "Release" CACHE STRING
6 6 "Choose the type of build, options are: Debug Release RelWithDebInfo MinSizeRel." FORCE)
7 7 endif(NOT CMAKE_BUILD_TYPE)
8 8
9 9 set(LFR_BP_SRC ${CMAKE_CURRENT_SOURCE_DIR}/LFR_basic-parameters/basic_parameters.c)
10 10
11 11 SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/sparc")
12 12
13 add_subdirectory(libgcov)
13 14 add_subdirectory(src)
14 #add_subdirectory(timegen)
Show file before | Show file after | Hide comments
@@ -1,8 +1,32
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 #ifndef GRSPW_H_INCLUDED
2 26 #define GRSPW_H_INCLUDED
3 27
4 28 int grspw_set_ie( unsigned char value, unsigned int *ctrlReg );
5 29 int grspw_set_tq( unsigned char value, unsigned int *ctrlReg );
6 30 int grspw_set_tr( unsigned char value, unsigned int *ctrlReg );
7 31
8 32 #endif // GRSPW_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,169 +1,156
1 1 #ifndef FSW_MISC_H_INCLUDED
2 2 #define FSW_MISC_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <stdio.h>
6 6 #include <grspw.h>
7 7 #include <grlib_regs.h>
8 8
9 9 #include "fsw_params.h"
10 10 #include "fsw_spacewire.h"
11 11 #include "lfr_cpu_usage_report.h"
12 12
13 #define LFR_RESET_CAUSE_UNKNOWN_CAUSE 0
14 13 #define WATCHDOG_LOOP_PRINTF 10
15 14 #define WATCHDOG_LOOP_DEBUG 3
16 15
17 #define DUMB_MESSAGE_NB 15
18 16 #define NB_RTEMS_EVENTS 32
19 17 #define EVENT_12 12
20 18 #define EVENT_13 13
21 19 #define EVENT_14 14
22 #define DUMB_MESSAGE_0 "in DUMB *** default"
23 20 #define DUMB_MESSAGE_1 "in DUMB *** timecode_irq_handler"
24 #define DUMB_MESSAGE_2 "in DUMB *** f3 buffer changed"
25 #define DUMB_MESSAGE_3 "in DUMB *** in SMIQ *** Error sending event to AVF0"
26 #define DUMB_MESSAGE_4 "in DUMB *** spectral_matrices_isr *** Error sending event to SMIQ"
27 #define DUMB_MESSAGE_5 "in DUMB *** waveforms_simulator_isr"
28 #define DUMB_MESSAGE_6 "VHDL SM *** two buffers f0 ready"
29 #define DUMB_MESSAGE_7 "ready for dump"
30 #define DUMB_MESSAGE_8 "VHDL ERR *** spectral matrix"
31 #define DUMB_MESSAGE_9 "tick"
32 #define DUMB_MESSAGE_10 "VHDL ERR *** waveform picker"
33 #define DUMB_MESSAGE_11 "VHDL ERR *** unexpected ready matrix values"
34 21 #define DUMB_MESSAGE_12 "WATCHDOG timer"
35 22 #define DUMB_MESSAGE_13 "TIMECODE timer"
36 #define DUMB_MESSAGE_14 "TIMECODE ISR"
37 23
38 24 enum lfr_reset_cause_t{
39 25 UNKNOWN_CAUSE,
40 26 POWER_ON,
41 27 TC_RESET,
42 28 WATCHDOG,
43 29 ERROR_RESET,
44 30 UNEXP_RESET
45 31 };
46 32
47 33 typedef struct{
48 34 unsigned char dpu_spw_parity;
49 35 unsigned char dpu_spw_disconnect;
50 36 unsigned char dpu_spw_escape;
51 37 unsigned char dpu_spw_credit;
52 38 unsigned char dpu_spw_write_sync;
53 39 unsigned char timecode_erroneous;
54 40 unsigned char timecode_missing;
55 41 unsigned char timecode_invalid;
56 42 unsigned char time_timecode_it;
57 43 unsigned char time_not_synchro;
58 44 unsigned char time_timecode_ctr;
59 45 unsigned char ahb_correctable;
60 46 } hk_lfr_le_t;
61 47
62 48 typedef struct{
63 49 unsigned char dpu_spw_early_eop;
64 50 unsigned char dpu_spw_invalid_addr;
65 51 unsigned char dpu_spw_eep;
66 52 unsigned char dpu_spw_rx_too_big;
67 53 } hk_lfr_me_t;
68 54
69 55 #define B00 196
70 56 #define B01 196
71 57 #define B02 0
72 58 #define B10 131
73 59 #define B11 -244
74 60 #define B12 131
75 61 #define B20 161
76 62 #define B21 -314
77 63 #define B22 161
78 64
79 65 #define A00 1
80 66 #define A01 -925
81 67 #define A02 0
82 68 #define A10 1
83 69 #define A11 -947
84 70 #define A12 439
85 71 #define A20 1
86 72 #define A21 -993
87 73 #define A22 486
88 74
89 75 #define GAIN_B0 12
90 76 #define GAIN_B1 11
91 77 #define GAIN_B2 10
92 78
93 79 #define GAIN_A0 10
94 80 #define GAIN_A1 9
95 81 #define GAIN_A2 9
96 82
97 83 #define NB_COEFFS 3
98 84 #define COEFF0 0
99 85 #define COEFF1 1
100 86 #define COEFF2 2
101 87
102 88 typedef struct filter_ctx
103 89 {
104 90 int W[NB_COEFFS][NB_COEFFS];
105 91 }filter_ctx;
106 92
107 93 extern gptimer_regs_t *gptimer_regs;
108 94 extern void ASR16_get_FPRF_IURF_ErrorCounters( unsigned int*, unsigned int* );
109 95 extern void CCR_getInstructionAndDataErrorCounters( unsigned int*, unsigned int* );
110 96
111 97 extern rtems_name name_hk_rate_monotonic; // name of the HK rate monotonic
112 98 extern rtems_id HK_id;// id of the HK rate monotonic period
113 99 extern rtems_name name_avgv_rate_monotonic; // name of the AVGV rate monotonic
114 100 extern rtems_id AVGV_id;// id of the AVGV rate monotonic period
115 101
116 102 void timer_configure( unsigned char timer, unsigned int clock_divider,
117 103 unsigned char interrupt_level, rtems_isr (*timer_isr)() );
118 104 void timer_start( unsigned char timer );
119 105 void timer_stop( unsigned char timer );
120 106 void timer_set_clock_divider(unsigned char timer, unsigned int clock_divider);
121 107
122 108 // WATCHDOG
123 109 rtems_isr watchdog_isr( rtems_vector_number vector );
124 110 void watchdog_configure(void);
125 111 void watchdog_stop(void);
126 112 void watchdog_reload(void);
127 113 void watchdog_start(void);
128 114
129 115 // SERIAL LINK
130 116 int send_console_outputs_on_apbuart_port( void );
131 117 int enable_apbuart_transmitter( void );
132 118 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value);
133 119
134 120 // RTEMS TASKS
135 121 rtems_task load_task( rtems_task_argument argument );
136 122 rtems_task hous_task( rtems_task_argument argument );
137 123 rtems_task avgv_task( rtems_task_argument argument );
138 124 rtems_task dumb_task( rtems_task_argument unused );
125 rtems_task scrubbing_task( rtems_task_argument unused );
126 rtems_task calibration_sweep_task( rtems_task_argument unused );
139 127
140 128 void init_housekeeping_parameters( void );
141 129 void increment_seq_counter(unsigned short *packetSequenceControl);
142 130 void getTime( unsigned char *time);
143 131 unsigned long long int getTimeAsUnsignedLongLongInt( );
144 void send_dumb_hk( void );
145 132 void get_temperatures( unsigned char *temperatures );
146 133 void get_v_e1_e2_f3( unsigned char *spacecraft_potential );
147 134 void get_cpu_load( unsigned char *resource_statistics );
148 135 void set_hk_lfr_sc_potential_flag( bool state );
149 136 void set_sy_lfr_pas_filter_enabled( bool state );
150 137 void set_sy_lfr_watchdog_enabled( bool state );
151 138 void set_hk_lfr_calib_enable( bool state );
152 139 void set_hk_lfr_reset_cause( enum lfr_reset_cause_t lfr_reset_cause );
153 140 void hk_lfr_le_me_he_update();
154 141 void set_hk_lfr_time_not_synchro();
155 142
156 143 extern int sched_yield( void );
157 144 extern void rtems_cpu_usage_reset();
158 145 extern ring_node *current_ring_node_f3;
159 146 extern ring_node *ring_node_to_send_cwf_f3;
160 147 extern ring_node waveform_ring_f3[];
161 148 extern unsigned short sequenceCounterHK;
162 149
163 150 extern unsigned char hk_lfr_q_sd_fifo_size_max;
164 151 extern unsigned char hk_lfr_q_rv_fifo_size_max;
165 152 extern unsigned char hk_lfr_q_p0_fifo_size_max;
166 153 extern unsigned char hk_lfr_q_p1_fifo_size_max;
167 154 extern unsigned char hk_lfr_q_p2_fifo_size_max;
168 155
169 156 #endif // FSW_MISC_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,36 +1,37
1 1 #ifndef LFR_CPU_USAGE_REPORT_H
2 2 #define LFR_CPU_USAGE_REPORT_H
3 3
4 4 #ifdef HAVE_CONFIG_H
5 5 #include "config.h"
6 6 #endif
7 7
8 8 #include <rtems.h>
9 9
10 10 #include <assert.h>
11 11 #include <string.h>
12 12 #include <stdlib.h>
13 13 #include <stdio.h>
14 14 #include <ctype.h>
15 15 #include <inttypes.h>
16 16
17 17 #include <rtems/cpuuse.h>
18 18 #include <rtems/bspIo.h>
19 19
20 20 #ifndef __RTEMS_USE_TICKS_FOR_STATISTICS__
21 21 #include <rtems/score/timestamp.h>
22 22 #endif
23 23
24 24 #ifndef __RTEMS_USE_TICKS_FOR_STATISTICS__
25 25 extern Timestamp_Control CPU_usage_Uptime_at_last_reset;
26 26 #else
27 27 extern uint32_t CPU_usage_Ticks_at_last_reset;
28 28 #endif
29 29
30 30 unsigned char lfr_rtems_cpu_usage_report( void );
31 31
32 #define CONST_10 10
32 33 #define CONST_100 100
34 #define CONST_255 255
33 35 #define CONST_1000 1000
34 #define CONST_100000 100000
35 36
36 37 #endif // LFR_CPU_USAGE_REPORT_H
Show file before | Show file after | Hide comments
@@ -1,373 +1,364
1 1 #ifndef FSW_PROCESSING_H_INCLUDED
2 2 #define FSW_PROCESSING_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <grspw.h>
6 6 #include <math.h>
7 7 #include <stdlib.h> // abs() is in the stdlib
8 8 #include <stdio.h>
9 9 #include <math.h>
10 10 #include <grlib_regs.h>
11 11
12 12 #include "fsw_params.h"
13 13
14 14 #define SBM_COEFF_PER_NORM_COEFF 2
15 15 #define MAX_SRC_DATA 780 // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
16 16 #define MAX_SRC_DATA_WITH_SPARE 143 // 13 bins * 11 Bytes
17 17
18 #define NODE_0 0
19 #define NODE_1 1
20 #define NODE_2 2
21 #define NODE_3 3
22 #define NODE_4 4
23 #define NODE_5 5
24 #define NODE_6 6
25 #define NODE_7 7
26
27 18 typedef struct ring_node_asm
28 19 {
29 20 struct ring_node_asm *next;
30 21 float matrix[ TOTAL_SIZE_SM ];
31 22 unsigned int status;
32 23 } ring_node_asm;
33 24
34 25 typedef struct
35 26 {
36 27 unsigned char targetLogicalAddress;
37 28 unsigned char protocolIdentifier;
38 29 unsigned char reserved;
39 30 unsigned char userApplication;
40 31 unsigned char packetID[BYTES_PER_PACKETID];
41 32 unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL];
42 33 unsigned char packetLength[BYTES_PER_PKT_LEN];
43 34 // DATA FIELD HEADER
44 35 unsigned char spare1_pusVersion_spare2;
45 36 unsigned char serviceType;
46 37 unsigned char serviceSubType;
47 38 unsigned char destinationID;
48 39 unsigned char time[BYTES_PER_TIME];
49 40 // AUXILIARY HEADER
50 41 unsigned char sid;
51 42 unsigned char pa_bia_status_info;
52 43 unsigned char sy_lfr_common_parameters_spare;
53 44 unsigned char sy_lfr_common_parameters;
54 45 unsigned char acquisitionTime[BYTES_PER_TIME];
55 46 unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR];
56 47 // SOURCE DATA
57 48 unsigned char data[ MAX_SRC_DATA ]; // MAX size is 26 bins * 30 Bytes [TM_LFR_SCIENCE_BURST_BP2_F1]
58 49 } bp_packet;
59 50
60 51 typedef struct
61 52 {
62 53 unsigned char targetLogicalAddress;
63 54 unsigned char protocolIdentifier;
64 55 unsigned char reserved;
65 56 unsigned char userApplication;
66 57 unsigned char packetID[BYTES_PER_PACKETID];
67 58 unsigned char packetSequenceControl[BYTES_PER_SEQ_CTRL];
68 59 unsigned char packetLength[BYTES_PER_PKT_LEN];
69 60 // DATA FIELD HEADER
70 61 unsigned char spare1_pusVersion_spare2;
71 62 unsigned char serviceType;
72 63 unsigned char serviceSubType;
73 64 unsigned char destinationID;
74 65 unsigned char time[BYTES_PER_TIME];
75 66 // AUXILIARY HEADER
76 67 unsigned char sid;
77 68 unsigned char pa_bia_status_info;
78 69 unsigned char sy_lfr_common_parameters_spare;
79 70 unsigned char sy_lfr_common_parameters;
80 71 unsigned char acquisitionTime[BYTES_PER_TIME];
81 72 unsigned char source_data_spare;
82 73 unsigned char pa_lfr_bp_blk_nr[BYTES_PER_BLKNR];
83 74 // SOURCE DATA
84 75 unsigned char data[ MAX_SRC_DATA_WITH_SPARE ]; // 13 bins * 11 Bytes
85 76 } bp_packet_with_spare; // only for TM_LFR_SCIENCE_NORMAL_BP1_F0 and F1
86 77
87 78 typedef struct asm_msg
88 79 {
89 80 ring_node_asm *norm;
90 81 ring_node_asm *burst_sbm;
91 82 rtems_event_set event;
92 83 unsigned int coarseTimeNORM;
93 84 unsigned int fineTimeNORM;
94 85 unsigned int coarseTimeSBM;
95 86 unsigned int fineTimeSBM;
96 87 unsigned int numberOfSMInASMNORM;
97 88 unsigned int numberOfSMInASMSBM;
98 89 } asm_msg;
99 90
100 91 extern unsigned char thisIsAnASMRestart;
101 92
102 93 extern volatile int sm_f0[ ];
103 94 extern volatile int sm_f1[ ];
104 95 extern volatile int sm_f2[ ];
105 96 extern unsigned int acquisitionDurations[];
106 97
107 98 // parameters
108 99 extern struct param_local_str param_local;
109 100 extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
110 101
111 102 // registers
112 103 extern time_management_regs_t *time_management_regs;
113 104 extern volatile spectral_matrix_regs_t *spectral_matrix_regs;
114 105
115 106 extern rtems_name misc_name[];
116 107 extern rtems_id Task_id[]; /* array of task ids */
117 108
118 109 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel);
119 110 // ISR
120 111 rtems_isr spectral_matrices_isr( rtems_vector_number vector );
121 112
122 113 //******************
123 114 // Spectral Matrices
124 115 void reset_nb_sm( void );
125 116 // SM
126 117 void SM_init_rings( void );
127 118 void SM_reset_current_ring_nodes( void );
128 119 // ASM
129 120 void ASM_generic_init_ring(ring_node_asm *ring, unsigned char nbNodes );
130 121
131 122 //*****************
132 123 // Basic Parameters
133 124
134 125 void BP_reset_current_ring_nodes( void );
135 126 void BP_init_header(bp_packet *packet,
136 127 unsigned int apid, unsigned char sid,
137 128 unsigned int packetLength , unsigned char blkNr);
138 129 void BP_init_header_with_spare(bp_packet_with_spare *packet,
139 130 unsigned int apid, unsigned char sid,
140 131 unsigned int packetLength, unsigned char blkNr );
141 132 void BP_send( char *data,
142 133 rtems_id queue_id,
143 134 unsigned int nbBytesToSend , unsigned int sid );
144 135 void BP_send_s1_s2(char *data,
145 136 rtems_id queue_id,
146 137 unsigned int nbBytesToSend, unsigned int sid );
147 138
148 139 //******************
149 140 // general functions
150 141 void reset_sm_status( void );
151 142 void reset_spectral_matrix_regs( void );
152 143 void set_time(unsigned char *time, unsigned char *timeInBuffer );
153 144 unsigned long long int get_acquisition_time( unsigned char *timePtr );
154 145 unsigned char getSID( rtems_event_set event );
155 146
156 147 extern rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id );
157 148 extern rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id );
158 149
159 150 //***************************************
160 151 // DEFINITIONS OF STATIC INLINE FUNCTIONS
161 152 static inline void SM_average(float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
162 153 ring_node *ring_node_tab[],
163 154 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
164 155 asm_msg *msgForMATR , unsigned char channel);
165 156
166 157 void ASM_patch( float *inputASM, float *outputASM );
167 158
168 159 void extractReImVectors(float *inputASM, float *outputASM, unsigned int asmComponent );
169 160
170 161 static inline void ASM_reorganize_and_divide(float *averaged_spec_mat, float *averaged_spec_mat_reorganized,
171 162 float divider );
172 163
173 164 static inline void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat,
174 165 float divider,
175 166 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart);
176 167
177 168 static inline void ASM_convert(volatile float *input_matrix, char *output_matrix);
178 169
179 170 unsigned char isPolluted( u_int64_t t0, u_int64_t t1, u_int64_t tbad0, u_int64_t tbad1 );
180 171
181 172 unsigned char acquisitionTimeIsValid(unsigned int coarseTime, unsigned int fineTime, unsigned char channel);
182 173
183 174 void SM_average( float *averaged_spec_mat_NORM, float *averaged_spec_mat_SBM,
184 175 ring_node *ring_node_tab[],
185 176 unsigned int nbAverageNORM, unsigned int nbAverageSBM,
186 177 asm_msg *msgForMATR, unsigned char channel )
187 178 {
188 179 float sum;
189 180 unsigned int i;
190 181 unsigned int k;
191 182 unsigned char incomingSMIsValid[NB_SM_BEFORE_AVF0_F1];
192 183 unsigned int numberOfValidSM;
193 184 unsigned char isValid;
194 185
195 186 //**************
196 187 // PAS FILTERING
197 188 // check acquisitionTime of the incoming data
198 189 numberOfValidSM = 0;
199 190 for (k=0; k<NB_SM_BEFORE_AVF0_F1; k++)
200 191 {
201 192 isValid = acquisitionTimeIsValid( ring_node_tab[k]->coarseTime, ring_node_tab[k]->fineTime, channel );
202 193 incomingSMIsValid[k] = isValid;
203 194 numberOfValidSM = numberOfValidSM + isValid;
204 195 }
205 196
206 197 //************************
207 198 // AVERAGE SPECTRAL MATRIX
208 199 for(i=0; i<TOTAL_SIZE_SM; i++)
209 200 {
210 201 sum = INIT_FLOAT;
211 202 for ( k = 0; k < NB_SM_BEFORE_AVF0_F1; k++ )
212 203 {
213 204 if (incomingSMIsValid[k] == MATRIX_IS_NOT_POLLUTED)
214 205 {
215 206 sum = sum + ( (int *) (ring_node_tab[0]->buffer_address) ) [ i ] ;
216 207 }
217 208 }
218 209
219 210 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
220 211 {
221 212 averaged_spec_mat_NORM[ i ] = sum;
222 213 averaged_spec_mat_SBM[ i ] = sum;
223 214 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
224 215 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
225 216 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
226 217 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
227 218 }
228 219 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
229 220 {
230 221 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
231 222 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
232 223 }
233 224 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
234 225 {
235 226 averaged_spec_mat_NORM[ i ] = ( averaged_spec_mat_NORM[ i ] + sum );
236 227 averaged_spec_mat_SBM[ i ] = sum;
237 228 msgForMATR->coarseTimeSBM = ring_node_tab[0]->coarseTime;
238 229 msgForMATR->fineTimeSBM = ring_node_tab[0]->fineTime;
239 230 }
240 231 else
241 232 {
242 233 averaged_spec_mat_NORM[ i ] = sum;
243 234 averaged_spec_mat_SBM[ i ] = ( averaged_spec_mat_SBM[ i ] + sum );
244 235 msgForMATR->coarseTimeNORM = ring_node_tab[0]->coarseTime;
245 236 msgForMATR->fineTimeNORM = ring_node_tab[0]->fineTime;
246 237 // PRINTF2("ERR *** in SM_average *** unexpected parameters %d %d\n", nbAverageNORM, nbAverageSBM)
247 238 }
248 239 }
249 240
250 241 //*******************
251 242 // UPDATE SM COUNTERS
252 243 if ( (nbAverageNORM == 0) && (nbAverageSBM == 0) )
253 244 {
254 245 msgForMATR->numberOfSMInASMNORM = numberOfValidSM;
255 246 msgForMATR->numberOfSMInASMSBM = numberOfValidSM;
256 247 }
257 248 else if ( (nbAverageNORM != 0) && (nbAverageSBM != 0) )
258 249 {
259 250 msgForMATR->numberOfSMInASMNORM = msgForMATR->numberOfSMInASMNORM + numberOfValidSM;
260 251 msgForMATR->numberOfSMInASMSBM = msgForMATR->numberOfSMInASMSBM + numberOfValidSM;
261 252 }
262 253 else if ( (nbAverageNORM != 0) && (nbAverageSBM == 0) )
263 254 {
264 255 msgForMATR->numberOfSMInASMNORM = msgForMATR->numberOfSMInASMNORM + numberOfValidSM;
265 256 msgForMATR->numberOfSMInASMSBM = numberOfValidSM;
266 257 }
267 258 else
268 259 {
269 260 msgForMATR->numberOfSMInASMNORM = numberOfValidSM;
270 261 msgForMATR->numberOfSMInASMSBM = msgForMATR->numberOfSMInASMSBM + numberOfValidSM;
271 262 }
272 263 }
273 264
274 265 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
275 266 {
276 267 int frequencyBin;
277 268 int asmComponent;
278 269 unsigned int offsetASM;
279 270 unsigned int offsetASMReorganized;
280 271
281 272 // BUILD DATA
282 273 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
283 274 {
284 275 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
285 276 {
286 277 offsetASMReorganized =
287 278 (frequencyBin * NB_VALUES_PER_SM)
288 279 + asmComponent;
289 280 offsetASM =
290 281 (asmComponent * NB_BINS_PER_SM)
291 282 + frequencyBin;
292 283 if ( divider != INIT_FLOAT )
293 284 {
294 285 averaged_spec_mat_reorganized[offsetASMReorganized ] = averaged_spec_mat[ offsetASM ] / divider;
295 286 }
296 287 else
297 288 {
298 289 averaged_spec_mat_reorganized[offsetASMReorganized ] = INIT_FLOAT;
299 290 }
300 291 }
301 292 }
302 293 }
303 294
304 295 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
305 296 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
306 297 {
307 298 int frequencyBin;
308 299 int asmComponent;
309 300 int offsetASM;
310 301 int offsetCompressed;
311 302 int k;
312 303
313 304 // BUILD DATA
314 305 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
315 306 {
316 307 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
317 308 {
318 309 offsetCompressed = // NO TIME OFFSET
319 310 (frequencyBin * NB_VALUES_PER_SM)
320 311 + asmComponent;
321 312 offsetASM = // NO TIME OFFSET
322 313 (asmComponent * NB_BINS_PER_SM)
323 314 + ASMIndexStart
324 315 + (frequencyBin * nbBinsToAverage);
325 316 compressed_spec_mat[ offsetCompressed ] = 0;
326 317 for ( k = 0; k < nbBinsToAverage; k++ )
327 318 {
328 319 compressed_spec_mat[offsetCompressed ] =
329 320 ( compressed_spec_mat[ offsetCompressed ]
330 321 + averaged_spec_mat[ offsetASM + k ] );
331 322 }
332 323 compressed_spec_mat[ offsetCompressed ] =
333 324 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
334 325 }
335 326 }
336 327 }
337 328
338 329 void ASM_convert( volatile float *input_matrix, char *output_matrix)
339 330 {
340 331 unsigned int frequencyBin;
341 332 unsigned int asmComponent;
342 333 char * pt_char_input;
343 334 char * pt_char_output;
344 335 unsigned int offsetInput;
345 336 unsigned int offsetOutput;
346 337
347 338 pt_char_input = (char*) &input_matrix;
348 339 pt_char_output = (char*) &output_matrix;
349 340
350 341 // convert all other data
351 342 for( frequencyBin=0; frequencyBin<NB_BINS_PER_SM; frequencyBin++)
352 343 {
353 344 for ( asmComponent=0; asmComponent<NB_VALUES_PER_SM; asmComponent++)
354 345 {
355 346 offsetInput = (frequencyBin*NB_VALUES_PER_SM) + asmComponent ;
356 347 offsetOutput = SM_BYTES_PER_VAL * ( (frequencyBin*NB_VALUES_PER_SM) + asmComponent ) ;
357 348 pt_char_input = (char*) &input_matrix [ offsetInput ];
358 349 pt_char_output = (char*) &output_matrix[ offsetOutput ];
359 350 pt_char_output[0] = pt_char_input[0]; // bits 31 downto 24 of the float
360 351 pt_char_output[1] = pt_char_input[1]; // bits 23 downto 16 of the float
361 352 }
362 353 }
363 354 }
364 355
365 356 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat,
366 357 float divider,
367 358 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage , unsigned char ASMIndexStart, unsigned char channel);
368 359
369 360 int getFBinMask(int k, unsigned char channel);
370 361
371 362 void init_kcoeff_sbm_from_kcoeff_norm( float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm);
372 363
373 364 #endif // FSW_PROCESSING_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,115 +1,117
1 1 #ifndef TC_HANDLER_H_INCLUDED
2 2 #define TC_HANDLER_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <leon.h>
6 6
7 7 #include "tc_load_dump_parameters.h"
8 8 #include "tc_acceptance.h"
9 9 #include "tm_lfr_tc_exe.h"
10 10 #include "wf_handler.h"
11 11 #include "fsw_processing.h"
12 12
13 13 #include "lfr_cpu_usage_report.h"
14 14
15 15 #define MAX_DELTA_COARSE_TIME 3
16 16 #define NB_SCIENCE_TASKS 10
17 17 #define NB_ASM_TASKS 6
18 18 #define STATUS_0 0
19 19 #define STATUS_1 1
20 20 #define STATUS_2 2
21 21 #define STATUS_3 3
22 22 #define STATUS_4 4
23 23 #define STATUS_5 5
24 24 #define STATUS_6 6
25 25 #define STATUS_7 7
26 26 #define STATUS_8 8
27 27 #define STATUS_9 9
28 28
29 29 #define CAL_F0 625.
30 30 #define CAL_F1 10000.
31 31 #define CAL_W0 (2. * pi * CAL_F0)
32 32 #define CAL_W1 (2. * pi * CAL_F1)
33 33 #define CAL_A0 1.
34 34 #define CAL_A1 2.
35 35 #define CAL_FS 160256.410
36 36 #define CAL_SCALE_FACTOR (0.250 / 0.000654) // 191, 500 mVpp, 2 sinus waves => 500 mVpp each, amplitude = 250 mV
37 37 #define CAL_NB_PTS 256
38 38 #define CAL_DATA_MASK 0xfff
39 39 #define CAL_F_DIVISOR 38 // 25 MHz => 160 256 (39 - 1)
40 #define CAL_F_DIVISOR_MIN 38
41 #define CAL_F_DIVISOR_MAX (38*2*2*2*2)
40 42 // INTERLEAVED MODE
41 43 #define CAL_FS_INTER 240384.615
42 44 #define CAL_NB_PTS_INTER 384
43 45 #define CAL_DATA_MASK_INTER 0x3f
44 46 #define CAL_DATA_SHIFT_INTER 12
45 47 #define BYTES_FOR_2_SAMPLES 3 // one need 3 bytes = 24 bits to store 3 samples of 12 bits in interleaved mode
46 48 #define STEPS_FOR_STORAGE_INTER 128
47 49 #define CAL_F_DIVISOR_INTER 26 // 25 MHz => 240 384
48 50
49 51 extern unsigned int lastValidEnterModeTime;
50 52 extern unsigned char oneTcLfrUpdateTimeReceived;
51 53
52 54 //****
53 55 // ISR
54 56 rtems_isr commutation_isr1( rtems_vector_number vector );
55 57 rtems_isr commutation_isr2( rtems_vector_number vector );
56 58
57 59 //***********
58 60 // RTEMS TASK
59 61 rtems_task actn_task( rtems_task_argument unused );
60 62
61 63 //***********
62 64 // TC ACTIONS
63 65 int action_reset( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time );
64 66 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id);
65 67 int action_update_info( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
66 68 int action_enable_calibration( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time );
67 69 int action_disable_calibration( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time );
68 70 int action_update_time( ccsdsTelecommandPacket_t *TC);
69 71
70 72 // mode transition
71 73 int check_mode_value( unsigned char requestedMode );
72 74 int check_mode_transition( unsigned char requestedMode );
73 75 void update_last_valid_transition_date( unsigned int transitionCoarseTime );
74 76 int check_transition_date( unsigned int transitionCoarseTime );
75 77 int stop_spectral_matrices( void );
76 78 int stop_current_mode( void );
77 79 int enter_mode_standby(void );
78 80 int enter_mode_normal( unsigned int transitionCoarseTime );
79 81 int enter_mode_burst( unsigned int transitionCoarseTime );
80 82 int enter_mode_sbm1( unsigned int transitionCoarseTime );
81 83 int enter_mode_sbm2( unsigned int transitionCoarseTime );
82 84 int restart_science_tasks( unsigned char lfrRequestedMode );
83 85 int restart_asm_tasks(unsigned char lfrRequestedMode );
84 86 int suspend_science_tasks(void);
85 87 int suspend_asm_tasks( void );
86 88 void launch_waveform_picker( unsigned char mode , unsigned int transitionCoarseTime );
87 89 void launch_spectral_matrix( void );
88 90 void set_sm_irq_onNewMatrix( unsigned char value );
89 91 void set_sm_irq_onError( unsigned char value );
90 92
91 93 // other functions
92 94 void updateLFRCurrentMode(unsigned char requestedMode);
93 95 void set_lfr_soft_reset( unsigned char value );
94 96 void reset_lfr( void );
95 97 // CALIBRATION
96 98 void setCalibrationPrescaler( unsigned int prescaler );
97 99 void setCalibrationDivisor( unsigned int divisionFactor );
98 100 void setCalibrationData( void );
99 101 void setCalibrationReload( bool state);
100 102 void setCalibrationEnable( bool state );
101 103 void setCalibrationInterleaved( bool state );
102 104 void setCalibration( bool state );
103 105 void configureCalibration( bool interleaved );
104 106 //
105 107 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC , unsigned char *time );
106 108 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC , unsigned char *time );
107 109 void close_action( ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id );
108 110
109 111 extern rtems_status_code get_message_queue_id_send( rtems_id *queue_id );
110 112 extern rtems_status_code get_message_queue_id_recv( rtems_id *queue_id );
111 113
112 114 #endif // TC_HANDLER_H_INCLUDED
113 115
114 116
115 117
Show file before | Show file after | Hide comments
@@ -1,124 +1,114
1 1 #ifndef TC_LOAD_DUMP_PARAMETERS_H
2 2 #define TC_LOAD_DUMP_PARAMETERS_H
3 3
4 4 #include <rtems.h>
5 5 #include <stdio.h>
6 6
7 7 #include "fsw_params.h"
8 8 #include "wf_handler.h"
9 9 #include "tm_lfr_tc_exe.h"
10 10 #include "fsw_misc.h"
11 11 #include "basic_parameters_params.h"
12 12 #include "avf0_prc0.h"
13 13
14 14 #define FLOAT_EQUAL_ZERO 0.001
15 15 #define NB_BINS_TO_REMOVE 3
16 16 #define FI_INTERVAL_COEFF 0.285
17 17 #define BIN_MIN 0
18 18 #define BIN_MAX 127
19 19 #define DELTAF_F0 96.
20 20 #define DELTAF_F1 16.
21 21 #define DELTAF_F2 1.
22 #define DELTAF_DIV 2.
23
24 #define BIT_RW1_F1 0x80
25 #define BIT_RW1_F2 0x40
26 #define BIT_RW2_F1 0x20
27 #define BIT_RW2_F2 0x10
28 #define BIT_RW3_F1 0x08
29 #define BIT_RW3_F2 0x04
30 #define BIT_RW4_F1 0x02
31 #define BIT_RW4_F2 0x01
32 22
33 23 #define WHEEL_1 1
34 24 #define WHEEL_2 2
35 25 #define WHEEL_3 3
36 26 #define WHEEL_4 4
37 27 #define FREQ_1 1
38 28 #define FREQ_2 2
39 29 #define FREQ_3 3
40 30 #define FREQ_4 4
41 31 #define FLAG_OFFSET_WHEELS_1_3 8
42 32 #define FLAG_OFFSET_WHEELS_2_4 4
43 33
44 34 #define FLAG_NAN 0 // Not A NUMBER
45 35 #define FLAG_IAN 1 // Is A Number
46 36
47 37 #define SBM_KCOEFF_PER_NORM_KCOEFF 2
48 38
49 39 extern unsigned short sequenceCounterParameterDump;
50 40 extern unsigned short sequenceCounters_TM_DUMP[];
51 41 extern float k_coeff_intercalib_f0_norm[ ];
52 42 extern float k_coeff_intercalib_f0_sbm[ ];
53 43 extern float k_coeff_intercalib_f1_norm[ ];
54 44 extern float k_coeff_intercalib_f1_sbm[ ];
55 45 extern float k_coeff_intercalib_f2[ ];
56 46 extern fbins_masks_t fbins_masks;
57 47
58 48 int action_load_common_par( ccsdsTelecommandPacket_t *TC );
59 49 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
60 50 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
61 51 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
62 52 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id , unsigned char *time);
63 53 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
64 54 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
65 55 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
66 56 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time);
67 57 int action_dump_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
68 58
69 59 // NORMAL
70 60 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
71 61 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC );
72 62 int set_sy_lfr_n_swf_p( ccsdsTelecommandPacket_t *TC );
73 63 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC );
74 64 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC );
75 65 int set_sy_lfr_n_bp_p1( ccsdsTelecommandPacket_t *TC );
76 66 int set_sy_lfr_n_cwf_long_f3( ccsdsTelecommandPacket_t *TC );
77 67
78 68 // BURST
79 69 int set_sy_lfr_b_bp_p0( ccsdsTelecommandPacket_t *TC );
80 70 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC );
81 71
82 72 // SBM1
83 73 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC );
84 74 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC );
85 75
86 76 // SBM2
87 77 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC );
88 78 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC );
89 79
90 80 // TC_LFR_UPDATE_INFO
91 81 unsigned int check_update_info_hk_lfr_mode( unsigned char mode );
92 82 unsigned int check_update_info_hk_tds_mode( unsigned char mode );
93 83 unsigned int check_update_info_hk_thr_mode( unsigned char mode );
94 84 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value );
95 85 void set_hk_lfr_sc_rw_f_flags( void );
96 86 int check_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value );
97 87 int check_all_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int *pos, float*value );
98 88 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC );
99 89 void setFBinMask(unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, float sy_lfr_rw_k );
100 90 void build_sy_lfr_rw_mask( unsigned int channel );
101 91 void build_sy_lfr_rw_masks();
102 92 void merge_fbins_masks( void );
103 93
104 94 // FBINS_MASK
105 95 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC );
106 96
107 97 // TC_LFR_LOAD_PARS_FILTER_PAR
108 98 int check_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value );
109 99 int check_all_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int *pos, float*value );
110 100 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id );
111 101
112 102 // KCOEFFICIENTS
113 103 int set_sy_lfr_kcoeff(ccsdsTelecommandPacket_t *TC , rtems_id queue_id);
114 104 void copyFloatByChar( unsigned char *destination, unsigned char *source );
115 105 void copyInt32ByChar( unsigned char *destination, unsigned char *source );
116 106 void copyInt16ByChar( unsigned char *destination, unsigned char *source );
117 107 void floatToChar( float value, unsigned char* ptr);
118 108
119 109 void init_parameter_dump( void );
120 110 void init_kcoefficients_dump( void );
121 111 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr );
122 112 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id );
123 113
124 114 #endif // TC_LOAD_DUMP_PARAMETERS_H
Show file before | Show file after | Hide comments
@@ -1,116 +1,115
1 1 #ifndef WF_HANDLER_H_INCLUDED
2 2 #define WF_HANDLER_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <grspw.h>
6 6 #include <stdio.h>
7 7 #include <math.h>
8 8 #include <fsw_params.h>
9 9
10 10 #include "fsw_init.h"
11 11 #include "fsw_params_wf_handler.h"
12 12
13 13 #define pi 3.14159265359
14 14 #define T0_IN_FINETIME ( 65536. / 24576. )
15 15 #define T1_IN_FINETIME ( 65536. / 4096. )
16 16 #define T2_IN_FINETIME ( 65536. / 256. )
17 17 #define T3_IN_FINETIME ( 65536. / 16. )
18 18
19 19 #define TICKS_PER_T1 16
20 20 #define TICKS_PER_T2 256
21 21 #define TICKS_PER_S 65536.
22 22 #define MS_PER_S 1000.
23 23
24 24 #define FREQ_F0 24576.
25 25 #define FREQ_F1 4096.
26 26 #define FREQ_F2 256.
27 #define FREQ_F3 16.
28 27
29 28 #define DELTAT_F0 2731 // (2048. / 24576. / 2.) * 65536. = 2730.667;
30 29 #define DELTAT_F1 16384 // (2048. / 4096. / 2.) * 65536. = 16384;
31 30 #define DELTAT_F2 262144 // (2048. / 256. / 2.) * 65536. = 262144;
32 31
33 32 #define OFFSET_2_BYTES 2
34 33
35 34 #define ONE_TICK_CORR_INTERVAL_0_MIN 0.5
36 35 #define ONE_TICK_CORR_INTERVAL_0_MAX 1.0
37 36 #define ONE_TICK_CORR_INTERVAL_1_MIN -1.0
38 37 #define ONE_TICK_CORR_INTERVAL_1_MAX -0.5
39 38 #define ONE_TICK_CORR 1
40 39 #define CORR_MULT 2
41 40
42 41 extern int fdSPW;
43 42
44 43 //*****************
45 44 // waveform buffers
46 45 extern volatile int wf_buffer_f0[ ];
47 46 extern volatile int wf_buffer_f1[ ];
48 47 extern volatile int wf_buffer_f2[ ];
49 48 extern volatile int wf_buffer_f3[ ];
50 49
51 50 extern waveform_picker_regs_0_1_18_t *waveform_picker_regs;
52 51 extern time_management_regs_t *time_management_regs;
53 52 extern Packet_TM_LFR_HK_t housekeeping_packet;
54 53 extern Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet;
55 54 extern struct param_local_str param_local;
56 55
57 56 extern unsigned short sequenceCounters_SCIENCE_NORMAL_BURST;
58 57 extern unsigned short sequenceCounters_SCIENCE_SBM1_SBM2;
59 58
60 59 extern rtems_id Task_id[]; /* array of task ids */
61 60
62 61 extern unsigned char lfrCurrentMode;
63 62
64 63 //**********
65 64 // RTEMS_ISR
66 65 void reset_extractSWF( void );
67 66 rtems_isr waveforms_isr( rtems_vector_number vector );
68 67
69 68 //***********
70 69 // RTEMS_TASK
71 70 rtems_task wfrm_task( rtems_task_argument argument );
72 71 rtems_task cwf3_task( rtems_task_argument argument );
73 72 rtems_task cwf2_task( rtems_task_argument argument );
74 73 rtems_task cwf1_task( rtems_task_argument argument );
75 74 rtems_task swbd_task( rtems_task_argument argument );
76 75
77 76 //******************
78 77 // general functions
79 78 void WFP_init_rings( void );
80 79 void init_ring( ring_node ring[], unsigned char nbNodes, volatile int buffer[] , unsigned int bufferSize );
81 80 void WFP_reset_current_ring_nodes( void );
82 81 //
83 82 int init_header_continuous_cwf3_light_table( Header_TM_LFR_SCIENCE_CWF_t *headerCWF );
84 83 //
85 84 int send_waveform_CWF3_light(ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id );
86 85 //
87 86 void compute_acquisition_time(unsigned int coarseTime, unsigned int fineTime,
88 87 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char *acquisitionTime );
89 88 void build_snapshot_from_ring(ring_node *ring_node_to_send, unsigned char frequencyChannel ,
90 89 unsigned long long acquisitionTimeF0_asLong, ring_node *ring_node_swf_extracted, int *swf_extracted);
91 90 double computeCorrection( unsigned char *timePtr );
92 91 void applyCorrection( double correction );
93 92 void snapshot_resynchronization( unsigned char *timePtr );
94 93 //
95 94 rtems_id get_pkts_queue_id( void );
96 95
97 96 //**************
98 97 // wfp registers
99 98 // RESET
100 99 void reset_wfp_burst_enable( void );
101 100 void reset_wfp_status( void );
102 101 void reset_wfp_buffer_addresses( void );
103 102 void reset_waveform_picker_regs( void );
104 103 // SET
105 104 void set_wfp_data_shaping(void);
106 105 void set_wfp_burst_enable_register( unsigned char mode );
107 106 void set_wfp_delta_snapshot( void );
108 107 void set_wfp_delta_f0_f0_2( void );
109 108 void set_wfp_delta_f1( void );
110 109 void set_wfp_delta_f2( void );
111 110
112 111 //*****************
113 112 // local parameters
114 113 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid );
115 114
116 115 #endif // WF_HANDLER_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,25 +1,39
1 1 set(rtems_dir /opt/rtems-4.10/)
2 2
3 3 set(CMAKE_SYSTEM_NAME rtems)
4 4 set(CMAKE_C_COMPILER ${rtems_dir}/bin/sparc-rtems-gcc)
5 5 set(CMAKE_CXX_COMPILER ${rtems_dir}/bin/sparc-rtems-g++)
6 6 set(CMAKE_LINKER ${rtems_dir}/bin/sparc-rtems-g++)
7 7 SET(CMAKE_EXE_LINKER_FLAGS "-static")
8 8 option(fix-b2bst "Activate -mfix-b2bst switch to mitigate \"LEON3FT Stale Cache Entry After Store with Data Tag Parity Error\" errata, GRLIB-TN-0009" ON)
9 9
10 option(Coverage "Enables code coverage" OFF)
11
12
13 set(CMAKE_C_FLAGS_RELEASE "-O3")
14 set(CMAKE_C_FLAGS_DEBUG "-O0")
15
16
10 17 if(fix-b2bst)
11 set(CMAKE_C_FLAGS_RELEASE "-O3 -mfix-b2bst")
12 else()
13 set(CMAKE_C_FLAGS_RELEASE "-O3")
18 set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -mfix-b2bst")
19 set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -mfix-b2bst")
14 20 endif()
15 21
22
16 23 set(CMAKE_C_LINK_EXECUTABLE "<CMAKE_LINKER> <FLAGS> -Xlinker -Map=<TARGET>.map <CMAKE_CXX_LINK_FLAGS> <LINK_FLAGS> <OBJECTS> -o <TARGET> <LINK_LIBRARIES>")
17 24
18 25 include_directories("${rtems_dir}/sparc-rtems/leon3/lib/include")
19 26
20 27 function (check_b2bst target bin)
21 28 add_custom_command(TARGET ${target}
22 29 POST_BUILD
23 30 COMMAND ${rtems_dir}/bin/sparc-rtems-objdump -d ${bin}/${target} | ${CMAKE_SOURCE_DIR}/sparc/leon3ft-b2bst-scan.tcl
24 31 )
25 32 endfunction()
33
34 function (build_srec target bin rev)
35 add_custom_command(TARGET ${target}
36 POST_BUILD
37 COMMAND ${rtems_dir}/bin/sparc-rtems-objcopy -j .data -F srec ${bin}/${target} RpwLfrApp_XXXX_data_rev-${rev}.srec && ${rtems_dir}/bin/sparc-rtems-objcopy -j .text -F srec ${bin}/${target} RpwLfrApp_XXXX_text_rev-${rev}.srec
38 )
39 endfunction()
Show file before | Show file after | Hide comments
@@ -1,112 +1,130
1 1 cmake_minimum_required (VERSION 2.6)
2 2 project (fsw)
3 3
4 4 include(sparc-rtems)
5 5 include(cppcheck)
6 6
7 7 include_directories("../header"
8 8 "../header/lfr_common_headers"
9 9 "../header/processing"
10 10 "../LFR_basic-parameters"
11 11 "../src")
12 12
13 13 set(SOURCES wf_handler.c
14 14 tc_handler.c
15 15 fsw_misc.c
16 16 fsw_init.c
17 17 fsw_globals.c
18 18 fsw_spacewire.c
19 19 tc_load_dump_parameters.c
20 20 tm_lfr_tc_exe.c
21 21 tc_acceptance.c
22 22 processing/fsw_processing.c
23 23 processing/avf0_prc0.c
24 24 processing/avf1_prc1.c
25 25 processing/avf2_prc2.c
26 26 lfr_cpu_usage_report.c
27 27 ${LFR_BP_SRC}
28 28 ../header/wf_handler.h
29 29 ../header/tc_handler.h
30 30 ../header/grlib_regs.h
31 31 ../header/fsw_misc.h
32 32 ../header/fsw_init.h
33 33 ../header/fsw_spacewire.h
34 34 ../header/tc_load_dump_parameters.h
35 35 ../header/tm_lfr_tc_exe.h
36 36 ../header/tc_acceptance.h
37 37 ../header/processing/fsw_processing.h
38 38 ../header/processing/avf0_prc0.h
39 39 ../header/processing/avf1_prc1.h
40 40 ../header/processing/avf2_prc2.h
41 41 ../header/fsw_params_wf_handler.h
42 42 ../header/lfr_cpu_usage_report.h
43 43 ../header/lfr_common_headers/ccsds_types.h
44 44 ../header/lfr_common_headers/fsw_params.h
45 45 ../header/lfr_common_headers/fsw_params_nb_bytes.h
46 46 ../header/lfr_common_headers/fsw_params_processing.h
47 47 ../header/lfr_common_headers/tm_byte_positions.h
48 48 ../LFR_basic-parameters/basic_parameters.h
49 49 ../LFR_basic-parameters/basic_parameters_params.h
50 50 ../header/GscMemoryLPP.hpp
51 51 )
52 52
53 53
54 54 option(FSW_verbose "Enable verbose LFR" OFF)
55 55 option(FSW_boot_messages "Enable LFR boot messages" OFF)
56 56 option(FSW_debug_messages "Enable LFR debug messages" OFF)
57 57 option(FSW_cpu_usage_report "Enable LFR cpu usage report" OFF)
58 58 option(FSW_stack_report "Enable LFR stack report" OFF)
59 59 option(FSW_vhdl_dev "?" OFF)
60 60 option(FSW_lpp_dpu_destid "Set to debug at LPP" OFF)
61 61 option(FSW_debug_watchdog "Enable debug watchdog" OFF)
62 62 option(FSW_debug_tch "?" OFF)
63 option(FSW_Instrument_Scrubbing "Enable scrubbing counter" OFF)
63 64
64 65 set(SW_VERSION_N1 "3" CACHE STRING "Choose N1 FSW Version." FORCE)
65 66 set(SW_VERSION_N2 "2" CACHE STRING "Choose N2 FSW Version." FORCE)
66 67 set(SW_VERSION_N3 "0" CACHE STRING "Choose N3 FSW Version." FORCE)
67 set(SW_VERSION_N4 "15" CACHE STRING "Choose N4 FSW Version." FORCE)
68 set(SW_VERSION_N4 "20" CACHE STRING "Choose N4 FSW Version." FORCE)
68 69
69 70 if(FSW_verbose)
70 71 add_definitions(-DPRINT_MESSAGES_ON_CONSOLE)
71 72 endif()
72 73 if(FSW_boot_messages)
73 74 add_definitions(-DBOOT_MESSAGES)
74 75 endif()
75 76 if(FSW_debug_messages)
76 77 add_definitions(-DDEBUG_MESSAGES)
77 78 endif()
78 79 if(FSW_cpu_usage_report)
79 80 add_definitions(-DPRINT_TASK_STATISTICS)
80 81 endif()
81 82 if(FSW_stack_report)
82 83 add_definitions(-DPRINT_STACK_REPORT)
83 84 endif()
84 85 if(FSW_vhdl_dev)
85 86 add_definitions(-DVHDL_DEV)
86 87 endif()
87 88 if(FSW_lpp_dpu_destid)
88 89 add_definitions(-DLPP_DPU_DESTID)
89 90 endif()
90 91 if(FSW_debug_watchdog)
91 92 add_definitions(-DDEBUG_WATCHDOG)
92 93 endif()
93 94 if(FSW_debug_tch)
94 95 add_definitions(-DDEBUG_TCH)
95 96 endif()
96 97
98
99
97 100 add_definitions(-DMSB_FIRST_TCH)
98 101
99 102 add_definitions(-DSWVERSION=-1-0)
100 103 add_definitions(-DSW_VERSION_N1=${SW_VERSION_N1})
101 104 add_definitions(-DSW_VERSION_N2=${SW_VERSION_N2})
102 105 add_definitions(-DSW_VERSION_N3=${SW_VERSION_N3})
103 106 add_definitions(-DSW_VERSION_N4=${SW_VERSION_N4})
104 107
105 108 add_executable(fsw ${SOURCES})
106 109
110 if(FSW_Instrument_Scrubbing)
111 add_definitions(-DENABLE_SCRUBBING_COUNTER)
112 endif()
113
114 if(Coverage)
115 target_link_libraries(fsw gcov)
116 SET_TARGET_PROPERTIES(fsw PROPERTIES COMPILE_FLAGS "-fprofile-arcs -ftest-coverage")
117 endif()
118
119
107 120 if(fix-b2bst)
108 121 check_b2bst(fsw ${CMAKE_CURRENT_BINARY_DIR})
109 122 endif()
110 123
124 if(NOT FSW_lpp_dpu_destid)
125 build_srec(fsw ${CMAKE_CURRENT_BINARY_DIR} "${SW_VERSION_N1}-${SW_VERSION_N2}-${SW_VERSION_N3}-${SW_VERSION_N4}")
126 endif()
127
128
111 129 add_test_cppcheck(fsw STYLE UNUSED_FUNCTIONS POSSIBLE_ERROR MISSING_INCLUDE)
112 130
Show file before | Show file after | Hide comments
@@ -1,96 +1,119
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
1 24 /** Global variables of the LFR flight software.
2 25 *
3 26 * @file
4 27 * @author P. LEROY
5 28 *
6 29 * Among global variables, there are:
7 30 * - RTEMS names and id.
8 31 * - APB configuration registers.
9 32 * - waveforms global buffers, used by the waveform picker hardware module to store data.
10 33 * - spectral matrices buffesr, used by the hardware module to store data.
11 34 * - variable related to LFR modes parameters.
12 35 * - the global HK packet buffer.
13 36 * - the global dump parameter buffer.
14 37 *
15 38 */
16 39
17 40 #include <rtems.h>
18 41 #include <grspw.h>
19 42
20 43 #include "ccsds_types.h"
21 44 #include "grlib_regs.h"
22 45 #include "fsw_params.h"
23 46 #include "fsw_params_wf_handler.h"
24 47
25 #define NB_OF_TASKS 20
48
26 49 #define NB_OF_MISC_NAMES 5
27 50
28 51 // RTEMS GLOBAL VARIABLES
29 52 rtems_name misc_name[NB_OF_MISC_NAMES] = {0};
30 rtems_name Task_name[NB_OF_TASKS] = {0}; /* array of task names */
31 rtems_id Task_id[NB_OF_TASKS] = {0}; /* array of task ids */
53 rtems_name Task_name[CONFIGURE_MAXIMUM_TASKS-1] = {0}; /* array of task names */
54 rtems_id Task_id[CONFIGURE_MAXIMUM_TASKS-1] = {0}; /* array of task ids */
32 55 rtems_name timecode_timer_name = 0;
33 56 rtems_id timecode_timer_id = RTEMS_ID_NONE;
34 57 rtems_name name_hk_rate_monotonic = 0; // name of the HK rate monotonic
35 58 rtems_id HK_id = RTEMS_ID_NONE;// id of the HK rate monotonic period
36 59 rtems_name name_avgv_rate_monotonic = 0; // name of the AVGV rate monotonic
37 60 rtems_id AVGV_id = RTEMS_ID_NONE;// id of the AVGV rate monotonic period
38 61 int fdSPW = 0;
39 62 int fdUART = 0;
40 63 unsigned char lfrCurrentMode = 0;
41 64 unsigned char pa_bia_status_info = 0;
42 65 unsigned char thisIsAnASMRestart = 0;
43 66 unsigned char oneTcLfrUpdateTimeReceived = 0;
44 67
45 68 // WAVEFORMS GLOBAL VARIABLES // 2048 * 3 * 4 + 2 * 4 = 24576 + 8 bytes = 24584
46 69 // 97 * 256 = 24832 => delta = 248 bytes = 62 words
47 70 // WAVEFORMS GLOBAL VARIABLES // 2688 * 3 * 4 + 2 * 4 = 32256 + 8 bytes = 32264
48 71 // 127 * 256 = 32512 => delta = 248 bytes = 62 words
49 72 // F0 F1 F2 F3
50 73 volatile int wf_buffer_f0[ NB_RING_NODES_F0 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
51 74 volatile int wf_buffer_f1[ NB_RING_NODES_F1 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
52 75 volatile int wf_buffer_f2[ NB_RING_NODES_F2 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
53 76 volatile int wf_buffer_f3[ NB_RING_NODES_F3 * WFRM_BUFFER ] __attribute__((aligned(0x100))) = {0};
54 77
55 78 //***********************************
56 79 // SPECTRAL MATRICES GLOBAL VARIABLES
57 80
58 81 // alignment constraints for the spectral matrices buffers => the first data after the time (8 bytes) shall be aligned on 0x00
59 82 volatile int sm_f0[ NB_RING_NODES_SM_F0 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
60 83 volatile int sm_f1[ NB_RING_NODES_SM_F1 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
61 84 volatile int sm_f2[ NB_RING_NODES_SM_F2 * TOTAL_SIZE_SM ] __attribute__((aligned(0x100))) = {0};
62 85
63 86 // APB CONFIGURATION REGISTERS
64 87 time_management_regs_t *time_management_regs = (time_management_regs_t*) REGS_ADDR_TIME_MANAGEMENT;
65 88 gptimer_regs_t *gptimer_regs = (gptimer_regs_t *) REGS_ADDR_GPTIMER;
66 89 waveform_picker_regs_0_1_18_t *waveform_picker_regs = (waveform_picker_regs_0_1_18_t*) REGS_ADDR_WAVEFORM_PICKER;
67 90 spectral_matrix_regs_t *spectral_matrix_regs = (spectral_matrix_regs_t*) REGS_ADDR_SPECTRAL_MATRIX;
68 91
69 92 // MODE PARAMETERS
70 93 Packet_TM_LFR_PARAMETER_DUMP_t parameter_dump_packet = {0};
71 94 struct param_local_str param_local = {0};
72 95 unsigned int lastValidEnterModeTime = {0};
73 96
74 97 // HK PACKETS
75 98 Packet_TM_LFR_HK_t housekeeping_packet = {0};
76 99 // message queues occupancy
77 100 unsigned char hk_lfr_q_sd_fifo_size_max = 0;
78 101 unsigned char hk_lfr_q_rv_fifo_size_max = 0;
79 102 unsigned char hk_lfr_q_p0_fifo_size_max = 0;
80 103 unsigned char hk_lfr_q_p1_fifo_size_max = 0;
81 104 unsigned char hk_lfr_q_p2_fifo_size_max = 0;
82 105 // sequence counters are incremented by APID (PID + CAT) and destination ID
83 106 unsigned short sequenceCounters_SCIENCE_NORMAL_BURST __attribute__((aligned(0x4))) = 0;
84 107 unsigned short sequenceCounters_SCIENCE_SBM1_SBM2 __attribute__((aligned(0x4))) = 0;
85 108 unsigned short sequenceCounters_TC_EXE[SEQ_CNT_NB_DEST_ID] __attribute__((aligned(0x4))) = {0};
86 109 unsigned short sequenceCounters_TM_DUMP[SEQ_CNT_NB_DEST_ID] __attribute__((aligned(0x4))) = {0};
87 110 unsigned short sequenceCounterHK __attribute__((aligned(0x4))) = {0};
88 111 spw_stats grspw_stats __attribute__((aligned(0x4))) = {0};
89 112
90 113 // TC_LFR_UPDATE_INFO
91 114 rw_f_t rw_f;
92 115
93 116 // TC_LFR_LOAD_FILTER_PAR
94 117 filterPar_t filterPar = {0};
95 118
96 119 fbins_masks_t fbins_masks = {0};
Show file before | Show file after | Hide comments
@@ -1,974 +1,1029
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** This is the RTEMS initialization module.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * This module contains two very different information:
7 31 * - specific instructions to configure the compilation of the RTEMS executive
8 32 * - functions related to the fligth softwre initialization, especially the INIT RTEMS task
9 33 *
10 34 */
11 35
12 //*************************
13 // GPL reminder to be added
14 //*************************
36 #include <rtems.h>
15 37
16 #include <rtems.h>
17 38
18 39 /* configuration information */
19 40
20 41 #define CONFIGURE_INIT
21 42
22 43 #include <bsp.h> /* for device driver prototypes */
23 44
24 45 /* configuration information */
25 46
26 #define CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
27 #define CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
28
29 #define CONFIGURE_MAXIMUM_TASKS 21 // number of tasks concurrently active including INIT
30 #define CONFIGURE_RTEMS_INIT_TASKS_TABLE
31 #define CONFIGURE_EXTRA_TASK_STACKS (3 * RTEMS_MINIMUM_STACK_SIZE)
32 #define CONFIGURE_LIBIO_MAXIMUM_FILE_DESCRIPTORS 32
33 #define CONFIGURE_INIT_TASK_PRIORITY 1 // instead of 100
34 #define CONFIGURE_INIT_TASK_MODE (RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT)
35 #define CONFIGURE_INIT_TASK_ATTRIBUTES (RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT)
36 #define CONFIGURE_MAXIMUM_DRIVERS 16
37 #define CONFIGURE_MAXIMUM_PERIODS 6 // [hous] [load] [avgv]
38 #define CONFIGURE_MAXIMUM_TIMERS 6 // [spiq] [link] [spacewire_reset_link]
39 #define CONFIGURE_MAXIMUM_MESSAGE_QUEUES 5
40 #ifdef PRINT_STACK_REPORT
41 #define CONFIGURE_STACK_CHECKER_ENABLED
42 #endif
47 #include <fsw_params.h>
43 48
44 49 #include <rtems/confdefs.h>
45 50
46 51 /* If --drvmgr was enabled during the configuration of the RTEMS kernel */
47 52 #ifdef RTEMS_DRVMGR_STARTUP
48 #ifdef LEON3
49 /* Add Timer and UART Driver */
53 #ifdef LEON3
54 /* Add Timer and UART Driver */
50 55
51 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
52 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
53 #endif
56 #ifdef CONFIGURE_APPLICATION_NEEDS_CLOCK_DRIVER
57 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GPTIMER
58 #endif
54 59
55 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
56 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
57 #endif
60 #ifdef CONFIGURE_APPLICATION_NEEDS_CONSOLE_DRIVER
61 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_APBUART
62 #endif
58 63
59 #endif
60 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
61
62 #include <drvmgr/drvmgr_confdefs.h>
64 #endif
65 #define CONFIGURE_DRIVER_AMBAPP_GAISLER_GRSPW /* GRSPW Driver */
66 #include <drvmgr/drvmgr_confdefs.h>
63 67 #endif
64 68
65 69 #include "fsw_init.h"
66 70 #include "fsw_config.c"
67 71 #include "GscMemoryLPP.hpp"
68 72
69 73 void initCache()
70 74 {
71 75 // ASI 2 contains a few control registers that have not been assigned as ancillary state registers.
72 76 // These should only be read and written using 32-bit LDA/STA instructions.
73 77 // All cache registers are accessed through load/store operations to the alternate address space (LDA/STA), using ASI = 2.
74 78 // The table below shows the register addresses:
75 79 // 0x00 Cache control register
76 80 // 0x04 Reserved
77 81 // 0x08 Instruction cache configuration register
78 82 // 0x0C Data cache configuration register
79 83
80 84 // Cache Control Register Leon3 / Leon3FT
81 85 // 31..30 29 28 27..24 23 22 21 20..19 18 17 16
82 86 // RFT PS TB DS FD FI FT ST IB
83 87 // 15 14 13..12 11..10 9..8 7..6 5 4 3..2 1..0
84 88 // IP DP ITE IDE DTE DDE DF IF DCS ICS
85 89
86 90 unsigned int cacheControlRegister;
87 91
88 92 CCR_resetCacheControlRegister();
89 93 ASR16_resetRegisterProtectionControlRegister();
90 94
91 95 cacheControlRegister = CCR_getValue();
92 96 PRINTF1("(0) CCR - Cache Control Register = %x\n", cacheControlRegister);
93 97 PRINTF1("(0) ASR16 = %x\n", *asr16Ptr);
94 98
95 99 CCR_enableInstructionCache(); // ICS bits
96 100 CCR_enableDataCache(); // DCS bits
97 101 CCR_enableInstructionBurstFetch(); // IB bit
98 102
99 103 faultTolerantScheme();
100 104
101 105 cacheControlRegister = CCR_getValue();
102 106 PRINTF1("(1) CCR - Cache Control Register = %x\n", cacheControlRegister);
103 107 PRINTF1("(1) ASR16 Register protection control register = %x\n", *asr16Ptr);
104 108
105 109 PRINTF("\n");
106 110 }
107 111
108 112 rtems_task Init( rtems_task_argument ignored )
109 113 {
110 114 /** This is the RTEMS INIT taks, it is the first task launched by the system.
111 115 *
112 116 * @param unused is the starting argument of the RTEMS task
113 117 *
114 118 * The INIT task create and run all other RTEMS tasks.
115 119 *
116 120 */
117 121
118 122 //***********
119 123 // INIT CACHE
120 124
121 125 unsigned char *vhdlVersion;
122 126
123 127 reset_lfr();
124 128
125 129 reset_local_time();
126 130
127 131 rtems_cpu_usage_reset();
128 132
129 133 rtems_status_code status;
130 134 rtems_status_code status_spw;
131 135 rtems_isr_entry old_isr_handler;
132 136
133 137 old_isr_handler = NULL;
134 138
135 139 // UART settings
136 140 enable_apbuart_transmitter();
137 141 set_apbuart_scaler_reload_register(REGS_ADDR_APBUART, APBUART_SCALER_RELOAD_VALUE);
138 142
139 143 DEBUG_PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n")
140 144
141 145
142 146 PRINTF("\n\n\n\n\n")
143 147
144 148 initCache();
145 149
146 150 PRINTF("*************************\n")
147 151 PRINTF("** LFR Flight Software **\n")
148 152
149 153 PRINTF1("** %d-", SW_VERSION_N1)
150 154 PRINTF1("%d-" , SW_VERSION_N2)
151 155 PRINTF1("%d-" , SW_VERSION_N3)
152 156 PRINTF1("%d **\n", SW_VERSION_N4)
153 157
154 158 vhdlVersion = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
155 159 PRINTF("** VHDL **\n")
156 160 PRINTF1("** %d-", vhdlVersion[1])
157 161 PRINTF1("%d-" , vhdlVersion[2])
158 162 PRINTF1("%d **\n", vhdlVersion[3])
159 163 PRINTF("*************************\n")
160 164 PRINTF("\n\n")
161 165
162 166 init_parameter_dump();
163 167 init_kcoefficients_dump();
164 168 init_local_mode_parameters();
165 169 init_housekeeping_parameters();
166 170 init_k_coefficients_prc0();
167 171 init_k_coefficients_prc1();
168 172 init_k_coefficients_prc2();
169 173 pa_bia_status_info = INIT_CHAR;
170 174
171 175 // initialize all reaction wheels frequencies to NaN
172 176 rw_f.cp_rpw_sc_rw1_f1 = NAN;
173 177 rw_f.cp_rpw_sc_rw1_f2 = NAN;
174 178 rw_f.cp_rpw_sc_rw1_f3 = NAN;
175 179 rw_f.cp_rpw_sc_rw1_f4 = NAN;
176 180 rw_f.cp_rpw_sc_rw2_f1 = NAN;
177 181 rw_f.cp_rpw_sc_rw2_f2 = NAN;
178 182 rw_f.cp_rpw_sc_rw2_f3 = NAN;
179 183 rw_f.cp_rpw_sc_rw2_f4 = NAN;
180 184 rw_f.cp_rpw_sc_rw3_f1 = NAN;
181 185 rw_f.cp_rpw_sc_rw3_f2 = NAN;
182 186 rw_f.cp_rpw_sc_rw3_f3 = NAN;
183 187 rw_f.cp_rpw_sc_rw3_f4 = NAN;
184 188 rw_f.cp_rpw_sc_rw4_f1 = NAN;
185 189 rw_f.cp_rpw_sc_rw4_f2 = NAN;
186 190 rw_f.cp_rpw_sc_rw4_f3 = NAN;
187 191 rw_f.cp_rpw_sc_rw4_f4 = NAN;
188 192
189 193 // initialize filtering parameters
190 194 filterPar.spare_sy_lfr_pas_filter_enabled = DEFAULT_SY_LFR_PAS_FILTER_ENABLED;
191 195 filterPar.sy_lfr_sc_rw_delta_f = DEFAULT_SY_LFR_SC_RW_DELTA_F;
192 196 filterPar.sy_lfr_pas_filter_tbad = DEFAULT_SY_LFR_PAS_FILTER_TBAD;
193 197 filterPar.sy_lfr_pas_filter_shift = DEFAULT_SY_LFR_PAS_FILTER_SHIFT;
194 198 filterPar.modulus_in_finetime = DEFAULT_MODULUS;
195 199 filterPar.tbad_in_finetime = DEFAULT_TBAD;
196 200 filterPar.offset_in_finetime = DEFAULT_OFFSET;
197 201 filterPar.shift_in_finetime = DEFAULT_SHIFT;
198 202 update_last_valid_transition_date( DEFAULT_LAST_VALID_TRANSITION_DATE );
199 203
200 204 // waveform picker initialization
201 205 WFP_init_rings();
202 206 LEON_Clear_interrupt( IRQ_SPARC_GPTIMER_WATCHDOG ); // initialize the waveform rings
203 207 WFP_reset_current_ring_nodes();
204 208 reset_waveform_picker_regs();
205 209
206 210 // spectral matrices initialization
207 211 SM_init_rings(); // initialize spectral matrices rings
208 212 SM_reset_current_ring_nodes();
209 213 reset_spectral_matrix_regs();
210 214
211 215 // configure calibration
212 216 configureCalibration( false ); // true means interleaved mode, false is for normal mode
213 217
214 218 updateLFRCurrentMode( LFR_MODE_STANDBY );
215 219
216 220 BOOT_PRINTF1("in INIT *** lfrCurrentMode is %d\n", lfrCurrentMode)
217 221
218 222 create_names(); // create all names
219 223
220 224 status = create_timecode_timer(); // create the timer used by timecode_irq_handler
221 225 if (status != RTEMS_SUCCESSFUL)
222 226 {
223 227 PRINTF1("in INIT *** ERR in create_timer_timecode, code %d", status)
224 228 }
225 229
226 230 status = create_message_queues(); // create message queues
227 231 if (status != RTEMS_SUCCESSFUL)
228 232 {
229 233 PRINTF1("in INIT *** ERR in create_message_queues, code %d", status)
230 234 }
231 235
232 236 status = create_all_tasks(); // create all tasks
233 237 if (status != RTEMS_SUCCESSFUL)
234 238 {
235 239 PRINTF1("in INIT *** ERR in create_all_tasks, code %d\n", status)
236 240 }
237 241
238 242 // **************************
239 243 // <SPACEWIRE INITIALIZATION>
240 244 status_spw = spacewire_open_link(); // (1) open the link
241 245 if ( status_spw != RTEMS_SUCCESSFUL )
242 246 {
243 247 PRINTF1("in INIT *** ERR spacewire_open_link code %d\n", status_spw )
244 248 }
245 249
246 250 if ( status_spw == RTEMS_SUCCESSFUL ) // (2) configure the link
247 251 {
248 252 status_spw = spacewire_configure_link( fdSPW );
249 253 if ( status_spw != RTEMS_SUCCESSFUL )
250 254 {
251 255 PRINTF1("in INIT *** ERR spacewire_configure_link code %d\n", status_spw )
252 256 }
253 257 }
254 258
255 259 if ( status_spw == RTEMS_SUCCESSFUL) // (3) start the link
256 260 {
257 261 status_spw = spacewire_start_link( fdSPW );
258 262 if ( status_spw != RTEMS_SUCCESSFUL )
259 263 {
260 264 PRINTF1("in INIT *** ERR spacewire_start_link code %d\n", status_spw )
261 265 }
262 266 }
263 267 // </SPACEWIRE INITIALIZATION>
264 268 // ***************************
265 269
266 270 status = start_all_tasks(); // start all tasks
267 271 if (status != RTEMS_SUCCESSFUL)
268 272 {
269 273 PRINTF1("in INIT *** ERR in start_all_tasks, code %d", status)
270 274 }
271 275
272 276 // start RECV and SEND *AFTER* SpaceWire Initialization, due to the timeout of the start call during the initialization
273 277 status = start_recv_send_tasks();
274 278 if ( status != RTEMS_SUCCESSFUL )
275 279 {
276 280 PRINTF1("in INIT *** ERR start_recv_send_tasks code %d\n", status )
277 281 }
278 282
279 283 // suspend science tasks, they will be restarted later depending on the mode
280 284 status = suspend_science_tasks(); // suspend science tasks (not done in stop_current_mode if current mode = STANDBY)
281 285 if (status != RTEMS_SUCCESSFUL)
282 286 {
283 287 PRINTF1("in INIT *** in suspend_science_tasks *** ERR code: %d\n", status)
284 288 }
285 289
286 290 // configure IRQ handling for the waveform picker unit
287 291 status = rtems_interrupt_catch( waveforms_isr,
288 292 IRQ_SPARC_WAVEFORM_PICKER,
289 293 &old_isr_handler) ;
290 294 // configure IRQ handling for the spectral matrices unit
291 295 status = rtems_interrupt_catch( spectral_matrices_isr,
292 296 IRQ_SPARC_SPECTRAL_MATRIX,
293 297 &old_isr_handler) ;
294 298
295 299 // if the spacewire link is not up then send an event to the SPIQ task for link recovery
296 300 if ( status_spw != RTEMS_SUCCESSFUL )
297 301 {
298 302 status = rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT );
299 303 if ( status != RTEMS_SUCCESSFUL ) {
300 304 PRINTF1("in INIT *** ERR rtems_event_send to SPIQ code %d\n", status )
301 305 }
302 306 }
303 307
304 308 BOOT_PRINTF("delete INIT\n")
305 309
306 310 set_hk_lfr_sc_potential_flag( true );
307 311
308 312 // start the timer to detect a missing spacewire timecode
309 313 // the timeout is larger because the spw IP needs to receive several valid timecodes before generating a tickout
310 314 // if a tickout is generated, the timer is restarted
311 315 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT_INIT, timecode_timer_routine, NULL );
312 316
313 317 grspw_timecode_callback = &timecode_irq_handler;
314 318
315 319 status = rtems_task_delete(RTEMS_SELF);
316 320
317 321 }
318 322
319 323 void init_local_mode_parameters( void )
320 324 {
321 325 /** This function initialize the param_local global variable with default values.
322 326 *
323 327 */
324 328
325 329 unsigned int i;
326 330
327 331 // LOCAL PARAMETERS
328 332
329 333 BOOT_PRINTF1("local_sbm1_nb_cwf_max %d \n", param_local.local_sbm1_nb_cwf_max)
330 334 BOOT_PRINTF1("local_sbm2_nb_cwf_max %d \n", param_local.local_sbm2_nb_cwf_max)
331 335
332 336 // init sequence counters
333 337
334 338 for(i = 0; i<SEQ_CNT_NB_DEST_ID; i++)
335 339 {
336 340 sequenceCounters_TC_EXE[i] = INIT_CHAR;
337 341 sequenceCounters_TM_DUMP[i] = INIT_CHAR;
338 342 }
339 343 sequenceCounters_SCIENCE_NORMAL_BURST = INIT_CHAR;
340 344 sequenceCounters_SCIENCE_SBM1_SBM2 = INIT_CHAR;
341 345 sequenceCounterHK = TM_PACKET_SEQ_CTRL_STANDALONE << TM_PACKET_SEQ_SHIFT;
342 346 }
343 347
344 348 void reset_local_time( void )
345 349 {
346 350 time_management_regs->ctrl = time_management_regs->ctrl | VAL_SOFTWARE_RESET; // [0010] software reset, coarse time = 0x80000000
347 351 }
348 352
349 353 void create_names( void ) // create all names for tasks and queues
350 354 {
351 355 /** This function creates all RTEMS names used in the software for tasks and queues.
352 356 *
353 357 * @return RTEMS directive status codes:
354 358 * - RTEMS_SUCCESSFUL - successful completion
355 359 *
356 360 */
357 361
358 362 // task names
359 363 Task_name[TASKID_AVGV] = rtems_build_name( 'A', 'V', 'G', 'V' );
360 364 Task_name[TASKID_RECV] = rtems_build_name( 'R', 'E', 'C', 'V' );
361 365 Task_name[TASKID_ACTN] = rtems_build_name( 'A', 'C', 'T', 'N' );
362 366 Task_name[TASKID_SPIQ] = rtems_build_name( 'S', 'P', 'I', 'Q' );
363 367 Task_name[TASKID_LOAD] = rtems_build_name( 'L', 'O', 'A', 'D' );
364 368 Task_name[TASKID_AVF0] = rtems_build_name( 'A', 'V', 'F', '0' );
365 369 Task_name[TASKID_SWBD] = rtems_build_name( 'S', 'W', 'B', 'D' );
366 370 Task_name[TASKID_WFRM] = rtems_build_name( 'W', 'F', 'R', 'M' );
367 371 Task_name[TASKID_DUMB] = rtems_build_name( 'D', 'U', 'M', 'B' );
368 372 Task_name[TASKID_HOUS] = rtems_build_name( 'H', 'O', 'U', 'S' );
369 373 Task_name[TASKID_PRC0] = rtems_build_name( 'P', 'R', 'C', '0' );
370 374 Task_name[TASKID_CWF3] = rtems_build_name( 'C', 'W', 'F', '3' );
371 375 Task_name[TASKID_CWF2] = rtems_build_name( 'C', 'W', 'F', '2' );
372 376 Task_name[TASKID_CWF1] = rtems_build_name( 'C', 'W', 'F', '1' );
373 377 Task_name[TASKID_SEND] = rtems_build_name( 'S', 'E', 'N', 'D' );
374 378 Task_name[TASKID_LINK] = rtems_build_name( 'L', 'I', 'N', 'K' );
375 379 Task_name[TASKID_AVF1] = rtems_build_name( 'A', 'V', 'F', '1' );
376 380 Task_name[TASKID_PRC1] = rtems_build_name( 'P', 'R', 'C', '1' );
377 381 Task_name[TASKID_AVF2] = rtems_build_name( 'A', 'V', 'F', '2' );
378 382 Task_name[TASKID_PRC2] = rtems_build_name( 'P', 'R', 'C', '2' );
383 Task_name[TASKID_SCRB] = rtems_build_name( 'S', 'C', 'R', 'B' );
384 Task_name[TASKID_CALI] = rtems_build_name( 'C', 'A', 'L', 'I' );
379 385
380 386 // rate monotonic period names
381 387 name_hk_rate_monotonic = rtems_build_name( 'H', 'O', 'U', 'S' );
382 388 name_avgv_rate_monotonic = rtems_build_name( 'A', 'V', 'G', 'V' );
383 389
384 390 misc_name[QUEUE_RECV] = rtems_build_name( 'Q', '_', 'R', 'V' );
385 391 misc_name[QUEUE_SEND] = rtems_build_name( 'Q', '_', 'S', 'D' );
386 392 misc_name[QUEUE_PRC0] = rtems_build_name( 'Q', '_', 'P', '0' );
387 393 misc_name[QUEUE_PRC1] = rtems_build_name( 'Q', '_', 'P', '1' );
388 394 misc_name[QUEUE_PRC2] = rtems_build_name( 'Q', '_', 'P', '2' );
389 395
390 396 timecode_timer_name = rtems_build_name( 'S', 'P', 'T', 'C' );
391 397 }
392 398
393 399 int create_all_tasks( void ) // create all tasks which run in the software
394 400 {
395 401 /** This function creates all RTEMS tasks used in the software.
396 402 *
397 403 * @return RTEMS directive status codes:
398 404 * - RTEMS_SUCCESSFUL - task created successfully
399 405 * - RTEMS_INVALID_ADDRESS - id is NULL
400 406 * - RTEMS_INVALID_NAME - invalid task name
401 407 * - RTEMS_INVALID_PRIORITY - invalid task priority
402 408 * - RTEMS_MP_NOT_CONFIGURED - multiprocessing not configured
403 409 * - RTEMS_TOO_MANY - too many tasks created
404 410 * - RTEMS_UNSATISFIED - not enough memory for stack/FP context
405 411 * - RTEMS_TOO_MANY - too many global objects
406 412 *
407 413 */
408 414
409 415 rtems_status_code status;
410 416
411 417 //**********
412 418 // SPACEWIRE
413 419 // RECV
414 420 status = rtems_task_create(
415 421 Task_name[TASKID_RECV], TASK_PRIORITY_RECV, RTEMS_MINIMUM_STACK_SIZE,
416 422 RTEMS_DEFAULT_MODES,
417 423 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_RECV]
418 424 );
419 425 if (status == RTEMS_SUCCESSFUL) // SEND
420 426 {
421 427 status = rtems_task_create(
422 428 Task_name[TASKID_SEND], TASK_PRIORITY_SEND, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
423 429 RTEMS_DEFAULT_MODES,
424 430 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SEND]
425 431 );
426 432 }
427 433 if (status == RTEMS_SUCCESSFUL) // LINK
428 434 {
429 435 status = rtems_task_create(
430 436 Task_name[TASKID_LINK], TASK_PRIORITY_LINK, RTEMS_MINIMUM_STACK_SIZE,
431 437 RTEMS_DEFAULT_MODES,
432 438 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LINK]
433 439 );
434 440 }
435 441 if (status == RTEMS_SUCCESSFUL) // ACTN
436 442 {
437 443 status = rtems_task_create(
438 444 Task_name[TASKID_ACTN], TASK_PRIORITY_ACTN, RTEMS_MINIMUM_STACK_SIZE,
439 445 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
440 446 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_ACTN]
441 447 );
442 448 }
443 449 if (status == RTEMS_SUCCESSFUL) // SPIQ
444 450 {
445 451 status = rtems_task_create(
446 452 Task_name[TASKID_SPIQ], TASK_PRIORITY_SPIQ, RTEMS_MINIMUM_STACK_SIZE,
447 453 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
448 454 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_SPIQ]
449 455 );
450 456 }
451 457
452 458 //******************
453 459 // SPECTRAL MATRICES
454 460 if (status == RTEMS_SUCCESSFUL) // AVF0
455 461 {
456 462 status = rtems_task_create(
457 463 Task_name[TASKID_AVF0], TASK_PRIORITY_AVF0, RTEMS_MINIMUM_STACK_SIZE,
458 464 RTEMS_DEFAULT_MODES,
459 465 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF0]
460 466 );
461 467 }
462 468 if (status == RTEMS_SUCCESSFUL) // PRC0
463 469 {
464 470 status = rtems_task_create(
465 471 Task_name[TASKID_PRC0], TASK_PRIORITY_PRC0, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
466 472 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
467 473 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC0]
468 474 );
469 475 }
470 476 if (status == RTEMS_SUCCESSFUL) // AVF1
471 477 {
472 478 status = rtems_task_create(
473 479 Task_name[TASKID_AVF1], TASK_PRIORITY_AVF1, RTEMS_MINIMUM_STACK_SIZE,
474 480 RTEMS_DEFAULT_MODES,
475 481 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF1]
476 482 );
477 483 }
478 484 if (status == RTEMS_SUCCESSFUL) // PRC1
479 485 {
480 486 status = rtems_task_create(
481 487 Task_name[TASKID_PRC1], TASK_PRIORITY_PRC1, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
482 488 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
483 489 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC1]
484 490 );
485 491 }
486 492 if (status == RTEMS_SUCCESSFUL) // AVF2
487 493 {
488 494 status = rtems_task_create(
489 495 Task_name[TASKID_AVF2], TASK_PRIORITY_AVF2, RTEMS_MINIMUM_STACK_SIZE,
490 496 RTEMS_DEFAULT_MODES,
491 497 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVF2]
492 498 );
493 499 }
494 500 if (status == RTEMS_SUCCESSFUL) // PRC2
495 501 {
496 502 status = rtems_task_create(
497 503 Task_name[TASKID_PRC2], TASK_PRIORITY_PRC2, RTEMS_MINIMUM_STACK_SIZE * STACK_SIZE_MULT,
498 504 RTEMS_DEFAULT_MODES | RTEMS_NO_PREEMPT,
499 505 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_PRC2]
500 506 );
501 507 }
502 508
503 509 //****************
504 510 // WAVEFORM PICKER
505 511 if (status == RTEMS_SUCCESSFUL) // WFRM
506 512 {
507 513 status = rtems_task_create(
508 514 Task_name[TASKID_WFRM], TASK_PRIORITY_WFRM, RTEMS_MINIMUM_STACK_SIZE,
509 515 RTEMS_DEFAULT_MODES,
510 516 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_WFRM]
511 517 );
512 518 }
513 519 if (status == RTEMS_SUCCESSFUL) // CWF3
514 520 {
515 521 status = rtems_task_create(
516 522 Task_name[TASKID_CWF3], TASK_PRIORITY_CWF3, RTEMS_MINIMUM_STACK_SIZE,
517 523 RTEMS_DEFAULT_MODES,
518 524 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF3]
519 525 );
520 526 }
521 527 if (status == RTEMS_SUCCESSFUL) // CWF2
522 528 {
523 529 status = rtems_task_create(
524 530 Task_name[TASKID_CWF2], TASK_PRIORITY_CWF2, RTEMS_MINIMUM_STACK_SIZE,
525 531 RTEMS_DEFAULT_MODES,
526 532 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF2]
527 533 );
528 534 }
529 535 if (status == RTEMS_SUCCESSFUL) // CWF1
530 536 {
531 537 status = rtems_task_create(
532 538 Task_name[TASKID_CWF1], TASK_PRIORITY_CWF1, RTEMS_MINIMUM_STACK_SIZE,
533 539 RTEMS_DEFAULT_MODES,
534 540 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CWF1]
535 541 );
536 542 }
537 543 if (status == RTEMS_SUCCESSFUL) // SWBD
538 544 {
539 545 status = rtems_task_create(
540 546 Task_name[TASKID_SWBD], TASK_PRIORITY_SWBD, RTEMS_MINIMUM_STACK_SIZE,
541 547 RTEMS_DEFAULT_MODES,
542 548 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SWBD]
543 549 );
544 550 }
545 551
546 552 //*****
547 553 // MISC
548 554 if (status == RTEMS_SUCCESSFUL) // LOAD
549 555 {
550 556 status = rtems_task_create(
551 557 Task_name[TASKID_LOAD], TASK_PRIORITY_LOAD, RTEMS_MINIMUM_STACK_SIZE,
552 558 RTEMS_DEFAULT_MODES,
553 559 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_LOAD]
554 560 );
555 561 }
556 562 if (status == RTEMS_SUCCESSFUL) // DUMB
557 563 {
558 564 status = rtems_task_create(
559 565 Task_name[TASKID_DUMB], TASK_PRIORITY_DUMB, RTEMS_MINIMUM_STACK_SIZE,
560 566 RTEMS_DEFAULT_MODES,
561 567 RTEMS_DEFAULT_ATTRIBUTES, &Task_id[TASKID_DUMB]
562 568 );
563 569 }
570 if (status == RTEMS_SUCCESSFUL) // SCRUBBING TASK
571 {
572 status = rtems_task_create(
573 Task_name[TASKID_SCRB], TASK_PRIORITY_SCRB, RTEMS_MINIMUM_STACK_SIZE,
574 RTEMS_DEFAULT_MODES,
575 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_SCRB]
576 );
577 }
564 578 if (status == RTEMS_SUCCESSFUL) // HOUS
565 579 {
566 580 status = rtems_task_create(
567 581 Task_name[TASKID_HOUS], TASK_PRIORITY_HOUS, RTEMS_MINIMUM_STACK_SIZE,
568 582 RTEMS_DEFAULT_MODES,
569 583 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_HOUS]
570 584 );
571 585 }
572 586 if (status == RTEMS_SUCCESSFUL) // AVGV
573 587 {
574 588 status = rtems_task_create(
575 589 Task_name[TASKID_AVGV], TASK_PRIORITY_AVGV, RTEMS_MINIMUM_STACK_SIZE,
576 590 RTEMS_DEFAULT_MODES,
577 591 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_AVGV]
578 592 );
579 593 }
594 if (status == RTEMS_SUCCESSFUL) // CALI
595 {
596 status = rtems_task_create(
597 Task_name[TASKID_CALI], TASK_PRIORITY_CALI, RTEMS_MINIMUM_STACK_SIZE,
598 RTEMS_DEFAULT_MODES,
599 RTEMS_DEFAULT_ATTRIBUTES | RTEMS_FLOATING_POINT, &Task_id[TASKID_CALI]
600 );
601 }
580 602
581 603 return status;
582 604 }
583 605
584 606 int start_recv_send_tasks( void )
585 607 {
586 608 rtems_status_code status;
587 609
588 610 status = rtems_task_start( Task_id[TASKID_RECV], recv_task, 1 );
589 611 if (status!=RTEMS_SUCCESSFUL) {
590 612 BOOT_PRINTF("in INIT *** Error starting TASK_RECV\n")
591 613 }
592 614
593 615 if (status == RTEMS_SUCCESSFUL) // SEND
594 616 {
595 617 status = rtems_task_start( Task_id[TASKID_SEND], send_task, 1 );
596 618 if (status!=RTEMS_SUCCESSFUL) {
597 619 BOOT_PRINTF("in INIT *** Error starting TASK_SEND\n")
598 620 }
599 621 }
600 622
601 623 return status;
602 624 }
603 625
604 626 int start_all_tasks( void ) // start all tasks except SEND RECV and HOUS
605 627 {
606 628 /** This function starts all RTEMS tasks used in the software.
607 629 *
608 630 * @return RTEMS directive status codes:
609 631 * - RTEMS_SUCCESSFUL - ask started successfully
610 632 * - RTEMS_INVALID_ADDRESS - invalid task entry point
611 633 * - RTEMS_INVALID_ID - invalid task id
612 634 * - RTEMS_INCORRECT_STATE - task not in the dormant state
613 635 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot start remote task
614 636 *
615 637 */
616 638 // starts all the tasks fot eh flight software
617 639
618 640 rtems_status_code status;
619 641
620 642 //**********
621 643 // SPACEWIRE
622 644 status = rtems_task_start( Task_id[TASKID_SPIQ], spiq_task, 1 );
623 645 if (status!=RTEMS_SUCCESSFUL) {
624 646 BOOT_PRINTF("in INIT *** Error starting TASK_SPIQ\n")
625 647 }
626 648
627 649 if (status == RTEMS_SUCCESSFUL) // LINK
628 650 {
629 651 status = rtems_task_start( Task_id[TASKID_LINK], link_task, 1 );
630 652 if (status!=RTEMS_SUCCESSFUL) {
631 653 BOOT_PRINTF("in INIT *** Error starting TASK_LINK\n")
632 654 }
633 655 }
634 656
635 657 if (status == RTEMS_SUCCESSFUL) // ACTN
636 658 {
637 659 status = rtems_task_start( Task_id[TASKID_ACTN], actn_task, 1 );
638 660 if (status!=RTEMS_SUCCESSFUL) {
639 661 BOOT_PRINTF("in INIT *** Error starting TASK_ACTN\n")
640 662 }
641 663 }
642 664
643 665 //******************
644 666 // SPECTRAL MATRICES
645 667 if (status == RTEMS_SUCCESSFUL) // AVF0
646 668 {
647 669 status = rtems_task_start( Task_id[TASKID_AVF0], avf0_task, LFR_MODE_STANDBY );
648 670 if (status!=RTEMS_SUCCESSFUL) {
649 671 BOOT_PRINTF("in INIT *** Error starting TASK_AVF0\n")
650 672 }
651 673 }
652 674 if (status == RTEMS_SUCCESSFUL) // PRC0
653 675 {
654 676 status = rtems_task_start( Task_id[TASKID_PRC0], prc0_task, LFR_MODE_STANDBY );
655 677 if (status!=RTEMS_SUCCESSFUL) {
656 678 BOOT_PRINTF("in INIT *** Error starting TASK_PRC0\n")
657 679 }
658 680 }
659 681 if (status == RTEMS_SUCCESSFUL) // AVF1
660 682 {
661 683 status = rtems_task_start( Task_id[TASKID_AVF1], avf1_task, LFR_MODE_STANDBY );
662 684 if (status!=RTEMS_SUCCESSFUL) {
663 685 BOOT_PRINTF("in INIT *** Error starting TASK_AVF1\n")
664 686 }
665 687 }
666 688 if (status == RTEMS_SUCCESSFUL) // PRC1
667 689 {
668 690 status = rtems_task_start( Task_id[TASKID_PRC1], prc1_task, LFR_MODE_STANDBY );
669 691 if (status!=RTEMS_SUCCESSFUL) {
670 692 BOOT_PRINTF("in INIT *** Error starting TASK_PRC1\n")
671 693 }
672 694 }
673 695 if (status == RTEMS_SUCCESSFUL) // AVF2
674 696 {
675 697 status = rtems_task_start( Task_id[TASKID_AVF2], avf2_task, 1 );
676 698 if (status!=RTEMS_SUCCESSFUL) {
677 699 BOOT_PRINTF("in INIT *** Error starting TASK_AVF2\n")
678 700 }
679 701 }
680 702 if (status == RTEMS_SUCCESSFUL) // PRC2
681 703 {
682 704 status = rtems_task_start( Task_id[TASKID_PRC2], prc2_task, 1 );
683 705 if (status!=RTEMS_SUCCESSFUL) {
684 706 BOOT_PRINTF("in INIT *** Error starting TASK_PRC2\n")
685 707 }
686 708 }
687 709
688 710 //****************
689 711 // WAVEFORM PICKER
690 712 if (status == RTEMS_SUCCESSFUL) // WFRM
691 713 {
692 714 status = rtems_task_start( Task_id[TASKID_WFRM], wfrm_task, 1 );
693 715 if (status!=RTEMS_SUCCESSFUL) {
694 716 BOOT_PRINTF("in INIT *** Error starting TASK_WFRM\n")
695 717 }
696 718 }
697 719 if (status == RTEMS_SUCCESSFUL) // CWF3
698 720 {
699 721 status = rtems_task_start( Task_id[TASKID_CWF3], cwf3_task, 1 );
700 722 if (status!=RTEMS_SUCCESSFUL) {
701 723 BOOT_PRINTF("in INIT *** Error starting TASK_CWF3\n")
702 724 }
703 725 }
704 726 if (status == RTEMS_SUCCESSFUL) // CWF2
705 727 {
706 728 status = rtems_task_start( Task_id[TASKID_CWF2], cwf2_task, 1 );
707 729 if (status!=RTEMS_SUCCESSFUL) {
708 730 BOOT_PRINTF("in INIT *** Error starting TASK_CWF2\n")
709 731 }
710 732 }
711 733 if (status == RTEMS_SUCCESSFUL) // CWF1
712 734 {
713 735 status = rtems_task_start( Task_id[TASKID_CWF1], cwf1_task, 1 );
714 736 if (status!=RTEMS_SUCCESSFUL) {
715 737 BOOT_PRINTF("in INIT *** Error starting TASK_CWF1\n")
716 738 }
717 739 }
718 740 if (status == RTEMS_SUCCESSFUL) // SWBD
719 741 {
720 742 status = rtems_task_start( Task_id[TASKID_SWBD], swbd_task, 1 );
721 743 if (status!=RTEMS_SUCCESSFUL) {
722 744 BOOT_PRINTF("in INIT *** Error starting TASK_SWBD\n")
723 745 }
724 746 }
725 747
726 748 //*****
727 749 // MISC
728 750 if (status == RTEMS_SUCCESSFUL) // HOUS
729 751 {
730 752 status = rtems_task_start( Task_id[TASKID_HOUS], hous_task, 1 );
731 753 if (status!=RTEMS_SUCCESSFUL) {
732 754 BOOT_PRINTF("in INIT *** Error starting TASK_HOUS\n")
733 755 }
734 756 }
735 757 if (status == RTEMS_SUCCESSFUL) // AVGV
736 758 {
737 759 status = rtems_task_start( Task_id[TASKID_AVGV], avgv_task, 1 );
738 760 if (status!=RTEMS_SUCCESSFUL) {
739 761 BOOT_PRINTF("in INIT *** Error starting TASK_AVGV\n")
740 762 }
741 763 }
742 764 if (status == RTEMS_SUCCESSFUL) // DUMB
743 765 {
744 766 status = rtems_task_start( Task_id[TASKID_DUMB], dumb_task, 1 );
745 767 if (status!=RTEMS_SUCCESSFUL) {
746 768 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
747 769 }
748 770 }
771 if (status == RTEMS_SUCCESSFUL) // SCRUBBING
772 {
773 status = rtems_task_start( Task_id[TASKID_SCRB], scrubbing_task, 1 );
774 if (status!=RTEMS_SUCCESSFUL) {
775 BOOT_PRINTF("in INIT *** Error starting TASK_DUMB\n")
776 }
777 }
749 778 if (status == RTEMS_SUCCESSFUL) // LOAD
750 779 {
751 780 status = rtems_task_start( Task_id[TASKID_LOAD], load_task, 1 );
752 781 if (status!=RTEMS_SUCCESSFUL) {
753 782 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
754 783 }
755 784 }
785 if (status == RTEMS_SUCCESSFUL) // CALI
786 {
787 status = rtems_task_start( Task_id[TASKID_CALI], calibration_sweep_task, 1 );
788 if (status!=RTEMS_SUCCESSFUL) {
789 BOOT_PRINTF("in INIT *** Error starting TASK_LOAD\n")
790 }
791 }
756 792
757 793 return status;
758 794 }
759 795
760 rtems_status_code create_message_queues( void ) // create the two message queues used in the software
796 rtems_status_code create_message_queues( void ) // create the five message queues used in the software
761 797 {
762 798 rtems_status_code status_recv;
763 799 rtems_status_code status_send;
764 800 rtems_status_code status_q_p0;
765 801 rtems_status_code status_q_p1;
766 802 rtems_status_code status_q_p2;
767 803 rtems_status_code ret;
768 804 rtems_id queue_id;
769 805
770 806 ret = RTEMS_SUCCESSFUL;
771 807 queue_id = RTEMS_ID_NONE;
772 808
773 809 //****************************************
774 810 // create the queue for handling valid TCs
775 811 status_recv = rtems_message_queue_create( misc_name[QUEUE_RECV],
776 812 MSG_QUEUE_COUNT_RECV, CCSDS_TC_PKT_MAX_SIZE,
777 813 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
778 814 if ( status_recv != RTEMS_SUCCESSFUL ) {
779 815 PRINTF1("in create_message_queues *** ERR creating QUEU queue, %d\n", status_recv)
780 816 }
781 817
782 818 //************************************************
783 819 // create the queue for handling TM packet sending
784 820 status_send = rtems_message_queue_create( misc_name[QUEUE_SEND],
785 821 MSG_QUEUE_COUNT_SEND, MSG_QUEUE_SIZE_SEND,
786 822 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
787 823 if ( status_send != RTEMS_SUCCESSFUL ) {
788 824 PRINTF1("in create_message_queues *** ERR creating PKTS queue, %d\n", status_send)
789 825 }
790 826
791 827 //*****************************************************************************
792 828 // create the queue for handling averaged spectral matrices for processing @ f0
793 829 status_q_p0 = rtems_message_queue_create( misc_name[QUEUE_PRC0],
794 830 MSG_QUEUE_COUNT_PRC0, MSG_QUEUE_SIZE_PRC0,
795 831 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
796 832 if ( status_q_p0 != RTEMS_SUCCESSFUL ) {
797 833 PRINTF1("in create_message_queues *** ERR creating Q_P0 queue, %d\n", status_q_p0)
798 834 }
799 835
800 836 //*****************************************************************************
801 837 // create the queue for handling averaged spectral matrices for processing @ f1
802 838 status_q_p1 = rtems_message_queue_create( misc_name[QUEUE_PRC1],
803 839 MSG_QUEUE_COUNT_PRC1, MSG_QUEUE_SIZE_PRC1,
804 840 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
805 841 if ( status_q_p1 != RTEMS_SUCCESSFUL ) {
806 842 PRINTF1("in create_message_queues *** ERR creating Q_P1 queue, %d\n", status_q_p1)
807 843 }
808 844
809 845 //*****************************************************************************
810 846 // create the queue for handling averaged spectral matrices for processing @ f2
811 847 status_q_p2 = rtems_message_queue_create( misc_name[QUEUE_PRC2],
812 848 MSG_QUEUE_COUNT_PRC2, MSG_QUEUE_SIZE_PRC2,
813 849 RTEMS_FIFO | RTEMS_LOCAL, &queue_id );
814 850 if ( status_q_p2 != RTEMS_SUCCESSFUL ) {
815 851 PRINTF1("in create_message_queues *** ERR creating Q_P2 queue, %d\n", status_q_p2)
816 852 }
817 853
818 854 if ( status_recv != RTEMS_SUCCESSFUL )
819 855 {
820 856 ret = status_recv;
821 857 }
822 858 else if( status_send != RTEMS_SUCCESSFUL )
823 859 {
824 860 ret = status_send;
825 861 }
826 862 else if( status_q_p0 != RTEMS_SUCCESSFUL )
827 863 {
828 864 ret = status_q_p0;
829 865 }
830 866 else if( status_q_p1 != RTEMS_SUCCESSFUL )
831 867 {
832 868 ret = status_q_p1;
833 869 }
834 870 else
835 871 {
836 872 ret = status_q_p2;
837 873 }
838 874
839 875 return ret;
840 876 }
841 877
842 878 rtems_status_code create_timecode_timer( void )
843 879 {
844 880 rtems_status_code status;
845 881
846 882 status = rtems_timer_create( timecode_timer_name, &timecode_timer_id );
847 883
848 884 if ( status != RTEMS_SUCCESSFUL )
849 885 {
850 886 PRINTF1("in create_timer_timecode *** ERR creating SPTC timer, %d\n", status)
851 887 }
852 888 else
853 889 {
854 890 PRINTF("in create_timer_timecode *** OK creating SPTC timer\n")
855 891 }
856 892
857 893 return status;
858 894 }
859 895
860 896 rtems_status_code get_message_queue_id_send( rtems_id *queue_id )
861 897 {
862 898 rtems_status_code status;
863 899 rtems_name queue_name;
864 900
865 901 queue_name = rtems_build_name( 'Q', '_', 'S', 'D' );
866 902
867 903 status = rtems_message_queue_ident( queue_name, 0, queue_id );
868 904
869 905 return status;
870 906 }
871 907
872 908 rtems_status_code get_message_queue_id_recv( rtems_id *queue_id )
873 909 {
874 910 rtems_status_code status;
875 911 rtems_name queue_name;
876 912
877 913 queue_name = rtems_build_name( 'Q', '_', 'R', 'V' );
878 914
879 915 status = rtems_message_queue_ident( queue_name, 0, queue_id );
880 916
881 917 return status;
882 918 }
883 919
884 920 rtems_status_code get_message_queue_id_prc0( rtems_id *queue_id )
885 921 {
886 922 rtems_status_code status;
887 923 rtems_name queue_name;
888 924
889 925 queue_name = rtems_build_name( 'Q', '_', 'P', '0' );
890 926
891 927 status = rtems_message_queue_ident( queue_name, 0, queue_id );
892 928
893 929 return status;
894 930 }
895 931
896 932 rtems_status_code get_message_queue_id_prc1( rtems_id *queue_id )
897 933 {
898 934 rtems_status_code status;
899 935 rtems_name queue_name;
900 936
901 937 queue_name = rtems_build_name( 'Q', '_', 'P', '1' );
902 938
903 939 status = rtems_message_queue_ident( queue_name, 0, queue_id );
904 940
905 941 return status;
906 942 }
907 943
908 944 rtems_status_code get_message_queue_id_prc2( rtems_id *queue_id )
909 945 {
910 946 rtems_status_code status;
911 947 rtems_name queue_name;
912 948
913 949 queue_name = rtems_build_name( 'Q', '_', 'P', '2' );
914 950
915 951 status = rtems_message_queue_ident( queue_name, 0, queue_id );
916 952
917 953 return status;
918 954 }
919 955
956 /**
957 * @brief update_queue_max_count returns max(fifo_size_max, pending_messages + 1)
958 * @param queue_id
959 * @param fifo_size_max
960 */
920 961 void update_queue_max_count( rtems_id queue_id, unsigned char*fifo_size_max )
921 962 {
922 963 u_int32_t count;
923 964 rtems_status_code status;
924 965
925 966 count = 0;
926 967
927 968 status = rtems_message_queue_get_number_pending( queue_id, &count );
928 969
929 970 count = count + 1;
930 971
931 972 if (status != RTEMS_SUCCESSFUL)
932 973 {
933 974 PRINTF1("in update_queue_max_count *** ERR = %d\n", status)
934 975 }
935 976 else
936 977 {
937 978 if (count > *fifo_size_max)
938 979 {
939 980 *fifo_size_max = count;
940 981 }
941 982 }
942 983 }
943 984
985 /**
986 * @brief init_ring initializes given ring buffer
987 * @param ring array of nodes to initialize
988 * @param nbNodes number of node in the ring buffer
989 * @param buffer memory space given to the ring buffer
990 * @param bufferSize size of the whole ring buffer memory space
991 *
992 * @details This function creates a circular buffer from a given number of nodes and a given memory space. It first sets all nodes attributes to thier defaults values
993 * and associates a portion of the given memory space with each node. Then it connects each nodes to build a circular buffer.
994 *
995 * Each node capacity will be bufferSize/nbNodes.
996 *
997 * https://en.wikipedia.org/wiki/Circular_buffer
998 */
944 999 void init_ring(ring_node ring[], unsigned char nbNodes, volatile int buffer[], unsigned int bufferSize )
945 1000 {
946 1001 unsigned char i;
947 1002
948 1003 //***************
949 1004 // BUFFER ADDRESS
950 1005 for(i=0; i<nbNodes; i++)
951 1006 {
952 1007 ring[i].coarseTime = INT32_ALL_F;
953 1008 ring[i].fineTime = INT32_ALL_F;
954 1009 ring[i].sid = INIT_CHAR;
955 1010 ring[i].status = INIT_CHAR;
956 1011 ring[i].buffer_address = (int) &buffer[ i * bufferSize ];
957 1012 }
958 1013
959 1014 //*****
960 1015 // NEXT
961 1016 ring[ nbNodes - 1 ].next = (ring_node*) &ring[ 0 ];
962 1017 for(i=0; i<nbNodes-1; i++)
963 1018 {
964 1019 ring[i].next = (ring_node*) &ring[ i + 1 ];
965 1020 }
966 1021
967 1022 //*********
968 1023 // PREVIOUS
969 1024 ring[ 0 ].previous = (ring_node*) &ring[ nbNodes - 1 ];
970 1025 for(i=1; i<nbNodes; i++)
971 1026 {
972 1027 ring[i].previous = (ring_node*) &ring[ i - 1 ];
973 1028 }
974 1029 }
Show file before | Show file after | Hide comments
@@ -1,1036 +1,1109
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** General usage functions and RTEMS tasks.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 */
7 31
8 32 #include "fsw_misc.h"
9 33
10 34 int16_t hk_lfr_sc_v_f3_as_int16 = 0;
11 35 int16_t hk_lfr_sc_e1_f3_as_int16 = 0;
12 36 int16_t hk_lfr_sc_e2_f3_as_int16 = 0;
13 37
14 38 void timer_configure(unsigned char timer, unsigned int clock_divider,
15 39 unsigned char interrupt_level, rtems_isr (*timer_isr)() )
16 40 {
17 41 /** This function configures a GPTIMER timer instantiated in the VHDL design.
18 42 *
19 43 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
20 44 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
21 45 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
22 46 * @param interrupt_level is the interrupt level that the timer drives.
23 47 * @param timer_isr is the interrupt subroutine that will be attached to the IRQ driven by the timer.
24 48 *
25 49 * Interrupt levels are described in the SPARC documentation sparcv8.pdf p.76
26 50 *
27 51 */
28 52
29 53 rtems_status_code status;
30 54 rtems_isr_entry old_isr_handler;
31 55
32 56 old_isr_handler = NULL;
33 57
34 58 gptimer_regs->timer[timer].ctrl = INIT_CHAR; // reset the control register
35 59
36 60 status = rtems_interrupt_catch( timer_isr, interrupt_level, &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels
37 61 if (status!=RTEMS_SUCCESSFUL)
38 62 {
39 63 PRINTF("in configure_timer *** ERR rtems_interrupt_catch\n")
40 64 }
41 65
42 66 timer_set_clock_divider( timer, clock_divider);
43 67 }
44 68
45 69 void timer_start(unsigned char timer)
46 70 {
47 71 /** This function starts a GPTIMER timer.
48 72 *
49 73 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
50 74 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
51 75 *
52 76 */
53 77
54 78 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_CLEAR_IRQ;
55 79 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_LD;
56 80 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_EN;
57 81 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_RS;
58 82 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_IE;
59 83 }
60 84
61 85 void timer_stop(unsigned char timer)
62 86 {
63 87 /** This function stops a GPTIMER timer.
64 88 *
65 89 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
66 90 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
67 91 *
68 92 */
69 93
70 94 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & GPTIMER_EN_MASK;
71 95 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & GPTIMER_IE_MASK;
72 96 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | GPTIMER_CLEAR_IRQ;
73 97 }
74 98
75 99 void timer_set_clock_divider(unsigned char timer, unsigned int clock_divider)
76 100 {
77 101 /** This function sets the clock divider of a GPTIMER timer.
78 102 *
79 103 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
80 104 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
81 105 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
82 106 *
83 107 */
84 108
85 109 gptimer_regs->timer[timer].reload = clock_divider; // base clock frequency is 1 MHz
86 110 }
87 111
88 // WATCHDOG
112 // WATCHDOG, this ISR should never be triggered.
89 113
90 114 rtems_isr watchdog_isr( rtems_vector_number vector )
91 115 {
92 116 rtems_status_code status_code;
93 117
94 118 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_12 );
95 119
96 120 PRINTF("watchdog_isr *** this is the end, exit(0)\n");
97 121
98 122 exit(0);
99 123 }
100 124
101 125 void watchdog_configure(void)
102 126 {
103 127 /** This function configure the watchdog.
104 128 *
105 129 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
106 130 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
107 131 *
108 132 * The watchdog is a timer provided by the GPTIMER IP core of the GRLIB.
109 133 *
110 134 */
111 135
112 136 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt during configuration
113 137
114 138 timer_configure( TIMER_WATCHDOG, CLKDIV_WATCHDOG, IRQ_SPARC_GPTIMER_WATCHDOG, watchdog_isr );
115 139
116 140 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
117 141 }
118 142
119 143 void watchdog_stop(void)
120 144 {
121 145 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt line
122 146 timer_stop( TIMER_WATCHDOG );
123 147 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
124 148 }
125 149
126 150 void watchdog_reload(void)
127 151 {
128 152 /** This function reloads the watchdog timer counter with the timer reload value.
129 153 *
130 154 * @param void
131 155 *
132 156 * @return void
133 157 *
134 158 */
135 159
136 160 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_LD;
137 161 }
138 162
139 163 void watchdog_start(void)
140 164 {
141 165 /** This function starts the watchdog timer.
142 166 *
143 167 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
144 168 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
145 169 *
146 170 */
147 171
148 172 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG );
149 173
150 174 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_CLEAR_IRQ;
151 175 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_LD;
152 176 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_EN;
153 177 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | GPTIMER_IE;
154 178
155 179 LEON_Unmask_interrupt( IRQ_GPTIMER_WATCHDOG );
156 180
157 181 }
158 182
159 183 int enable_apbuart_transmitter( void ) // set the bit 1, TE Transmitter Enable to 1 in the APBUART control register
160 184 {
161 185 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
162 186
163 187 apbuart_regs->ctrl = APBUART_CTRL_REG_MASK_TE;
164 188
165 189 return 0;
166 190 }
167 191
168 192 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value)
169 193 {
170 194 /** This function sets the scaler reload register of the apbuart module
171 195 *
172 196 * @param regs is the address of the apbuart registers in memory
173 197 * @param value is the value that will be stored in the scaler register
174 198 *
175 199 * The value shall be set by the software to get data on the serial interface.
176 200 *
177 201 */
178 202
179 203 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs;
180 204
181 205 apbuart_regs->scaler = value;
182 206
183 207 BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value)
184 208 }
185 209
186 //************
187 // RTEMS TASKS
210 /**
211 * @brief load_task starts and keeps the watchdog alive.
212 * @param argument
213 * @return
214 */
188 215
189 216 rtems_task load_task(rtems_task_argument argument)
190 217 {
191 218 BOOT_PRINTF("in LOAD *** \n")
192 219
193 220 rtems_status_code status;
194 221 unsigned int i;
195 222 unsigned int j;
196 223 rtems_name name_watchdog_rate_monotonic; // name of the watchdog rate monotonic
197 224 rtems_id watchdog_period_id; // id of the watchdog rate monotonic period
198 225
199 226 watchdog_period_id = RTEMS_ID_NONE;
200 227
201 228 name_watchdog_rate_monotonic = rtems_build_name( 'L', 'O', 'A', 'D' );
202 229
203 230 status = rtems_rate_monotonic_create( name_watchdog_rate_monotonic, &watchdog_period_id );
204 231 if( status != RTEMS_SUCCESSFUL ) {
205 232 PRINTF1( "in LOAD *** rtems_rate_monotonic_create failed with status of %d\n", status )
206 233 }
207 234
208 235 i = 0;
209 236 j = 0;
210 237
211 238 watchdog_configure();
212 239
213 240 watchdog_start();
214 241
215 242 set_sy_lfr_watchdog_enabled( true );
216 243
217 244 while(1){
218 245 status = rtems_rate_monotonic_period( watchdog_period_id, WATCHDOG_PERIOD );
219 246 watchdog_reload();
220 247 i = i + 1;
221 248 if ( i == WATCHDOG_LOOP_PRINTF )
222 249 {
223 250 i = 0;
224 251 j = j + 1;
225 252 PRINTF1("%d\n", j)
226 253 }
227 254 #ifdef DEBUG_WATCHDOG
228 255 if (j == WATCHDOG_LOOP_DEBUG )
229 256 {
230 257 status = rtems_task_delete(RTEMS_SELF);
231 258 }
232 259 #endif
233 260 }
234 261 }
235 262
263 /**
264 * @brief hous_task produces and sends HK each seconds
265 * @param argument
266 * @return
267 */
236 268 rtems_task hous_task(rtems_task_argument argument)
237 269 {
238 270 rtems_status_code status;
239 271 rtems_status_code spare_status;
240 272 rtems_id queue_id;
241 273 rtems_rate_monotonic_period_status period_status;
242 274 bool isSynchronized;
243 275
244 276 queue_id = RTEMS_ID_NONE;
245 277 memset(&period_status, 0, sizeof(rtems_rate_monotonic_period_status));
246 278 isSynchronized = false;
247 279
248 280 status = get_message_queue_id_send( &queue_id );
249 281 if (status != RTEMS_SUCCESSFUL)
250 282 {
251 283 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
252 284 }
253 285
254 286 BOOT_PRINTF("in HOUS ***\n");
255 287
256 288 if (rtems_rate_monotonic_ident( name_hk_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
257 289 status = rtems_rate_monotonic_create( name_hk_rate_monotonic, &HK_id );
258 290 if( status != RTEMS_SUCCESSFUL ) {
259 291 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
260 292 }
261 293 }
262 294
263 295 status = rtems_rate_monotonic_cancel(HK_id);
264 296 if( status != RTEMS_SUCCESSFUL ) {
265 297 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status );
266 298 }
267 299 else {
268 300 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n");
269 301 }
270 302
271 303 // startup phase
272 304 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
273 305 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
274 306 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
275 307 while( (period_status.state != RATE_MONOTONIC_EXPIRED)
276 308 && (isSynchronized == false) ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
277 309 {
278 310 if ((time_management_regs->coarse_time & VAL_LFR_SYNCHRONIZED) == INT32_ALL_0) // check time synchronization
279 311 {
280 312 isSynchronized = true;
281 313 }
282 314 else
283 315 {
284 316 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
285 317
286 318 status = rtems_task_wake_after( HK_SYNC_WAIT ); // wait HK_SYNCH_WAIT 100 ms = 10 * 10 ms
287 319 }
288 320 }
289 321 status = rtems_rate_monotonic_cancel(HK_id);
290 322 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
291 323
292 324 set_hk_lfr_reset_cause( POWER_ON );
293 325
294 326 while(1){ // launch the rate monotonic task
295 327 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
296 328 if ( status != RTEMS_SUCCESSFUL ) {
297 329 PRINTF1( "in HOUS *** ERR period: %d\n", status);
298 330 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
299 331 }
300 332 else {
301 333 housekeeping_packet.packetSequenceControl[BYTE_0] = (unsigned char) (sequenceCounterHK >> SHIFT_1_BYTE);
302 334 housekeeping_packet.packetSequenceControl[BYTE_1] = (unsigned char) (sequenceCounterHK );
303 335 increment_seq_counter( &sequenceCounterHK );
304 336
305 337 housekeeping_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
306 338 housekeeping_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
307 339 housekeeping_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
308 340 housekeeping_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
309 341 housekeeping_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
310 342 housekeeping_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
311 343
312 344 spacewire_update_hk_lfr_link_state( &housekeeping_packet.lfr_status_word[0] );
313 345
314 346 spacewire_read_statistics();
315 347
316 348 update_hk_with_grspw_stats();
317 349
318 350 set_hk_lfr_time_not_synchro();
319 351
320 352 housekeeping_packet.hk_lfr_q_sd_fifo_size_max = hk_lfr_q_sd_fifo_size_max;
321 353 housekeeping_packet.hk_lfr_q_rv_fifo_size_max = hk_lfr_q_rv_fifo_size_max;
322 354 housekeeping_packet.hk_lfr_q_p0_fifo_size_max = hk_lfr_q_p0_fifo_size_max;
323 355 housekeeping_packet.hk_lfr_q_p1_fifo_size_max = hk_lfr_q_p1_fifo_size_max;
324 356 housekeeping_packet.hk_lfr_q_p2_fifo_size_max = hk_lfr_q_p2_fifo_size_max;
325 357
326 358 housekeeping_packet.sy_lfr_common_parameters_spare = parameter_dump_packet.sy_lfr_common_parameters_spare;
327 359 housekeeping_packet.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
328 360 get_temperatures( housekeeping_packet.hk_lfr_temp_scm );
329 361 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
330 362 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
331 363
332 364 hk_lfr_le_me_he_update();
333 365
334 366 // SEND PACKET
335 367 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
336 368 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
337 369 if (status != RTEMS_SUCCESSFUL) {
338 370 PRINTF1("in HOUS *** ERR send: %d\n", status)
339 371 }
340 372 }
341 373 }
342 374
343 375 PRINTF("in HOUS *** deleting task\n")
344 376
345 377 status = rtems_task_delete( RTEMS_SELF ); // should not return
346 378
347 379 return;
348 380 }
349 381
382 /**
383 * @brief filter is a Direct-Form-II filter implementation, mostly used to filter electric field for HK
384 * @param x, new sample
385 * @param ctx, filter context, used to store previous input and output samples
386 * @return a new filtered sample
387 */
350 388 int filter( int x, filter_ctx* ctx )
351 389 {
352 390 static const int b[NB_COEFFS][NB_COEFFS]={ {B00, B01, B02}, {B10, B11, B12}, {B20, B21, B22} };
353 391 static const int a[NB_COEFFS][NB_COEFFS]={ {A00, A01, A02}, {A10, A11, A12}, {A20, A21, A22} };
354 392 static const int b_gain[NB_COEFFS]={GAIN_B0, GAIN_B1, GAIN_B2};
355 393 static const int a_gain[NB_COEFFS]={GAIN_A0, GAIN_A1, GAIN_A2};
356 394
357 395 int_fast32_t W;
358 396 int i;
359 397
360 398 W = INIT_INT;
361 399 i = INIT_INT;
362 400
363 401 //Direct-Form-II
364 402 for ( i = 0; i < NB_COEFFS; i++ )
365 403 {
366 404 x = x << a_gain[i];
367 405 W = (x - ( a[i][COEFF1] * ctx->W[i][COEFF0] )
368 406 - ( a[i][COEFF2] * ctx->W[i][COEFF1] ) ) >> a_gain[i];
369 407 x = ( b[i][COEFF0] * W )
370 408 + ( b[i][COEFF1] * ctx->W[i][COEFF0] )
371 409 + ( b[i][COEFF2] * ctx->W[i][COEFF1] );
372 410 x = x >> b_gain[i];
373 411 ctx->W[i][1] = ctx->W[i][0];
374 412 ctx->W[i][0] = W;
375 413 }
376 414 return x;
377 415 }
378 416
417 /**
418 * @brief avgv_task pruduces HK rate elctrical field from F3 data
419 * @param argument
420 * @return
421 */
379 422 rtems_task avgv_task(rtems_task_argument argument)
380 423 {
381 424 #define MOVING_AVERAGE 16
382 425 rtems_status_code status;
383 426 static int32_t v[MOVING_AVERAGE] = {0};
384 427 static int32_t e1[MOVING_AVERAGE] = {0};
385 428 static int32_t e2[MOVING_AVERAGE] = {0};
386 429 static int old_v = 0;
387 430 static int old_e1 = 0;
388 431 static int old_e2 = 0;
389 432 int32_t current_v;
390 433 int32_t current_e1;
391 434 int32_t current_e2;
392 435 int32_t average_v;
393 436 int32_t average_e1;
394 437 int32_t average_e2;
395 438 int32_t newValue_v;
396 439 int32_t newValue_e1;
397 440 int32_t newValue_e2;
398 441 unsigned char k;
399 442 unsigned char indexOfOldValue;
400 443
401 444 static filter_ctx ctx_v = { { {0,0,0}, {0,0,0}, {0,0,0} } };
402 445 static filter_ctx ctx_e1 = { { {0,0,0}, {0,0,0}, {0,0,0} } };
403 446 static filter_ctx ctx_e2 = { { {0,0,0}, {0,0,0}, {0,0,0} } };
404 447
405 448 BOOT_PRINTF("in AVGV ***\n");
406 449
407 450 if (rtems_rate_monotonic_ident( name_avgv_rate_monotonic, &AVGV_id) != RTEMS_SUCCESSFUL) {
408 451 status = rtems_rate_monotonic_create( name_avgv_rate_monotonic, &AVGV_id );
409 452 if( status != RTEMS_SUCCESSFUL ) {
410 453 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
411 454 }
412 455 }
413 456
414 457 status = rtems_rate_monotonic_cancel(AVGV_id);
415 458 if( status != RTEMS_SUCCESSFUL ) {
416 459 PRINTF1( "ERR *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id) ***code: %d\n", status );
417 460 }
418 461 else {
419 462 DEBUG_PRINTF("OK *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id)\n");
420 463 }
421 464
422 465 // initialize values
423 466 indexOfOldValue = MOVING_AVERAGE - 1;
424 467 current_v = 0;
425 468 current_e1 = 0;
426 469 current_e2 = 0;
427 470 average_v = 0;
428 471 average_e1 = 0;
429 472 average_e2 = 0;
430 473 newValue_v = 0;
431 474 newValue_e1 = 0;
432 475 newValue_e2 = 0;
433 476
434 477 k = INIT_CHAR;
435 478
436 479 while(1)
437 480 { // launch the rate monotonic task
438 481 status = rtems_rate_monotonic_period( AVGV_id, AVGV_PERIOD );
439 482 if ( status != RTEMS_SUCCESSFUL )
440 483 {
441 484 PRINTF1( "in AVGV *** ERR period: %d\n", status);
442 485 }
443 486 else
444 487 {
445 488 current_v = waveform_picker_regs->v;
446 489 current_e1 = waveform_picker_regs->e1;
447 490 current_e2 = waveform_picker_regs->e2;
448 491 if ( (current_v != old_v)
449 492 || (current_e1 != old_e1)
450 493 || (current_e2 != old_e2))
451 494 {
452 495 average_v = filter( current_v, &ctx_v );
453 496 average_e1 = filter( current_e1, &ctx_e1 );
454 497 average_e2 = filter( current_e2, &ctx_e2 );
455 498
456 499 //update int16 values
457 500 hk_lfr_sc_v_f3_as_int16 = (int16_t) average_v;
458 501 hk_lfr_sc_e1_f3_as_int16 = (int16_t) average_e1;
459 502 hk_lfr_sc_e2_f3_as_int16 = (int16_t) average_e2;
460 503 }
461 504 old_v = current_v;
462 505 old_e1 = current_e1;
463 506 old_e2 = current_e2;
464 507 }
465 508 }
466 509
467 510 PRINTF("in AVGV *** deleting task\n");
468 511
469 512 status = rtems_task_delete( RTEMS_SELF ); // should not return
470 513
471 514 return;
472 515 }
473 516
474 517 rtems_task dumb_task( rtems_task_argument unused )
475 518 {
476 519 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
477 520 *
478 521 * @param unused is the starting argument of the RTEMS task
479 522 *
480 523 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
481 524 *
482 525 */
483 526
484 527 unsigned int i;
485 528 unsigned int intEventOut;
486 529 unsigned int coarse_time = 0;
487 530 unsigned int fine_time = 0;
488 531 rtems_event_set event_out;
489 532
490 533 event_out = EVENT_SETS_NONE_PENDING;
491 534
492 535 BOOT_PRINTF("in DUMB *** \n")
493 536
494 537 while(1){
495 538 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
496 539 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
497 540 | RTEMS_EVENT_8 | RTEMS_EVENT_9 | RTEMS_EVENT_12 | RTEMS_EVENT_13
498 541 | RTEMS_EVENT_14,
499 542 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
500 543 intEventOut = (unsigned int) event_out;
501 544 for ( i=0; i<NB_RTEMS_EVENTS; i++)
502 545 {
503 546 if ( ((intEventOut >> i) & 1) != 0)
504 547 {
505 548 coarse_time = time_management_regs->coarse_time;
506 549 fine_time = time_management_regs->fine_time;
507 550 if (i==EVENT_12)
508 551 {
509 552 PRINTF1("%s\n", DUMB_MESSAGE_12)
510 553 }
511 554 if (i==EVENT_13)
512 555 {
513 556 PRINTF1("%s\n", DUMB_MESSAGE_13)
514 557 }
515 558 if (i==EVENT_14)
516 559 {
517 560 PRINTF1("%s\n", DUMB_MESSAGE_1)
518 561 }
519 562 }
520 563 }
521 564 }
522 565 }
523 566
567 rtems_task scrubbing_task( rtems_task_argument unused )
568 {
569 /** This RTEMS taks is used to avoid entering IDLE task and also scrub memory to increase scubbing frequency.
570 *
571 * @param unused is the starting argument of the RTEMS task
572 *
573 * The scrubbing reads continuously memory when no other tasks are ready.
574 *
575 */
576
577 BOOT_PRINTF("in SCRUBBING *** \n");
578 volatile int i=0;
579 volatile float valuef = 1.;
580 volatile uint32_t* RAM=(uint32_t*)0x40000000;
581 volatile uint32_t value;
582 #ifdef ENABLE_SCRUBBING_COUNTER
583 housekeeping_packet.lfr_fpga_version[BYTE_0] = 0;
584 #endif
585 while(1){
586 i=(i+1)%(1024*1024);
587 valuef += 10.f*(float)RAM[i];
588 #ifdef ENABLE_SCRUBBING_COUNTER
589 if(i==0)
590 {
591 housekeeping_packet.lfr_fpga_version[BYTE_0] += 1;
592 }
593 #endif
594 }
595 }
596
597 rtems_task calibration_sweep_task( rtems_task_argument unused )
598 {
599 /** This RTEMS taks is used to change calibration signal smapling frequency between snapshots.
600 *
601 * @param unused is the starting argument of the RTEMS task
602 *
603 * If calibration is enabled, this task will divide by two the calibration signal smapling frequency between snapshots.
604 * When minimum sampling frequency is reach it will jump to maximum sampling frequency to loop indefinitely.
605 *
606 */
607 rtems_event_set event_out;
608 BOOT_PRINTF("in calibration sweep *** \n");
609 rtems_interval ticks_per_seconds = rtems_clock_get_ticks_per_second();
610 while(1){
611 // Waiting for next F0 snapshot
612 rtems_event_receive(RTEMS_EVENT_CAL_SWEEP_WAKE, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out);
613 if(time_management_regs->calDACCtrl & BIT_CAL_ENABLE)
614 {
615 unsigned int delta_snapshot;
616 delta_snapshot = (parameter_dump_packet.sy_lfr_n_swf_p[0] * CONST_256)
617 + parameter_dump_packet.sy_lfr_n_swf_p[1];
618 // We are woken almost in the center of a snapshot -> let's wait for sy_lfr_n_swf_p / 2
619 rtems_task_wake_after( ticks_per_seconds * delta_snapshot / 2);
620 if(time_management_regs->calDivisor >= CAL_F_DIVISOR_MAX){
621 time_management_regs->calDivisor = CAL_F_DIVISOR_MIN;
622 }
623 else{
624 time_management_regs->calDivisor *= 2;
625 }
626 }
627
628
629
630 }
631
632 }
633
634
524 635 //*****************************
525 636 // init housekeeping parameters
526 637
527 638 void init_housekeeping_parameters( void )
528 639 {
529 640 /** This function initialize the housekeeping_packet global variable with default values.
530 641 *
531 642 */
532 643
533 644 unsigned int i = 0;
534 645 unsigned char *parameters;
535 646 unsigned char sizeOfHK;
536 647
537 648 sizeOfHK = sizeof( Packet_TM_LFR_HK_t );
538 649
539 650 parameters = (unsigned char*) &housekeeping_packet;
540 651
541 652 for(i = 0; i< sizeOfHK; i++)
542 653 {
543 654 parameters[i] = INIT_CHAR;
544 655 }
545 656
546 657 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
547 658 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
548 659 housekeeping_packet.reserved = DEFAULT_RESERVED;
549 660 housekeeping_packet.userApplication = CCSDS_USER_APP;
550 661 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> SHIFT_1_BYTE);
551 662 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
552 663 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
553 664 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
554 665 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> SHIFT_1_BYTE);
555 666 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
556 667 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
557 668 housekeeping_packet.serviceType = TM_TYPE_HK;
558 669 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
559 670 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
560 671 housekeeping_packet.sid = SID_HK;
561 672
562 673 // init status word
563 674 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
564 675 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
565 676 // init software version
566 677 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
567 678 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
568 679 housekeeping_packet.lfr_sw_version[BYTE_2] = SW_VERSION_N3;
569 680 housekeeping_packet.lfr_sw_version[BYTE_3] = SW_VERSION_N4;
570 681 // init fpga version
571 682 parameters = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
572 683 housekeeping_packet.lfr_fpga_version[BYTE_0] = parameters[BYTE_1]; // n1
573 684 housekeeping_packet.lfr_fpga_version[BYTE_1] = parameters[BYTE_2]; // n2
574 685 housekeeping_packet.lfr_fpga_version[BYTE_2] = parameters[BYTE_3]; // n3
575 686
576 687 housekeeping_packet.hk_lfr_q_sd_fifo_size = MSG_QUEUE_COUNT_SEND;
577 688 housekeeping_packet.hk_lfr_q_rv_fifo_size = MSG_QUEUE_COUNT_RECV;
578 689 housekeeping_packet.hk_lfr_q_p0_fifo_size = MSG_QUEUE_COUNT_PRC0;
579 690 housekeeping_packet.hk_lfr_q_p1_fifo_size = MSG_QUEUE_COUNT_PRC1;
580 691 housekeeping_packet.hk_lfr_q_p2_fifo_size = MSG_QUEUE_COUNT_PRC2;
581 692 }
582 693
583 694 void increment_seq_counter( unsigned short *packetSequenceControl )
584 695 {
585 696 /** This function increment the sequence counter passes in argument.
586 697 *
587 698 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
588 699 *
589 700 */
590 701
591 702 unsigned short segmentation_grouping_flag;
592 703 unsigned short sequence_cnt;
593 704
594 705 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE; // keep bits 7 downto 6
595 706 sequence_cnt = (*packetSequenceControl) & SEQ_CNT_MASK; // [0011 1111 1111 1111]
596 707
597 708 if ( sequence_cnt < SEQ_CNT_MAX)
598 709 {
599 710 sequence_cnt = sequence_cnt + 1;
600 711 }
601 712 else
602 713 {
603 714 sequence_cnt = 0;
604 715 }
605 716
606 717 *packetSequenceControl = segmentation_grouping_flag | sequence_cnt ;
607 718 }
608 719
609 720 void getTime( unsigned char *time)
610 721 {
611 722 /** This function write the current local time in the time buffer passed in argument.
612 723 *
613 724 */
614 725
615 726 time[0] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_3_BYTES);
616 727 time[1] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_2_BYTES);
617 728 time[2] = (unsigned char) (time_management_regs->coarse_time>>SHIFT_1_BYTE);
618 729 time[3] = (unsigned char) (time_management_regs->coarse_time);
619 730 time[4] = (unsigned char) (time_management_regs->fine_time>>SHIFT_1_BYTE);
620 731 time[5] = (unsigned char) (time_management_regs->fine_time);
621 732 }
622 733
623 734 unsigned long long int getTimeAsUnsignedLongLongInt( )
624 735 {
625 736 /** This function write the current local time in the time buffer passed in argument.
626 737 *
627 738 */
628 739 unsigned long long int time;
629 740
630 741 time = ( (unsigned long long int) (time_management_regs->coarse_time & COARSE_TIME_MASK) << SHIFT_2_BYTES )
631 742 + time_management_regs->fine_time;
632 743
633 744 return time;
634 745 }
635 746
636 void send_dumb_hk( void )
637 {
638 Packet_TM_LFR_HK_t dummy_hk_packet;
639 unsigned char *parameters;
640 unsigned int i;
641 rtems_id queue_id;
642
643 queue_id = RTEMS_ID_NONE;
644
645 dummy_hk_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
646 dummy_hk_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
647 dummy_hk_packet.reserved = DEFAULT_RESERVED;
648 dummy_hk_packet.userApplication = CCSDS_USER_APP;
649 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> SHIFT_1_BYTE);
650 dummy_hk_packet.packetID[1] = (unsigned char) (APID_TM_HK);
651 dummy_hk_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
652 dummy_hk_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
653 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> SHIFT_1_BYTE);
654 dummy_hk_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
655 dummy_hk_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
656 dummy_hk_packet.serviceType = TM_TYPE_HK;
657 dummy_hk_packet.serviceSubType = TM_SUBTYPE_HK;
658 dummy_hk_packet.destinationID = TM_DESTINATION_ID_GROUND;
659 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
660 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
661 dummy_hk_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
662 dummy_hk_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
663 dummy_hk_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
664 dummy_hk_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
665 dummy_hk_packet.sid = SID_HK;
666
667 // init status word
668 dummy_hk_packet.lfr_status_word[0] = INT8_ALL_F;
669 dummy_hk_packet.lfr_status_word[1] = INT8_ALL_F;
670 // init software version
671 dummy_hk_packet.lfr_sw_version[0] = SW_VERSION_N1;
672 dummy_hk_packet.lfr_sw_version[1] = SW_VERSION_N2;
673 dummy_hk_packet.lfr_sw_version[BYTE_2] = SW_VERSION_N3;
674 dummy_hk_packet.lfr_sw_version[BYTE_3] = SW_VERSION_N4;
675 // init fpga version
676 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + APB_OFFSET_VHDL_REV);
677 dummy_hk_packet.lfr_fpga_version[BYTE_0] = parameters[BYTE_1]; // n1
678 dummy_hk_packet.lfr_fpga_version[BYTE_1] = parameters[BYTE_2]; // n2
679 dummy_hk_packet.lfr_fpga_version[BYTE_2] = parameters[BYTE_3]; // n3
680
681 parameters = (unsigned char *) &dummy_hk_packet.hk_lfr_cpu_load;
682
683 for (i=0; i<(BYTE_POS_HK_REACTION_WHEELS_FREQUENCY - BYTE_POS_HK_LFR_CPU_LOAD); i++)
684 {
685 parameters[i] = INT8_ALL_F;
686 }
687
688 get_message_queue_id_send( &queue_id );
689
690 rtems_message_queue_send( queue_id, &dummy_hk_packet,
691 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
692 }
693
694 747 void get_temperatures( unsigned char *temperatures )
695 748 {
696 749 unsigned char* temp_scm_ptr;
697 750 unsigned char* temp_pcb_ptr;
698 751 unsigned char* temp_fpga_ptr;
699 752
700 753 // SEL1 SEL0
701 754 // 0 0 => PCB
702 755 // 0 1 => FPGA
703 756 // 1 0 => SCM
704 757
705 758 temp_scm_ptr = (unsigned char *) &time_management_regs->temp_scm;
706 759 temp_pcb_ptr = (unsigned char *) &time_management_regs->temp_pcb;
707 760 temp_fpga_ptr = (unsigned char *) &time_management_regs->temp_fpga;
708 761
709 762 temperatures[ BYTE_0 ] = temp_scm_ptr[ BYTE_2 ];
710 763 temperatures[ BYTE_1 ] = temp_scm_ptr[ BYTE_3 ];
711 764 temperatures[ BYTE_2 ] = temp_pcb_ptr[ BYTE_2 ];
712 765 temperatures[ BYTE_3 ] = temp_pcb_ptr[ BYTE_3 ];
713 766 temperatures[ BYTE_4 ] = temp_fpga_ptr[ BYTE_2 ];
714 767 temperatures[ BYTE_5 ] = temp_fpga_ptr[ BYTE_3 ];
715 768 }
716 769
717 770 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
718 771 {
719 772 unsigned char* v_ptr;
720 773 unsigned char* e1_ptr;
721 774 unsigned char* e2_ptr;
722 775
723 776 v_ptr = (unsigned char *) &hk_lfr_sc_v_f3_as_int16;
724 777 e1_ptr = (unsigned char *) &hk_lfr_sc_e1_f3_as_int16;
725 778 e2_ptr = (unsigned char *) &hk_lfr_sc_e2_f3_as_int16;
726 779
727 780 spacecraft_potential[BYTE_0] = v_ptr[0];
728 781 spacecraft_potential[BYTE_1] = v_ptr[1];
729 782 spacecraft_potential[BYTE_2] = e1_ptr[0];
730 783 spacecraft_potential[BYTE_3] = e1_ptr[1];
731 784 spacecraft_potential[BYTE_4] = e2_ptr[0];
732 785 spacecraft_potential[BYTE_5] = e2_ptr[1];
733 786 }
734 787
788 /**
789 * @brief get_cpu_load, computes CPU load, CPU load average and CPU load max
790 * @param resource_statistics stores:
791 * - CPU load at index 0
792 * - CPU load max at index 1
793 * - CPU load average at index 2
794 *
795 * The CPU load average is computed on the last 60 values with a simple moving average.
796 */
735 797 void get_cpu_load( unsigned char *resource_statistics )
736 798 {
799 #define LOAD_AVG_SIZE 60
800 static unsigned char cpu_load_hist[LOAD_AVG_SIZE]={0};
801 static char old_avg_pos=0;
802 static unsigned int cpu_load_avg;
737 803 unsigned char cpu_load;
738 804
739 805 cpu_load = lfr_rtems_cpu_usage_report();
740 806
741 807 // HK_LFR_CPU_LOAD
742 resource_statistics[0] = cpu_load;
808 resource_statistics[BYTE_0] = cpu_load;
743 809
744 810 // HK_LFR_CPU_LOAD_MAX
745 if (cpu_load > resource_statistics[1])
811 if (cpu_load > resource_statistics[BYTE_1])
746 812 {
747 resource_statistics[1] = cpu_load;
813 resource_statistics[BYTE_1] = cpu_load;
748 814 }
749 815
816 cpu_load_avg = cpu_load_avg - (unsigned int)cpu_load_hist[(int)old_avg_pos] + (unsigned int)cpu_load;
817 cpu_load_hist[(int)old_avg_pos] = cpu_load;
818 old_avg_pos += 1;
819 old_avg_pos %= LOAD_AVG_SIZE;
750 820 // CPU_LOAD_AVE
751 resource_statistics[BYTE_2] = 0;
752
821 resource_statistics[BYTE_2] = (unsigned char)(cpu_load_avg / LOAD_AVG_SIZE);
822 // this will change the way LFR compute usage
753 823 #ifndef PRINT_TASK_STATISTICS
754 824 rtems_cpu_usage_reset();
755 825 #endif
756 826
757 827 }
758 828
759 829 void set_hk_lfr_sc_potential_flag( bool state )
760 830 {
761 831 if (state == true)
762 832 {
763 833 housekeeping_packet.lfr_status_word[1] =
764 834 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_SC_POTENTIAL_FLAG_BIT; // [0100 0000]
765 835 }
766 836 else
767 837 {
768 838 housekeeping_packet.lfr_status_word[1] =
769 839 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_SC_POTENTIAL_FLAG_MASK; // [1011 1111]
770 840 }
771 841 }
772 842
773 843 void set_sy_lfr_pas_filter_enabled( bool state )
774 844 {
775 845 if (state == true)
776 846 {
777 847 housekeeping_packet.lfr_status_word[1] =
778 848 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_PAS_FILTER_ENABLED_BIT; // [0010 0000]
779 849 }
780 850 else
781 851 {
782 852 housekeeping_packet.lfr_status_word[1] =
783 853 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_PAS_FILTER_ENABLED_MASK; // [1101 1111]
784 854 }
785 855 }
786 856
787 857 void set_sy_lfr_watchdog_enabled( bool state )
788 858 {
789 859 if (state == true)
790 860 {
791 861 housekeeping_packet.lfr_status_word[1] =
792 862 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_WATCHDOG_BIT; // [0001 0000]
793 863 }
794 864 else
795 865 {
796 866 housekeeping_packet.lfr_status_word[1] =
797 867 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_WATCHDOG_MASK; // [1110 1111]
798 868 }
799 869 }
800 870
801 871 void set_hk_lfr_calib_enable( bool state )
802 872 {
803 873 if (state == true)
804 874 {
805 875 housekeeping_packet.lfr_status_word[1] =
806 876 housekeeping_packet.lfr_status_word[1] | STATUS_WORD_CALIB_BIT; // [0000 1000]
807 877 }
808 878 else
809 879 {
810 880 housekeeping_packet.lfr_status_word[1] =
811 881 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_CALIB_MASK; // [1111 0111]
812 882 }
813 883 }
814 884
815 885 void set_hk_lfr_reset_cause( enum lfr_reset_cause_t lfr_reset_cause )
816 886 {
817 887 housekeeping_packet.lfr_status_word[1] =
818 888 housekeeping_packet.lfr_status_word[1] & STATUS_WORD_RESET_CAUSE_MASK; // [1111 1000]
819 889
820 890 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1]
821 891 | (lfr_reset_cause & STATUS_WORD_RESET_CAUSE_BITS ); // [0000 0111]
822 892
823 893 }
824 894
825 895 void increment_hk_counter( unsigned char newValue, unsigned char oldValue, unsigned int *counter )
826 896 {
827 897 int delta;
828 898
829 899 delta = 0;
830 900
831 901 if (newValue >= oldValue)
832 902 {
833 903 delta = newValue - oldValue;
834 904 }
835 905 else
836 906 {
837 907 delta = (CONST_256 - oldValue) + newValue;
838 908 }
839 909
840 910 *counter = *counter + delta;
841 911 }
842 912
913 // Low severity error counters update
843 914 void hk_lfr_le_update( void )
844 915 {
845 916 static hk_lfr_le_t old_hk_lfr_le = {0};
846 917 hk_lfr_le_t new_hk_lfr_le;
847 918 unsigned int counter;
848 919
849 920 counter = (((unsigned int) housekeeping_packet.hk_lfr_le_cnt[0]) * CONST_256) + housekeeping_packet.hk_lfr_le_cnt[1];
850 921
851 922 // DPU
852 923 new_hk_lfr_le.dpu_spw_parity = housekeeping_packet.hk_lfr_dpu_spw_parity;
853 924 new_hk_lfr_le.dpu_spw_disconnect= housekeeping_packet.hk_lfr_dpu_spw_disconnect;
854 925 new_hk_lfr_le.dpu_spw_escape = housekeeping_packet.hk_lfr_dpu_spw_escape;
855 926 new_hk_lfr_le.dpu_spw_credit = housekeeping_packet.hk_lfr_dpu_spw_credit;
856 927 new_hk_lfr_le.dpu_spw_write_sync= housekeeping_packet.hk_lfr_dpu_spw_write_sync;
857 928 // TIMECODE
858 929 new_hk_lfr_le.timecode_erroneous= housekeeping_packet.hk_lfr_timecode_erroneous;
859 930 new_hk_lfr_le.timecode_missing = housekeeping_packet.hk_lfr_timecode_missing;
860 931 new_hk_lfr_le.timecode_invalid = housekeeping_packet.hk_lfr_timecode_invalid;
861 932 // TIME
862 933 new_hk_lfr_le.time_timecode_it = housekeeping_packet.hk_lfr_time_timecode_it;
863 934 new_hk_lfr_le.time_not_synchro = housekeeping_packet.hk_lfr_time_not_synchro;
864 935 new_hk_lfr_le.time_timecode_ctr = housekeeping_packet.hk_lfr_time_timecode_ctr;
865 936 //AHB
866 937 new_hk_lfr_le.ahb_correctable = housekeeping_packet.hk_lfr_ahb_correctable;
867 938 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
868 939 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
869 940
870 941 // update the le counter
871 942 // DPU
872 943 increment_hk_counter( new_hk_lfr_le.dpu_spw_parity, old_hk_lfr_le.dpu_spw_parity, &counter );
873 944 increment_hk_counter( new_hk_lfr_le.dpu_spw_disconnect,old_hk_lfr_le.dpu_spw_disconnect, &counter );
874 945 increment_hk_counter( new_hk_lfr_le.dpu_spw_escape, old_hk_lfr_le.dpu_spw_escape, &counter );
875 946 increment_hk_counter( new_hk_lfr_le.dpu_spw_credit, old_hk_lfr_le.dpu_spw_credit, &counter );
876 947 increment_hk_counter( new_hk_lfr_le.dpu_spw_write_sync,old_hk_lfr_le.dpu_spw_write_sync, &counter );
877 948 // TIMECODE
878 949 increment_hk_counter( new_hk_lfr_le.timecode_erroneous,old_hk_lfr_le.timecode_erroneous, &counter );
879 950 increment_hk_counter( new_hk_lfr_le.timecode_missing, old_hk_lfr_le.timecode_missing, &counter );
880 951 increment_hk_counter( new_hk_lfr_le.timecode_invalid, old_hk_lfr_le.timecode_invalid, &counter );
881 952 // TIME
882 953 increment_hk_counter( new_hk_lfr_le.time_timecode_it, old_hk_lfr_le.time_timecode_it, &counter );
883 954 increment_hk_counter( new_hk_lfr_le.time_not_synchro, old_hk_lfr_le.time_not_synchro, &counter );
884 955 increment_hk_counter( new_hk_lfr_le.time_timecode_ctr, old_hk_lfr_le.time_timecode_ctr, &counter );
885 956 // AHB
886 957 increment_hk_counter( new_hk_lfr_le.ahb_correctable, old_hk_lfr_le.ahb_correctable, &counter );
887 958
888 959 // DPU
889 960 old_hk_lfr_le.dpu_spw_parity = new_hk_lfr_le.dpu_spw_parity;
890 961 old_hk_lfr_le.dpu_spw_disconnect= new_hk_lfr_le.dpu_spw_disconnect;
891 962 old_hk_lfr_le.dpu_spw_escape = new_hk_lfr_le.dpu_spw_escape;
892 963 old_hk_lfr_le.dpu_spw_credit = new_hk_lfr_le.dpu_spw_credit;
893 964 old_hk_lfr_le.dpu_spw_write_sync= new_hk_lfr_le.dpu_spw_write_sync;
894 965 // TIMECODE
895 966 old_hk_lfr_le.timecode_erroneous= new_hk_lfr_le.timecode_erroneous;
896 967 old_hk_lfr_le.timecode_missing = new_hk_lfr_le.timecode_missing;
897 968 old_hk_lfr_le.timecode_invalid = new_hk_lfr_le.timecode_invalid;
898 969 // TIME
899 970 old_hk_lfr_le.time_timecode_it = new_hk_lfr_le.time_timecode_it;
900 971 old_hk_lfr_le.time_not_synchro = new_hk_lfr_le.time_not_synchro;
901 972 old_hk_lfr_le.time_timecode_ctr = new_hk_lfr_le.time_timecode_ctr;
902 973 //AHB
903 974 old_hk_lfr_le.ahb_correctable = new_hk_lfr_le.ahb_correctable;
904 975 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
905 976 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
906 977
907 978 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
908 979 // LE
909 980 housekeeping_packet.hk_lfr_le_cnt[0] = (unsigned char) ((counter & BYTE0_MASK) >> SHIFT_1_BYTE);
910 981 housekeeping_packet.hk_lfr_le_cnt[1] = (unsigned char) (counter & BYTE1_MASK);
911 982 }
912 983
984 // Medium severity error counters update
913 985 void hk_lfr_me_update( void )
914 986 {
915 987 static hk_lfr_me_t old_hk_lfr_me = {0};
916 988 hk_lfr_me_t new_hk_lfr_me;
917 989 unsigned int counter;
918 990
919 991 counter = (((unsigned int) housekeeping_packet.hk_lfr_me_cnt[0]) * CONST_256) + housekeeping_packet.hk_lfr_me_cnt[1];
920 992
921 993 // get the current values
922 994 new_hk_lfr_me.dpu_spw_early_eop = housekeeping_packet.hk_lfr_dpu_spw_early_eop;
923 995 new_hk_lfr_me.dpu_spw_invalid_addr = housekeeping_packet.hk_lfr_dpu_spw_invalid_addr;
924 996 new_hk_lfr_me.dpu_spw_eep = housekeeping_packet.hk_lfr_dpu_spw_eep;
925 997 new_hk_lfr_me.dpu_spw_rx_too_big = housekeeping_packet.hk_lfr_dpu_spw_rx_too_big;
926 998
927 999 // update the me counter
928 1000 increment_hk_counter( new_hk_lfr_me.dpu_spw_early_eop, old_hk_lfr_me.dpu_spw_early_eop, &counter );
929 1001 increment_hk_counter( new_hk_lfr_me.dpu_spw_invalid_addr, old_hk_lfr_me.dpu_spw_invalid_addr, &counter );
930 1002 increment_hk_counter( new_hk_lfr_me.dpu_spw_eep, old_hk_lfr_me.dpu_spw_eep, &counter );
931 1003 increment_hk_counter( new_hk_lfr_me.dpu_spw_rx_too_big, old_hk_lfr_me.dpu_spw_rx_too_big, &counter );
932 1004
933 1005 // store the counters for the next time
934 1006 old_hk_lfr_me.dpu_spw_early_eop = new_hk_lfr_me.dpu_spw_early_eop;
935 1007 old_hk_lfr_me.dpu_spw_invalid_addr = new_hk_lfr_me.dpu_spw_invalid_addr;
936 1008 old_hk_lfr_me.dpu_spw_eep = new_hk_lfr_me.dpu_spw_eep;
937 1009 old_hk_lfr_me.dpu_spw_rx_too_big = new_hk_lfr_me.dpu_spw_rx_too_big;
938 1010
939 1011 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
940 1012 // ME
941 1013 housekeeping_packet.hk_lfr_me_cnt[0] = (unsigned char) ((counter & BYTE0_MASK) >> SHIFT_1_BYTE);
942 1014 housekeeping_packet.hk_lfr_me_cnt[1] = (unsigned char) (counter & BYTE1_MASK);
943 1015 }
944 1016
1017 // High severity error counters update
945 1018 void hk_lfr_le_me_he_update()
946 1019 {
947 1020
948 1021 unsigned int hk_lfr_he_cnt;
949 1022
950 1023 hk_lfr_he_cnt = (((unsigned int) housekeeping_packet.hk_lfr_he_cnt[0]) * 256) + housekeeping_packet.hk_lfr_he_cnt[1];
951 1024
952 1025 //update the low severity error counter
953 1026 hk_lfr_le_update( );
954 1027
955 1028 //update the medium severity error counter
956 1029 hk_lfr_me_update();
957 1030
958 1031 //update the high severity error counter
959 1032 hk_lfr_he_cnt = 0;
960 1033
961 1034 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
962 1035 // HE
963 1036 housekeeping_packet.hk_lfr_he_cnt[0] = (unsigned char) ((hk_lfr_he_cnt & BYTE0_MASK) >> SHIFT_1_BYTE);
964 1037 housekeeping_packet.hk_lfr_he_cnt[1] = (unsigned char) (hk_lfr_he_cnt & BYTE1_MASK);
965 1038
966 1039 }
967 1040
968 1041 void set_hk_lfr_time_not_synchro()
969 1042 {
970 1043 static unsigned char synchroLost = 1;
971 1044 int synchronizationBit;
972 1045
973 1046 // get the synchronization bit
974 1047 synchronizationBit =
975 1048 (time_management_regs->coarse_time & VAL_LFR_SYNCHRONIZED) >> BIT_SYNCHRONIZATION; // 1000 0000 0000 0000
976 1049
977 1050 switch (synchronizationBit)
978 1051 {
979 1052 case 0:
980 1053 if (synchroLost == 1)
981 1054 {
982 1055 synchroLost = 0;
983 1056 }
984 1057 break;
985 1058 case 1:
986 1059 if (synchroLost == 0 )
987 1060 {
988 1061 synchroLost = 1;
989 1062 increase_unsigned_char_counter(&housekeeping_packet.hk_lfr_time_not_synchro);
990 1063 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_NOT_SYNCHRO );
991 1064 }
992 1065 break;
993 1066 default:
994 1067 PRINTF1("in hk_lfr_time_not_synchro *** unexpected value for synchronizationBit = %d\n", synchronizationBit);
995 1068 break;
996 1069 }
997 1070
998 1071 }
999 1072
1000 1073 void set_hk_lfr_ahb_correctable() // CRITICITY L
1001 1074 {
1002 1075 /** This function builds the error counter hk_lfr_ahb_correctable using the statistics provided
1003 1076 * by the Cache Control Register (ASI 2, offset 0) and in the Register Protection Control Register (ASR16) on the
1004 1077 * detected errors in the cache, in the integer unit and in the floating point unit.
1005 1078 *
1006 1079 * @param void
1007 1080 *
1008 1081 * @return void
1009 1082 *
1010 1083 * All errors are summed to set the value of the hk_lfr_ahb_correctable counter.
1011 1084 *
1012 1085 */
1013 1086
1014 1087 unsigned int ahb_correctable;
1015 1088 unsigned int instructionErrorCounter;
1016 1089 unsigned int dataErrorCounter;
1017 1090 unsigned int fprfErrorCounter;
1018 1091 unsigned int iurfErrorCounter;
1019 1092
1020 1093 instructionErrorCounter = 0;
1021 1094 dataErrorCounter = 0;
1022 1095 fprfErrorCounter = 0;
1023 1096 iurfErrorCounter = 0;
1024 1097
1025 1098 CCR_getInstructionAndDataErrorCounters( &instructionErrorCounter, &dataErrorCounter);
1026 1099 ASR16_get_FPRF_IURF_ErrorCounters( &fprfErrorCounter, &iurfErrorCounter);
1027 1100
1028 1101 ahb_correctable = instructionErrorCounter
1029 1102 + dataErrorCounter
1030 1103 + fprfErrorCounter
1031 1104 + iurfErrorCounter
1032 1105 + housekeeping_packet.hk_lfr_ahb_correctable;
1033 1106
1034 1107 housekeeping_packet.hk_lfr_ahb_correctable = (unsigned char) (ahb_correctable & INT8_ALL_F); // [1111 1111]
1035 1108
1036 1109 }
Show file before | Show file after | Hide comments
@@ -1,1633 +1,1632
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions related to the SpaceWire interface.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * A group of functions to handle SpaceWire transmissions:
7 31 * - configuration of the SpaceWire link
8 32 * - SpaceWire related interruption requests processing
9 33 * - transmission of TeleMetry packets by a dedicated RTEMS task
10 34 * - reception of TeleCommands by a dedicated RTEMS task
11 35 *
12 36 */
13 37
14 38 #include "fsw_spacewire.h"
15 39
16 40 rtems_name semq_name = 0;
17 41 rtems_id semq_id = RTEMS_ID_NONE;
18 42
19 43 //*****************
20 44 // waveform headers
21 45 Header_TM_LFR_SCIENCE_CWF_t headerCWF = {0};
22 46 Header_TM_LFR_SCIENCE_SWF_t headerSWF = {0};
23 47 Header_TM_LFR_SCIENCE_ASM_t headerASM = {0};
24 48
25 49 unsigned char previousTimecodeCtr = 0;
26 50 unsigned int *grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
27 51
28 52 //***********
29 53 // RTEMS TASK
30 54 rtems_task spiq_task(rtems_task_argument unused)
31 55 {
32 56 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
33 57 *
34 58 * @param unused is the starting argument of the RTEMS task
35 59 *
36 60 */
37 61
38 62 rtems_event_set event_out;
39 63 rtems_status_code status;
40 64 int linkStatus;
41 65
42 66 event_out = EVENT_SETS_NONE_PENDING;
43 67 linkStatus = 0;
44 68
45 69 BOOT_PRINTF("in SPIQ *** \n")
46 70
47 71 while(true){
48 72 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
49 73 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
50 74
51 75 // [0] SUSPEND RECV AND SEND TASKS
52 76 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
53 77 if ( status != RTEMS_SUCCESSFUL ) {
54 78 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
55 79 }
56 80 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
57 81 if ( status != RTEMS_SUCCESSFUL ) {
58 82 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
59 83 }
60 84
61 85 // [1] CHECK THE LINK
62 86 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
63 87 if ( linkStatus != SPW_LINK_OK) {
64 88 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
65 89 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
66 90 }
67 91
68 92 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
69 93 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
70 94 if ( linkStatus != SPW_LINK_OK ) // [2.a] not in run state, reset the link
71 95 {
72 96 spacewire_read_statistics();
73 97 status = spacewire_several_connect_attemps( );
74 98 }
75 99 else // [2.b] in run state, start the link
76 100 {
77 101 status = spacewire_stop_and_start_link( fdSPW ); // start the link
78 102 if ( status != RTEMS_SUCCESSFUL)
79 103 {
80 104 PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status)
81 105 }
82 106 }
83 107
84 108 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
85 109 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
86 110 {
87 111 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
88 112 if ( status != RTEMS_SUCCESSFUL ) {
89 113 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
90 114 }
91 115 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
92 116 if ( status != RTEMS_SUCCESSFUL ) {
93 117 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
94 118 }
95 119 }
96 120 else // [3.b] the link is not in run state, go in STANDBY mode
97 121 {
98 122 status = enter_mode_standby();
99 123 if ( status != RTEMS_SUCCESSFUL )
100 124 {
101 125 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
102 126 }
103 127 {
104 128 updateLFRCurrentMode( LFR_MODE_STANDBY );
105 129 }
106 130 // wake the LINK task up to wait for the link recovery
107 131 status = rtems_event_send ( Task_id[TASKID_LINK], RTEMS_EVENT_0 );
108 132 status = rtems_task_suspend( RTEMS_SELF );
109 133 }
110 134 }
111 135 }
112 136
113 137 rtems_task recv_task( rtems_task_argument unused )
114 138 {
115 139 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
116 140 *
117 141 * @param unused is the starting argument of the RTEMS task
118 142 *
119 143 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
120 144 * 1. It reads the incoming data.
121 145 * 2. Launches the acceptance procedure.
122 146 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
123 147 *
124 148 */
125 149
126 150 int len;
127 151 ccsdsTelecommandPacket_t __attribute__((aligned(4))) currentTC;
128 152 unsigned char computed_CRC[ BYTES_PER_CRC ];
129 153 unsigned char currentTC_LEN_RCV[ BYTES_PER_PKT_LEN ];
130 154 unsigned char destinationID;
131 155 unsigned int estimatedPacketLength;
132 156 unsigned int parserCode;
133 157 rtems_status_code status;
134 158 rtems_id queue_recv_id;
135 159 rtems_id queue_send_id;
136 160
137 161 memset( &currentTC, 0, sizeof(ccsdsTelecommandPacket_t) );
138 162 destinationID = 0;
139 163 queue_recv_id = RTEMS_ID_NONE;
140 164 queue_send_id = RTEMS_ID_NONE;
141 165
142 166 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
143 167
144 168 status = get_message_queue_id_recv( &queue_recv_id );
145 169 if (status != RTEMS_SUCCESSFUL)
146 170 {
147 171 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
148 172 }
149 173
150 174 status = get_message_queue_id_send( &queue_send_id );
151 175 if (status != RTEMS_SUCCESSFUL)
152 176 {
153 177 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
154 178 }
155 179
156 180 BOOT_PRINTF("in RECV *** \n")
157 181
158 182 while(1)
159 183 {
160 184 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
161 185 if (len == -1){ // error during the read call
162 186 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
163 187 }
164 188 else {
165 189 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
166 190 PRINTF("in RECV *** packet lenght too short\n")
167 191 }
168 192 else {
169 193 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - PROTID_RES_APP); // => -3 is for Prot ID, Reserved and User App bytes
170 194 PRINTF1("incoming TC with Length (byte): %d\n", len - 3);
171 195 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> SHIFT_1_BYTE);
172 196 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
173 197 // CHECK THE TC
174 198 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
175 199 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
176 200 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
177 201 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
178 202 || (parserCode == WRONG_SRC_ID) )
179 203 { // send TM_LFR_TC_EXE_CORRUPTED
180 204 PRINTF1("TC corrupted received, with code: %d\n", parserCode);
181 205 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
182 206 &&
183 207 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
184 208 )
185 209 {
186 210 if ( parserCode == WRONG_SRC_ID )
187 211 {
188 212 destinationID = SID_TC_GROUND;
189 213 }
190 214 else
191 215 {
192 216 destinationID = currentTC.sourceID;
193 217 }
194 218 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
195 219 computed_CRC, currentTC_LEN_RCV,
196 220 destinationID );
197 221 }
198 222 }
199 223 else
200 224 { // send valid TC to the action launcher
201 225 status = rtems_message_queue_send( queue_recv_id, &currentTC,
202 226 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + PROTID_RES_APP);
203 227 }
204 228 }
205 229 }
206 230
207 231 update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max );
208 232
209 233 }
210 234 }
211 235
212 236 rtems_task send_task( rtems_task_argument argument)
213 237 {
214 238 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
215 239 *
216 240 * @param unused is the starting argument of the RTEMS task
217 241 *
218 242 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
219 243 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
220 244 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
221 245 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
222 246 * data it contains.
223 247 *
224 248 */
225 249
226 250 rtems_status_code status; // RTEMS status code
227 251 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
228 252 ring_node *incomingRingNodePtr;
229 253 int ring_node_address;
230 254 char *charPtr;
231 255 spw_ioctl_pkt_send *spw_ioctl_send;
232 256 size_t size; // size of the incoming TC packet
233 257 rtems_id queue_send_id;
234 258 unsigned int sid;
235 259 unsigned char sidAsUnsignedChar;
236 260 unsigned char type;
237 261
238 262 incomingRingNodePtr = NULL;
239 263 ring_node_address = 0;
240 264 charPtr = (char *) &ring_node_address;
241 265 size = 0;
242 266 queue_send_id = RTEMS_ID_NONE;
243 267 sid = 0;
244 268 sidAsUnsignedChar = 0;
245 269
246 270 init_header_cwf( &headerCWF );
247 271 init_header_swf( &headerSWF );
248 272 init_header_asm( &headerASM );
249 273
250 274 status = get_message_queue_id_send( &queue_send_id );
251 275 if (status != RTEMS_SUCCESSFUL)
252 276 {
253 277 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
254 278 }
255 279
256 280 BOOT_PRINTF("in SEND *** \n")
257 281
258 282 while(1)
259 283 {
260 284 status = rtems_message_queue_receive( queue_send_id, incomingData, &size,
261 285 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
262 286
263 287 if (status!=RTEMS_SUCCESSFUL)
264 288 {
265 289 PRINTF1("in SEND *** (1) ERR = %d\n", status)
266 290 }
267 291 else
268 292 {
269 293 if ( size == sizeof(ring_node*) )
270 294 {
271 295 charPtr[0] = incomingData[0];
272 296 charPtr[1] = incomingData[1];
273 297 charPtr[BYTE_2] = incomingData[BYTE_2];
274 298 charPtr[BYTE_3] = incomingData[BYTE_3];
275 299 incomingRingNodePtr = (ring_node*) ring_node_address;
276 300 sid = incomingRingNodePtr->sid;
277 301 if ( (sid==SID_NORM_CWF_LONG_F3)
278 302 || (sid==SID_BURST_CWF_F2 )
279 303 || (sid==SID_SBM1_CWF_F1 )
280 304 || (sid==SID_SBM2_CWF_F2 ))
281 305 {
282 306 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
283 307 }
284 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
308 else if ( (sid==SID_NORM_SWF_F0) || (sid==SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
285 309 {
286 310 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
287 311 }
288 else if ( (sid==SID_NORM_CWF_F3) )
312 else if (sid==SID_NORM_CWF_F3)
289 313 {
290 314 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
291 315 }
292 316 else if (sid==SID_NORM_ASM_F0)
293 317 {
294 318 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
295 319 }
296 320 else if (sid==SID_NORM_ASM_F1)
297 321 {
298 322 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
299 323 }
300 324 else if (sid==SID_NORM_ASM_F2)
301 325 {
302 326 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
303 327 }
304 else if ( sid==TM_CODE_K_DUMP )
328 else if (sid==TM_CODE_K_DUMP)
305 329 {
306 330 spw_send_k_dump( incomingRingNodePtr );
307 331 }
308 332 else
309 333 {
310 334 PRINTF1("unexpected sid = %d\n", sid);
311 335 }
312 336 }
313 337 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
314 338 {
315 339 sidAsUnsignedChar = (unsigned char) incomingData[ PACKET_POS_PA_LFR_SID_PKT ];
316 340 sid = sidAsUnsignedChar;
317 341 type = (unsigned char) incomingData[ PACKET_POS_SERVICE_TYPE ];
318 342 if (type == TM_TYPE_LFR_SCIENCE) // this is a BP packet, all other types are handled differently
319 343 // SET THE SEQUENCE_CNT PARAMETER IN CASE OF BP0 OR BP1 PACKETS
320 344 {
321 345 increment_seq_counter_source_id( (unsigned char*) &incomingData[ PACKET_POS_SEQUENCE_CNT ], sid );
322 346 }
323 347
324 348 status = write( fdSPW, incomingData, size );
325 349 if (status == -1){
326 350 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
327 351 }
328 352 }
329 353 else // the incoming message is a spw_ioctl_pkt_send structure
330 354 {
331 355 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
332 356 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
333 357 if (status == -1){
334 358 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
335 359 }
336 360 }
337 361 }
338 362
339 363 update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max );
340 364
341 365 }
342 366 }
343 367
344 368 rtems_task link_task( rtems_task_argument argument )
345 369 {
346 370 rtems_event_set event_out;
347 371 rtems_status_code status;
348 372 int linkStatus;
349 373
350 374 event_out = EVENT_SETS_NONE_PENDING;
351 375 linkStatus = 0;
352 376
353 377 BOOT_PRINTF("in LINK ***\n")
354 378
355 379 while(1)
356 380 {
357 381 // wait for an RTEMS_EVENT
358 382 rtems_event_receive( RTEMS_EVENT_0,
359 383 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
360 384 PRINTF("in LINK *** wait for the link\n")
361 385 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
362 386 while( linkStatus != SPW_LINK_OK) // wait for the link
363 387 {
364 388 status = rtems_task_wake_after( SPW_LINK_WAIT ); // monitor the link each 100ms
365 389 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
366 390 watchdog_reload();
367 391 }
368 392
369 393 spacewire_read_statistics();
370 394 status = spacewire_stop_and_start_link( fdSPW );
371 395
372 396 if (status != RTEMS_SUCCESSFUL)
373 397 {
374 398 PRINTF1("in LINK *** ERR link not started %d\n", status)
375 399 }
376 400 else
377 401 {
378 402 PRINTF("in LINK *** OK link started\n")
379 403 }
380 404
381 405 // restart the SPIQ task
382 406 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
383 407 if ( status != RTEMS_SUCCESSFUL ) {
384 408 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
385 409 }
386 410
387 411 // restart RECV and SEND
388 412 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
389 413 if ( status != RTEMS_SUCCESSFUL ) {
390 414 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
391 415 }
392 416 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
393 417 if ( status != RTEMS_SUCCESSFUL ) {
394 418 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
395 419 }
396 420 }
397 421 }
398 422
399 423 //****************
400 424 // OTHER FUNCTIONS
401 425 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
402 426 {
403 427 /** This function opens the SpaceWire link.
404 428 *
405 429 * @return a valid file descriptor in case of success, -1 in case of a failure
406 430 *
407 431 */
408 432 rtems_status_code status;
409 433
410 434 status = RTEMS_SUCCESSFUL;
411 435
412 436 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
413 437 if ( fdSPW < 0 ) {
414 438 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
415 439 }
416 440 else
417 441 {
418 442 status = RTEMS_SUCCESSFUL;
419 443 }
420 444
421 445 return status;
422 446 }
423 447
424 448 int spacewire_start_link( int fd )
425 449 {
426 450 rtems_status_code status;
427 451
428 452 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
429 453 // -1 default hardcoded driver timeout
430 454
431 455 return status;
432 456 }
433 457
434 458 int spacewire_stop_and_start_link( int fd )
435 459 {
436 460 rtems_status_code status;
437 461
438 462 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
439 463 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
440 464 // -1 default hardcoded driver timeout
441 465
442 466 return status;
443 467 }
444 468
445 469 int spacewire_configure_link( int fd )
446 470 {
447 471 /** This function configures the SpaceWire link.
448 472 *
449 473 * @return GR-RTEMS-DRIVER directive status codes:
450 474 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
451 475 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
452 476 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
453 477 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
454 478 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
455 479 * - 5 EIO - Error when writing to grswp hardware registers.
456 480 * - 2 ENOENT - No such file or directory
457 481 */
458 482
459 483 rtems_status_code status;
460 484
461 485 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
462 486 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
463 487 spw_ioctl_packetsize packetsize;
464 488
465 489 packetsize.rxsize = SPW_RXSIZE;
466 490 packetsize.txdsize = SPW_TXDSIZE;
467 491 packetsize.txhsize = SPW_TXHSIZE;
468 492
469 493 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
470 494 if (status!=RTEMS_SUCCESSFUL) {
471 495 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
472 496 }
473 497 //
474 498 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
475 499 if (status!=RTEMS_SUCCESSFUL) {
476 500 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
477 501 }
478 502 //
479 503 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
480 504 if (status!=RTEMS_SUCCESSFUL) {
481 505 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
482 506 }
483 507 //
484 508 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
485 509 if (status!=RTEMS_SUCCESSFUL) {
486 510 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
487 511 }
488 512 //
489 513 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
490 514 if (status!=RTEMS_SUCCESSFUL) {
491 515 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
492 516 }
493 517 //
494 518 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
495 519 if (status!=RTEMS_SUCCESSFUL) {
496 520 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
497 521 }
498 522 //
499 523 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, CONF_TCODE_CTRL); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
500 524 if (status!=RTEMS_SUCCESSFUL) {
501 525 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
502 526 }
503 527 //
504 528 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_PACKETSIZE, packetsize); // set rxsize, txdsize and txhsize
505 529 if (status!=RTEMS_SUCCESSFUL) {
506 530 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_PACKETSIZE,\n")
507 531 }
508 532
509 533 return status;
510 534 }
511 535
512 536 int spacewire_several_connect_attemps( void )
513 537 {
514 538 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
515 539 *
516 540 * @return RTEMS directive status code:
517 541 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
518 542 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
519 543 *
520 544 */
521 545
522 546 rtems_status_code status_spw;
523 547 rtems_status_code status;
524 548 int i;
525 549
526 550 status_spw = RTEMS_SUCCESSFUL;
527 551
528 552 i = 0;
529 553 while (i < SY_LFR_DPU_CONNECT_ATTEMPT)
530 554 {
531 555 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
532 556
533 557 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
534 558
535 559 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
536 560
537 561 status_spw = spacewire_stop_and_start_link( fdSPW );
538 562
539 563 if ( status_spw != RTEMS_SUCCESSFUL )
540 564 {
541 565 i = i + 1;
542 566 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw);
543 567 }
544 568 else
545 569 {
546 570 i = SY_LFR_DPU_CONNECT_ATTEMPT;
547 571 }
548 572 }
549 573
550 574 return status_spw;
551 575 }
552 576
553 577 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
554 578 {
555 579 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
556 580 *
557 581 * @param val is the value, 0 or 1, used to set the value of the NP bit.
558 582 * @param regAddr is the address of the GRSPW control register.
559 583 *
560 584 * NP is the bit 20 of the GRSPW control register.
561 585 *
562 586 */
563 587
564 588 unsigned int *spwptr = (unsigned int*) regAddr;
565 589
566 590 if (val == 1) {
567 591 *spwptr = *spwptr | SPW_BIT_NP; // [NP] set the No port force bit
568 592 }
569 593 if (val== 0) {
570 594 *spwptr = *spwptr & SPW_BIT_NP_MASK;
571 595 }
572 596 }
573 597
574 598 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
575 599 {
576 600 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
577 601 *
578 602 * @param val is the value, 0 or 1, used to set the value of the RE bit.
579 603 * @param regAddr is the address of the GRSPW control register.
580 604 *
581 605 * RE is the bit 16 of the GRSPW control register.
582 606 *
583 607 */
584 608
585 609 unsigned int *spwptr = (unsigned int*) regAddr;
586 610
587 611 if (val == 1)
588 612 {
589 613 *spwptr = *spwptr | SPW_BIT_RE; // [RE] set the RMAP Enable bit
590 614 }
591 615 if (val== 0)
592 616 {
593 617 *spwptr = *spwptr & SPW_BIT_RE_MASK;
594 618 }
595 619 }
596 620
597 621 void spacewire_read_statistics( void )
598 622 {
599 623 /** This function reads the SpaceWire statistics from the grspw RTEMS driver.
600 624 *
601 625 * @param void
602 626 *
603 627 * @return void
604 628 *
605 629 * Once they are read, the counters are stored in a global variable used during the building of the
606 630 * HK packets.
607 631 *
608 632 */
609 633
610 634 rtems_status_code status;
611 635 spw_stats current;
612 636
613 637 memset(&current, 0, sizeof(spw_stats));
614 638
615 639 spacewire_get_last_error();
616 640
617 641 // read the current statistics
618 642 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
619 643
620 644 // clear the counters
621 645 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_CLR_STATISTICS );
622 646
623 // typedef struct {
624 // unsigned int tx_link_err; // NOT IN HK
625 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
626 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
627 // unsigned int rx_eep_err;
628 // unsigned int rx_truncated;
629 // unsigned int parity_err;
630 // unsigned int escape_err;
631 // unsigned int credit_err;
632 // unsigned int write_sync_err;
633 // unsigned int disconnect_err;
634 // unsigned int early_ep;
635 // unsigned int invalid_address;
636 // unsigned int packets_sent;
637 // unsigned int packets_received;
638 // } spw_stats;
639
640 647 // rx_eep_err
641 648 grspw_stats.rx_eep_err = grspw_stats.rx_eep_err + current.rx_eep_err;
642 649 // rx_truncated
643 650 grspw_stats.rx_truncated = grspw_stats.rx_truncated + current.rx_truncated;
644 651 // parity_err
645 652 grspw_stats.parity_err = grspw_stats.parity_err + current.parity_err;
646 653 // escape_err
647 654 grspw_stats.escape_err = grspw_stats.escape_err + current.escape_err;
648 655 // credit_err
649 656 grspw_stats.credit_err = grspw_stats.credit_err + current.credit_err;
650 657 // write_sync_err
651 658 grspw_stats.write_sync_err = grspw_stats.write_sync_err + current.write_sync_err;
652 659 // disconnect_err
653 660 grspw_stats.disconnect_err = grspw_stats.disconnect_err + current.disconnect_err;
654 661 // early_ep
655 662 grspw_stats.early_ep = grspw_stats.early_ep + current.early_ep;
656 663 // invalid_address
657 664 grspw_stats.invalid_address = grspw_stats.invalid_address + current.invalid_address;
658 665 // packets_sent
659 666 grspw_stats.packets_sent = grspw_stats.packets_sent + current.packets_sent;
660 667 // packets_received
661 668 grspw_stats.packets_received= grspw_stats.packets_received + current.packets_received;
662 669
663 670 }
664 671
665 672 void spacewire_get_last_error( void )
666 673 {
667 674 static spw_stats previous = {0};
668 675 spw_stats current;
669 676 rtems_status_code status;
670 677
671 678 unsigned int hk_lfr_last_er_rid;
672 679 unsigned char hk_lfr_last_er_code;
673 680 int coarseTime;
674 681 int fineTime;
675 682 unsigned char update_hk_lfr_last_er;
676 683
677 684 memset(&current, 0, sizeof(spw_stats));
678 685 hk_lfr_last_er_rid = INIT_CHAR;
679 686 hk_lfr_last_er_code = INIT_CHAR;
680 687 update_hk_lfr_last_er = INIT_CHAR;
681 688
682 689 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
683 690
684 691 // get current time
685 692 coarseTime = time_management_regs->coarse_time;
686 693 fineTime = time_management_regs->fine_time;
687 694
688 // typedef struct {
689 // unsigned int tx_link_err; // NOT IN HK
690 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
691 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
692 // unsigned int rx_eep_err;
693 // unsigned int rx_truncated;
694 // unsigned int parity_err;
695 // unsigned int escape_err;
696 // unsigned int credit_err;
697 // unsigned int write_sync_err;
698 // unsigned int disconnect_err;
699 // unsigned int early_ep;
700 // unsigned int invalid_address;
701 // unsigned int packets_sent;
702 // unsigned int packets_received;
703 // } spw_stats;
704
705 695 // tx_link_err *** no code associated to this field
706 696 // rx_rmap_header_crc_err *** LE *** in HK
707 697 if (previous.rx_rmap_header_crc_err != current.rx_rmap_header_crc_err)
708 698 {
709 699 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
710 700 hk_lfr_last_er_code = CODE_HEADER_CRC;
711 701 update_hk_lfr_last_er = 1;
712 702 }
713 703 // rx_rmap_data_crc_err *** LE *** NOT IN HK
714 704 if (previous.rx_rmap_data_crc_err != current.rx_rmap_data_crc_err)
715 705 {
716 706 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
717 707 hk_lfr_last_er_code = CODE_DATA_CRC;
718 708 update_hk_lfr_last_er = 1;
719 709 }
720 710 // rx_eep_err
721 711 if (previous.rx_eep_err != current.rx_eep_err)
722 712 {
723 713 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
724 714 hk_lfr_last_er_code = CODE_EEP;
725 715 update_hk_lfr_last_er = 1;
726 716 }
727 717 // rx_truncated
728 718 if (previous.rx_truncated != current.rx_truncated)
729 719 {
730 720 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
731 721 hk_lfr_last_er_code = CODE_RX_TOO_BIG;
732 722 update_hk_lfr_last_er = 1;
733 723 }
734 724 // parity_err
735 725 if (previous.parity_err != current.parity_err)
736 726 {
737 727 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
738 728 hk_lfr_last_er_code = CODE_PARITY;
739 729 update_hk_lfr_last_er = 1;
740 730 }
741 731 // escape_err
742 732 if (previous.parity_err != current.parity_err)
743 733 {
744 734 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
745 735 hk_lfr_last_er_code = CODE_ESCAPE;
746 736 update_hk_lfr_last_er = 1;
747 737 }
748 738 // credit_err
749 739 if (previous.credit_err != current.credit_err)
750 740 {
751 741 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
752 742 hk_lfr_last_er_code = CODE_CREDIT;
753 743 update_hk_lfr_last_er = 1;
754 744 }
755 745 // write_sync_err
756 746 if (previous.write_sync_err != current.write_sync_err)
757 747 {
758 748 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
759 749 hk_lfr_last_er_code = CODE_WRITE_SYNC;
760 750 update_hk_lfr_last_er = 1;
761 751 }
762 752 // disconnect_err
763 753 if (previous.disconnect_err != current.disconnect_err)
764 754 {
765 755 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
766 756 hk_lfr_last_er_code = CODE_DISCONNECT;
767 757 update_hk_lfr_last_er = 1;
768 758 }
769 759 // early_ep
770 760 if (previous.early_ep != current.early_ep)
771 761 {
772 762 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
773 763 hk_lfr_last_er_code = CODE_EARLY_EOP_EEP;
774 764 update_hk_lfr_last_er = 1;
775 765 }
776 766 // invalid_address
777 767 if (previous.invalid_address != current.invalid_address)
778 768 {
779 769 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
780 770 hk_lfr_last_er_code = CODE_INVALID_ADDRESS;
781 771 update_hk_lfr_last_er = 1;
782 772 }
783 773
784 774 // if a field has changed, update the hk_last_er fields
785 775 if (update_hk_lfr_last_er == 1)
786 776 {
787 777 update_hk_lfr_last_er_fields( hk_lfr_last_er_rid, hk_lfr_last_er_code );
788 778 }
789 779
790 780 previous = current;
791 781 }
792 782
793 783 void update_hk_lfr_last_er_fields(unsigned int rid, unsigned char code)
794 784 {
795 785 unsigned char *coarseTimePtr;
796 786 unsigned char *fineTimePtr;
797 787
798 788 coarseTimePtr = (unsigned char*) &time_management_regs->coarse_time;
799 789 fineTimePtr = (unsigned char*) &time_management_regs->fine_time;
800 790
801 791 housekeeping_packet.hk_lfr_last_er_rid[0] = (unsigned char) ((rid & BYTE0_MASK) >> SHIFT_1_BYTE );
802 792 housekeeping_packet.hk_lfr_last_er_rid[1] = (unsigned char) (rid & BYTE1_MASK);
803 793 housekeeping_packet.hk_lfr_last_er_code = code;
804 794 housekeeping_packet.hk_lfr_last_er_time[0] = coarseTimePtr[0];
805 795 housekeeping_packet.hk_lfr_last_er_time[1] = coarseTimePtr[1];
806 796 housekeeping_packet.hk_lfr_last_er_time[BYTE_2] = coarseTimePtr[BYTE_2];
807 797 housekeeping_packet.hk_lfr_last_er_time[BYTE_3] = coarseTimePtr[BYTE_3];
808 798 housekeeping_packet.hk_lfr_last_er_time[BYTE_4] = fineTimePtr[BYTE_2];
809 799 housekeeping_packet.hk_lfr_last_er_time[BYTE_5] = fineTimePtr[BYTE_3];
810 800 }
811 801
812 802 void update_hk_with_grspw_stats( void )
813 803 {
814 804 //****************************
815 805 // DPU_SPACEWIRE_IF_STATISTICS
816 806 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (grspw_stats.packets_received >> SHIFT_1_BYTE);
817 807 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (grspw_stats.packets_received);
818 808 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (grspw_stats.packets_sent >> SHIFT_1_BYTE);
819 809 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (grspw_stats.packets_sent);
820 810
821 811 //******************************************
822 812 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
823 813 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) grspw_stats.parity_err;
824 814 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) grspw_stats.disconnect_err;
825 815 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) grspw_stats.escape_err;
826 816 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) grspw_stats.credit_err;
827 817 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) grspw_stats.write_sync_err;
828 818
829 819 //*********************************************
830 820 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
831 821 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) grspw_stats.early_ep;
832 822 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) grspw_stats.invalid_address;
833 823 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) grspw_stats.rx_eep_err;
834 824 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) grspw_stats.rx_truncated;
835 825 }
836 826
837 827 void spacewire_update_hk_lfr_link_state( unsigned char *hk_lfr_status_word_0 )
838 828 {
839 829 unsigned int *statusRegisterPtr;
840 830 unsigned char linkState;
841 831
842 832 statusRegisterPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_STATUS_REGISTER);
843 833 linkState =
844 834 (unsigned char) ( ( (*statusRegisterPtr) >> SPW_LINK_STAT_POS) & STATUS_WORD_LINK_STATE_BITS); // [0000 0111]
845 835
846 836 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 & STATUS_WORD_LINK_STATE_MASK; // [1111 1000] set link state to 0
847 837
848 838 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 | linkState; // update hk_lfr_dpu_spw_link_state
849 839 }
850 840
851 841 void increase_unsigned_char_counter( unsigned char *counter )
852 842 {
853 843 // update the number of valid timecodes that have been received
854 844 if (*counter == UINT8_MAX)
855 845 {
856 846 *counter = 0;
857 847 }
858 848 else
859 849 {
860 850 *counter = *counter + 1;
861 851 }
862 852 }
863 853
864 854 unsigned int check_timecode_and_previous_timecode_coherency(unsigned char currentTimecodeCtr)
865 855 {
866 856 /** This function checks the coherency between the incoming timecode and the last valid timecode.
867 857 *
868 858 * @param currentTimecodeCtr is the incoming timecode
869 859 *
870 860 * @return returned codes::
871 861 * - LFR_DEFAULT
872 862 * - LFR_SUCCESSFUL
873 863 *
874 864 */
875 865
876 866 static unsigned char firstTickout = 1;
877 867 unsigned char ret;
878 868
879 869 ret = LFR_DEFAULT;
880 870
881 871 if (firstTickout == 0)
882 872 {
883 873 if (currentTimecodeCtr == 0)
884 874 {
885 875 if (previousTimecodeCtr == SPW_TIMECODE_MAX)
886 876 {
887 877 ret = LFR_SUCCESSFUL;
888 878 }
889 879 else
890 880 {
891 881 ret = LFR_DEFAULT;
892 882 }
893 883 }
894 884 else
895 885 {
896 886 if (currentTimecodeCtr == (previousTimecodeCtr +1))
897 887 {
898 888 ret = LFR_SUCCESSFUL;
899 889 }
900 890 else
901 891 {
902 892 ret = LFR_DEFAULT;
903 893 }
904 894 }
905 895 }
906 896 else
907 897 {
908 898 firstTickout = 0;
909 899 ret = LFR_SUCCESSFUL;
910 900 }
911 901
912 902 return ret;
913 903 }
914 904
915 905 unsigned int check_timecode_and_internal_time_coherency(unsigned char timecode, unsigned char internalTime)
916 906 {
917 907 unsigned int ret;
918 908
919 909 ret = LFR_DEFAULT;
920 910
921 911 if (timecode == internalTime)
922 912 {
923 913 ret = LFR_SUCCESSFUL;
924 914 }
925 915 else
926 916 {
927 917 ret = LFR_DEFAULT;
928 918 }
929 919
930 920 return ret;
931 921 }
932 922
933 923 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
934 924 {
935 925 // a tickout has been emitted, perform actions on the incoming timecode
936 926
937 927 unsigned char incomingTimecode;
938 928 unsigned char updateTime;
939 929 unsigned char internalTime;
940 930 rtems_status_code status;
941 931
942 932 incomingTimecode = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
943 933 updateTime = time_management_regs->coarse_time_load & TIMECODE_MASK;
944 934 internalTime = time_management_regs->coarse_time & TIMECODE_MASK;
945 935
946 936 housekeeping_packet.hk_lfr_dpu_spw_last_timc = incomingTimecode;
947 937
948 938 // update the number of tickout that have been generated
949 939 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt );
950 940
951 941 //**************************
952 942 // HK_LFR_TIMECODE_ERRONEOUS
953 943 // MISSING and INVALID are handled by the timecode_timer_routine service routine
954 944 if (check_timecode_and_previous_timecode_coherency( incomingTimecode ) == LFR_DEFAULT)
955 945 {
956 946 // this is unexpected but a tickout could have been raised despite of the timecode being erroneous
957 947 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_erroneous );
958 948 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_ERRONEOUS );
959 949 }
960 950
961 951 //************************
962 952 // HK_LFR_TIME_TIMECODE_IT
963 953 // check the coherency between the SpaceWire timecode and the Internal Time
964 954 if (check_timecode_and_internal_time_coherency( incomingTimecode, internalTime ) == LFR_DEFAULT)
965 955 {
966 956 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_it );
967 957 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_IT );
968 958 }
969 959
970 960 //********************
971 961 // HK_LFR_TIMECODE_CTR
972 962 // check the value of the timecode with respect to the last TC_LFR_UPDATE_TIME => SSS-CP-FS-370
973 963 if (oneTcLfrUpdateTimeReceived == 1)
974 964 {
975 965 if ( incomingTimecode != updateTime )
976 966 {
977 967 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_ctr );
978 968 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_CTR );
979 969 }
980 970 }
981 971
982 972 // launch the timecode timer to detect missing or invalid timecodes
983 973 previousTimecodeCtr = incomingTimecode; // update the previousTimecodeCtr value
984 974 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT, timecode_timer_routine, NULL );
985 975 if (status != RTEMS_SUCCESSFUL)
986 976 {
987 977 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_14 );
988 978 }
989 979 }
990 980
991 981 rtems_timer_service_routine timecode_timer_routine( rtems_id timer_id, void *user_data )
992 982 {
993 983 static unsigned char initStep = 1;
994 984
995 985 unsigned char currentTimecodeCtr;
996 986
997 987 currentTimecodeCtr = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
998 988
999 989 if (initStep == 1)
1000 990 {
1001 991 if (currentTimecodeCtr == previousTimecodeCtr)
1002 992 {
1003 993 //************************
1004 994 // HK_LFR_TIMECODE_MISSING
1005 995 // the timecode value has not changed, no valid timecode has been received, the timecode is MISSING
1006 996 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
1007 997 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
1008 998 }
1009 999 else if (currentTimecodeCtr == (previousTimecodeCtr+1))
1010 1000 {
1011 1001 // the timecode value has changed and the value is valid, this is unexpected because
1012 1002 // the timer should not have fired, the timecode_irq_handler should have been raised
1013 1003 }
1014 1004 else
1015 1005 {
1016 1006 //************************
1017 1007 // HK_LFR_TIMECODE_INVALID
1018 1008 // the timecode value has changed and the value is not valid, no tickout has been generated
1019 1009 // this is why the timer has fired
1020 1010 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_invalid );
1021 1011 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_INVALID );
1022 1012 }
1023 1013 }
1024 1014 else
1025 1015 {
1026 1016 initStep = 1;
1027 1017 //************************
1028 1018 // HK_LFR_TIMECODE_MISSING
1029 1019 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
1030 1020 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
1031 1021 }
1032 1022
1033 1023 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_13 );
1034 1024 }
1035 1025
1036 1026 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
1037 1027 {
1038 1028 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1039 1029 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1040 1030 header->reserved = DEFAULT_RESERVED;
1041 1031 header->userApplication = CCSDS_USER_APP;
1042 1032 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
1043 1033 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
1044 1034 header->packetLength[0] = INIT_CHAR;
1045 1035 header->packetLength[1] = INIT_CHAR;
1046 1036 // DATA FIELD HEADER
1047 1037 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1048 1038 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1049 1039 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1050 1040 header->destinationID = TM_DESTINATION_ID_GROUND;
1051 1041 header->time[BYTE_0] = INIT_CHAR;
1052 1042 header->time[BYTE_1] = INIT_CHAR;
1053 1043 header->time[BYTE_2] = INIT_CHAR;
1054 1044 header->time[BYTE_3] = INIT_CHAR;
1055 1045 header->time[BYTE_4] = INIT_CHAR;
1056 1046 header->time[BYTE_5] = INIT_CHAR;
1057 1047 // AUXILIARY DATA HEADER
1058 1048 header->sid = INIT_CHAR;
1059 1049 header->pa_bia_status_info = DEFAULT_HKBIA;
1060 1050 header->blkNr[0] = INIT_CHAR;
1061 1051 header->blkNr[1] = INIT_CHAR;
1062 1052 }
1063 1053
1064 1054 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
1065 1055 {
1066 1056 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1067 1057 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1068 1058 header->reserved = DEFAULT_RESERVED;
1069 1059 header->userApplication = CCSDS_USER_APP;
1070 1060 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1071 1061 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1072 1062 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1073 1063 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1074 1064 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> SHIFT_1_BYTE);
1075 1065 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1076 1066 // DATA FIELD HEADER
1077 1067 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1078 1068 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1079 1069 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1080 1070 header->destinationID = TM_DESTINATION_ID_GROUND;
1081 1071 header->time[BYTE_0] = INIT_CHAR;
1082 1072 header->time[BYTE_1] = INIT_CHAR;
1083 1073 header->time[BYTE_2] = INIT_CHAR;
1084 1074 header->time[BYTE_3] = INIT_CHAR;
1085 1075 header->time[BYTE_4] = INIT_CHAR;
1086 1076 header->time[BYTE_5] = INIT_CHAR;
1087 1077 // AUXILIARY DATA HEADER
1088 1078 header->sid = INIT_CHAR;
1089 1079 header->pa_bia_status_info = DEFAULT_HKBIA;
1090 1080 header->pktCnt = PKTCNT_SWF; // PKT_CNT
1091 1081 header->pktNr = INIT_CHAR;
1092 1082 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> SHIFT_1_BYTE);
1093 1083 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1094 1084 }
1095 1085
1096 1086 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
1097 1087 {
1098 1088 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1099 1089 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1100 1090 header->reserved = DEFAULT_RESERVED;
1101 1091 header->userApplication = CCSDS_USER_APP;
1102 1092 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1103 1093 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1104 1094 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1105 1095 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1106 1096 header->packetLength[0] = INIT_CHAR;
1107 1097 header->packetLength[1] = INIT_CHAR;
1108 1098 // DATA FIELD HEADER
1109 1099 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1110 1100 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1111 1101 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
1112 1102 header->destinationID = TM_DESTINATION_ID_GROUND;
1113 1103 header->time[BYTE_0] = INIT_CHAR;
1114 1104 header->time[BYTE_1] = INIT_CHAR;
1115 1105 header->time[BYTE_2] = INIT_CHAR;
1116 1106 header->time[BYTE_3] = INIT_CHAR;
1117 1107 header->time[BYTE_4] = INIT_CHAR;
1118 1108 header->time[BYTE_5] = INIT_CHAR;
1119 1109 // AUXILIARY DATA HEADER
1120 1110 header->sid = INIT_CHAR;
1121 1111 header->pa_bia_status_info = INIT_CHAR;
1122 1112 header->pa_lfr_pkt_cnt_asm = INIT_CHAR;
1123 1113 header->pa_lfr_pkt_nr_asm = INIT_CHAR;
1124 1114 header->pa_lfr_asm_blk_nr[0] = INIT_CHAR;
1125 1115 header->pa_lfr_asm_blk_nr[1] = INIT_CHAR;
1126 1116 }
1127 1117
1128 1118 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
1129 1119 Header_TM_LFR_SCIENCE_CWF_t *header )
1130 1120 {
1131 1121 /** This function sends CWF CCSDS packets (F2, F1 or F0).
1132 1122 *
1133 1123 * @param waveform points to the buffer containing the data that will be send.
1134 1124 * @param sid is the source identifier of the data that will be sent.
1135 1125 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1136 1126 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1137 1127 * contain information to setup the transmission of the data packets.
1138 1128 *
1139 1129 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1140 1130 *
1141 1131 */
1142 1132
1143 1133 unsigned int i;
1144 1134 int ret;
1145 1135 unsigned int coarseTime;
1146 1136 unsigned int fineTime;
1147 1137 rtems_status_code status;
1148 1138 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1149 1139 int *dataPtr;
1150 1140 unsigned char sid;
1151 1141
1152 1142 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1153 1143 spw_ioctl_send_CWF.options = 0;
1154 1144
1155 1145 ret = LFR_DEFAULT;
1156 1146 sid = (unsigned char) ring_node_to_send->sid;
1157 1147
1158 1148 coarseTime = ring_node_to_send->coarseTime;
1159 1149 fineTime = ring_node_to_send->fineTime;
1160 1150 dataPtr = (int*) ring_node_to_send->buffer_address;
1161 1151
1162 1152 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> SHIFT_1_BYTE);
1163 1153 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1164 1154 header->pa_bia_status_info = pa_bia_status_info;
1165 1155 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1166 1156 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> SHIFT_1_BYTE);
1167 1157 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1168 1158
1169 1159 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
1170 1160 {
1171 1161 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
1172 1162 spw_ioctl_send_CWF.hdr = (char*) header;
1173 1163 // BUILD THE DATA
1174 1164 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
1175 1165
1176 1166 // SET PACKET SEQUENCE CONTROL
1177 1167 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1178 1168
1179 1169 // SET SID
1180 1170 header->sid = sid;
1181 1171
1182 1172 // SET PACKET TIME
1183 1173 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
1184 1174 //
1185 1175 header->time[0] = header->acquisitionTime[0];
1186 1176 header->time[1] = header->acquisitionTime[1];
1187 1177 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1188 1178 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1189 1179 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1190 1180 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1191 1181
1192 1182 // SET PACKET ID
1193 1183 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
1194 1184 {
1195 1185 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> SHIFT_1_BYTE);
1196 1186 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
1197 1187 }
1198 1188 else
1199 1189 {
1200 1190 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1201 1191 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1202 1192 }
1203 1193
1204 1194 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1205 1195 if (status != RTEMS_SUCCESSFUL) {
1206 1196 ret = LFR_DEFAULT;
1207 1197 }
1208 1198 }
1209 1199
1210 1200 return ret;
1211 1201 }
1212 1202
1213 1203 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
1214 1204 Header_TM_LFR_SCIENCE_SWF_t *header )
1215 1205 {
1216 1206 /** This function sends SWF CCSDS packets (F2, F1 or F0).
1217 1207 *
1218 1208 * @param waveform points to the buffer containing the data that will be send.
1219 1209 * @param sid is the source identifier of the data that will be sent.
1220 1210 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
1221 1211 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1222 1212 * contain information to setup the transmission of the data packets.
1223 1213 *
1224 1214 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1225 1215 *
1226 1216 */
1227 1217
1228 1218 unsigned int i;
1229 1219 int ret;
1230 1220 unsigned int coarseTime;
1231 1221 unsigned int fineTime;
1232 1222 rtems_status_code status;
1233 1223 spw_ioctl_pkt_send spw_ioctl_send_SWF;
1234 1224 int *dataPtr;
1235 1225 unsigned char sid;
1236 1226
1237 1227 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
1238 1228 spw_ioctl_send_SWF.options = 0;
1239 1229
1240 1230 ret = LFR_DEFAULT;
1241 1231
1242 1232 coarseTime = ring_node_to_send->coarseTime;
1243 1233 fineTime = ring_node_to_send->fineTime;
1244 1234 dataPtr = (int*) ring_node_to_send->buffer_address;
1245 1235 sid = ring_node_to_send->sid;
1246 1236
1247 1237 header->pa_bia_status_info = pa_bia_status_info;
1248 1238 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1249 1239
1250 1240 for (i=0; i<PKTCNT_SWF; i++) // send waveform
1251 1241 {
1252 1242 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
1253 1243 spw_ioctl_send_SWF.hdr = (char*) header;
1254 1244
1255 1245 // SET PACKET SEQUENCE CONTROL
1256 1246 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1257 1247
1258 1248 // SET PACKET LENGTH AND BLKNR
1259 1249 if (i == (PKTCNT_SWF-1))
1260 1250 {
1261 1251 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
1262 1252 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> SHIFT_1_BYTE);
1263 1253 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
1264 1254 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> SHIFT_1_BYTE);
1265 1255 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
1266 1256 }
1267 1257 else
1268 1258 {
1269 1259 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
1270 1260 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> SHIFT_1_BYTE);
1271 1261 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
1272 1262 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> SHIFT_1_BYTE);
1273 1263 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
1274 1264 }
1275 1265
1276 1266 // SET PACKET TIME
1277 1267 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
1278 1268 //
1279 1269 header->time[BYTE_0] = header->acquisitionTime[BYTE_0];
1280 1270 header->time[BYTE_1] = header->acquisitionTime[BYTE_1];
1281 1271 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1282 1272 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1283 1273 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1284 1274 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1285 1275
1286 1276 // SET SID
1287 1277 header->sid = sid;
1288 1278
1289 1279 // SET PKTNR
1290 1280 header->pktNr = i+1; // PKT_NR
1291 1281
1292 1282 // SEND PACKET
1293 1283 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
1294 1284 if (status != RTEMS_SUCCESSFUL) {
1295 1285 ret = LFR_DEFAULT;
1296 1286 }
1297 1287 }
1298 1288
1299 1289 return ret;
1300 1290 }
1301 1291
1302 1292 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
1303 1293 Header_TM_LFR_SCIENCE_CWF_t *header )
1304 1294 {
1305 1295 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
1306 1296 *
1307 1297 * @param waveform points to the buffer containing the data that will be send.
1308 1298 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1309 1299 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1310 1300 * contain information to setup the transmission of the data packets.
1311 1301 *
1312 1302 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
1313 1303 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
1314 1304 *
1315 1305 */
1316 1306
1317 1307 unsigned int i;
1318 1308 int ret;
1319 1309 unsigned int coarseTime;
1320 1310 unsigned int fineTime;
1321 1311 rtems_status_code status;
1322 1312 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1323 1313 char *dataPtr;
1324 1314 unsigned char sid;
1325 1315
1326 1316 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1327 1317 spw_ioctl_send_CWF.options = 0;
1328 1318
1329 1319 ret = LFR_DEFAULT;
1330 1320 sid = ring_node_to_send->sid;
1331 1321
1332 1322 coarseTime = ring_node_to_send->coarseTime;
1333 1323 fineTime = ring_node_to_send->fineTime;
1334 1324 dataPtr = (char*) ring_node_to_send->buffer_address;
1335 1325
1336 1326 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> SHIFT_1_BYTE);
1337 1327 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
1338 1328 header->pa_bia_status_info = pa_bia_status_info;
1339 1329 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1340 1330 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> SHIFT_1_BYTE);
1341 1331 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
1342 1332
1343 1333 //*********************
1344 1334 // SEND CWF3_light DATA
1345 1335 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
1346 1336 {
1347 1337 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1348 1338 spw_ioctl_send_CWF.hdr = (char*) header;
1349 1339 // BUILD THE DATA
1350 1340 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1351 1341
1352 1342 // SET PACKET SEQUENCE COUNTER
1353 1343 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1354 1344
1355 1345 // SET SID
1356 1346 header->sid = sid;
1357 1347
1358 1348 // SET PACKET TIME
1359 1349 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1360 1350 //
1361 1351 header->time[BYTE_0] = header->acquisitionTime[BYTE_0];
1362 1352 header->time[BYTE_1] = header->acquisitionTime[BYTE_1];
1363 1353 header->time[BYTE_2] = header->acquisitionTime[BYTE_2];
1364 1354 header->time[BYTE_3] = header->acquisitionTime[BYTE_3];
1365 1355 header->time[BYTE_4] = header->acquisitionTime[BYTE_4];
1366 1356 header->time[BYTE_5] = header->acquisitionTime[BYTE_5];
1367 1357
1368 1358 // SET PACKET ID
1369 1359 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE);
1370 1360 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1371 1361
1372 1362 // SEND PACKET
1373 1363 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1374 1364 if (status != RTEMS_SUCCESSFUL) {
1375 1365 ret = LFR_DEFAULT;
1376 1366 }
1377 1367 }
1378 1368
1379 1369 return ret;
1380 1370 }
1381 1371
1382 1372 void spw_send_asm_f0( ring_node *ring_node_to_send,
1383 1373 Header_TM_LFR_SCIENCE_ASM_t *header )
1384 1374 {
1385 1375 unsigned int i;
1386 1376 unsigned int length = 0;
1387 1377 rtems_status_code status;
1388 1378 unsigned int sid;
1389 1379 float *spectral_matrix;
1390 1380 int coarseTime;
1391 1381 int fineTime;
1392 1382 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1393 1383
1394 1384 sid = ring_node_to_send->sid;
1395 1385 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1396 1386 coarseTime = ring_node_to_send->coarseTime;
1397 1387 fineTime = ring_node_to_send->fineTime;
1398 1388
1399 1389 header->pa_bia_status_info = pa_bia_status_info;
1400 1390 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1401 1391
1402 1392 for (i=0; i<PKTCNT_ASM; i++)
1403 1393 {
1404 1394 if ((i==0) || (i==1))
1405 1395 {
1406 1396 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1407 1397 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1408 1398 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1409 1399 ];
1410 1400 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1411 1401 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1412 1402 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> SHIFT_1_BYTE ); // BLK_NR MSB
1413 1403 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1414 1404 }
1415 1405 else
1416 1406 {
1417 1407 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1418 1408 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1419 1409 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1420 1410 ];
1421 1411 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1422 1412 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1423 1413 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1424 1414 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1425 1415 }
1426 1416
1427 1417 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1428 1418 spw_ioctl_send_ASM.hdr = (char *) header;
1429 1419 spw_ioctl_send_ASM.options = 0;
1430 1420
1431 1421 // (2) BUILD THE HEADER
1432 1422 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1433 1423 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1434 1424 header->packetLength[1] = (unsigned char) (length);
1435 1425 header->sid = (unsigned char) sid; // SID
1436 1426 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1437 1427 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1438 1428
1439 1429 // (3) SET PACKET TIME
1440 1430 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1441 1431 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1442 1432 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1443 1433 header->time[BYTE_3] = (unsigned char) (coarseTime);
1444 1434 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1445 1435 header->time[BYTE_5] = (unsigned char) (fineTime);
1446 1436 //
1447 1437 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1448 1438 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1449 1439 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1450 1440 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1451 1441 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1452 1442 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1453 1443
1454 1444 // (4) SEND PACKET
1455 1445 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1456 1446 if (status != RTEMS_SUCCESSFUL) {
1457 1447 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1458 1448 }
1459 1449 }
1460 1450 }
1461 1451
1462 1452 void spw_send_asm_f1( ring_node *ring_node_to_send,
1463 1453 Header_TM_LFR_SCIENCE_ASM_t *header )
1464 1454 {
1465 1455 unsigned int i;
1466 1456 unsigned int length = 0;
1467 1457 rtems_status_code status;
1468 1458 unsigned int sid;
1469 1459 float *spectral_matrix;
1470 1460 int coarseTime;
1471 1461 int fineTime;
1472 1462 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1473 1463
1474 1464 sid = ring_node_to_send->sid;
1475 1465 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1476 1466 coarseTime = ring_node_to_send->coarseTime;
1477 1467 fineTime = ring_node_to_send->fineTime;
1478 1468
1479 1469 header->pa_bia_status_info = pa_bia_status_info;
1480 1470 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1481 1471
1482 1472 for (i=0; i<PKTCNT_ASM; i++)
1483 1473 {
1484 1474 if ((i==0) || (i==1))
1485 1475 {
1486 1476 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1487 1477 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1488 1478 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1489 1479 ];
1490 1480 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1491 1481 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1492 1482 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> SHIFT_1_BYTE ); // BLK_NR MSB
1493 1483 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1494 1484 }
1495 1485 else
1496 1486 {
1497 1487 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1498 1488 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1499 1489 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1500 1490 ];
1501 1491 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1502 1492 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1503 1493 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1504 1494 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1505 1495 }
1506 1496
1507 1497 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1508 1498 spw_ioctl_send_ASM.hdr = (char *) header;
1509 1499 spw_ioctl_send_ASM.options = 0;
1510 1500
1511 1501 // (2) BUILD THE HEADER
1512 1502 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1513 1503 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1514 1504 header->packetLength[1] = (unsigned char) (length);
1515 1505 header->sid = (unsigned char) sid; // SID
1516 1506 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1517 1507 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1518 1508
1519 1509 // (3) SET PACKET TIME
1520 1510 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1521 1511 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1522 1512 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1523 1513 header->time[BYTE_3] = (unsigned char) (coarseTime);
1524 1514 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1525 1515 header->time[BYTE_5] = (unsigned char) (fineTime);
1526 1516 //
1527 1517 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1528 1518 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1529 1519 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1530 1520 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1531 1521 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1532 1522 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1533 1523
1534 1524 // (4) SEND PACKET
1535 1525 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1536 1526 if (status != RTEMS_SUCCESSFUL) {
1537 1527 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1538 1528 }
1539 1529 }
1540 1530 }
1541 1531
1532 /**
1533 * @brief spw_send_asm_f2 Sends an ASM packet at F2 over spacewire
1534 * @param ring_node_to_send node pointing to the actual buffer to send
1535 * @param header
1536 */
1542 1537 void spw_send_asm_f2( ring_node *ring_node_to_send,
1543 1538 Header_TM_LFR_SCIENCE_ASM_t *header )
1544 1539 {
1545 1540 unsigned int i;
1546 1541 unsigned int length = 0;
1547 1542 rtems_status_code status;
1548 1543 unsigned int sid;
1549 1544 float *spectral_matrix;
1550 1545 int coarseTime;
1551 1546 int fineTime;
1552 1547 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1553 1548
1554 1549 sid = ring_node_to_send->sid;
1555 1550 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1556 1551 coarseTime = ring_node_to_send->coarseTime;
1557 1552 fineTime = ring_node_to_send->fineTime;
1558 1553
1559 1554 header->pa_bia_status_info = pa_bia_status_info;
1560 1555 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1561 1556
1562 1557 for (i=0; i<PKTCNT_ASM; i++)
1563 1558 {
1564 1559
1565 1560 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1566 1561 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1567 1562 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1568 1563 ];
1569 1564 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1570 1565 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1571 1566 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> SHIFT_1_BYTE ); // BLK_NR MSB
1572 1567 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1573 1568
1574 1569 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1575 1570 spw_ioctl_send_ASM.hdr = (char *) header;
1576 1571 spw_ioctl_send_ASM.options = 0;
1577 1572
1578 1573 // (2) BUILD THE HEADER
1579 1574 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1580 1575 header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE);
1581 1576 header->packetLength[1] = (unsigned char) (length);
1582 1577 header->sid = (unsigned char) sid; // SID
1583 1578 header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM;
1584 1579 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1585 1580
1586 1581 // (3) SET PACKET TIME
1587 1582 header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES);
1588 1583 header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES);
1589 1584 header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE);
1590 1585 header->time[BYTE_3] = (unsigned char) (coarseTime);
1591 1586 header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE);
1592 1587 header->time[BYTE_5] = (unsigned char) (fineTime);
1593 1588 //
1594 1589 header->acquisitionTime[BYTE_0] = header->time[BYTE_0];
1595 1590 header->acquisitionTime[BYTE_1] = header->time[BYTE_1];
1596 1591 header->acquisitionTime[BYTE_2] = header->time[BYTE_2];
1597 1592 header->acquisitionTime[BYTE_3] = header->time[BYTE_3];
1598 1593 header->acquisitionTime[BYTE_4] = header->time[BYTE_4];
1599 1594 header->acquisitionTime[BYTE_5] = header->time[BYTE_5];
1600 1595
1601 1596 // (4) SEND PACKET
1602 1597 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1603 1598 if (status != RTEMS_SUCCESSFUL) {
1604 1599 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1605 1600 }
1606 1601 }
1607 1602 }
1608 1603
1604 /**
1605 * @brief spw_send_k_dump Sends k coefficients dump packet over spacewire
1606 * @param ring_node_to_send node pointing to the actual buffer to send
1607 */
1609 1608 void spw_send_k_dump( ring_node *ring_node_to_send )
1610 1609 {
1611 1610 rtems_status_code status;
1612 1611 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1613 1612 unsigned int packetLength;
1614 1613 unsigned int size;
1615 1614
1616 1615 PRINTF("spw_send_k_dump\n")
1617 1616
1618 1617 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1619 1618
1620 1619 packetLength = (kcoefficients_dump->packetLength[0] * CONST_256) + kcoefficients_dump->packetLength[1];
1621 1620
1622 1621 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1623 1622
1624 1623 PRINTF2("packetLength %d, size %d\n", packetLength, size )
1625 1624
1626 1625 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1627 1626
1628 1627 if (status == -1){
1629 1628 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1630 1629 }
1631 1630
1632 1631 ring_node_to_send->status = INIT_CHAR;
1633 1632 }
Show file before | Show file after | Hide comments
@@ -1,118 +1,69
1 1 /*
2 2 * CPU Usage Reporter
3 3 *
4 4 * COPYRIGHT (c) 1989-2009
5 5 * On-Line Applications Research Corporation (OAR).
6 6 *
7 7 * The license and distribution terms for this file may be
8 8 * found in the file LICENSE in this distribution or at
9 9 * http://www.rtems.com/license/LICENSE.
10 10 *
11 11 * $Id$
12 12 */
13 13
14 14 #include "lfr_cpu_usage_report.h"
15 #include "fsw_params.h"
16
17 extern rtems_id Task_id[];
15 18
16 19 unsigned char lfr_rtems_cpu_usage_report( void )
17 20 {
18 21 uint32_t api_index;
22 uint32_t information_index;
19 23 Thread_Control *the_thread;
20 24 Objects_Information *information;
21 25 uint32_t ival;
22 26 uint32_t fval;
23 27 #ifndef __RTEMS_USE_TICKS_FOR_STATISTICS__
24 28 Timestamp_Control uptime;
25 29 Timestamp_Control total;
26 30 Timestamp_Control ran;
31
27 32 #else
28 uint32_t total_units = 0;
33 #error "Can't compute CPU usage using ticks on LFR"
29 34 #endif
30 35
31 36 unsigned char cpu_load;
32 37
33 38 ival = 0;
34 39 cpu_load = 0;
35 40
36 /*
37 * When not using nanosecond CPU usage resolution, we have to count
38 * the number of "ticks" we gave credit for to give the user a rough
39 * guideline as to what each number means proportionally.
40 */
41 #ifndef __RTEMS_USE_TICKS_FOR_STATISTICS__
42 _TOD_Get_uptime( &uptime );
43 _Timestamp_Subtract( &CPU_usage_Uptime_at_last_reset, &uptime, &total );
44 #else
45 for ( api_index = 1 ; api_index <= OBJECTS_APIS_LAST ; api_index++ ) {
46 if ( !_Objects_Information_table[ api_index ] ) { }
47 else
48 {
49 information = _Objects_Information_table[ api_index ][ 1 ];
50 if ( information != NULL )
51 {
52 for ( i=1 ; i <= information->maximum ; i++ ) {
53 the_thread = (Thread_Control *)information->local_table[ i ];
54
55 if ( the_thread != NULL ) {
56 total_units += the_thread->cpu_time_used; }
57 }
58 }
59 }
60 }
61 #endif
62
63 41 for ( api_index = 1 ; api_index <= OBJECTS_APIS_LAST ; api_index++ )
64 42 {
65 43 if ( !_Objects_Information_table[ api_index ] ) { }
66 44 else
67 45 {
68 46 information = _Objects_Information_table[ api_index ][ 1 ];
69 47 if ( information != NULL )
70 48 {
71 the_thread = (Thread_Control *)information->local_table[ 1 ];
72
73 if ( the_thread == NULL ) { }
74 else
49 for(information_index=1;information_index<=information->maximum;information_index++)
75 50 {
76 #ifndef __RTEMS_USE_TICKS_FOR_STATISTICS__
77 /*
78 * If this is the currently executing thread, account for time
79 * since the last context switch.
80 */
81 ran = the_thread->cpu_time_used;
82 if ( _Thread_Executing->Object.id == the_thread->Object.id )
51 the_thread = (Thread_Control *)information->local_table[ information_index ];
52
53 if ( the_thread == NULL) { }
54 else if(the_thread->Object.id == Task_id[TASKID_SCRB]) // Only measure scrubbing task load, CPU load is 100%-Scrubbing
83 55 {
84 Timestamp_Control used;
85 _Timestamp_Subtract(
86 &_Thread_Time_of_last_context_switch, &uptime, &used
87 );
88 _Timestamp_Add_to( &ran, &used );
56 _TOD_Get_uptime( &uptime );
57 _Timestamp_Subtract( &CPU_usage_Uptime_at_last_reset, &uptime, &total );
58 ran = the_thread->cpu_time_used;
59 _Timestamp_Divide( &ran, &total, &ival, &fval);
60 cpu_load = (unsigned char)(CONST_255 - ((ival*CONST_10+fval/CONST_100)*CONST_256/CONST_1000));
89 61 }
90 _Timestamp_Divide( &ran, &total, &ival, &fval );
91
92 #else
93 if (total_units != 0)
94 {
95 uint64_t ival_64;
96
97 ival_64 = the_thread->cpu_time_used;
98 ival_64 *= CONST_100000;
99 ival = ival_64 / total_units;
100 }
101 else
102 {
103 ival = 0;
104 }
105
106 fval = ival % CONST_1000;
107 ival /= CONST_1000;
108 #endif
109 62 }
110 63 }
111 64 }
112 65 }
113 cpu_load = (unsigned char) (CONST_100 - ival);
114
115 66 return cpu_load;
116 67 }
117 68
118 69
Show file before | Show file after | Hide comments
@@ -1,423 +1,447
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions related to data processing.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 31 *
8 32 */
9 33
10 34 #include "avf0_prc0.h"
11 35
12 36 nb_sm_before_bp_asm_f0 nb_sm_before_f0 = {0};
13 37
14 38 //***
15 39 // F0
16 40 ring_node_asm asm_ring_norm_f0 [ NB_RING_NODES_ASM_NORM_F0 ] = {0};
17 41 ring_node_asm asm_ring_burst_sbm_f0 [ NB_RING_NODES_ASM_BURST_SBM_F0 ] = {0};
18 42
19 43 ring_node ring_to_send_asm_f0 [ NB_RING_NODES_ASM_F0 ] = {0};
20 44 int buffer_asm_f0 [ NB_RING_NODES_ASM_F0 * TOTAL_SIZE_SM ] = {0};
21 45
22 46 float asm_f0_patched_norm [ TOTAL_SIZE_SM ] = {0};
23 47 float asm_f0_patched_burst_sbm [ TOTAL_SIZE_SM ] = {0};
24 48 float asm_f0_reorganized [ TOTAL_SIZE_SM ] = {0};
25 49
26 50 float compressed_sm_norm_f0[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F0] = {0};
27 51 float compressed_sm_sbm_f0 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F0 ] = {0};
28 52
29 53 float k_coeff_intercalib_f0_norm[ NB_BINS_COMPRESSED_SM_F0 * NB_K_COEFF_PER_BIN ] = {0}; // 11 * 32 = 352
30 54 float k_coeff_intercalib_f0_sbm[ NB_BINS_COMPRESSED_SM_SBM_F0 * NB_K_COEFF_PER_BIN ] = {0}; // 22 * 32 = 704
31 55
32 56 //************
33 57 // RTEMS TASKS
34 58
35 59 rtems_task avf0_task( rtems_task_argument lfrRequestedMode )
36 60 {
37 61 int i;
38 62
39 63 rtems_event_set event_out;
40 64 rtems_status_code status;
41 65 rtems_id queue_id_prc0;
42 66 asm_msg msgForPRC;
43 67 ring_node *nodeForAveraging;
44 68 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0_F1];
45 69 ring_node_asm *current_ring_node_asm_burst_sbm_f0;
46 70 ring_node_asm *current_ring_node_asm_norm_f0;
47 71
48 72 unsigned int nb_norm_bp1;
49 73 unsigned int nb_norm_bp2;
50 74 unsigned int nb_norm_asm;
51 75 unsigned int nb_sbm_bp1;
52 76 unsigned int nb_sbm_bp2;
53 77
54 78 nb_norm_bp1 = 0;
55 79 nb_norm_bp2 = 0;
56 80 nb_norm_asm = 0;
57 81 nb_sbm_bp1 = 0;
58 82 nb_sbm_bp2 = 0;
59 83 event_out = EVENT_SETS_NONE_PENDING;
60 84 queue_id_prc0 = RTEMS_ID_NONE;
61 85
62 86 reset_nb_sm_f0( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
63 87 ASM_generic_init_ring( asm_ring_norm_f0, NB_RING_NODES_ASM_NORM_F0 );
64 88 ASM_generic_init_ring( asm_ring_burst_sbm_f0, NB_RING_NODES_ASM_BURST_SBM_F0 );
65 89 current_ring_node_asm_norm_f0 = asm_ring_norm_f0;
66 90 current_ring_node_asm_burst_sbm_f0 = asm_ring_burst_sbm_f0;
67 91
68 92 BOOT_PRINTF1("in AVFO *** lfrRequestedMode = %d\n", (int) lfrRequestedMode);
69 93
70 94 status = get_message_queue_id_prc0( &queue_id_prc0 );
71 95 if (status != RTEMS_SUCCESSFUL)
72 96 {
73 97 PRINTF1("in MATR *** ERR get_message_queue_id_prc0 %d\n", status)
74 98 }
75 99
76 100 while(1){
77 101 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
78 102
79 103 //****************************************
80 104 // initialize the mesage for the MATR task
81 105 msgForPRC.norm = current_ring_node_asm_norm_f0;
82 106 msgForPRC.burst_sbm = current_ring_node_asm_burst_sbm_f0;
83 107 msgForPRC.event = EVENT_SETS_NONE_PENDING; // this composite event will be sent to the PRC0 task
84 108 //
85 109 //****************************************
86 110
87 111 nodeForAveraging = getRingNodeForAveraging( 0 );
88 112
89 113 ring_node_tab[NB_SM_BEFORE_AVF0_F1-1] = nodeForAveraging;
90 114 for ( i = 1; i < (NB_SM_BEFORE_AVF0_F1); i++ )
91 115 {
92 116 nodeForAveraging = nodeForAveraging->previous;
93 117 ring_node_tab[NB_SM_BEFORE_AVF0_F1 - i - 1] = nodeForAveraging;
94 118 }
95 119
96 120 // compute the average and store it in the averaged_sm_f1 buffer
97 121 SM_average( current_ring_node_asm_norm_f0->matrix,
98 122 current_ring_node_asm_burst_sbm_f0->matrix,
99 123 ring_node_tab,
100 124 nb_norm_bp1, nb_sbm_bp1,
101 125 &msgForPRC, 0 ); // 0 => frequency channel 0
102 126
103 127 // update nb_average
104 128 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0_F1;
105 129 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0_F1;
106 130 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0_F1;
107 131 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0_F1;
108 132 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0_F1;
109 133
110 134 if (nb_sbm_bp1 == nb_sm_before_f0.burst_sbm_bp1)
111 135 {
112 136 nb_sbm_bp1 = 0;
113 137 // set another ring for the ASM storage
114 138 current_ring_node_asm_burst_sbm_f0 = current_ring_node_asm_burst_sbm_f0->next;
115 139 if ( lfrCurrentMode == LFR_MODE_BURST )
116 140 {
117 141 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP1_F0;
118 142 }
119 143 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
120 144 {
121 145 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP1_F0;
122 146 }
123 147 }
124 148
125 149 if (nb_sbm_bp2 == nb_sm_before_f0.burst_sbm_bp2)
126 150 {
127 151 nb_sbm_bp2 = 0;
128 152 if ( lfrCurrentMode == LFR_MODE_BURST )
129 153 {
130 154 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP2_F0;
131 155 }
132 156 else if ( (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
133 157 {
134 158 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP2_F0;
135 159 }
136 160 }
137 161
138 162 if (nb_norm_bp1 == nb_sm_before_f0.norm_bp1)
139 163 {
140 164 nb_norm_bp1 = 0;
141 165 // set another ring for the ASM storage
142 166 current_ring_node_asm_norm_f0 = current_ring_node_asm_norm_f0->next;
143 167 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
144 168 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
145 169 {
146 170 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F0;
147 171 }
148 172 }
149 173
150 174 if (nb_norm_bp2 == nb_sm_before_f0.norm_bp2)
151 175 {
152 176 nb_norm_bp2 = 0;
153 177 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
154 178 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
155 179 {
156 180 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F0;
157 181 }
158 182 }
159 183
160 184 if (nb_norm_asm == nb_sm_before_f0.norm_asm)
161 185 {
162 186 nb_norm_asm = 0;
163 187 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
164 188 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
165 189 {
166 190 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F0;
167 191 }
168 192 }
169 193
170 194 //*************************
171 195 // send the message to PRC
172 196 if (msgForPRC.event != EVENT_SETS_NONE_PENDING)
173 197 {
174 198 status = rtems_message_queue_send( queue_id_prc0, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC0);
175 199 }
176 200
177 201 if (status != RTEMS_SUCCESSFUL) {
178 202 PRINTF1("in AVF0 *** Error sending message to PRC, code %d\n", status)
179 203 }
180 204 }
181 205 }
182 206
183 207 rtems_task prc0_task( rtems_task_argument lfrRequestedMode )
184 208 {
185 209 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
186 210 size_t size; // size of the incoming TC packet
187 211 asm_msg *incomingMsg;
188 212 //
189 213 unsigned char sid;
190 214 rtems_status_code status;
191 215 rtems_id queue_id;
192 216 rtems_id queue_id_q_p0;
193 217 bp_packet_with_spare __attribute__((aligned(4))) packet_norm_bp1;
194 218 bp_packet __attribute__((aligned(4))) packet_norm_bp2;
195 219 bp_packet __attribute__((aligned(4))) packet_sbm_bp1;
196 220 bp_packet __attribute__((aligned(4))) packet_sbm_bp2;
197 221 ring_node *current_ring_node_to_send_asm_f0;
198 222 float nbSMInASMNORM;
199 223 float nbSMInASMSBM;
200 224
201 225 size = 0;
202 226 queue_id = RTEMS_ID_NONE;
203 227 queue_id_q_p0 = RTEMS_ID_NONE;
204 228 memset( &packet_norm_bp1, 0, sizeof(bp_packet_with_spare) );
205 229 memset( &packet_norm_bp2, 0, sizeof(bp_packet) );
206 230 memset( &packet_sbm_bp1, 0, sizeof(bp_packet) );
207 231 memset( &packet_sbm_bp2, 0, sizeof(bp_packet) );
208 232
209 233 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
210 234 init_ring( ring_to_send_asm_f0, NB_RING_NODES_ASM_F0, (volatile int*) buffer_asm_f0, TOTAL_SIZE_SM );
211 235 current_ring_node_to_send_asm_f0 = ring_to_send_asm_f0;
212 236
213 237 //*************
214 238 // NORM headers
215 239 BP_init_header_with_spare( &packet_norm_bp1,
216 240 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F0,
217 241 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0, NB_BINS_COMPRESSED_SM_F0 );
218 242 BP_init_header( &packet_norm_bp2,
219 243 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F0,
220 244 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0, NB_BINS_COMPRESSED_SM_F0);
221 245
222 246 //****************************
223 247 // BURST SBM1 and SBM2 headers
224 248 if ( lfrRequestedMode == LFR_MODE_BURST )
225 249 {
226 250 BP_init_header( &packet_sbm_bp1,
227 251 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F0,
228 252 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
229 253 BP_init_header( &packet_sbm_bp2,
230 254 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F0,
231 255 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
232 256 }
233 257 else if ( lfrRequestedMode == LFR_MODE_SBM1 )
234 258 {
235 259 BP_init_header( &packet_sbm_bp1,
236 260 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP1_F0,
237 261 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
238 262 BP_init_header( &packet_sbm_bp2,
239 263 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM1_BP2_F0,
240 264 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
241 265 }
242 266 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
243 267 {
244 268 BP_init_header( &packet_sbm_bp1,
245 269 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F0,
246 270 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
247 271 BP_init_header( &packet_sbm_bp2,
248 272 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F0,
249 273 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0, NB_BINS_COMPRESSED_SM_SBM_F0);
250 274 }
251 275 else
252 276 {
253 277 PRINTF1("in PRC0 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
254 278 }
255 279
256 280 status = get_message_queue_id_send( &queue_id );
257 281 if (status != RTEMS_SUCCESSFUL)
258 282 {
259 283 PRINTF1("in PRC0 *** ERR get_message_queue_id_send %d\n", status)
260 284 }
261 285 status = get_message_queue_id_prc0( &queue_id_q_p0);
262 286 if (status != RTEMS_SUCCESSFUL)
263 287 {
264 288 PRINTF1("in PRC0 *** ERR get_message_queue_id_prc0 %d\n", status)
265 289 }
266 290
267 291 BOOT_PRINTF1("in PRC0 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
268 292
269 293 while(1){
270 294 status = rtems_message_queue_receive( queue_id_q_p0, incomingData, &size, //************************************
271 295 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
272 296
273 297 incomingMsg = (asm_msg*) incomingData;
274 298
275 299 ASM_patch( incomingMsg->norm->matrix, asm_f0_patched_norm );
276 300 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f0_patched_burst_sbm );
277 301
278 302 nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM;
279 303 nbSMInASMSBM = incomingMsg->numberOfSMInASMSBM;
280 304
281 305 //****************
282 306 //****************
283 307 // BURST SBM1 SBM2
284 308 //****************
285 309 //****************
286 310 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F0 ) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F0 ) )
287 311 {
288 312 sid = getSID( incomingMsg->event );
289 313 // 1) compress the matrix for Basic Parameters calculation
290 314 ASM_compress_reorganize_and_divide_mask( asm_f0_patched_burst_sbm, compressed_sm_sbm_f0,
291 315 nbSMInASMSBM,
292 316 NB_BINS_COMPRESSED_SM_SBM_F0, NB_BINS_TO_AVERAGE_ASM_SBM_F0,
293 317 ASM_F0_INDICE_START, CHANNELF0);
294 318 // 2) compute the BP1 set
295 319 BP1_set( compressed_sm_sbm_f0, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp1.data );
296 320 // 3) send the BP1 set
297 321 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
298 322 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
299 323 packet_sbm_bp1.pa_bia_status_info = pa_bia_status_info;
300 324 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
301 325 BP_send_s1_s2( (char *) &packet_sbm_bp1, queue_id,
302 326 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F0 + PACKET_LENGTH_DELTA,
303 327 sid);
304 328 // 4) compute the BP2 set if needed
305 329 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F0) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F0) )
306 330 {
307 331 // 1) compute the BP2 set
308 332 BP2_set( compressed_sm_sbm_f0, NB_BINS_COMPRESSED_SM_SBM_F0, packet_sbm_bp2.data );
309 333 // 2) send the BP2 set
310 334 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
311 335 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
312 336 packet_sbm_bp2.pa_bia_status_info = pa_bia_status_info;
313 337 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
314 338 BP_send_s1_s2( (char *) &packet_sbm_bp2, queue_id,
315 339 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F0 + PACKET_LENGTH_DELTA,
316 340 sid);
317 341 }
318 342 }
319 343
320 344 //*****
321 345 //*****
322 346 // NORM
323 347 //*****
324 348 //*****
325 349 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F0)
326 350 {
327 351 // 1) compress the matrix for Basic Parameters calculation
328 352 ASM_compress_reorganize_and_divide_mask( asm_f0_patched_norm, compressed_sm_norm_f0,
329 353 nbSMInASMNORM,
330 354 NB_BINS_COMPRESSED_SM_F0, NB_BINS_TO_AVERAGE_ASM_F0,
331 355 ASM_F0_INDICE_START, CHANNELF0 );
332 356 // 2) compute the BP1 set
333 357 BP1_set( compressed_sm_norm_f0, k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp1.data );
334 358 // 3) send the BP1 set
335 359 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
336 360 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
337 361 packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
338 362 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
339 363 BP_send( (char *) &packet_norm_bp1, queue_id,
340 364 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F0 + PACKET_LENGTH_DELTA,
341 365 SID_NORM_BP1_F0 );
342 366 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F0)
343 367 {
344 368 // 1) compute the BP2 set using the same ASM as the one used for BP1
345 369 BP2_set( compressed_sm_norm_f0, NB_BINS_COMPRESSED_SM_F0, packet_norm_bp2.data );
346 370 // 2) send the BP2 set
347 371 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
348 372 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
349 373 packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
350 374 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
351 375 BP_send( (char *) &packet_norm_bp2, queue_id,
352 376 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F0 + PACKET_LENGTH_DELTA,
353 377 SID_NORM_BP2_F0);
354 378 }
355 379 }
356 380
357 381 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F0)
358 382 {
359 383 // 1) reorganize the ASM and divide
360 384 ASM_reorganize_and_divide( asm_f0_patched_norm,
361 385 (float*) current_ring_node_to_send_asm_f0->buffer_address,
362 386 nbSMInASMNORM );
363 387 current_ring_node_to_send_asm_f0->coarseTime = incomingMsg->coarseTimeNORM;
364 388 current_ring_node_to_send_asm_f0->fineTime = incomingMsg->fineTimeNORM;
365 389 current_ring_node_to_send_asm_f0->sid = SID_NORM_ASM_F0;
366 390
367 391 // 3) send the spectral matrix packets
368 392 status = rtems_message_queue_send( queue_id, &current_ring_node_to_send_asm_f0, sizeof( ring_node* ) );
369 393
370 394 // change asm ring node
371 395 current_ring_node_to_send_asm_f0 = current_ring_node_to_send_asm_f0->next;
372 396 }
373 397
374 398 update_queue_max_count( queue_id_q_p0, &hk_lfr_q_p0_fifo_size_max );
375 399
376 400 }
377 401 }
378 402
379 403 //**********
380 404 // FUNCTIONS
381 405
382 406 void reset_nb_sm_f0( unsigned char lfrMode )
383 407 {
384 408 nb_sm_before_f0.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * NB_SM_PER_S_F0;
385 409 nb_sm_before_f0.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * NB_SM_PER_S_F0;
386 410 nb_sm_before_f0.norm_asm =
387 411 ( (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256) + parameter_dump_packet.sy_lfr_n_asm_p[1]) * NB_SM_PER_S_F0;
388 412 nb_sm_before_f0.sbm1_bp1 = parameter_dump_packet.sy_lfr_s1_bp_p0 * NB_SM_PER_S1_BP_P0; // 0.25 s per digit
389 413 nb_sm_before_f0.sbm1_bp2 = parameter_dump_packet.sy_lfr_s1_bp_p1 * NB_SM_PER_S_F0;
390 414 nb_sm_before_f0.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * NB_SM_PER_S_F0;
391 415 nb_sm_before_f0.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * NB_SM_PER_S_F0;
392 416 nb_sm_before_f0.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * NB_SM_PER_S_F0;
393 417 nb_sm_before_f0.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * NB_SM_PER_S_F0;
394 418
395 419 if (lfrMode == LFR_MODE_SBM1)
396 420 {
397 421 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm1_bp1;
398 422 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm1_bp2;
399 423 }
400 424 else if (lfrMode == LFR_MODE_SBM2)
401 425 {
402 426 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.sbm2_bp1;
403 427 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.sbm2_bp2;
404 428 }
405 429 else if (lfrMode == LFR_MODE_BURST)
406 430 {
407 431 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
408 432 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
409 433 }
410 434 else
411 435 {
412 436 nb_sm_before_f0.burst_sbm_bp1 = nb_sm_before_f0.burst_bp1;
413 437 nb_sm_before_f0.burst_sbm_bp2 = nb_sm_before_f0.burst_bp2;
414 438 }
415 439 }
416 440
417 441 void init_k_coefficients_prc0( void )
418 442 {
419 443 init_k_coefficients( k_coeff_intercalib_f0_norm, NB_BINS_COMPRESSED_SM_F0 );
420 444
421 445 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f0_norm, k_coeff_intercalib_f0_sbm, NB_BINS_COMPRESSED_SM_F0);
422 446 }
423 447
Show file before | Show file after | Hide comments
@@ -1,407 +1,431
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions related to data processing.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 31 *
8 32 */
9 33
10 34 #include "avf1_prc1.h"
11 35
12 36 nb_sm_before_bp_asm_f1 nb_sm_before_f1 = {0};
13 37
14 38 //***
15 39 // F1
16 40 ring_node_asm asm_ring_norm_f1 [ NB_RING_NODES_ASM_NORM_F1 ] = {0};
17 41 ring_node_asm asm_ring_burst_sbm_f1 [ NB_RING_NODES_ASM_BURST_SBM_F1 ] = {0};
18 42
19 43 ring_node ring_to_send_asm_f1 [ NB_RING_NODES_ASM_F1 ] = {0};
20 44 int buffer_asm_f1 [ NB_RING_NODES_ASM_F1 * TOTAL_SIZE_SM ] = {0};
21 45
22 46 float asm_f1_patched_norm [ TOTAL_SIZE_SM ] = {0};
23 47 float asm_f1_patched_burst_sbm [ TOTAL_SIZE_SM ] = {0};
24 48 float asm_f1_reorganized [ TOTAL_SIZE_SM ] = {0};
25 49
26 50 float compressed_sm_norm_f1[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F1] = {0};
27 51 float compressed_sm_sbm_f1 [ TOTAL_SIZE_COMPRESSED_ASM_SBM_F1 ] = {0};
28 52
29 53 float k_coeff_intercalib_f1_norm[ NB_BINS_COMPRESSED_SM_F1 * NB_K_COEFF_PER_BIN ] = {0}; // 13 * 32 = 416
30 54 float k_coeff_intercalib_f1_sbm[ NB_BINS_COMPRESSED_SM_SBM_F1 * NB_K_COEFF_PER_BIN ] = {0}; // 26 * 32 = 832
31 55
32 56 //************
33 57 // RTEMS TASKS
34 58
35 59 rtems_task avf1_task( rtems_task_argument lfrRequestedMode )
36 60 {
37 61 int i;
38 62
39 63 rtems_event_set event_out;
40 64 rtems_status_code status;
41 65 rtems_id queue_id_prc1;
42 66 asm_msg msgForPRC;
43 67 ring_node *nodeForAveraging;
44 68 ring_node *ring_node_tab[NB_SM_BEFORE_AVF0_F1];
45 69 ring_node_asm *current_ring_node_asm_burst_sbm_f1;
46 70 ring_node_asm *current_ring_node_asm_norm_f1;
47 71
48 72 unsigned int nb_norm_bp1;
49 73 unsigned int nb_norm_bp2;
50 74 unsigned int nb_norm_asm;
51 75 unsigned int nb_sbm_bp1;
52 76 unsigned int nb_sbm_bp2;
53 77
54 78 event_out = EVENT_SETS_NONE_PENDING;
55 79 queue_id_prc1 = RTEMS_ID_NONE;
56 80
57 81 nb_norm_bp1 = 0;
58 82 nb_norm_bp2 = 0;
59 83 nb_norm_asm = 0;
60 84 nb_sbm_bp1 = 0;
61 85 nb_sbm_bp2 = 0;
62 86
63 87 reset_nb_sm_f1( lfrRequestedMode ); // reset the sm counters that drive the BP and ASM computations / transmissions
64 88 ASM_generic_init_ring( asm_ring_norm_f1, NB_RING_NODES_ASM_NORM_F1 );
65 89 ASM_generic_init_ring( asm_ring_burst_sbm_f1, NB_RING_NODES_ASM_BURST_SBM_F1 );
66 90 current_ring_node_asm_norm_f1 = asm_ring_norm_f1;
67 91 current_ring_node_asm_burst_sbm_f1 = asm_ring_burst_sbm_f1;
68 92
69 93 BOOT_PRINTF1("in AVF1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
70 94
71 95 status = get_message_queue_id_prc1( &queue_id_prc1 );
72 96 if (status != RTEMS_SUCCESSFUL)
73 97 {
74 98 PRINTF1("in AVF1 *** ERR get_message_queue_id_prc1 %d\n", status)
75 99 }
76 100
77 101 while(1){
78 102 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
79 103
80 104 //****************************************
81 105 // initialize the mesage for the MATR task
82 106 msgForPRC.norm = current_ring_node_asm_norm_f1;
83 107 msgForPRC.burst_sbm = current_ring_node_asm_burst_sbm_f1;
84 108 msgForPRC.event = EVENT_SETS_NONE_PENDING; // this composite event will be sent to the PRC1 task
85 109 //
86 110 //****************************************
87 111
88 112 nodeForAveraging = getRingNodeForAveraging( 1 );
89 113
90 114 ring_node_tab[NB_SM_BEFORE_AVF0_F1-1] = nodeForAveraging;
91 115 for ( i = 1; i < (NB_SM_BEFORE_AVF0_F1); i++ )
92 116 {
93 117 nodeForAveraging = nodeForAveraging->previous;
94 118 ring_node_tab[NB_SM_BEFORE_AVF0_F1 - i - 1] = nodeForAveraging;
95 119 }
96 120
97 121 // compute the average and store it in the averaged_sm_f1 buffer
98 122 SM_average( current_ring_node_asm_norm_f1->matrix,
99 123 current_ring_node_asm_burst_sbm_f1->matrix,
100 124 ring_node_tab,
101 125 nb_norm_bp1, nb_sbm_bp1,
102 126 &msgForPRC, 1 ); // 1 => frequency channel 1
103 127
104 128 // update nb_average
105 129 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF0_F1;
106 130 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF0_F1;
107 131 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF0_F1;
108 132 nb_sbm_bp1 = nb_sbm_bp1 + NB_SM_BEFORE_AVF0_F1;
109 133 nb_sbm_bp2 = nb_sbm_bp2 + NB_SM_BEFORE_AVF0_F1;
110 134
111 135 if (nb_sbm_bp1 == nb_sm_before_f1.burst_sbm_bp1)
112 136 {
113 137 nb_sbm_bp1 = 0;
114 138 // set another ring for the ASM storage
115 139 current_ring_node_asm_burst_sbm_f1 = current_ring_node_asm_burst_sbm_f1->next;
116 140 if ( lfrCurrentMode == LFR_MODE_BURST )
117 141 {
118 142 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP1_F1;
119 143 }
120 144 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
121 145 {
122 146 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP1_F1;
123 147 }
124 148 }
125 149
126 150 if (nb_sbm_bp2 == nb_sm_before_f1.burst_sbm_bp2)
127 151 {
128 152 nb_sbm_bp2 = 0;
129 153 if ( lfrCurrentMode == LFR_MODE_BURST )
130 154 {
131 155 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_BURST_BP2_F1;
132 156 }
133 157 else if ( lfrCurrentMode == LFR_MODE_SBM2 )
134 158 {
135 159 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_SBM_BP2_F1;
136 160 }
137 161 }
138 162
139 163 if (nb_norm_bp1 == nb_sm_before_f1.norm_bp1)
140 164 {
141 165 nb_norm_bp1 = 0;
142 166 // set another ring for the ASM storage
143 167 current_ring_node_asm_norm_f1 = current_ring_node_asm_norm_f1->next;
144 168 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
145 169 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
146 170 {
147 171 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F1;
148 172 }
149 173 }
150 174
151 175 if (nb_norm_bp2 == nb_sm_before_f1.norm_bp2)
152 176 {
153 177 nb_norm_bp2 = 0;
154 178 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
155 179 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
156 180 {
157 181 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F1;
158 182 }
159 183 }
160 184
161 185 if (nb_norm_asm == nb_sm_before_f1.norm_asm)
162 186 {
163 187 nb_norm_asm = 0;
164 188 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
165 189 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
166 190 {
167 191 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F1;
168 192 }
169 193 }
170 194
171 195 //*************************
172 196 // send the message to PRC
173 197 if (msgForPRC.event != EVENT_SETS_NONE_PENDING)
174 198 {
175 199 status = rtems_message_queue_send( queue_id_prc1, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC1);
176 200 }
177 201
178 202 if (status != RTEMS_SUCCESSFUL) {
179 203 PRINTF1("in AVF1 *** Error sending message to PRC1, code %d\n", status)
180 204 }
181 205 }
182 206 }
183 207
184 208 rtems_task prc1_task( rtems_task_argument lfrRequestedMode )
185 209 {
186 210 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
187 211 size_t size; // size of the incoming TC packet
188 212 asm_msg *incomingMsg;
189 213 //
190 214 unsigned char sid;
191 215 rtems_status_code status;
192 216 rtems_id queue_id_send;
193 217 rtems_id queue_id_q_p1;
194 218 bp_packet_with_spare __attribute__((aligned(4))) packet_norm_bp1;
195 219 bp_packet __attribute__((aligned(4))) packet_norm_bp2;
196 220 bp_packet __attribute__((aligned(4))) packet_sbm_bp1;
197 221 bp_packet __attribute__((aligned(4))) packet_sbm_bp2;
198 222 ring_node *current_ring_node_to_send_asm_f1;
199 223 float nbSMInASMNORM;
200 224 float nbSMInASMSBM;
201 225
202 226 size = 0;
203 227 queue_id_send = RTEMS_ID_NONE;
204 228 queue_id_q_p1 = RTEMS_ID_NONE;
205 229 memset( &packet_norm_bp1, 0, sizeof(bp_packet_with_spare) );
206 230 memset( &packet_norm_bp2, 0, sizeof(bp_packet) );
207 231 memset( &packet_sbm_bp1, 0, sizeof(bp_packet) );
208 232 memset( &packet_sbm_bp2, 0, sizeof(bp_packet) );
209 233
210 234 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
211 235 init_ring( ring_to_send_asm_f1, NB_RING_NODES_ASM_F1, (volatile int*) buffer_asm_f1, TOTAL_SIZE_SM );
212 236 current_ring_node_to_send_asm_f1 = ring_to_send_asm_f1;
213 237
214 238 //*************
215 239 // NORM headers
216 240 BP_init_header_with_spare( &packet_norm_bp1,
217 241 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F1,
218 242 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1, NB_BINS_COMPRESSED_SM_F1 );
219 243 BP_init_header( &packet_norm_bp2,
220 244 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F1,
221 245 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1, NB_BINS_COMPRESSED_SM_F1);
222 246
223 247 //***********************
224 248 // BURST and SBM2 headers
225 249 if ( lfrRequestedMode == LFR_MODE_BURST )
226 250 {
227 251 BP_init_header( &packet_sbm_bp1,
228 252 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP1_F1,
229 253 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
230 254 BP_init_header( &packet_sbm_bp2,
231 255 APID_TM_SCIENCE_NORMAL_BURST, SID_BURST_BP2_F1,
232 256 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
233 257 }
234 258 else if ( lfrRequestedMode == LFR_MODE_SBM2 )
235 259 {
236 260 BP_init_header( &packet_sbm_bp1,
237 261 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP1_F1,
238 262 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
239 263 BP_init_header( &packet_sbm_bp2,
240 264 APID_TM_SCIENCE_SBM1_SBM2, SID_SBM2_BP2_F1,
241 265 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1, NB_BINS_COMPRESSED_SM_SBM_F1);
242 266 }
243 267 else
244 268 {
245 269 PRINTF1("in PRC1 *** lfrRequestedMode is %d, several headers not initialized\n", (unsigned int) lfrRequestedMode)
246 270 }
247 271
248 272 status = get_message_queue_id_send( &queue_id_send );
249 273 if (status != RTEMS_SUCCESSFUL)
250 274 {
251 275 PRINTF1("in PRC1 *** ERR get_message_queue_id_send %d\n", status)
252 276 }
253 277 status = get_message_queue_id_prc1( &queue_id_q_p1);
254 278 if (status != RTEMS_SUCCESSFUL)
255 279 {
256 280 PRINTF1("in PRC1 *** ERR get_message_queue_id_prc1 %d\n", status)
257 281 }
258 282
259 283 BOOT_PRINTF1("in PRC1 *** lfrRequestedMode = %d\n", (int) lfrRequestedMode)
260 284
261 285 while(1){
262 286 status = rtems_message_queue_receive( queue_id_q_p1, incomingData, &size, //************************************
263 287 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF0
264 288
265 289 incomingMsg = (asm_msg*) incomingData;
266 290
267 291 ASM_patch( incomingMsg->norm->matrix, asm_f1_patched_norm );
268 292 ASM_patch( incomingMsg->burst_sbm->matrix, asm_f1_patched_burst_sbm );
269 293
270 294 nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM;
271 295 nbSMInASMSBM = incomingMsg->numberOfSMInASMSBM;
272 296
273 297 //***********
274 298 //***********
275 299 // BURST SBM2
276 300 //***********
277 301 //***********
278 302 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP1_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP1_F1) )
279 303 {
280 304 sid = getSID( incomingMsg->event );
281 305 // 1) compress the matrix for Basic Parameters calculation
282 306 ASM_compress_reorganize_and_divide_mask( asm_f1_patched_burst_sbm, compressed_sm_sbm_f1,
283 307 nbSMInASMSBM,
284 308 NB_BINS_COMPRESSED_SM_SBM_F1, NB_BINS_TO_AVERAGE_ASM_SBM_F1,
285 309 ASM_F1_INDICE_START, CHANNELF1);
286 310 // 2) compute the BP1 set
287 311 BP1_set( compressed_sm_sbm_f1, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp1.data );
288 312 // 3) send the BP1 set
289 313 set_time( packet_sbm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
290 314 set_time( packet_sbm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
291 315 packet_sbm_bp1.pa_bia_status_info = pa_bia_status_info;
292 316 packet_sbm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
293 317 BP_send_s1_s2( (char *) &packet_sbm_bp1, queue_id_send,
294 318 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP1_F1 + PACKET_LENGTH_DELTA,
295 319 sid );
296 320 // 4) compute the BP2 set if needed
297 321 if ( (incomingMsg->event & RTEMS_EVENT_BURST_BP2_F1) || (incomingMsg->event & RTEMS_EVENT_SBM_BP2_F1) )
298 322 {
299 323 // 1) compute the BP2 set
300 324 BP2_set( compressed_sm_sbm_f1, NB_BINS_COMPRESSED_SM_SBM_F1, packet_sbm_bp2.data );
301 325 // 2) send the BP2 set
302 326 set_time( packet_sbm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeSBM );
303 327 set_time( packet_sbm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeSBM );
304 328 packet_sbm_bp2.pa_bia_status_info = pa_bia_status_info;
305 329 packet_sbm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
306 330 BP_send_s1_s2( (char *) &packet_sbm_bp2, queue_id_send,
307 331 PACKET_LENGTH_TM_LFR_SCIENCE_SBM_BP2_F1 + PACKET_LENGTH_DELTA,
308 332 sid );
309 333 }
310 334 }
311 335
312 336 //*****
313 337 //*****
314 338 // NORM
315 339 //*****
316 340 //*****
317 341 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F1)
318 342 {
319 343 // 1) compress the matrix for Basic Parameters calculation
320 344 ASM_compress_reorganize_and_divide_mask( asm_f1_patched_norm, compressed_sm_norm_f1,
321 345 nbSMInASMNORM,
322 346 NB_BINS_COMPRESSED_SM_F1, NB_BINS_TO_AVERAGE_ASM_F1,
323 347 ASM_F1_INDICE_START, CHANNELF1 );
324 348 // 2) compute the BP1 set
325 349 BP1_set( compressed_sm_norm_f1, k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp1.data );
326 350 // 3) send the BP1 set
327 351 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
328 352 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
329 353 packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
330 354 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
331 355 BP_send( (char *) &packet_norm_bp1, queue_id_send,
332 356 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F1 + PACKET_LENGTH_DELTA,
333 357 SID_NORM_BP1_F1 );
334 358 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F1)
335 359 {
336 360 // 1) compute the BP2 set
337 361 BP2_set( compressed_sm_norm_f1, NB_BINS_COMPRESSED_SM_F1, packet_norm_bp2.data );
338 362 // 2) send the BP2 set
339 363 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
340 364 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
341 365 packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
342 366 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
343 367 BP_send( (char *) &packet_norm_bp2, queue_id_send,
344 368 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F1 + PACKET_LENGTH_DELTA,
345 369 SID_NORM_BP2_F1 );
346 370 }
347 371 }
348 372
349 373 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F1)
350 374 {
351 375 // 1) reorganize the ASM and divide
352 376 ASM_reorganize_and_divide( asm_f1_patched_norm,
353 377 (float*) current_ring_node_to_send_asm_f1->buffer_address,
354 378 nbSMInASMNORM );
355 379 current_ring_node_to_send_asm_f1->coarseTime = incomingMsg->coarseTimeNORM;
356 380 current_ring_node_to_send_asm_f1->fineTime = incomingMsg->fineTimeNORM;
357 381 current_ring_node_to_send_asm_f1->sid = SID_NORM_ASM_F1;
358 382
359 383 // 3) send the spectral matrix packets
360 384 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f1, sizeof( ring_node* ) );
361 385
362 386 // change asm ring node
363 387 current_ring_node_to_send_asm_f1 = current_ring_node_to_send_asm_f1->next;
364 388 }
365 389
366 390 update_queue_max_count( queue_id_q_p1, &hk_lfr_q_p1_fifo_size_max );
367 391
368 392 }
369 393 }
370 394
371 395 //**********
372 396 // FUNCTIONS
373 397
374 398 void reset_nb_sm_f1( unsigned char lfrMode )
375 399 {
376 400 nb_sm_before_f1.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0 * NB_SM_PER_S_F1;
377 401 nb_sm_before_f1.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1 * NB_SM_PER_S_F1;
378 402 nb_sm_before_f1.norm_asm =
379 403 ( (parameter_dump_packet.sy_lfr_n_asm_p[0] * 256) + parameter_dump_packet.sy_lfr_n_asm_p[1]) * NB_SM_PER_S_F1;
380 404 nb_sm_before_f1.sbm2_bp1 = parameter_dump_packet.sy_lfr_s2_bp_p0 * NB_SM_PER_S_F1;
381 405 nb_sm_before_f1.sbm2_bp2 = parameter_dump_packet.sy_lfr_s2_bp_p1 * NB_SM_PER_S_F1;
382 406 nb_sm_before_f1.burst_bp1 = parameter_dump_packet.sy_lfr_b_bp_p0 * NB_SM_PER_S_F1;
383 407 nb_sm_before_f1.burst_bp2 = parameter_dump_packet.sy_lfr_b_bp_p1 * NB_SM_PER_S_F1;
384 408
385 409 if (lfrMode == LFR_MODE_SBM2)
386 410 {
387 411 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.sbm2_bp1;
388 412 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.sbm2_bp2;
389 413 }
390 414 else if (lfrMode == LFR_MODE_BURST)
391 415 {
392 416 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
393 417 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
394 418 }
395 419 else
396 420 {
397 421 nb_sm_before_f1.burst_sbm_bp1 = nb_sm_before_f1.burst_bp1;
398 422 nb_sm_before_f1.burst_sbm_bp2 = nb_sm_before_f1.burst_bp2;
399 423 }
400 424 }
401 425
402 426 void init_k_coefficients_prc1( void )
403 427 {
404 428 init_k_coefficients( k_coeff_intercalib_f1_norm, NB_BINS_COMPRESSED_SM_F1 );
405 429
406 430 init_kcoeff_sbm_from_kcoeff_norm( k_coeff_intercalib_f1_norm, k_coeff_intercalib_f1_sbm, NB_BINS_COMPRESSED_SM_F1);
407 431 }
Show file before | Show file after | Hide comments
@@ -1,332 +1,356
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions related to data processing.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 31 *
8 32 */
9 33
10 34 #include "avf2_prc2.h"
11 35
12 36 nb_sm_before_bp_asm_f2 nb_sm_before_f2 = {0};
13 37
14 38 //***
15 39 // F2
16 40 ring_node_asm asm_ring_norm_f2 [ NB_RING_NODES_ASM_NORM_F2 ] = {0};
17 41
18 42 ring_node ring_to_send_asm_f2 [ NB_RING_NODES_ASM_F2 ] = {0};
19 43 int buffer_asm_f2 [ NB_RING_NODES_ASM_F2 * TOTAL_SIZE_SM ] = {0};
20 44
21 45 float asm_f2_patched_norm [ TOTAL_SIZE_SM ] = {0};
22 46 float asm_f2_reorganized [ TOTAL_SIZE_SM ] = {0};
23 47
24 48 float compressed_sm_norm_f2[ TOTAL_SIZE_COMPRESSED_ASM_NORM_F2] = {0};
25 49
26 50 float k_coeff_intercalib_f2[ NB_BINS_COMPRESSED_SM_F2 * NB_K_COEFF_PER_BIN ] = {0}; // 12 * 32 = 384
27 51
28 52 //************
29 53 // RTEMS TASKS
30 54
31 55 //***
32 56 // F2
33 57 rtems_task avf2_task( rtems_task_argument argument )
34 58 {
35 59 rtems_event_set event_out;
36 60 rtems_status_code status;
37 61 rtems_id queue_id_prc2;
38 62 asm_msg msgForPRC;
39 63 ring_node *nodeForAveraging;
40 64 ring_node_asm *current_ring_node_asm_norm_f2;
41 65
42 66 unsigned int nb_norm_bp1;
43 67 unsigned int nb_norm_bp2;
44 68 unsigned int nb_norm_asm;
45 69
46 70 event_out = EVENT_SETS_NONE_PENDING;
47 71 queue_id_prc2 = RTEMS_ID_NONE;
48 72 nb_norm_bp1 = 0;
49 73 nb_norm_bp2 = 0;
50 74 nb_norm_asm = 0;
51 75
52 76 reset_nb_sm_f2( ); // reset the sm counters that drive the BP and ASM computations / transmissions
53 77 ASM_generic_init_ring( asm_ring_norm_f2, NB_RING_NODES_ASM_NORM_F2 );
54 78 current_ring_node_asm_norm_f2 = asm_ring_norm_f2;
55 79
56 80 BOOT_PRINTF("in AVF2 ***\n")
57 81
58 82 status = get_message_queue_id_prc2( &queue_id_prc2 );
59 83 if (status != RTEMS_SUCCESSFUL)
60 84 {
61 85 PRINTF1("in AVF2 *** ERR get_message_queue_id_prc2 %d\n", status)
62 86 }
63 87
64 88 while(1){
65 89 rtems_event_receive(RTEMS_EVENT_0, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT0
66 90
67 91 //****************************************
68 92 // initialize the mesage for the MATR task
69 93 msgForPRC.norm = current_ring_node_asm_norm_f2;
70 94 msgForPRC.burst_sbm = NULL;
71 95 msgForPRC.event = EVENT_SETS_NONE_PENDING; // this composite event will be sent to the PRC2 task
72 96 //
73 97 //****************************************
74 98
75 99 nodeForAveraging = getRingNodeForAveraging( CHANNELF2 );
76 100
77 101 // compute the average and store it in the averaged_sm_f2 buffer
78 102 SM_average_f2( current_ring_node_asm_norm_f2->matrix,
79 103 nodeForAveraging,
80 104 nb_norm_bp1,
81 105 &msgForPRC );
82 106
83 107 // update nb_average
84 108 nb_norm_bp1 = nb_norm_bp1 + NB_SM_BEFORE_AVF2;
85 109 nb_norm_bp2 = nb_norm_bp2 + NB_SM_BEFORE_AVF2;
86 110 nb_norm_asm = nb_norm_asm + NB_SM_BEFORE_AVF2;
87 111
88 112 if (nb_norm_bp1 == nb_sm_before_f2.norm_bp1)
89 113 {
90 114 nb_norm_bp1 = 0;
91 115 // set another ring for the ASM storage
92 116 current_ring_node_asm_norm_f2 = current_ring_node_asm_norm_f2->next;
93 117 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
94 118 || (lfrCurrentMode == LFR_MODE_SBM2) )
95 119 {
96 120 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP1_F2;
97 121 }
98 122 }
99 123
100 124 if (nb_norm_bp2 == nb_sm_before_f2.norm_bp2)
101 125 {
102 126 nb_norm_bp2 = 0;
103 127 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
104 128 || (lfrCurrentMode == LFR_MODE_SBM2) )
105 129 {
106 130 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_BP2_F2;
107 131 }
108 132 }
109 133
110 134 if (nb_norm_asm == nb_sm_before_f2.norm_asm)
111 135 {
112 136 nb_norm_asm = 0;
113 137 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
114 138 || (lfrCurrentMode == LFR_MODE_SBM2) )
115 139 {
116 140 msgForPRC.event = msgForPRC.event | RTEMS_EVENT_NORM_ASM_F2;
117 141 }
118 142 }
119 143
120 144 //*************************
121 145 // send the message to PRC2
122 146 if (msgForPRC.event != EVENT_SETS_NONE_PENDING)
123 147 {
124 148 status = rtems_message_queue_send( queue_id_prc2, (char *) &msgForPRC, MSG_QUEUE_SIZE_PRC2);
125 149 }
126 150
127 151 if (status != RTEMS_SUCCESSFUL) {
128 152 PRINTF1("in AVF2 *** Error sending message to PRC2, code %d\n", status)
129 153 }
130 154 }
131 155 }
132 156
133 157 rtems_task prc2_task( rtems_task_argument argument )
134 158 {
135 159 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
136 160 size_t size; // size of the incoming TC packet
137 161 asm_msg *incomingMsg;
138 162 //
139 163 rtems_status_code status;
140 164 rtems_id queue_id_send;
141 165 rtems_id queue_id_q_p2;
142 166 bp_packet __attribute__((aligned(4))) packet_norm_bp1;
143 167 bp_packet __attribute__((aligned(4))) packet_norm_bp2;
144 168 ring_node *current_ring_node_to_send_asm_f2;
145 169 float nbSMInASMNORM;
146 170
147 171 unsigned long long int localTime;
148 172
149 173 size = 0;
150 174 queue_id_send = RTEMS_ID_NONE;
151 175 queue_id_q_p2 = RTEMS_ID_NONE;
152 176 memset( &packet_norm_bp1, 0, sizeof(bp_packet) );
153 177 memset( &packet_norm_bp2, 0, sizeof(bp_packet) );
154 178
155 179 // init the ring of the averaged spectral matrices which will be transmitted to the DPU
156 180 init_ring( ring_to_send_asm_f2, NB_RING_NODES_ASM_F2, (volatile int*) buffer_asm_f2, TOTAL_SIZE_SM );
157 181 current_ring_node_to_send_asm_f2 = ring_to_send_asm_f2;
158 182
159 183 //*************
160 184 // NORM headers
161 185 BP_init_header( &packet_norm_bp1,
162 186 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP1_F2,
163 187 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2, NB_BINS_COMPRESSED_SM_F2 );
164 188 BP_init_header( &packet_norm_bp2,
165 189 APID_TM_SCIENCE_NORMAL_BURST, SID_NORM_BP2_F2,
166 190 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2, NB_BINS_COMPRESSED_SM_F2 );
167 191
168 192 status = get_message_queue_id_send( &queue_id_send );
169 193 if (status != RTEMS_SUCCESSFUL)
170 194 {
171 195 PRINTF1("in PRC2 *** ERR get_message_queue_id_send %d\n", status)
172 196 }
173 197 status = get_message_queue_id_prc2( &queue_id_q_p2);
174 198 if (status != RTEMS_SUCCESSFUL)
175 199 {
176 200 PRINTF1("in PRC2 *** ERR get_message_queue_id_prc2 %d\n", status)
177 201 }
178 202
179 203 BOOT_PRINTF("in PRC2 ***\n")
180 204
181 205 while(1){
182 206 status = rtems_message_queue_receive( queue_id_q_p2, incomingData, &size, //************************************
183 207 RTEMS_WAIT, RTEMS_NO_TIMEOUT ); // wait for a message coming from AVF2
184 208
185 209 incomingMsg = (asm_msg*) incomingData;
186 210
187 211 ASM_patch( incomingMsg->norm->matrix, asm_f2_patched_norm );
188 212
189 213 localTime = getTimeAsUnsignedLongLongInt( );
190 214
191 215 nbSMInASMNORM = incomingMsg->numberOfSMInASMNORM;
192 216
193 217 //*****
194 218 //*****
195 219 // NORM
196 220 //*****
197 221 //*****
198 222 // 1) compress the matrix for Basic Parameters calculation
199 223 ASM_compress_reorganize_and_divide_mask( asm_f2_patched_norm, compressed_sm_norm_f2,
200 224 nbSMInASMNORM,
201 225 NB_BINS_COMPRESSED_SM_F2, NB_BINS_TO_AVERAGE_ASM_F2,
202 226 ASM_F2_INDICE_START, CHANNELF2 );
203 227 // BP1_F2
204 228 if (incomingMsg->event & RTEMS_EVENT_NORM_BP1_F2)
205 229 {
206 230 // 1) compute the BP1 set
207 231 BP1_set( compressed_sm_norm_f2, k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp1.data );
208 232 // 2) send the BP1 set
209 233 set_time( packet_norm_bp1.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
210 234 set_time( packet_norm_bp1.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
211 235 packet_norm_bp1.pa_bia_status_info = pa_bia_status_info;
212 236 packet_norm_bp1.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
213 237 BP_send( (char *) &packet_norm_bp1, queue_id_send,
214 238 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP1_F2 + PACKET_LENGTH_DELTA,
215 239 SID_NORM_BP1_F2 );
216 240 }
217 241 // BP2_F2
218 242 if (incomingMsg->event & RTEMS_EVENT_NORM_BP2_F2)
219 243 {
220 244 // 1) compute the BP2 set
221 245 BP2_set( compressed_sm_norm_f2, NB_BINS_COMPRESSED_SM_F2, packet_norm_bp2.data );
222 246 // 2) send the BP2 set
223 247 set_time( packet_norm_bp2.time, (unsigned char *) &incomingMsg->coarseTimeNORM );
224 248 set_time( packet_norm_bp2.acquisitionTime, (unsigned char *) &incomingMsg->coarseTimeNORM );
225 249 packet_norm_bp2.pa_bia_status_info = pa_bia_status_info;
226 250 packet_norm_bp2.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
227 251 BP_send( (char *) &packet_norm_bp2, queue_id_send,
228 252 PACKET_LENGTH_TM_LFR_SCIENCE_NORM_BP2_F2 + PACKET_LENGTH_DELTA,
229 253 SID_NORM_BP2_F2 );
230 254 }
231 255
232 256 if (incomingMsg->event & RTEMS_EVENT_NORM_ASM_F2)
233 257 {
234 258 // 1) reorganize the ASM and divide
235 259 ASM_reorganize_and_divide( asm_f2_patched_norm,
236 260 (float*) current_ring_node_to_send_asm_f2->buffer_address,
237 261 nb_sm_before_f2.norm_bp1 );
238 262 current_ring_node_to_send_asm_f2->coarseTime = incomingMsg->coarseTimeNORM;
239 263 current_ring_node_to_send_asm_f2->fineTime = incomingMsg->fineTimeNORM;
240 264 current_ring_node_to_send_asm_f2->sid = SID_NORM_ASM_F2;
241 265
242 266 // 3) send the spectral matrix packets
243 267 status = rtems_message_queue_send( queue_id_send, &current_ring_node_to_send_asm_f2, sizeof( ring_node* ) );
244 268
245 269 // change asm ring node
246 270 current_ring_node_to_send_asm_f2 = current_ring_node_to_send_asm_f2->next;
247 271 }
248 272
249 273 update_queue_max_count( queue_id_q_p2, &hk_lfr_q_p2_fifo_size_max );
250 274
251 275 }
252 276 }
253 277
254 278 //**********
255 279 // FUNCTIONS
256 280
257 281 void reset_nb_sm_f2( void )
258 282 {
259 283 nb_sm_before_f2.norm_bp1 = parameter_dump_packet.sy_lfr_n_bp_p0;
260 284 nb_sm_before_f2.norm_bp2 = parameter_dump_packet.sy_lfr_n_bp_p1;
261 285 nb_sm_before_f2.norm_asm = (parameter_dump_packet.sy_lfr_n_asm_p[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_asm_p[1];
262 286 }
263 287
264 288 void SM_average_f2( float *averaged_spec_mat_f2,
265 289 ring_node *ring_node,
266 290 unsigned int nbAverageNormF2,
267 291 asm_msg *msgForMATR )
268 292 {
269 293 float sum;
270 294 unsigned int i;
271 295 unsigned char keepMatrix;
272 296
273 297 // test acquisitionTime validity
274 298 keepMatrix = acquisitionTimeIsValid( ring_node->coarseTime, ring_node->fineTime, CHANNELF2 );
275 299
276 300 for(i=0; i<TOTAL_SIZE_SM; i++)
277 301 {
278 302 sum = ( (int *) (ring_node->buffer_address) ) [ i ];
279 303 if ( (nbAverageNormF2 == 0) ) // average initialization
280 304 {
281 305 if (keepMatrix == MATRIX_IS_NOT_POLLUTED) // keep the matrix and add it to the average
282 306 {
283 307 averaged_spec_mat_f2[ i ] = sum;
284 308 }
285 309 else // drop the matrix and initialize the average
286 310 {
287 311 averaged_spec_mat_f2[ i ] = INIT_FLOAT;
288 312 }
289 313 msgForMATR->coarseTimeNORM = ring_node->coarseTime;
290 314 msgForMATR->fineTimeNORM = ring_node->fineTime;
291 315 }
292 316 else
293 317 {
294 318 if (keepMatrix == MATRIX_IS_NOT_POLLUTED) // keep the matrix and add it to the average
295 319 {
296 320 averaged_spec_mat_f2[ i ] = ( averaged_spec_mat_f2[ i ] + sum );
297 321 }
298 322 else
299 323 {
300 324 // nothing to do, the matrix is not valid
301 325 }
302 326 }
303 327 }
304 328
305 329 if (keepMatrix == 1)
306 330 {
307 331 if ( (nbAverageNormF2 == 0) )
308 332 {
309 333 msgForMATR->numberOfSMInASMNORM = 1;
310 334 }
311 335 else
312 336 {
313 337 msgForMATR->numberOfSMInASMNORM++;
314 338 }
315 339 }
316 340 else
317 341 {
318 342 if ( (nbAverageNormF2 == 0) )
319 343 {
320 344 msgForMATR->numberOfSMInASMNORM = 0;
321 345 }
322 346 else
323 347 {
324 348 // nothing to do
325 349 }
326 350 }
327 351 }
328 352
329 353 void init_k_coefficients_prc2( void )
330 354 {
331 355 init_k_coefficients( k_coeff_intercalib_f2, NB_BINS_COMPRESSED_SM_F2);
332 356 }
Show file before | Show file after | Hide comments
@@ -1,840 +1,898
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions related to data processing.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 31 *
8 32 */
9 33
10 34 #include "fsw_processing.h"
11 35 #include "fsw_processing_globals.c"
12 36 #include "fsw_init.h"
13 37
14 38 unsigned int nb_sm_f0 = 0;
15 39 unsigned int nb_sm_f0_aux_f1= 0;
16 40 unsigned int nb_sm_f1 = 0;
17 41 unsigned int nb_sm_f0_aux_f2= 0;
18 42
19 43 typedef enum restartState_t
20 44 {
21 45 WAIT_FOR_F2,
22 46 WAIT_FOR_F1,
23 47 WAIT_FOR_F0
24 48 } restartState;
25 49
26 50 //************************
27 51 // spectral matrices rings
28 52 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ] = {0};
29 53 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ] = {0};
30 54 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ] = {0};
31 55 ring_node *current_ring_node_sm_f0 = NULL;
32 56 ring_node *current_ring_node_sm_f1 = NULL;
33 57 ring_node *current_ring_node_sm_f2 = NULL;
34 58 ring_node *ring_node_for_averaging_sm_f0= NULL;
35 59 ring_node *ring_node_for_averaging_sm_f1= NULL;
36 60 ring_node *ring_node_for_averaging_sm_f2= NULL;
37 61
38 62 //
39 63 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
40 64 {
41 65 ring_node *node;
42 66
43 67 node = NULL;
44 68 switch ( frequencyChannel ) {
45 69 case CHANNELF0:
46 70 node = ring_node_for_averaging_sm_f0;
47 71 break;
48 72 case CHANNELF1:
49 73 node = ring_node_for_averaging_sm_f1;
50 74 break;
51 75 case CHANNELF2:
52 76 node = ring_node_for_averaging_sm_f2;
53 77 break;
54 78 default:
55 79 break;
56 80 }
57 81
58 82 return node;
59 83 }
60 84
61 85 //***********************************************************
62 86 // Interrupt Service Routine for spectral matrices processing
63 87
64 88 void spectral_matrices_isr_f0( int statusReg )
65 89 {
66 90 unsigned char status;
67 91 rtems_status_code status_code;
68 92 ring_node *full_ring_node;
69 93
70 94 status = (unsigned char) (statusReg & BITS_STATUS_F0); // [0011] get the status_ready_matrix_f0_x bits
71 95
72 96 switch(status)
73 97 {
74 98 case 0:
75 99 break;
76 100 case BIT_READY_0_1:
77 101 // UNEXPECTED VALUE
78 102 spectral_matrix_regs->status = BIT_READY_0_1; // [0011]
79 103 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
80 104 break;
81 105 case BIT_READY_0:
82 106 full_ring_node = current_ring_node_sm_f0->previous;
83 107 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
84 108 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
85 109 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
86 110 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
87 111 // if there are enough ring nodes ready, wake up an AVFx task
88 112 nb_sm_f0 = nb_sm_f0 + 1;
89 113 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
90 114 {
91 115 ring_node_for_averaging_sm_f0 = full_ring_node;
92 116 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
93 117 {
94 118 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
95 119 }
96 120 nb_sm_f0 = 0;
97 121 }
98 122 spectral_matrix_regs->status = BIT_READY_0; // [0000 0001]
99 123 break;
100 124 case BIT_READY_1:
101 125 full_ring_node = current_ring_node_sm_f0->previous;
102 126 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
103 127 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
104 128 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
105 129 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
106 130 // if there are enough ring nodes ready, wake up an AVFx task
107 131 nb_sm_f0 = nb_sm_f0 + 1;
108 132 if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
109 133 {
110 134 ring_node_for_averaging_sm_f0 = full_ring_node;
111 135 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
112 136 {
113 137 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
114 138 }
115 139 nb_sm_f0 = 0;
116 140 }
117 141 spectral_matrix_regs->status = BIT_READY_1; // [0000 0010]
118 142 break;
119 143 default:
120 144 break;
121 145 }
122 146 }
123 147
124 148 void spectral_matrices_isr_f1( int statusReg )
125 149 {
126 150 rtems_status_code status_code;
127 151 unsigned char status;
128 152 ring_node *full_ring_node;
129 153
130 154 status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS); // [1100] get the status_ready_matrix_f1_x bits
131 155
132 156 switch(status)
133 157 {
134 158 case 0:
135 159 break;
136 160 case BIT_READY_0_1:
137 161 // UNEXPECTED VALUE
138 162 spectral_matrix_regs->status = BITS_STATUS_F1; // [1100]
139 163 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
140 164 break;
141 165 case BIT_READY_0:
142 166 full_ring_node = current_ring_node_sm_f1->previous;
143 167 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
144 168 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
145 169 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
146 170 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
147 171 // if there are enough ring nodes ready, wake up an AVFx task
148 172 nb_sm_f1 = nb_sm_f1 + 1;
149 173 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
150 174 {
151 175 ring_node_for_averaging_sm_f1 = full_ring_node;
152 176 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
153 177 {
154 178 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
155 179 }
156 180 nb_sm_f1 = 0;
157 181 }
158 182 spectral_matrix_regs->status = BIT_STATUS_F1_0; // [0000 0100]
159 183 break;
160 184 case BIT_READY_1:
161 185 full_ring_node = current_ring_node_sm_f1->previous;
162 186 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
163 187 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
164 188 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
165 189 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
166 190 // if there are enough ring nodes ready, wake up an AVFx task
167 191 nb_sm_f1 = nb_sm_f1 + 1;
168 192 if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
169 193 {
170 194 ring_node_for_averaging_sm_f1 = full_ring_node;
171 195 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
172 196 {
173 197 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
174 198 }
175 199 nb_sm_f1 = 0;
176 200 }
177 201 spectral_matrix_regs->status = BIT_STATUS_F1_1; // [1000 0000]
178 202 break;
179 203 default:
180 204 break;
181 205 }
182 206 }
183 207
184 208 void spectral_matrices_isr_f2( int statusReg )
185 209 {
186 210 unsigned char status;
187 211 rtems_status_code status_code;
188 212
189 213 status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS); // [0011 0000] get the status_ready_matrix_f2_x bits
190 214
191 215 switch(status)
192 216 {
193 217 case 0:
194 218 break;
195 219 case BIT_READY_0_1:
196 220 // UNEXPECTED VALUE
197 221 spectral_matrix_regs->status = BITS_STATUS_F2; // [0011 0000]
198 222 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
199 223 break;
200 224 case BIT_READY_0:
201 225 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
202 226 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
203 227 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
204 228 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
205 229 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
206 230 spectral_matrix_regs->status = BIT_STATUS_F2_0; // [0001 0000]
207 231 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
208 232 {
209 233 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
210 234 }
211 235 break;
212 236 case BIT_READY_1:
213 237 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
214 238 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
215 239 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
216 240 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
217 241 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
218 242 spectral_matrix_regs->status = BIT_STATUS_F2_1; // [0010 0000]
219 243 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
220 244 {
221 245 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
222 246 }
223 247 break;
224 248 default:
225 249 break;
226 250 }
227 251 }
228 252
229 253 void spectral_matrix_isr_error_handler( int statusReg )
230 254 {
231 255 // STATUS REGISTER
232 256 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
233 257 // 10 9 8
234 258 // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
235 259 // 7 6 5 4 3 2 1 0
236 260 // [bad_component_err] not defined in the last version of the VHDL code
237 261
238 262 rtems_status_code status_code;
239 263
240 264 //***************************************************
241 265 // the ASM status register is copied in the HK packet
242 266 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS); // [0111 1000 0000]
243 267
244 268 if (statusReg & BITS_SM_ERR) // [0111 1100 0000]
245 269 {
246 270 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
247 271 }
248 272
249 273 spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR;
250 274
251 275 }
252 276
253 277 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
254 278 {
255 279 // STATUS REGISTER
256 280 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
257 281 // 10 9 8
258 282 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
259 283 // 7 6 5 4 3 2 1 0
260 284
261 285 int statusReg;
262 286
263 287 static restartState state = WAIT_FOR_F2;
264 288
265 289 statusReg = spectral_matrix_regs->status;
266 290
267 291 if (thisIsAnASMRestart == 0)
268 292 { // this is not a restart sequence, process incoming matrices normally
269 293 spectral_matrices_isr_f0( statusReg );
270 294
271 295 spectral_matrices_isr_f1( statusReg );
272 296
273 297 spectral_matrices_isr_f2( statusReg );
274 298 }
275 299 else
276 300 { // a restart sequence has to be launched
277 301 switch (state) {
278 302 case WAIT_FOR_F2:
279 303 if ((statusReg & BITS_STATUS_F2) != INIT_CHAR) // [0011 0000] check the status_ready_matrix_f2_x bits
280 304 {
281 305 state = WAIT_FOR_F1;
282 306 }
283 307 break;
284 308 case WAIT_FOR_F1:
285 309 if ((statusReg & BITS_STATUS_F1) != INIT_CHAR) // [0000 1100] check the status_ready_matrix_f1_x bits
286 310 {
287 311 state = WAIT_FOR_F0;
288 312 }
289 313 break;
290 314 case WAIT_FOR_F0:
291 315 if ((statusReg & BITS_STATUS_F0) != INIT_CHAR) // [0000 0011] check the status_ready_matrix_f0_x bits
292 316 {
293 317 state = WAIT_FOR_F2;
294 318 thisIsAnASMRestart = 0;
295 319 }
296 320 break;
297 321 default:
298 322 break;
299 323 }
300 324 reset_sm_status();
301 325 }
302 326
303 327 spectral_matrix_isr_error_handler( statusReg );
304 328
305 329 }
306 330
307 331 //******************
308 332 // Spectral Matrices
309 333
310 334 void reset_nb_sm( void )
311 335 {
312 336 nb_sm_f0 = 0;
313 337 nb_sm_f0_aux_f1 = 0;
314 338 nb_sm_f0_aux_f2 = 0;
315 339
316 340 nb_sm_f1 = 0;
317 341 }
318 342
319 343 void SM_init_rings( void )
320 344 {
321 345 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
322 346 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
323 347 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
324 348
325 349 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
326 350 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
327 351 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
328 352 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
329 353 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
330 354 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
331 355 }
332 356
333 357 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
334 358 {
335 359 unsigned char i;
336 360
337 361 ring[ nbNodes - 1 ].next
338 362 = (ring_node_asm*) &ring[ 0 ];
339 363
340 364 for(i=0; i<nbNodes-1; i++)
341 365 {
342 366 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
343 367 }
344 368 }
345 369
346 370 void SM_reset_current_ring_nodes( void )
347 371 {
348 372 current_ring_node_sm_f0 = sm_ring_f0[0].next;
349 373 current_ring_node_sm_f1 = sm_ring_f1[0].next;
350 374 current_ring_node_sm_f2 = sm_ring_f2[0].next;
351 375
352 376 ring_node_for_averaging_sm_f0 = NULL;
353 377 ring_node_for_averaging_sm_f1 = NULL;
354 378 ring_node_for_averaging_sm_f2 = NULL;
355 379 }
356 380
357 381 //*****************
358 382 // Basic Parameters
359 383
360 384 void BP_init_header( bp_packet *packet,
361 385 unsigned int apid, unsigned char sid,
362 386 unsigned int packetLength, unsigned char blkNr )
363 387 {
364 388 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
365 389 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
366 390 packet->reserved = INIT_CHAR;
367 391 packet->userApplication = CCSDS_USER_APP;
368 392 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
369 393 packet->packetID[1] = (unsigned char) (apid);
370 394 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
371 395 packet->packetSequenceControl[1] = INIT_CHAR;
372 396 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
373 397 packet->packetLength[1] = (unsigned char) (packetLength);
374 398 // DATA FIELD HEADER
375 399 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
376 400 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
377 401 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
378 402 packet->destinationID = TM_DESTINATION_ID_GROUND;
379 403 packet->time[BYTE_0] = INIT_CHAR;
380 404 packet->time[BYTE_1] = INIT_CHAR;
381 405 packet->time[BYTE_2] = INIT_CHAR;
382 406 packet->time[BYTE_3] = INIT_CHAR;
383 407 packet->time[BYTE_4] = INIT_CHAR;
384 408 packet->time[BYTE_5] = INIT_CHAR;
385 409 // AUXILIARY DATA HEADER
386 410 packet->sid = sid;
387 411 packet->pa_bia_status_info = INIT_CHAR;
388 412 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
389 413 packet->sy_lfr_common_parameters = INIT_CHAR;
390 414 packet->acquisitionTime[BYTE_0] = INIT_CHAR;
391 415 packet->acquisitionTime[BYTE_1] = INIT_CHAR;
392 416 packet->acquisitionTime[BYTE_2] = INIT_CHAR;
393 417 packet->acquisitionTime[BYTE_3] = INIT_CHAR;
394 418 packet->acquisitionTime[BYTE_4] = INIT_CHAR;
395 419 packet->acquisitionTime[BYTE_5] = INIT_CHAR;
396 420 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
397 421 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
398 422 }
399 423
400 424 void BP_init_header_with_spare( bp_packet_with_spare *packet,
401 425 unsigned int apid, unsigned char sid,
402 426 unsigned int packetLength , unsigned char blkNr)
403 427 {
404 428 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
405 429 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
406 430 packet->reserved = INIT_CHAR;
407 431 packet->userApplication = CCSDS_USER_APP;
408 432 packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
409 433 packet->packetID[1] = (unsigned char) (apid);
410 434 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
411 435 packet->packetSequenceControl[1] = INIT_CHAR;
412 436 packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
413 437 packet->packetLength[1] = (unsigned char) (packetLength);
414 438 // DATA FIELD HEADER
415 439 packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
416 440 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
417 441 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
418 442 packet->destinationID = TM_DESTINATION_ID_GROUND;
419 443 // AUXILIARY DATA HEADER
420 444 packet->sid = sid;
421 445 packet->pa_bia_status_info = INIT_CHAR;
422 446 packet->sy_lfr_common_parameters_spare = INIT_CHAR;
423 447 packet->sy_lfr_common_parameters = INIT_CHAR;
424 448 packet->time[BYTE_0] = INIT_CHAR;
425 449 packet->time[BYTE_1] = INIT_CHAR;
426 450 packet->time[BYTE_2] = INIT_CHAR;
427 451 packet->time[BYTE_3] = INIT_CHAR;
428 452 packet->time[BYTE_4] = INIT_CHAR;
429 453 packet->time[BYTE_5] = INIT_CHAR;
430 454 packet->source_data_spare = INIT_CHAR;
431 455 packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB
432 456 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
433 457 }
434 458
435 459 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
436 460 {
437 461 rtems_status_code status;
438 462
439 463 // SEND PACKET
440 464 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
441 465 if (status != RTEMS_SUCCESSFUL)
442 466 {
443 467 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
444 468 }
445 469 }
446 470
447 471 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
448 472 {
449 473 /** This function is used to send the BP paquets when needed.
450 474 *
451 475 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
452 476 *
453 477 * @return void
454 478 *
455 479 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
456 480 * BURST paquets are sent everytime.
457 481 *
458 482 */
459 483
460 484 rtems_status_code status;
461 485
462 486 // SEND PACKET
463 487 // before lastValidTransitionDate, the data are drops even if they are ready
464 488 // this guarantees that no SBM packets will be received before the requested enter mode time
465 489 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
466 490 {
467 491 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
468 492 if (status != RTEMS_SUCCESSFUL)
469 493 {
470 494 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
471 495 }
472 496 }
473 497 }
474 498
475 499 //******************
476 500 // general functions
477 501
478 502 void reset_sm_status( void )
479 503 {
480 504 // error
481 505 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
482 506 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
483 507 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
484 508 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
485 509
486 510 spectral_matrix_regs->status = BITS_STATUS_REG; // [0111 1111 1111]
487 511 }
488 512
489 513 void reset_spectral_matrix_regs( void )
490 514 {
491 515 /** This function resets the spectral matrices module registers.
492 516 *
493 517 * The registers affected by this function are located at the following offset addresses:
494 518 *
495 519 * - 0x00 config
496 520 * - 0x04 status
497 521 * - 0x08 matrixF0_Address0
498 522 * - 0x10 matrixFO_Address1
499 523 * - 0x14 matrixF1_Address
500 524 * - 0x18 matrixF2_Address
501 525 *
502 526 */
503 527
504 528 set_sm_irq_onError( 0 );
505 529
506 530 set_sm_irq_onNewMatrix( 0 );
507 531
508 532 reset_sm_status();
509 533
510 534 // F1
511 535 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
512 536 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
513 537 // F2
514 538 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
515 539 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
516 540 // F3
517 541 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
518 542 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
519 543
520 544 spectral_matrix_regs->matrix_length = DEFAULT_MATRIX_LENGTH; // 25 * 128 / 16 = 200 = 0xc8
521 545 }
522 546
523 547 void set_time( unsigned char *time, unsigned char * timeInBuffer )
524 548 {
525 549 time[BYTE_0] = timeInBuffer[BYTE_0];
526 550 time[BYTE_1] = timeInBuffer[BYTE_1];
527 551 time[BYTE_2] = timeInBuffer[BYTE_2];
528 552 time[BYTE_3] = timeInBuffer[BYTE_3];
529 553 time[BYTE_4] = timeInBuffer[BYTE_6];
530 554 time[BYTE_5] = timeInBuffer[BYTE_7];
531 555 }
532 556
533 557 unsigned long long int get_acquisition_time( unsigned char *timePtr )
534 558 {
535 559 unsigned long long int acquisitionTimeAslong;
536 560 acquisitionTimeAslong = INIT_CHAR;
537 561 acquisitionTimeAslong =
538 562 ( (unsigned long long int) (timePtr[BYTE_0] & SYNC_BIT_MASK) << SHIFT_5_BYTES ) // [0111 1111] mask the synchronization bit
539 563 + ( (unsigned long long int) timePtr[BYTE_1] << SHIFT_4_BYTES )
540 564 + ( (unsigned long long int) timePtr[BYTE_2] << SHIFT_3_BYTES )
541 565 + ( (unsigned long long int) timePtr[BYTE_3] << SHIFT_2_BYTES )
542 566 + ( (unsigned long long int) timePtr[BYTE_6] << SHIFT_1_BYTE )
543 567 + ( (unsigned long long int) timePtr[BYTE_7] );
544 568 return acquisitionTimeAslong;
545 569 }
546 570
547 571 unsigned char getSID( rtems_event_set event )
548 572 {
549 573 unsigned char sid;
550 574
551 575 rtems_event_set eventSetBURST;
552 576 rtems_event_set eventSetSBM;
553 577
554 578 sid = 0;
555 579
556 580 //******
557 581 // BURST
558 582 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
559 583 | RTEMS_EVENT_BURST_BP1_F1
560 584 | RTEMS_EVENT_BURST_BP2_F0
561 585 | RTEMS_EVENT_BURST_BP2_F1;
562 586
563 587 //****
564 588 // SBM
565 589 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
566 590 | RTEMS_EVENT_SBM_BP1_F1
567 591 | RTEMS_EVENT_SBM_BP2_F0
568 592 | RTEMS_EVENT_SBM_BP2_F1;
569 593
570 594 if (event & eventSetBURST)
571 595 {
572 596 sid = SID_BURST_BP1_F0;
573 597 }
574 598 else if (event & eventSetSBM)
575 599 {
576 600 sid = SID_SBM1_BP1_F0;
577 601 }
578 602 else
579 603 {
580 604 sid = 0;
581 605 }
582 606
583 607 return sid;
584 608 }
585 609
610 /**
611 * @brief extractReImVectors converts a given ASM component from interleaved to split representation
612 * @param inputASM
613 * @param outputASM
614 * @param asmComponent
615 */
586 616 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
587 617 {
588 618 unsigned int i;
589 619 float re;
590 620 float im;
591 621
592 622 for (i=0; i<NB_BINS_PER_SM; i++){
593 623 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) ];
594 624 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) + 1];
595 625 outputASM[ ( asmComponent *NB_BINS_PER_SM) + i] = re;
596 626 outputASM[ ((asmComponent+1)*NB_BINS_PER_SM) + i] = im;
597 627 }
598 628 }
599 629
630 /**
631 * @brief copyReVectors copies real part of a given ASM from inputASM to outputASM
632 * @param inputASM
633 * @param outputASM
634 * @param asmComponent
635 */
600 636 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
601 637 {
602 638 unsigned int i;
603 639 float re;
604 640
605 641 for (i=0; i<NB_BINS_PER_SM; i++){
606 642 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
607 643 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
608 644 }
609 645 }
610 646
647 /**
648 * @brief ASM_patch, converts ASM from interleaved to split representation
649 * @param inputASM
650 * @param outputASM
651 * @note inputASM and outputASM must be different, in other words this function can't do in place convertion
652 * @see extractReImVectors
653 */
611 654 void ASM_patch( float *inputASM, float *outputASM )
612 655 {
613 656 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B2); // b1b2
614 657 extractReImVectors( inputASM, outputASM, ASM_COMP_B1B3 ); // b1b3
615 658 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E1 ); // b1e1
616 659 extractReImVectors( inputASM, outputASM, ASM_COMP_B1E2 ); // b1e2
617 660 extractReImVectors( inputASM, outputASM, ASM_COMP_B2B3 ); // b2b3
618 661 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E1 ); // b2e1
619 662 extractReImVectors( inputASM, outputASM, ASM_COMP_B2E2 ); // b2e2
620 663 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E1 ); // b3e1
621 664 extractReImVectors( inputASM, outputASM, ASM_COMP_B3E2 ); // b3e2
622 665 extractReImVectors( inputASM, outputASM, ASM_COMP_E1E2 ); // e1e2
623 666
624 667 copyReVectors(inputASM, outputASM, ASM_COMP_B1B1 ); // b1b1
625 668 copyReVectors(inputASM, outputASM, ASM_COMP_B2B2 ); // b2b2
626 669 copyReVectors(inputASM, outputASM, ASM_COMP_B3B3); // b3b3
627 670 copyReVectors(inputASM, outputASM, ASM_COMP_E1E1); // e1e1
628 671 copyReVectors(inputASM, outputASM, ASM_COMP_E2E2); // e2e2
629 672 }
630 673
631 674 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
632 675 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
633 676 unsigned char ASMIndexStart,
634 677 unsigned char channel )
635 678 {
636 679 //*************
637 680 // input format
638 681 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
639 682 //**************
640 683 // output format
641 684 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
642 685 //************
643 686 // compression
644 687 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
645 688 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
646 689
647 690 int frequencyBin;
648 691 int asmComponent;
649 692 int offsetASM;
650 693 int offsetCompressed;
651 694 int offsetFBin;
652 695 int fBinMask;
653 696 int k;
654 697
655 698 // BUILD DATA
656 699 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
657 700 {
658 701 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
659 702 {
660 703 offsetCompressed = // NO TIME OFFSET
661 704 (frequencyBin * NB_VALUES_PER_SM)
662 705 + asmComponent;
663 706 offsetASM = // NO TIME OFFSET
664 707 (asmComponent * NB_BINS_PER_SM)
665 708 + ASMIndexStart
666 709 + (frequencyBin * nbBinsToAverage);
667 710 offsetFBin = ASMIndexStart
668 711 + (frequencyBin * nbBinsToAverage);
669 712 compressed_spec_mat[ offsetCompressed ] = 0;
670 713 for ( k = 0; k < nbBinsToAverage; k++ )
671 714 {
672 715 fBinMask = getFBinMask( offsetFBin + k, channel );
673 716 compressed_spec_mat[offsetCompressed ] = compressed_spec_mat[ offsetCompressed ]
674 717 + (averaged_spec_mat[ offsetASM + k ] * fBinMask);
675 718 }
676 719 if (divider != 0)
677 720 {
678 721 compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
679 722 }
680 723 else
681 724 {
682 725 compressed_spec_mat[ offsetCompressed ] = INIT_FLOAT;
683 726 }
684 727 }
685 728 }
686 729
687 730 }
688 731
689 732 int getFBinMask( int index, unsigned char channel )
690 733 {
691 734 unsigned int indexInChar;
692 735 unsigned int indexInTheChar;
693 736 int fbin;
694 737 unsigned char *sy_lfr_fbins_fx_word1;
695 738
696 739 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
697 740
698 741 switch(channel)
699 742 {
700 743 case CHANNELF0:
701 744 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
702 745 break;
703 746 case CHANNELF1:
704 747 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
705 748 break;
706 749 case CHANNELF2:
707 750 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
708 751 break;
709 752 default:
710 753 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
711 754 }
712 755
713 756 indexInChar = index >> SHIFT_3_BITS;
714 757 indexInTheChar = index - (indexInChar * BITS_PER_BYTE);
715 758
716 759 fbin = (int) ((sy_lfr_fbins_fx_word1[ BYTES_PER_MASK - 1 - indexInChar] >> indexInTheChar) & 1);
717 760
718 761 return fbin;
719 762 }
720 763
764 /**
765 * @brief isPolluted returns MATRIX_IS_POLLUTED if there is any overlap between t0:t1 and tbad0:tbad1 ranges
766 * @param t0 Start acquisition time
767 * @param t1 End of acquisition time
768 * @param tbad0 Start time of poluting signal
769 * @param tbad1 End time of poluting signal
770 * @return
771 */
721 772 unsigned char isPolluted( u_int64_t t0, u_int64_t t1, u_int64_t tbad0, u_int64_t tbad1 )
722 773 {
723 774 unsigned char polluted;
724 775
725 776 polluted = MATRIX_IS_NOT_POLLUTED;
726 777
727 778 if ( ((tbad0 < t0) && (t0 < tbad1)) // t0 is inside the polluted range
728 779 || ((tbad0 < t1) && (t1 < tbad1)) // t1 is inside the polluted range
729 780 || ((t0 < tbad0) && (tbad1 < t1)) // the polluted range is inside the signal range
730 781 || ((tbad0 < t0) && (t1 < tbad1))) // the signal range is inside the polluted range
731 782 {
732 783 polluted = MATRIX_IS_POLLUTED;
733 784 }
734 785
735 786 return polluted;
736 787 }
737 788
789 /**
790 * @brief acquisitionTimeIsValid checks if the given acquisition time is poluted by PAS
791 * @param coarseTime Coarse acquisition time of the given SM
792 * @param fineTime Fine acquisition time of the given ASM
793 * @param channel Frequency channel to check, will impact SM time footprint
794 * @return MATRIX_IS_POLLUTED if there is any time overlap between SM and PAS poluting signal
795 */
738 796 unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel)
739 797 {
740 798 u_int64_t t0;
741 799 u_int64_t t1;
742 800 u_int64_t tc;
743 801 u_int64_t tbad0;
744 802 u_int64_t tbad1;
745 803
746 804 u_int64_t modulusInFineTime;
747 805 u_int64_t offsetInFineTime;
748 806 u_int64_t shiftInFineTime;
749 807 u_int64_t tbadInFineTime;
750 808
751 809 u_int64_t timecodeReference;
752 810
753 811 unsigned char pasFilteringIsEnabled;
754 812 unsigned char ret;
755 813
756 814 // compute acquisition time from caoarseTime and fineTime
757 815 t0 = ( ((u_int64_t)coarseTime) << SHIFT_2_BYTES ) + (u_int64_t) fineTime;
758 816 t1 = t0;
759 817 tc = t0;
760 818 tbad0 = t0;
761 819 tbad1 = t0;
762 820
763 821 switch(channel)
764 822 {
765 823 case CHANNELF0:
766 824 t1 = t0 + ACQUISITION_DURATION_F0;
767 825 tc = t0 + HALF_ACQUISITION_DURATION_F0;
768 826 break;
769 827 case CHANNELF1:
770 828 t1 = t0 + ACQUISITION_DURATION_F1;
771 829 tc = t0 + HALF_ACQUISITION_DURATION_F1;
772 830 break;
773 831 case CHANNELF2:
774 832 t1 = t0 + ACQUISITION_DURATION_F2;
775 833 tc = t0 + HALF_ACQUISITION_DURATION_F2;
776 834 break;
777 835 default:
778 836 break;
779 837 }
780 838
781 839 // compute the acquitionTime range
782 840 modulusInFineTime = filterPar.modulus_in_finetime;
783 841 offsetInFineTime = filterPar.offset_in_finetime;
784 842 shiftInFineTime = filterPar.shift_in_finetime;
785 843 tbadInFineTime = filterPar.tbad_in_finetime;
786 844 timecodeReference = INIT_INT;
787 845
788 846 pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001]
789 847 ret = MATRIX_IS_NOT_POLLUTED;
790 848
791 849 if ( (tbadInFineTime == 0) || (pasFilteringIsEnabled == 0) )
792 850 {
793 851 ret = MATRIX_IS_NOT_POLLUTED;
794 852 }
795 853 else
796 854 {
797 855 // INTERSECTION TEST #1
798 856 timecodeReference = (tc - (tc % modulusInFineTime)) - modulusInFineTime ;
799 857 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
800 858 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
801 859 ret = isPolluted( t0, t1, tbad0, tbad1 );
802 860
803 861 // INTERSECTION TEST #2
804 862 if (ret == MATRIX_IS_NOT_POLLUTED)
805 863 {
806 864 timecodeReference = (tc - (tc % modulusInFineTime)) ;
807 865 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
808 866 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
809 867 ret = isPolluted( t0, t1, tbad0, tbad1 );
810 868 }
811 869
812 870 // INTERSECTION TEST #3
813 871 if (ret == MATRIX_IS_NOT_POLLUTED)
814 872 {
815 873 timecodeReference = (tc - (tc % modulusInFineTime)) + modulusInFineTime ;
816 874 tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
817 875 tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
818 876 ret = isPolluted( t0, t1, tbad0, tbad1 );
819 877 }
820 878 }
821 879
822 880 return ret;
823 881 }
824 882
825 883 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
826 884 {
827 885 unsigned char bin;
828 886 unsigned char kcoeff;
829 887
830 888 for (bin=0; bin<nb_bins_norm; bin++)
831 889 {
832 890 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
833 891 {
834 892 output_kcoeff[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF ]
835 893 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
836 894 output_kcoeff[ ( ( (bin * NB_K_COEFF_PER_BIN ) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ) + 1 ]
837 895 = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
838 896 }
839 897 }
840 898 }
Show file before | Show file after | Hide comments
@@ -1,481 +1,504
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
1 24 /** Functions related to TeleCommand acceptance.
2 25 *
3 26 * @file
4 27 * @author P. LEROY
5 28 *
6 29 * A group of functions to handle TeleCommands parsing.\n
7 30 *
8 31 */
9 32
10 33 #include "tc_acceptance.h"
11 34 #include <stdio.h>
12 35
13 36 unsigned int lookUpTableForCRC[CONST_256] = {0};
14 37
15 38 //**********************
16 39 // GENERAL USE FUNCTIONS
17 40 unsigned int Crc_opt( unsigned char D, unsigned int Chk)
18 41 {
19 42 /** This function generate the CRC for one byte and returns the value of the new syndrome.
20 43 *
21 44 * @param D is the current byte of data.
22 45 * @param Chk is the current syndrom value.
23 46 *
24 47 * @return the value of the new syndrome on two bytes.
25 48 *
26 49 */
27 50
28 51 return(((Chk << SHIFT_1_BYTE) & BYTE0_MASK)^lookUpTableForCRC [(((Chk >> SHIFT_1_BYTE)^D) & BYTE1_MASK)]);
29 52 }
30 53
31 54 void initLookUpTableForCRC( void )
32 55 {
33 56 /** This function is used to initiates the look-up table for fast CRC computation.
34 57 *
35 58 * The global table lookUpTableForCRC[256] is initiated.
36 59 *
37 60 */
38 61
39 62 unsigned int i;
40 63 unsigned int tmp;
41 64
42 65 for (i=0; i<CONST_256; i++)
43 66 {
44 67 tmp = 0;
45 68 if((i & BIT_0) != 0) {
46 69 tmp = tmp ^ CONST_CRC_0;
47 70 }
48 71 if((i & BIT_1) != 0) {
49 72 tmp = tmp ^ CONST_CRC_1;
50 73 }
51 74 if((i & BIT_2) != 0) {
52 75 tmp = tmp ^ CONST_CRC_2;
53 76 }
54 77 if((i & BIT_3) != 0) {
55 78 tmp = tmp ^ CONST_CRC_3;
56 79 }
57 80 if((i & BIT_4) != 0) {
58 81 tmp = tmp ^ CONST_CRC_4;
59 82 }
60 83 if((i & BIT_5) != 0) {
61 84 tmp = tmp ^ CONST_CRC_5;
62 85 }
63 86 if((i & BIT_6) != 0) {
64 87 tmp = tmp ^ CONST_CRC_6;
65 88 }
66 89 if((i & BIT_7) != 0) {
67 90 tmp = tmp ^ CONST_CRC_7;
68 91 }
69 92 lookUpTableForCRC[i] = tmp;
70 93 }
71 94 }
72 95
73 96 void GetCRCAsTwoBytes(unsigned char* data, unsigned char* crcAsTwoBytes, unsigned int sizeOfData)
74 97 {
75 98 /** This function calculates a two bytes Cyclic Redundancy Code.
76 99 *
77 100 * @param data points to a buffer containing the data on which to compute the CRC.
78 101 * @param crcAsTwoBytes points points to a two bytes buffer in which the CRC is stored.
79 102 * @param sizeOfData is the number of bytes of *data* used to compute the CRC.
80 103 *
81 104 * The specification of the Cyclic Redundancy Code is described in the following document: ECSS-E-70-41-A.
82 105 *
83 106 */
84 107
85 108 unsigned int Chk;
86 109 int j;
87 110 Chk = CRC_RESET; // reset the syndrom to all ones
88 111 for (j=0; j<sizeOfData; j++) {
89 112 Chk = Crc_opt(data[j], Chk);
90 113 }
91 114 crcAsTwoBytes[0] = (unsigned char) (Chk >> SHIFT_1_BYTE);
92 115 crcAsTwoBytes[1] = (unsigned char) (Chk & BYTE1_MASK);
93 116 }
94 117
95 118 //*********************
96 119 // ACCEPTANCE FUNCTIONS
97 120 int tc_parser(ccsdsTelecommandPacket_t * TCPacket, unsigned int estimatedPacketLength, unsigned char *computed_CRC)
98 121 {
99 122 /** This function parses TeleCommands.
100 123 *
101 124 * @param TC points to the TeleCommand that will be parsed.
102 125 * @param estimatedPacketLength is the PACKET_LENGTH field calculated from the effective length of the received packet.
103 126 *
104 127 * @return Status code of the parsing.
105 128 *
106 129 * The parsing checks:
107 130 * - process id
108 131 * - category
109 132 * - length: a global check is performed and a per subtype check also
110 133 * - type
111 134 * - subtype
112 135 * - crc
113 136 *
114 137 */
115 138
116 139 int status;
117 140 int status_crc;
118 141 unsigned char pid;
119 142 unsigned char category;
120 143 unsigned int packetLength;
121 144 unsigned char packetType;
122 145 unsigned char packetSubtype;
123 146 unsigned char sid;
124 147
125 148 status = CCSDS_TM_VALID;
126 149
127 150 // APID check *** APID on 2 bytes
128 151 pid = ((TCPacket->packetID[0] & BITS_PID_0) << SHIFT_4_BITS)
129 152 + ( (TCPacket->packetID[1] >> SHIFT_4_BITS) & BITS_PID_1 ); // PID = 11 *** 7 bits xxxxx210 7654xxxx
130 153 category = (TCPacket->packetID[1] & BITS_CAT); // PACKET_CATEGORY = 12 *** 4 bits xxxxxxxx xxxx3210
131 154 packetLength = (TCPacket->packetLength[0] * CONST_256) + TCPacket->packetLength[1];
132 155 packetType = TCPacket->serviceType;
133 156 packetSubtype = TCPacket->serviceSubType;
134 157 sid = TCPacket->sourceID;
135 158
136 159 if ( pid != CCSDS_PROCESS_ID ) // CHECK THE PROCESS ID
137 160 {
138 161 status = ILLEGAL_APID;
139 162 }
140 163 if (status == CCSDS_TM_VALID) // CHECK THE CATEGORY
141 164 {
142 165 if ( category != CCSDS_PACKET_CATEGORY )
143 166 {
144 167 status = ILLEGAL_APID;
145 168 }
146 169 }
147 170 if (status == CCSDS_TM_VALID) // CHECK THE PACKET_LENGTH FIELD AND THE ESTIMATED PACKET_LENGTH COMPLIANCE
148 171 {
149 172 if (packetLength != estimatedPacketLength ) {
150 173 status = WRONG_LEN_PKT;
151 174 }
152 175 }
153 176 if (status == CCSDS_TM_VALID) // CHECK THAT THE PACKET DOES NOT EXCEED THE MAX SIZE
154 177 {
155 178 if ( packetLength > CCSDS_TC_PKT_MAX_SIZE ) {
156 179 status = WRONG_LEN_PKT;
157 180 }
158 181 }
159 182 if (status == CCSDS_TM_VALID) // CHECK THE TYPE
160 183 {
161 184 status = tc_check_type( packetType );
162 185 }
163 186 if (status == CCSDS_TM_VALID) // CHECK THE SUBTYPE
164 187 {
165 188 status = tc_check_type_subtype( packetType, packetSubtype );
166 189 }
167 190 if (status == CCSDS_TM_VALID) // CHECK THE SID
168 191 {
169 192 status = tc_check_sid( sid );
170 193 }
171 194 if (status == CCSDS_TM_VALID) // CHECK THE SUBTYPE AND LENGTH COMPLIANCE
172 195 {
173 196 status = tc_check_length( packetSubtype, packetLength );
174 197 }
175 198 status_crc = tc_check_crc( TCPacket, estimatedPacketLength, computed_CRC );
176 199 if (status == CCSDS_TM_VALID ) // CHECK CRC
177 200 {
178 201 status = status_crc;
179 202 }
180 203
181 204 return status;
182 205 }
183 206
184 207 int tc_check_type( unsigned char packetType )
185 208 {
186 209 /** This function checks that the type of a TeleCommand is valid.
187 210 *
188 211 * @param packetType is the type to check.
189 212 *
190 213 * @return Status code CCSDS_TM_VALID or ILL_TYPE.
191 214 *
192 215 */
193 216
194 217 int status;
195 218
196 219 status = ILL_TYPE;
197 220
198 221 if ( (packetType == TC_TYPE_GEN) || (packetType == TC_TYPE_TIME))
199 222 {
200 223 status = CCSDS_TM_VALID;
201 224 }
202 225 else
203 226 {
204 227 status = ILL_TYPE;
205 228 }
206 229
207 230 return status;
208 231 }
209 232
210 233 int tc_check_type_subtype( unsigned char packetType, unsigned char packetSubType )
211 234 {
212 235 /** This function checks that the subtype of a TeleCommand is valid and coherent with the type.
213 236 *
214 237 * @param packetType is the type of the TC.
215 238 * @param packetSubType is the subtype to check.
216 239 *
217 240 * @return Status code CCSDS_TM_VALID or ILL_SUBTYPE.
218 241 *
219 242 */
220 243
221 244 int status;
222 245
223 246 switch(packetType)
224 247 {
225 248 case TC_TYPE_GEN:
226 249 if ( (packetSubType == TC_SUBTYPE_RESET)
227 250 || (packetSubType == TC_SUBTYPE_LOAD_COMM)
228 251 || (packetSubType == TC_SUBTYPE_LOAD_NORM) || (packetSubType == TC_SUBTYPE_LOAD_BURST)
229 252 || (packetSubType == TC_SUBTYPE_LOAD_SBM1) || (packetSubType == TC_SUBTYPE_LOAD_SBM2)
230 253 || (packetSubType == TC_SUBTYPE_DUMP)
231 254 || (packetSubType == TC_SUBTYPE_ENTER)
232 255 || (packetSubType == TC_SUBTYPE_UPDT_INFO)
233 256 || (packetSubType == TC_SUBTYPE_EN_CAL) || (packetSubType == TC_SUBTYPE_DIS_CAL)
234 257 || (packetSubType == TC_SUBTYPE_LOAD_K) || (packetSubType == TC_SUBTYPE_DUMP_K)
235 258 || (packetSubType == TC_SUBTYPE_LOAD_FBINS)
236 259 || (packetSubType == TC_SUBTYPE_LOAD_FILTER_PAR))
237 260 {
238 261 status = CCSDS_TM_VALID;
239 262 }
240 263 else
241 264 {
242 265 status = ILL_SUBTYPE;
243 266 }
244 267 break;
245 268
246 269 case TC_TYPE_TIME:
247 270 if (packetSubType == TC_SUBTYPE_UPDT_TIME)
248 271 {
249 272 status = CCSDS_TM_VALID;
250 273 }
251 274 else
252 275 {
253 276 status = ILL_SUBTYPE;
254 277 }
255 278 break;
256 279
257 280 default:
258 281 status = ILL_SUBTYPE;
259 282 break;
260 283 }
261 284
262 285 return status;
263 286 }
264 287
265 288 int tc_check_sid( unsigned char sid )
266 289 {
267 290 /** This function checks that the sid of a TeleCommand is valid.
268 291 *
269 292 * @param sid is the sid to check.
270 293 *
271 294 * @return Status code CCSDS_TM_VALID or CORRUPTED.
272 295 *
273 296 */
274 297
275 298 int status;
276 299
277 300 status = WRONG_SRC_ID;
278 301
279 302 if ( (sid == SID_TC_MISSION_TIMELINE) || (sid == SID_TC_TC_SEQUENCES) || (sid == SID_TC_RECOVERY_ACTION_CMD)
280 303 || (sid == SID_TC_BACKUP_MISSION_TIMELINE)
281 304 || (sid == SID_TC_DIRECT_CMD) || (sid == SID_TC_SPARE_GRD_SRC1) || (sid == SID_TC_SPARE_GRD_SRC2)
282 305 || (sid == SID_TC_OBCP) || (sid == SID_TC_SYSTEM_CONTROL) || (sid == SID_TC_AOCS)
283 306 || (sid == SID_TC_RPW_INTERNAL))
284 307 {
285 308 status = CCSDS_TM_VALID;
286 309 }
287 310 else
288 311 {
289 312 status = WRONG_SRC_ID;
290 313 }
291 314
292 315 return status;
293 316 }
294 317
295 318 int tc_check_length( unsigned char packetSubType, unsigned int length )
296 319 {
297 320 /** This function checks that the subtype and the length are compliant.
298 321 *
299 322 * @param packetSubType is the subtype to check.
300 323 * @param length is the length to check.
301 324 *
302 325 * @return Status code CCSDS_TM_VALID or ILL_TYPE.
303 326 *
304 327 */
305 328
306 329 int status;
307 330
308 331 status = LFR_SUCCESSFUL;
309 332
310 333 switch(packetSubType)
311 334 {
312 335 case TC_SUBTYPE_RESET:
313 336 if (length!=(TC_LEN_RESET-CCSDS_TC_TM_PACKET_OFFSET)) {
314 337 status = WRONG_LEN_PKT;
315 338 }
316 339 else {
317 340 status = CCSDS_TM_VALID;
318 341 }
319 342 break;
320 343 case TC_SUBTYPE_LOAD_COMM:
321 344 if (length!=(TC_LEN_LOAD_COMM-CCSDS_TC_TM_PACKET_OFFSET)) {
322 345 status = WRONG_LEN_PKT;
323 346 }
324 347 else {
325 348 status = CCSDS_TM_VALID;
326 349 }
327 350 break;
328 351 case TC_SUBTYPE_LOAD_NORM:
329 352 if (length!=(TC_LEN_LOAD_NORM-CCSDS_TC_TM_PACKET_OFFSET)) {
330 353 status = WRONG_LEN_PKT;
331 354 }
332 355 else {
333 356 status = CCSDS_TM_VALID;
334 357 }
335 358 break;
336 359 case TC_SUBTYPE_LOAD_BURST:
337 360 if (length!=(TC_LEN_LOAD_BURST-CCSDS_TC_TM_PACKET_OFFSET)) {
338 361 status = WRONG_LEN_PKT;
339 362 }
340 363 else {
341 364 status = CCSDS_TM_VALID;
342 365 }
343 366 break;
344 367 case TC_SUBTYPE_LOAD_SBM1:
345 368 if (length!=(TC_LEN_LOAD_SBM1-CCSDS_TC_TM_PACKET_OFFSET)) {
346 369 status = WRONG_LEN_PKT;
347 370 }
348 371 else {
349 372 status = CCSDS_TM_VALID;
350 373 }
351 374 break;
352 375 case TC_SUBTYPE_LOAD_SBM2:
353 376 if (length!=(TC_LEN_LOAD_SBM2-CCSDS_TC_TM_PACKET_OFFSET)) {
354 377 status = WRONG_LEN_PKT;
355 378 }
356 379 else {
357 380 status = CCSDS_TM_VALID;
358 381 }
359 382 break;
360 383 case TC_SUBTYPE_DUMP:
361 384 if (length!=(TC_LEN_DUMP-CCSDS_TC_TM_PACKET_OFFSET)) {
362 385 status = WRONG_LEN_PKT;
363 386 }
364 387 else {
365 388 status = CCSDS_TM_VALID;
366 389 }
367 390 break;
368 391 case TC_SUBTYPE_ENTER:
369 392 if (length!=(TC_LEN_ENTER-CCSDS_TC_TM_PACKET_OFFSET)) {
370 393 status = WRONG_LEN_PKT;
371 394 }
372 395 else {
373 396 status = CCSDS_TM_VALID;
374 397 }
375 398 break;
376 399 case TC_SUBTYPE_UPDT_INFO:
377 400 if (length!=(TC_LEN_UPDT_INFO-CCSDS_TC_TM_PACKET_OFFSET)) {
378 401 status = WRONG_LEN_PKT;
379 402 }
380 403 else {
381 404 status = CCSDS_TM_VALID;
382 405 }
383 406 break;
384 407 case TC_SUBTYPE_EN_CAL:
385 408 if (length!=(TC_LEN_EN_CAL-CCSDS_TC_TM_PACKET_OFFSET)) {
386 409 status = WRONG_LEN_PKT;
387 410 }
388 411 else {
389 412 status = CCSDS_TM_VALID;
390 413 }
391 414 break;
392 415 case TC_SUBTYPE_DIS_CAL:
393 416 if (length!=(TC_LEN_DIS_CAL-CCSDS_TC_TM_PACKET_OFFSET)) {
394 417 status = WRONG_LEN_PKT;
395 418 }
396 419 else {
397 420 status = CCSDS_TM_VALID;
398 421 }
399 422 break;
400 423 case TC_SUBTYPE_LOAD_K:
401 424 if (length!=(TC_LEN_LOAD_K-CCSDS_TC_TM_PACKET_OFFSET)) {
402 425 status = WRONG_LEN_PKT;
403 426 }
404 427 else {
405 428 status = CCSDS_TM_VALID;
406 429 }
407 430 break;
408 431 case TC_SUBTYPE_DUMP_K:
409 432 if (length!=(TC_LEN_DUMP_K-CCSDS_TC_TM_PACKET_OFFSET)) {
410 433 status = WRONG_LEN_PKT;
411 434 }
412 435 else {
413 436 status = CCSDS_TM_VALID;
414 437 }
415 438 break;
416 439 case TC_SUBTYPE_LOAD_FBINS:
417 440 if (length!=(TC_LEN_LOAD_FBINS-CCSDS_TC_TM_PACKET_OFFSET)) {
418 441 status = WRONG_LEN_PKT;
419 442 }
420 443 else {
421 444 status = CCSDS_TM_VALID;
422 445 }
423 446 break;
424 447 case TC_SUBTYPE_LOAD_FILTER_PAR:
425 448 if (length!=(TC_LEN_LOAD_FILTER_PAR-CCSDS_TC_TM_PACKET_OFFSET)) {
426 449 status = WRONG_LEN_PKT;
427 450 }
428 451 else {
429 452 status = CCSDS_TM_VALID;
430 453 }
431 454 break;
432 455 case TC_SUBTYPE_UPDT_TIME:
433 456 if (length!=(TC_LEN_UPDT_TIME-CCSDS_TC_TM_PACKET_OFFSET)) {
434 457 status = WRONG_LEN_PKT;
435 458 }
436 459 else {
437 460 status = CCSDS_TM_VALID;
438 461 }
439 462 break;
440 463 default: // if the subtype is not a legal value, return ILL_SUBTYPE
441 464 status = ILL_SUBTYPE;
442 465 break ;
443 466 }
444 467
445 468 return status;
446 469 }
447 470
448 471 int tc_check_crc( ccsdsTelecommandPacket_t * TCPacket, unsigned int length, unsigned char *computed_CRC )
449 472 {
450 473 /** This function checks the CRC validity of the corresponding TeleCommand packet.
451 474 *
452 475 * @param TCPacket points to the TeleCommand packet to check.
453 476 * @param length is the length of the TC packet.
454 477 *
455 478 * @return Status code CCSDS_TM_VALID or INCOR_CHECKSUM.
456 479 *
457 480 */
458 481
459 482 int status;
460 483 unsigned char * CCSDSContent;
461 484
462 485 status = INCOR_CHECKSUM;
463 486
464 487 CCSDSContent = (unsigned char*) TCPacket->packetID;
465 488 GetCRCAsTwoBytes(CCSDSContent, computed_CRC, length + CCSDS_TC_TM_PACKET_OFFSET - BYTES_PER_CRC); // 2 CRC bytes removed from the calculation of the CRC
466 489
467 490 if (computed_CRC[0] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET - BYTES_PER_CRC]) {
468 491 status = INCOR_CHECKSUM;
469 492 }
470 493 else if (computed_CRC[1] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -1]) {
471 494 status = INCOR_CHECKSUM;
472 495 }
473 496 else {
474 497 status = CCSDS_TM_VALID;
475 498 }
476 499
477 500 return status;
478 501 }
479 502
480 503
481 504
Show file before | Show file after | Hide comments
@@ -1,1673 +1,1696
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
1 24 /** Functions and tasks related to TeleCommand handling.
2 25 *
3 26 * @file
4 27 * @author P. LEROY
5 28 *
6 29 * A group of functions to handle TeleCommands:\n
7 30 * action launching\n
8 31 * TC parsing\n
9 32 * ...
10 33 *
11 34 */
12 35
13 36 #include "tc_handler.h"
14 37 #include "math.h"
15 38
16 39 //***********
17 40 // RTEMS TASK
18 41
19 42 rtems_task actn_task( rtems_task_argument unused )
20 43 {
21 44 /** This RTEMS task is responsible for launching actions upton the reception of valid TeleCommands.
22 45 *
23 46 * @param unused is the starting argument of the RTEMS task
24 47 *
25 48 * The ACTN task waits for data coming from an RTEMS msesage queue. When data arrives, it launches specific actions depending
26 49 * on the incoming TeleCommand.
27 50 *
28 51 */
29 52
30 53 int result;
31 54 rtems_status_code status; // RTEMS status code
32 55 ccsdsTelecommandPacket_t __attribute__((aligned(4))) TC; // TC sent to the ACTN task
33 56 size_t size; // size of the incoming TC packet
34 57 unsigned char subtype; // subtype of the current TC packet
35 58 unsigned char time[BYTES_PER_TIME];
36 59 rtems_id queue_rcv_id;
37 60 rtems_id queue_snd_id;
38 61
39 62 memset(&TC, 0, sizeof(ccsdsTelecommandPacket_t));
40 63 size = 0;
41 64 queue_rcv_id = RTEMS_ID_NONE;
42 65 queue_snd_id = RTEMS_ID_NONE;
43 66
44 67 status = get_message_queue_id_recv( &queue_rcv_id );
45 68 if (status != RTEMS_SUCCESSFUL)
46 69 {
47 70 PRINTF1("in ACTN *** ERR get_message_queue_id_recv %d\n", status)
48 71 }
49 72
50 73 status = get_message_queue_id_send( &queue_snd_id );
51 74 if (status != RTEMS_SUCCESSFUL)
52 75 {
53 76 PRINTF1("in ACTN *** ERR get_message_queue_id_send %d\n", status)
54 77 }
55 78
56 79 result = LFR_SUCCESSFUL;
57 80 subtype = 0; // subtype of the current TC packet
58 81
59 82 BOOT_PRINTF("in ACTN *** \n");
60 83
61 84 while(1)
62 85 {
63 86 status = rtems_message_queue_receive( queue_rcv_id, (char*) &TC, &size,
64 87 RTEMS_WAIT, RTEMS_NO_TIMEOUT);
65 88 getTime( time ); // set time to the current time
66 89 if (status!=RTEMS_SUCCESSFUL)
67 90 {
68 91 PRINTF1("ERR *** in task ACTN *** error receiving a message, code %d \n", status)
69 92 }
70 93 else
71 94 {
72 95 subtype = TC.serviceSubType;
73 96 switch(subtype)
74 97 {
75 98 case TC_SUBTYPE_RESET:
76 99 result = action_reset( &TC, queue_snd_id, time );
77 100 close_action( &TC, result, queue_snd_id );
78 101 break;
79 102 case TC_SUBTYPE_LOAD_COMM:
80 103 result = action_load_common_par( &TC );
81 104 close_action( &TC, result, queue_snd_id );
82 105 break;
83 106 case TC_SUBTYPE_LOAD_NORM:
84 107 result = action_load_normal_par( &TC, queue_snd_id, time );
85 108 close_action( &TC, result, queue_snd_id );
86 109 break;
87 110 case TC_SUBTYPE_LOAD_BURST:
88 111 result = action_load_burst_par( &TC, queue_snd_id, time );
89 112 close_action( &TC, result, queue_snd_id );
90 113 break;
91 114 case TC_SUBTYPE_LOAD_SBM1:
92 115 result = action_load_sbm1_par( &TC, queue_snd_id, time );
93 116 close_action( &TC, result, queue_snd_id );
94 117 break;
95 118 case TC_SUBTYPE_LOAD_SBM2:
96 119 result = action_load_sbm2_par( &TC, queue_snd_id, time );
97 120 close_action( &TC, result, queue_snd_id );
98 121 break;
99 122 case TC_SUBTYPE_DUMP:
100 123 result = action_dump_par( &TC, queue_snd_id );
101 124 close_action( &TC, result, queue_snd_id );
102 125 break;
103 126 case TC_SUBTYPE_ENTER:
104 127 result = action_enter_mode( &TC, queue_snd_id );
105 128 close_action( &TC, result, queue_snd_id );
106 129 break;
107 130 case TC_SUBTYPE_UPDT_INFO:
108 131 result = action_update_info( &TC, queue_snd_id );
109 132 close_action( &TC, result, queue_snd_id );
110 133 break;
111 134 case TC_SUBTYPE_EN_CAL:
112 135 result = action_enable_calibration( &TC, queue_snd_id, time );
113 136 close_action( &TC, result, queue_snd_id );
114 137 break;
115 138 case TC_SUBTYPE_DIS_CAL:
116 139 result = action_disable_calibration( &TC, queue_snd_id, time );
117 140 close_action( &TC, result, queue_snd_id );
118 141 break;
119 142 case TC_SUBTYPE_LOAD_K:
120 143 result = action_load_kcoefficients( &TC, queue_snd_id, time );
121 144 close_action( &TC, result, queue_snd_id );
122 145 break;
123 146 case TC_SUBTYPE_DUMP_K:
124 147 result = action_dump_kcoefficients( &TC, queue_snd_id, time );
125 148 close_action( &TC, result, queue_snd_id );
126 149 break;
127 150 case TC_SUBTYPE_LOAD_FBINS:
128 151 result = action_load_fbins_mask( &TC, queue_snd_id, time );
129 152 close_action( &TC, result, queue_snd_id );
130 153 break;
131 154 case TC_SUBTYPE_LOAD_FILTER_PAR:
132 155 result = action_load_filter_par( &TC, queue_snd_id, time );
133 156 close_action( &TC, result, queue_snd_id );
134 157 break;
135 158 case TC_SUBTYPE_UPDT_TIME:
136 159 result = action_update_time( &TC );
137 160 close_action( &TC, result, queue_snd_id );
138 161 break;
139 162 default:
140 163 break;
141 164 }
142 165 }
143 166 }
144 167 }
145 168
146 169 //***********
147 170 // TC ACTIONS
148 171
149 172 int action_reset(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
150 173 {
151 174 /** This function executes specific actions when a TC_LFR_RESET TeleCommand has been received.
152 175 *
153 176 * @param TC points to the TeleCommand packet that is being processed
154 177 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
155 178 *
156 179 */
157 180
158 181 PRINTF("this is the end!!!\n");
159 182 exit(0);
160 183
161 184 send_tm_lfr_tc_exe_not_implemented( TC, queue_id, time );
162 185
163 186 return LFR_DEFAULT;
164 187 }
165 188
166 189 int action_enter_mode(ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
167 190 {
168 191 /** This function executes specific actions when a TC_LFR_ENTER_MODE TeleCommand has been received.
169 192 *
170 193 * @param TC points to the TeleCommand packet that is being processed
171 194 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
172 195 *
173 196 */
174 197
175 198 rtems_status_code status;
176 199 unsigned char requestedMode;
177 200 unsigned int transitionCoarseTime;
178 201 unsigned char * bytePosPtr;
179 202
180 203 bytePosPtr = (unsigned char *) &TC->packetID;
181 204 requestedMode = bytePosPtr[ BYTE_POS_CP_MODE_LFR_SET ];
182 205 copyInt32ByChar( (char*) &transitionCoarseTime, &bytePosPtr[ BYTE_POS_CP_LFR_ENTER_MODE_TIME ] );
183 206 transitionCoarseTime = transitionCoarseTime & COARSE_TIME_MASK;
184 207 status = check_mode_value( requestedMode );
185 208
186 209 if ( status != LFR_SUCCESSFUL ) // the mode value is inconsistent
187 210 {
188 211 send_tm_lfr_tc_exe_inconsistent( TC, queue_id, BYTE_POS_CP_MODE_LFR_SET, requestedMode );
189 212 }
190 213
191 214 else // the mode value is valid, check the transition
192 215 {
193 216 status = check_mode_transition(requestedMode);
194 217 if (status != LFR_SUCCESSFUL)
195 218 {
196 219 PRINTF("ERR *** in action_enter_mode *** check_mode_transition\n")
197 220 send_tm_lfr_tc_exe_not_executable( TC, queue_id );
198 221 }
199 222 }
200 223
201 224 if ( status == LFR_SUCCESSFUL ) // the transition is valid, check the date
202 225 {
203 226 status = check_transition_date( transitionCoarseTime );
204 227 if (status != LFR_SUCCESSFUL)
205 228 {
206 229 PRINTF("ERR *** in action_enter_mode *** check_transition_date\n");
207 230 send_tm_lfr_tc_exe_not_executable(TC, queue_id );
208 231 }
209 232 }
210 233
211 234 if ( status == LFR_SUCCESSFUL ) // the date is valid, enter the mode
212 235 {
213 236 PRINTF1("OK *** in action_enter_mode *** enter mode %d\n", requestedMode);
214 237
215 238 switch(requestedMode)
216 239 {
217 240 case LFR_MODE_STANDBY:
218 241 status = enter_mode_standby();
219 242 break;
220 243 case LFR_MODE_NORMAL:
221 244 status = enter_mode_normal( transitionCoarseTime );
222 245 break;
223 246 case LFR_MODE_BURST:
224 247 status = enter_mode_burst( transitionCoarseTime );
225 248 break;
226 249 case LFR_MODE_SBM1:
227 250 status = enter_mode_sbm1( transitionCoarseTime );
228 251 break;
229 252 case LFR_MODE_SBM2:
230 253 status = enter_mode_sbm2( transitionCoarseTime );
231 254 break;
232 255 default:
233 256 break;
234 257 }
235 258
236 259 if (status != RTEMS_SUCCESSFUL)
237 260 {
238 261 status = LFR_EXE_ERROR;
239 262 }
240 263 }
241 264
242 265 return status;
243 266 }
244 267
245 268 int action_update_info(ccsdsTelecommandPacket_t *TC, rtems_id queue_id)
246 269 {
247 270 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
248 271 *
249 272 * @param TC points to the TeleCommand packet that is being processed
250 273 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
251 274 *
252 275 * @return LFR directive status code:
253 276 * - LFR_DEFAULT
254 277 * - LFR_SUCCESSFUL
255 278 *
256 279 */
257 280
258 281 unsigned int val;
259 282 unsigned int status;
260 283 unsigned char mode;
261 284 unsigned char * bytePosPtr;
262 285 int pos;
263 286 float value;
264 287
265 288 pos = INIT_CHAR;
266 289 value = INIT_FLOAT;
267 290
268 291 status = LFR_DEFAULT;
269 292
270 293 bytePosPtr = (unsigned char *) &TC->packetID;
271 294
272 295 // check LFR mode
273 296 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET5 ] & BITS_LFR_MODE) >> SHIFT_LFR_MODE;
274 297 status = check_update_info_hk_lfr_mode( mode );
275 298 if (status == LFR_SUCCESSFUL) // check TDS mode
276 299 {
277 300 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & BITS_TDS_MODE) >> SHIFT_TDS_MODE;
278 301 status = check_update_info_hk_tds_mode( mode );
279 302 }
280 303 if (status == LFR_SUCCESSFUL) // check THR mode
281 304 {
282 305 mode = (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET6 ] & BITS_THR_MODE);
283 306 status = check_update_info_hk_thr_mode( mode );
284 307 }
285 308 if (status == LFR_SUCCESSFUL) // check reaction wheels frequencies
286 309 {
287 310 status = check_all_sy_lfr_rw_f(TC, &pos, &value);
288 311 }
289 312
290 313 // if the parameters checking succeeds, udpate all parameters
291 314 if (status == LFR_SUCCESSFUL)
292 315 {
293 316 // pa_bia_status_info
294 317 // => pa_bia_mode_mux_set 3 bits
295 318 // => pa_bia_mode_hv_enabled 1 bit
296 319 // => pa_bia_mode_bias1_enabled 1 bit
297 320 // => pa_bia_mode_bias2_enabled 1 bit
298 321 // => pa_bia_mode_bias3_enabled 1 bit
299 322 // => pa_bia_on_off (cp_dpu_bias_on_off)
300 323 pa_bia_status_info = bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET2 ] & BITS_BIA; // [1111 1110]
301 324 pa_bia_status_info = pa_bia_status_info
302 325 | (bytePosPtr[ BYTE_POS_UPDATE_INFO_PARAMETERS_SET1 ] & 1);
303 326
304 327 // REACTION_WHEELS_FREQUENCY, copy the incoming parameters in the local variable (to be copied in HK packets)
305 328 getReactionWheelsFrequencies( TC );
306 329 set_hk_lfr_sc_rw_f_flags();
307 330 build_sy_lfr_rw_masks();
308 331
309 332 // once the masks are built, they have to be merged with the fbins_mask
310 333 merge_fbins_masks();
311 334
312 335 // increase the TC_LFR_UPDATE_INFO counter
313 336 if (status == LFR_SUCCESSFUL) // if the parameter check is successful
314 337 {
315 338 val = (housekeeping_packet.hk_lfr_update_info_tc_cnt[0] * CONST_256)
316 339 + housekeeping_packet.hk_lfr_update_info_tc_cnt[1];
317 340 val++;
318 341 housekeeping_packet.hk_lfr_update_info_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
319 342 housekeeping_packet.hk_lfr_update_info_tc_cnt[1] = (unsigned char) (val);
320 343 }
321 344 }
322 345
323 346 return status;
324 347 }
325 348
326 349 int action_enable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
327 350 {
328 351 /** This function executes specific actions when a TC_LFR_ENABLE_CALIBRATION TeleCommand has been received.
329 352 *
330 353 * @param TC points to the TeleCommand packet that is being processed
331 354 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
332 355 *
333 356 */
334 357
335 358 int result;
336 359
337 360 result = LFR_DEFAULT;
338 361
339 362 setCalibration( true );
340 363
341 364 result = LFR_SUCCESSFUL;
342 365
343 366 return result;
344 367 }
345 368
346 369 int action_disable_calibration(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
347 370 {
348 371 /** This function executes specific actions when a TC_LFR_DISABLE_CALIBRATION TeleCommand has been received.
349 372 *
350 373 * @param TC points to the TeleCommand packet that is being processed
351 374 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
352 375 *
353 376 */
354 377
355 378 int result;
356 379
357 380 result = LFR_DEFAULT;
358 381
359 382 setCalibration( false );
360 383
361 384 result = LFR_SUCCESSFUL;
362 385
363 386 return result;
364 387 }
365 388
366 389 int action_update_time(ccsdsTelecommandPacket_t *TC)
367 390 {
368 391 /** This function executes specific actions when a TC_LFR_UPDATE_TIME TeleCommand has been received.
369 392 *
370 393 * @param TC points to the TeleCommand packet that is being processed
371 394 * @param queue_id is the id of the queue which handles TM transmission by the SpaceWire driver
372 395 *
373 396 * @return LFR_SUCCESSFUL
374 397 *
375 398 */
376 399
377 400 unsigned int val;
378 401
379 402 time_management_regs->coarse_time_load = (TC->dataAndCRC[BYTE_0] << SHIFT_3_BYTES)
380 403 + (TC->dataAndCRC[BYTE_1] << SHIFT_2_BYTES)
381 404 + (TC->dataAndCRC[BYTE_2] << SHIFT_1_BYTE)
382 405 + TC->dataAndCRC[BYTE_3];
383 406
384 407 val = (housekeeping_packet.hk_lfr_update_time_tc_cnt[0] * CONST_256)
385 408 + housekeeping_packet.hk_lfr_update_time_tc_cnt[1];
386 409 val++;
387 410 housekeeping_packet.hk_lfr_update_time_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
388 411 housekeeping_packet.hk_lfr_update_time_tc_cnt[1] = (unsigned char) (val);
389 412
390 413 oneTcLfrUpdateTimeReceived = 1;
391 414
392 415 return LFR_SUCCESSFUL;
393 416 }
394 417
395 418 //*******************
396 419 // ENTERING THE MODES
397 420 int check_mode_value( unsigned char requestedMode )
398 421 {
399 422 int status;
400 423
401 424 status = LFR_DEFAULT;
402 425
403 426 if ( (requestedMode != LFR_MODE_STANDBY)
404 427 && (requestedMode != LFR_MODE_NORMAL) && (requestedMode != LFR_MODE_BURST)
405 428 && (requestedMode != LFR_MODE_SBM1) && (requestedMode != LFR_MODE_SBM2) )
406 429 {
407 430 status = LFR_DEFAULT;
408 431 }
409 432 else
410 433 {
411 434 status = LFR_SUCCESSFUL;
412 435 }
413 436
414 437 return status;
415 438 }
416 439
417 440 int check_mode_transition( unsigned char requestedMode )
418 441 {
419 442 /** This function checks the validity of the transition requested by the TC_LFR_ENTER_MODE.
420 443 *
421 444 * @param requestedMode is the mode requested by the TC_LFR_ENTER_MODE
422 445 *
423 446 * @return LFR directive status codes:
424 447 * - LFR_SUCCESSFUL - the transition is authorized
425 448 * - LFR_DEFAULT - the transition is not authorized
426 449 *
427 450 */
428 451
429 452 int status;
430 453
431 454 switch (requestedMode)
432 455 {
433 456 case LFR_MODE_STANDBY:
434 457 if ( lfrCurrentMode == LFR_MODE_STANDBY ) {
435 458 status = LFR_DEFAULT;
436 459 }
437 460 else
438 461 {
439 462 status = LFR_SUCCESSFUL;
440 463 }
441 464 break;
442 465 case LFR_MODE_NORMAL:
443 466 if ( lfrCurrentMode == LFR_MODE_NORMAL ) {
444 467 status = LFR_DEFAULT;
445 468 }
446 469 else {
447 470 status = LFR_SUCCESSFUL;
448 471 }
449 472 break;
450 473 case LFR_MODE_BURST:
451 474 if ( lfrCurrentMode == LFR_MODE_BURST ) {
452 475 status = LFR_DEFAULT;
453 476 }
454 477 else {
455 478 status = LFR_SUCCESSFUL;
456 479 }
457 480 break;
458 481 case LFR_MODE_SBM1:
459 482 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
460 483 status = LFR_DEFAULT;
461 484 }
462 485 else {
463 486 status = LFR_SUCCESSFUL;
464 487 }
465 488 break;
466 489 case LFR_MODE_SBM2:
467 490 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
468 491 status = LFR_DEFAULT;
469 492 }
470 493 else {
471 494 status = LFR_SUCCESSFUL;
472 495 }
473 496 break;
474 497 default:
475 498 status = LFR_DEFAULT;
476 499 break;
477 500 }
478 501
479 502 return status;
480 503 }
481 504
482 505 void update_last_valid_transition_date( unsigned int transitionCoarseTime )
483 506 {
484 507 if (transitionCoarseTime == 0)
485 508 {
486 509 lastValidEnterModeTime = time_management_regs->coarse_time + 1;
487 510 PRINTF1("lastValidEnterModeTime = 0x%x (transitionCoarseTime = 0 => coarse_time+1)\n", lastValidEnterModeTime);
488 511 }
489 512 else
490 513 {
491 514 lastValidEnterModeTime = transitionCoarseTime;
492 515 PRINTF1("lastValidEnterModeTime = 0x%x\n", transitionCoarseTime);
493 516 }
494 517 }
495 518
496 519 int check_transition_date( unsigned int transitionCoarseTime )
497 520 {
498 521 int status;
499 522 unsigned int localCoarseTime;
500 523 unsigned int deltaCoarseTime;
501 524
502 525 status = LFR_SUCCESSFUL;
503 526
504 527 if (transitionCoarseTime == 0) // transition time = 0 means an instant transition
505 528 {
506 529 status = LFR_SUCCESSFUL;
507 530 }
508 531 else
509 532 {
510 533 localCoarseTime = time_management_regs->coarse_time & COARSE_TIME_MASK;
511 534
512 535 PRINTF2("localTime = %x, transitionTime = %x\n", localCoarseTime, transitionCoarseTime);
513 536
514 537 if ( transitionCoarseTime <= localCoarseTime ) // SSS-CP-EQS-322
515 538 {
516 539 status = LFR_DEFAULT;
517 540 PRINTF("ERR *** in check_transition_date *** transitionCoarseTime <= localCoarseTime\n");
518 541 }
519 542
520 543 if (status == LFR_SUCCESSFUL)
521 544 {
522 545 deltaCoarseTime = transitionCoarseTime - localCoarseTime;
523 546 if ( deltaCoarseTime > MAX_DELTA_COARSE_TIME ) // SSS-CP-EQS-323
524 547 {
525 548 status = LFR_DEFAULT;
526 549 PRINTF1("ERR *** in check_transition_date *** deltaCoarseTime = %x\n", deltaCoarseTime)
527 550 }
528 551 }
529 552 }
530 553
531 554 return status;
532 555 }
533 556
534 557 int restart_asm_activities( unsigned char lfrRequestedMode )
535 558 {
536 559 rtems_status_code status;
537 560
538 561 status = stop_spectral_matrices();
539 562
540 563 thisIsAnASMRestart = 1;
541 564
542 565 status = restart_asm_tasks( lfrRequestedMode );
543 566
544 567 launch_spectral_matrix();
545 568
546 569 return status;
547 570 }
548 571
549 572 int stop_spectral_matrices( void )
550 573 {
551 574 /** This function stops and restarts the current mode average spectral matrices activities.
552 575 *
553 576 * @return RTEMS directive status codes:
554 577 * - RTEMS_SUCCESSFUL - task restarted successfully
555 578 * - RTEMS_INVALID_ID - task id invalid
556 579 * - RTEMS_ALREADY_SUSPENDED - task already suspended
557 580 *
558 581 */
559 582
560 583 rtems_status_code status;
561 584
562 585 status = RTEMS_SUCCESSFUL;
563 586
564 587 // (1) mask interruptions
565 588 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // mask spectral matrix interrupt
566 589
567 590 // (2) reset spectral matrices registers
568 591 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
569 592 reset_sm_status();
570 593
571 594 // (3) clear interruptions
572 595 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
573 596
574 597 // suspend several tasks
575 598 if (lfrCurrentMode != LFR_MODE_STANDBY) {
576 599 status = suspend_asm_tasks();
577 600 }
578 601
579 602 if (status != RTEMS_SUCCESSFUL)
580 603 {
581 604 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
582 605 }
583 606
584 607 return status;
585 608 }
586 609
587 610 int stop_current_mode( void )
588 611 {
589 612 /** This function stops the current mode by masking interrupt lines and suspending science tasks.
590 613 *
591 614 * @return RTEMS directive status codes:
592 615 * - RTEMS_SUCCESSFUL - task restarted successfully
593 616 * - RTEMS_INVALID_ID - task id invalid
594 617 * - RTEMS_ALREADY_SUSPENDED - task already suspended
595 618 *
596 619 */
597 620
598 621 rtems_status_code status;
599 622
600 623 status = RTEMS_SUCCESSFUL;
601 624
602 625 // (1) mask interruptions
603 626 LEON_Mask_interrupt( IRQ_WAVEFORM_PICKER ); // mask waveform picker interrupt
604 627 LEON_Mask_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
605 628
606 629 // (2) reset waveform picker registers
607 630 reset_wfp_burst_enable(); // reset burst and enable bits
608 631 reset_wfp_status(); // reset all the status bits
609 632
610 633 // (3) reset spectral matrices registers
611 634 set_sm_irq_onNewMatrix( 0 ); // stop the spectral matrices
612 635 reset_sm_status();
613 636
614 637 // reset lfr VHDL module
615 638 reset_lfr();
616 639
617 640 reset_extractSWF(); // reset the extractSWF flag to false
618 641
619 642 // (4) clear interruptions
620 643 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER ); // clear waveform picker interrupt
621 644 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX ); // clear spectral matrix interrupt
622 645
623 646 // suspend several tasks
624 647 if (lfrCurrentMode != LFR_MODE_STANDBY) {
625 648 status = suspend_science_tasks();
626 649 }
627 650
628 651 if (status != RTEMS_SUCCESSFUL)
629 652 {
630 653 PRINTF1("in stop_current_mode *** in suspend_science_tasks *** ERR code: %d\n", status)
631 654 }
632 655
633 656 return status;
634 657 }
635 658
636 659 int enter_mode_standby( void )
637 660 {
638 661 /** This function is used to put LFR in the STANDBY mode.
639 662 *
640 663 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
641 664 *
642 665 * @return RTEMS directive status codes:
643 666 * - RTEMS_SUCCESSFUL - task restarted successfully
644 667 * - RTEMS_INVALID_ID - task id invalid
645 668 * - RTEMS_INCORRECT_STATE - task never started
646 669 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
647 670 *
648 671 * The STANDBY mode does not depends on a specific transition date, the effect of the TC_LFR_ENTER_MODE
649 672 * is immediate.
650 673 *
651 674 */
652 675
653 676 int status;
654 677
655 678 status = stop_current_mode(); // STOP THE CURRENT MODE
656 679
657 680 #ifdef PRINT_TASK_STATISTICS
658 681 rtems_cpu_usage_report();
659 682 #endif
660 683
661 684 #ifdef PRINT_STACK_REPORT
662 685 PRINTF("stack report selected\n")
663 686 rtems_stack_checker_report_usage();
664 687 #endif
665 688
666 689 return status;
667 690 }
668 691
669 692 int enter_mode_normal( unsigned int transitionCoarseTime )
670 693 {
671 694 /** This function is used to start the NORMAL mode.
672 695 *
673 696 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
674 697 *
675 698 * @return RTEMS directive status codes:
676 699 * - RTEMS_SUCCESSFUL - task restarted successfully
677 700 * - RTEMS_INVALID_ID - task id invalid
678 701 * - RTEMS_INCORRECT_STATE - task never started
679 702 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
680 703 *
681 704 * The way the NORMAL mode is started depends on the LFR current mode. If LFR is in SBM1 or SBM2,
682 705 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected.
683 706 *
684 707 */
685 708
686 709 int status;
687 710
688 711 #ifdef PRINT_TASK_STATISTICS
689 712 rtems_cpu_usage_reset();
690 713 #endif
691 714
692 715 status = RTEMS_UNSATISFIED;
693 716
694 717 switch( lfrCurrentMode )
695 718 {
696 719 case LFR_MODE_STANDBY:
697 720 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart science tasks
698 721 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
699 722 {
700 723 launch_spectral_matrix( );
701 724 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
702 725 }
703 726 break;
704 727 case LFR_MODE_BURST:
705 728 status = stop_current_mode(); // stop the current mode
706 729 status = restart_science_tasks( LFR_MODE_NORMAL ); // restart the science tasks
707 730 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
708 731 {
709 732 launch_spectral_matrix( );
710 733 launch_waveform_picker( LFR_MODE_NORMAL, transitionCoarseTime );
711 734 }
712 735 break;
713 736 case LFR_MODE_SBM1:
714 737 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
715 738 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
716 739 update_last_valid_transition_date( transitionCoarseTime );
717 740 break;
718 741 case LFR_MODE_SBM2:
719 742 status = restart_asm_activities( LFR_MODE_NORMAL ); // this is necessary to restart ASM tasks to update the parameters
720 743 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
721 744 update_last_valid_transition_date( transitionCoarseTime );
722 745 break;
723 746 default:
724 747 break;
725 748 }
726 749
727 750 if (status != RTEMS_SUCCESSFUL)
728 751 {
729 752 PRINTF1("ERR *** in enter_mode_normal *** status = %d\n", status)
730 753 status = RTEMS_UNSATISFIED;
731 754 }
732 755
733 756 return status;
734 757 }
735 758
736 759 int enter_mode_burst( unsigned int transitionCoarseTime )
737 760 {
738 761 /** This function is used to start the BURST mode.
739 762 *
740 763 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
741 764 *
742 765 * @return RTEMS directive status codes:
743 766 * - RTEMS_SUCCESSFUL - task restarted successfully
744 767 * - RTEMS_INVALID_ID - task id invalid
745 768 * - RTEMS_INCORRECT_STATE - task never started
746 769 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
747 770 *
748 771 * The way the BURST mode is started does not depend on the LFR current mode.
749 772 *
750 773 */
751 774
752 775
753 776 int status;
754 777
755 778 #ifdef PRINT_TASK_STATISTICS
756 779 rtems_cpu_usage_reset();
757 780 #endif
758 781
759 782 status = stop_current_mode(); // stop the current mode
760 783 status = restart_science_tasks( LFR_MODE_BURST ); // restart the science tasks
761 784 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
762 785 {
763 786 launch_spectral_matrix( );
764 787 launch_waveform_picker( LFR_MODE_BURST, transitionCoarseTime );
765 788 }
766 789
767 790 if (status != RTEMS_SUCCESSFUL)
768 791 {
769 792 PRINTF1("ERR *** in enter_mode_burst *** status = %d\n", status)
770 793 status = RTEMS_UNSATISFIED;
771 794 }
772 795
773 796 return status;
774 797 }
775 798
776 799 int enter_mode_sbm1( unsigned int transitionCoarseTime )
777 800 {
778 801 /** This function is used to start the SBM1 mode.
779 802 *
780 803 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
781 804 *
782 805 * @return RTEMS directive status codes:
783 806 * - RTEMS_SUCCESSFUL - task restarted successfully
784 807 * - RTEMS_INVALID_ID - task id invalid
785 808 * - RTEMS_INCORRECT_STATE - task never started
786 809 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
787 810 *
788 811 * The way the SBM1 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM2,
789 812 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
790 813 * cases, the acquisition is completely restarted.
791 814 *
792 815 */
793 816
794 817 int status;
795 818
796 819 #ifdef PRINT_TASK_STATISTICS
797 820 rtems_cpu_usage_reset();
798 821 #endif
799 822
800 823 status = RTEMS_UNSATISFIED;
801 824
802 825 switch( lfrCurrentMode )
803 826 {
804 827 case LFR_MODE_STANDBY:
805 828 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart science tasks
806 829 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
807 830 {
808 831 launch_spectral_matrix( );
809 832 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
810 833 }
811 834 break;
812 835 case LFR_MODE_NORMAL: // lfrCurrentMode will be updated after the execution of close_action
813 836 status = restart_asm_activities( LFR_MODE_SBM1 );
814 837 status = LFR_SUCCESSFUL;
815 838 update_last_valid_transition_date( transitionCoarseTime );
816 839 break;
817 840 case LFR_MODE_BURST:
818 841 status = stop_current_mode(); // stop the current mode
819 842 status = restart_science_tasks( LFR_MODE_SBM1 ); // restart the science tasks
820 843 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
821 844 {
822 845 launch_spectral_matrix( );
823 846 launch_waveform_picker( LFR_MODE_SBM1, transitionCoarseTime );
824 847 }
825 848 break;
826 849 case LFR_MODE_SBM2:
827 850 status = restart_asm_activities( LFR_MODE_SBM1 );
828 851 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
829 852 update_last_valid_transition_date( transitionCoarseTime );
830 853 break;
831 854 default:
832 855 break;
833 856 }
834 857
835 858 if (status != RTEMS_SUCCESSFUL)
836 859 {
837 860 PRINTF1("ERR *** in enter_mode_sbm1 *** status = %d\n", status);
838 861 status = RTEMS_UNSATISFIED;
839 862 }
840 863
841 864 return status;
842 865 }
843 866
844 867 int enter_mode_sbm2( unsigned int transitionCoarseTime )
845 868 {
846 869 /** This function is used to start the SBM2 mode.
847 870 *
848 871 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
849 872 *
850 873 * @return RTEMS directive status codes:
851 874 * - RTEMS_SUCCESSFUL - task restarted successfully
852 875 * - RTEMS_INVALID_ID - task id invalid
853 876 * - RTEMS_INCORRECT_STATE - task never started
854 877 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
855 878 *
856 879 * The way the SBM2 mode is started depends on the LFR current mode. If LFR is in NORMAL or SBM1,
857 880 * the snapshots are not restarted, only ASM, BP and CWF data generation are affected. In other
858 881 * cases, the acquisition is completely restarted.
859 882 *
860 883 */
861 884
862 885 int status;
863 886
864 887 #ifdef PRINT_TASK_STATISTICS
865 888 rtems_cpu_usage_reset();
866 889 #endif
867 890
868 891 status = RTEMS_UNSATISFIED;
869 892
870 893 switch( lfrCurrentMode )
871 894 {
872 895 case LFR_MODE_STANDBY:
873 896 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart science tasks
874 897 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
875 898 {
876 899 launch_spectral_matrix( );
877 900 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
878 901 }
879 902 break;
880 903 case LFR_MODE_NORMAL:
881 904 status = restart_asm_activities( LFR_MODE_SBM2 );
882 905 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
883 906 update_last_valid_transition_date( transitionCoarseTime );
884 907 break;
885 908 case LFR_MODE_BURST:
886 909 status = stop_current_mode(); // stop the current mode
887 910 status = restart_science_tasks( LFR_MODE_SBM2 ); // restart the science tasks
888 911 if (status == RTEMS_SUCCESSFUL) // relaunch spectral_matrix and waveform_picker modules
889 912 {
890 913 launch_spectral_matrix( );
891 914 launch_waveform_picker( LFR_MODE_SBM2, transitionCoarseTime );
892 915 }
893 916 break;
894 917 case LFR_MODE_SBM1:
895 918 status = restart_asm_activities( LFR_MODE_SBM2 );
896 919 status = LFR_SUCCESSFUL; // lfrCurrentMode will be updated after the execution of close_action
897 920 update_last_valid_transition_date( transitionCoarseTime );
898 921 break;
899 922 default:
900 923 break;
901 924 }
902 925
903 926 if (status != RTEMS_SUCCESSFUL)
904 927 {
905 928 PRINTF1("ERR *** in enter_mode_sbm2 *** status = %d\n", status)
906 929 status = RTEMS_UNSATISFIED;
907 930 }
908 931
909 932 return status;
910 933 }
911 934
912 935 int restart_science_tasks( unsigned char lfrRequestedMode )
913 936 {
914 937 /** This function is used to restart all science tasks.
915 938 *
916 939 * @return RTEMS directive status codes:
917 940 * - RTEMS_SUCCESSFUL - task restarted successfully
918 941 * - RTEMS_INVALID_ID - task id invalid
919 942 * - RTEMS_INCORRECT_STATE - task never started
920 943 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
921 944 *
922 945 * Science tasks are AVF0, PRC0, WFRM, CWF3, CW2, CWF1
923 946 *
924 947 */
925 948
926 949 rtems_status_code status[NB_SCIENCE_TASKS];
927 950 rtems_status_code ret;
928 951
929 952 ret = RTEMS_SUCCESSFUL;
930 953
931 954 status[STATUS_0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
932 955 if (status[STATUS_0] != RTEMS_SUCCESSFUL)
933 956 {
934 957 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[STATUS_0])
935 958 }
936 959
937 960 status[STATUS_1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
938 961 if (status[STATUS_1] != RTEMS_SUCCESSFUL)
939 962 {
940 963 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[STATUS_1])
941 964 }
942 965
943 966 status[STATUS_2] = rtems_task_restart( Task_id[TASKID_WFRM],1 );
944 967 if (status[STATUS_2] != RTEMS_SUCCESSFUL)
945 968 {
946 969 PRINTF1("in restart_science_task *** WFRM ERR %d\n", status[STATUS_2])
947 970 }
948 971
949 972 status[STATUS_3] = rtems_task_restart( Task_id[TASKID_CWF3],1 );
950 973 if (status[STATUS_3] != RTEMS_SUCCESSFUL)
951 974 {
952 975 PRINTF1("in restart_science_task *** CWF3 ERR %d\n", status[STATUS_3])
953 976 }
954 977
955 978 status[STATUS_4] = rtems_task_restart( Task_id[TASKID_CWF2],1 );
956 979 if (status[STATUS_4] != RTEMS_SUCCESSFUL)
957 980 {
958 981 PRINTF1("in restart_science_task *** CWF2 ERR %d\n", status[STATUS_4])
959 982 }
960 983
961 984 status[STATUS_5] = rtems_task_restart( Task_id[TASKID_CWF1],1 );
962 985 if (status[STATUS_5] != RTEMS_SUCCESSFUL)
963 986 {
964 987 PRINTF1("in restart_science_task *** CWF1 ERR %d\n", status[STATUS_5])
965 988 }
966 989
967 990 status[STATUS_6] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
968 991 if (status[STATUS_6] != RTEMS_SUCCESSFUL)
969 992 {
970 993 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[STATUS_6])
971 994 }
972 995
973 996 status[STATUS_7] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
974 997 if (status[STATUS_7] != RTEMS_SUCCESSFUL)
975 998 {
976 999 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[STATUS_7])
977 1000 }
978 1001
979 1002 status[STATUS_8] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
980 1003 if (status[STATUS_8] != RTEMS_SUCCESSFUL)
981 1004 {
982 1005 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[STATUS_8])
983 1006 }
984 1007
985 1008 status[STATUS_9] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
986 1009 if (status[STATUS_9] != RTEMS_SUCCESSFUL)
987 1010 {
988 1011 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[STATUS_9])
989 1012 }
990 1013
991 1014 if ( (status[STATUS_0] != RTEMS_SUCCESSFUL) || (status[STATUS_1] != RTEMS_SUCCESSFUL) ||
992 1015 (status[STATUS_2] != RTEMS_SUCCESSFUL) || (status[STATUS_3] != RTEMS_SUCCESSFUL) ||
993 1016 (status[STATUS_4] != RTEMS_SUCCESSFUL) || (status[STATUS_5] != RTEMS_SUCCESSFUL) ||
994 1017 (status[STATUS_6] != RTEMS_SUCCESSFUL) || (status[STATUS_7] != RTEMS_SUCCESSFUL) ||
995 1018 (status[STATUS_8] != RTEMS_SUCCESSFUL) || (status[STATUS_9] != RTEMS_SUCCESSFUL) )
996 1019 {
997 1020 ret = RTEMS_UNSATISFIED;
998 1021 }
999 1022
1000 1023 return ret;
1001 1024 }
1002 1025
1003 1026 int restart_asm_tasks( unsigned char lfrRequestedMode )
1004 1027 {
1005 1028 /** This function is used to restart average spectral matrices tasks.
1006 1029 *
1007 1030 * @return RTEMS directive status codes:
1008 1031 * - RTEMS_SUCCESSFUL - task restarted successfully
1009 1032 * - RTEMS_INVALID_ID - task id invalid
1010 1033 * - RTEMS_INCORRECT_STATE - task never started
1011 1034 * - RTEMS_ILLEGAL_ON_REMOTE_OBJECT - cannot restart remote task
1012 1035 *
1013 1036 * ASM tasks are AVF0, PRC0, AVF1, PRC1, AVF2 and PRC2
1014 1037 *
1015 1038 */
1016 1039
1017 1040 rtems_status_code status[NB_ASM_TASKS];
1018 1041 rtems_status_code ret;
1019 1042
1020 1043 ret = RTEMS_SUCCESSFUL;
1021 1044
1022 1045 status[STATUS_0] = rtems_task_restart( Task_id[TASKID_AVF0], lfrRequestedMode );
1023 1046 if (status[STATUS_0] != RTEMS_SUCCESSFUL)
1024 1047 {
1025 1048 PRINTF1("in restart_science_task *** AVF0 ERR %d\n", status[STATUS_0])
1026 1049 }
1027 1050
1028 1051 status[STATUS_1] = rtems_task_restart( Task_id[TASKID_PRC0], lfrRequestedMode );
1029 1052 if (status[STATUS_1] != RTEMS_SUCCESSFUL)
1030 1053 {
1031 1054 PRINTF1("in restart_science_task *** PRC0 ERR %d\n", status[STATUS_1])
1032 1055 }
1033 1056
1034 1057 status[STATUS_2] = rtems_task_restart( Task_id[TASKID_AVF1], lfrRequestedMode );
1035 1058 if (status[STATUS_2] != RTEMS_SUCCESSFUL)
1036 1059 {
1037 1060 PRINTF1("in restart_science_task *** AVF1 ERR %d\n", status[STATUS_2])
1038 1061 }
1039 1062
1040 1063 status[STATUS_3] = rtems_task_restart( Task_id[TASKID_PRC1],lfrRequestedMode );
1041 1064 if (status[STATUS_3] != RTEMS_SUCCESSFUL)
1042 1065 {
1043 1066 PRINTF1("in restart_science_task *** PRC1 ERR %d\n", status[STATUS_3])
1044 1067 }
1045 1068
1046 1069 status[STATUS_4] = rtems_task_restart( Task_id[TASKID_AVF2], 1 );
1047 1070 if (status[STATUS_4] != RTEMS_SUCCESSFUL)
1048 1071 {
1049 1072 PRINTF1("in restart_science_task *** AVF2 ERR %d\n", status[STATUS_4])
1050 1073 }
1051 1074
1052 1075 status[STATUS_5] = rtems_task_restart( Task_id[TASKID_PRC2], 1 );
1053 1076 if (status[STATUS_5] != RTEMS_SUCCESSFUL)
1054 1077 {
1055 1078 PRINTF1("in restart_science_task *** PRC2 ERR %d\n", status[STATUS_5])
1056 1079 }
1057 1080
1058 1081 if ( (status[STATUS_0] != RTEMS_SUCCESSFUL) || (status[STATUS_1] != RTEMS_SUCCESSFUL) ||
1059 1082 (status[STATUS_2] != RTEMS_SUCCESSFUL) || (status[STATUS_3] != RTEMS_SUCCESSFUL) ||
1060 1083 (status[STATUS_4] != RTEMS_SUCCESSFUL) || (status[STATUS_5] != RTEMS_SUCCESSFUL) )
1061 1084 {
1062 1085 ret = RTEMS_UNSATISFIED;
1063 1086 }
1064 1087
1065 1088 return ret;
1066 1089 }
1067 1090
1068 1091 int suspend_science_tasks( void )
1069 1092 {
1070 1093 /** This function suspends the science tasks.
1071 1094 *
1072 1095 * @return RTEMS directive status codes:
1073 1096 * - RTEMS_SUCCESSFUL - task restarted successfully
1074 1097 * - RTEMS_INVALID_ID - task id invalid
1075 1098 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1076 1099 *
1077 1100 */
1078 1101
1079 1102 rtems_status_code status;
1080 1103
1081 1104 PRINTF("in suspend_science_tasks\n")
1082 1105
1083 1106 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1084 1107 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1085 1108 {
1086 1109 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1087 1110 }
1088 1111 else
1089 1112 {
1090 1113 status = RTEMS_SUCCESSFUL;
1091 1114 }
1092 1115 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1093 1116 {
1094 1117 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1095 1118 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1096 1119 {
1097 1120 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1098 1121 }
1099 1122 else
1100 1123 {
1101 1124 status = RTEMS_SUCCESSFUL;
1102 1125 }
1103 1126 }
1104 1127 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1105 1128 {
1106 1129 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1107 1130 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1108 1131 {
1109 1132 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1110 1133 }
1111 1134 else
1112 1135 {
1113 1136 status = RTEMS_SUCCESSFUL;
1114 1137 }
1115 1138 }
1116 1139 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1117 1140 {
1118 1141 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1119 1142 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1120 1143 {
1121 1144 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1122 1145 }
1123 1146 else
1124 1147 {
1125 1148 status = RTEMS_SUCCESSFUL;
1126 1149 }
1127 1150 }
1128 1151 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1129 1152 {
1130 1153 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1131 1154 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1132 1155 {
1133 1156 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1134 1157 }
1135 1158 else
1136 1159 {
1137 1160 status = RTEMS_SUCCESSFUL;
1138 1161 }
1139 1162 }
1140 1163 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1141 1164 {
1142 1165 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1143 1166 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1144 1167 {
1145 1168 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1146 1169 }
1147 1170 else
1148 1171 {
1149 1172 status = RTEMS_SUCCESSFUL;
1150 1173 }
1151 1174 }
1152 1175 if (status == RTEMS_SUCCESSFUL) // suspend WFRM
1153 1176 {
1154 1177 status = rtems_task_suspend( Task_id[TASKID_WFRM] );
1155 1178 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1156 1179 {
1157 1180 PRINTF1("in suspend_science_task *** WFRM ERR %d\n", status)
1158 1181 }
1159 1182 else
1160 1183 {
1161 1184 status = RTEMS_SUCCESSFUL;
1162 1185 }
1163 1186 }
1164 1187 if (status == RTEMS_SUCCESSFUL) // suspend CWF3
1165 1188 {
1166 1189 status = rtems_task_suspend( Task_id[TASKID_CWF3] );
1167 1190 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1168 1191 {
1169 1192 PRINTF1("in suspend_science_task *** CWF3 ERR %d\n", status)
1170 1193 }
1171 1194 else
1172 1195 {
1173 1196 status = RTEMS_SUCCESSFUL;
1174 1197 }
1175 1198 }
1176 1199 if (status == RTEMS_SUCCESSFUL) // suspend CWF2
1177 1200 {
1178 1201 status = rtems_task_suspend( Task_id[TASKID_CWF2] );
1179 1202 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1180 1203 {
1181 1204 PRINTF1("in suspend_science_task *** CWF2 ERR %d\n", status)
1182 1205 }
1183 1206 else
1184 1207 {
1185 1208 status = RTEMS_SUCCESSFUL;
1186 1209 }
1187 1210 }
1188 1211 if (status == RTEMS_SUCCESSFUL) // suspend CWF1
1189 1212 {
1190 1213 status = rtems_task_suspend( Task_id[TASKID_CWF1] );
1191 1214 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1192 1215 {
1193 1216 PRINTF1("in suspend_science_task *** CWF1 ERR %d\n", status)
1194 1217 }
1195 1218 else
1196 1219 {
1197 1220 status = RTEMS_SUCCESSFUL;
1198 1221 }
1199 1222 }
1200 1223
1201 1224 return status;
1202 1225 }
1203 1226
1204 1227 int suspend_asm_tasks( void )
1205 1228 {
1206 1229 /** This function suspends the science tasks.
1207 1230 *
1208 1231 * @return RTEMS directive status codes:
1209 1232 * - RTEMS_SUCCESSFUL - task restarted successfully
1210 1233 * - RTEMS_INVALID_ID - task id invalid
1211 1234 * - RTEMS_ALREADY_SUSPENDED - task already suspended
1212 1235 *
1213 1236 */
1214 1237
1215 1238 rtems_status_code status;
1216 1239
1217 1240 PRINTF("in suspend_science_tasks\n")
1218 1241
1219 1242 status = rtems_task_suspend( Task_id[TASKID_AVF0] ); // suspend AVF0
1220 1243 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1221 1244 {
1222 1245 PRINTF1("in suspend_science_task *** AVF0 ERR %d\n", status)
1223 1246 }
1224 1247 else
1225 1248 {
1226 1249 status = RTEMS_SUCCESSFUL;
1227 1250 }
1228 1251
1229 1252 if (status == RTEMS_SUCCESSFUL) // suspend PRC0
1230 1253 {
1231 1254 status = rtems_task_suspend( Task_id[TASKID_PRC0] );
1232 1255 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1233 1256 {
1234 1257 PRINTF1("in suspend_science_task *** PRC0 ERR %d\n", status)
1235 1258 }
1236 1259 else
1237 1260 {
1238 1261 status = RTEMS_SUCCESSFUL;
1239 1262 }
1240 1263 }
1241 1264
1242 1265 if (status == RTEMS_SUCCESSFUL) // suspend AVF1
1243 1266 {
1244 1267 status = rtems_task_suspend( Task_id[TASKID_AVF1] );
1245 1268 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1246 1269 {
1247 1270 PRINTF1("in suspend_science_task *** AVF1 ERR %d\n", status)
1248 1271 }
1249 1272 else
1250 1273 {
1251 1274 status = RTEMS_SUCCESSFUL;
1252 1275 }
1253 1276 }
1254 1277
1255 1278 if (status == RTEMS_SUCCESSFUL) // suspend PRC1
1256 1279 {
1257 1280 status = rtems_task_suspend( Task_id[TASKID_PRC1] );
1258 1281 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1259 1282 {
1260 1283 PRINTF1("in suspend_science_task *** PRC1 ERR %d\n", status)
1261 1284 }
1262 1285 else
1263 1286 {
1264 1287 status = RTEMS_SUCCESSFUL;
1265 1288 }
1266 1289 }
1267 1290
1268 1291 if (status == RTEMS_SUCCESSFUL) // suspend AVF2
1269 1292 {
1270 1293 status = rtems_task_suspend( Task_id[TASKID_AVF2] );
1271 1294 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1272 1295 {
1273 1296 PRINTF1("in suspend_science_task *** AVF2 ERR %d\n", status)
1274 1297 }
1275 1298 else
1276 1299 {
1277 1300 status = RTEMS_SUCCESSFUL;
1278 1301 }
1279 1302 }
1280 1303
1281 1304 if (status == RTEMS_SUCCESSFUL) // suspend PRC2
1282 1305 {
1283 1306 status = rtems_task_suspend( Task_id[TASKID_PRC2] );
1284 1307 if ((status != RTEMS_SUCCESSFUL) && (status != RTEMS_ALREADY_SUSPENDED))
1285 1308 {
1286 1309 PRINTF1("in suspend_science_task *** PRC2 ERR %d\n", status)
1287 1310 }
1288 1311 else
1289 1312 {
1290 1313 status = RTEMS_SUCCESSFUL;
1291 1314 }
1292 1315 }
1293 1316
1294 1317 return status;
1295 1318 }
1296 1319
1297 1320 void launch_waveform_picker( unsigned char mode, unsigned int transitionCoarseTime )
1298 1321 {
1299 1322
1300 1323 WFP_reset_current_ring_nodes();
1301 1324
1302 1325 reset_waveform_picker_regs();
1303 1326
1304 1327 set_wfp_burst_enable_register( mode );
1305 1328
1306 1329 LEON_Clear_interrupt( IRQ_WAVEFORM_PICKER );
1307 1330 LEON_Unmask_interrupt( IRQ_WAVEFORM_PICKER );
1308 1331
1309 1332 if (transitionCoarseTime == 0)
1310 1333 {
1311 1334 // instant transition means transition on the next valid date
1312 1335 // this is mandatory to have a good snapshot period and a good correction of the snapshot period
1313 1336 waveform_picker_regs->start_date = time_management_regs->coarse_time + 1;
1314 1337 }
1315 1338 else
1316 1339 {
1317 1340 waveform_picker_regs->start_date = transitionCoarseTime;
1318 1341 }
1319 1342
1320 1343 update_last_valid_transition_date(waveform_picker_regs->start_date);
1321 1344
1322 1345 }
1323 1346
1324 1347 void launch_spectral_matrix( void )
1325 1348 {
1326 1349 SM_reset_current_ring_nodes();
1327 1350
1328 1351 reset_spectral_matrix_regs();
1329 1352
1330 1353 reset_nb_sm();
1331 1354
1332 1355 set_sm_irq_onNewMatrix( 1 );
1333 1356
1334 1357 LEON_Clear_interrupt( IRQ_SPECTRAL_MATRIX );
1335 1358 LEON_Unmask_interrupt( IRQ_SPECTRAL_MATRIX );
1336 1359
1337 1360 }
1338 1361
1339 1362 void set_sm_irq_onNewMatrix( unsigned char value )
1340 1363 {
1341 1364 if (value == 1)
1342 1365 {
1343 1366 spectral_matrix_regs->config = spectral_matrix_regs->config | BIT_IRQ_ON_NEW_MATRIX;
1344 1367 }
1345 1368 else
1346 1369 {
1347 1370 spectral_matrix_regs->config = spectral_matrix_regs->config & MASK_IRQ_ON_NEW_MATRIX; // 1110
1348 1371 }
1349 1372 }
1350 1373
1351 1374 void set_sm_irq_onError( unsigned char value )
1352 1375 {
1353 1376 if (value == 1)
1354 1377 {
1355 1378 spectral_matrix_regs->config = spectral_matrix_regs->config | BIT_IRQ_ON_ERROR;
1356 1379 }
1357 1380 else
1358 1381 {
1359 1382 spectral_matrix_regs->config = spectral_matrix_regs->config & MASK_IRQ_ON_ERROR; // 1101
1360 1383 }
1361 1384 }
1362 1385
1363 1386 //*****************************
1364 1387 // CONFIGURE CALIBRATION SIGNAL
1365 1388 void setCalibrationPrescaler( unsigned int prescaler )
1366 1389 {
1367 1390 // prescaling of the master clock (25 MHz)
1368 1391 // master clock is divided by 2^prescaler
1369 1392 time_management_regs->calPrescaler = prescaler;
1370 1393 }
1371 1394
1372 1395 void setCalibrationDivisor( unsigned int divisionFactor )
1373 1396 {
1374 1397 // division of the prescaled clock by the division factor
1375 1398 time_management_regs->calDivisor = divisionFactor;
1376 1399 }
1377 1400
1378 1401 void setCalibrationData( void )
1379 1402 {
1380 1403 /** This function is used to store the values used to drive the DAC in order to generate the SCM calibration signal
1381 1404 *
1382 1405 * @param void
1383 1406 *
1384 1407 * @return void
1385 1408 *
1386 1409 */
1387 1410
1388 1411 unsigned int k;
1389 1412 unsigned short data;
1390 1413 float val;
1391 1414 float Ts;
1392 1415
1393 1416 time_management_regs->calDataPtr = INIT_CHAR;
1394 1417
1395 1418 Ts = 1 / CAL_FS;
1396 1419
1397 1420 // build the signal for the SCM calibration
1398 1421 for (k = 0; k < CAL_NB_PTS; k++)
1399 1422 {
1400 1423 val = CAL_A0 * sin( CAL_W0 * k * Ts )
1401 1424 + CAL_A1 * sin( CAL_W1 * k * Ts );
1402 1425 data = (unsigned short) ((val * CAL_SCALE_FACTOR) + CONST_2048);
1403 1426 time_management_regs->calData = data & CAL_DATA_MASK;
1404 1427 }
1405 1428 }
1406 1429
1407 1430 void setCalibrationDataInterleaved( void )
1408 1431 {
1409 1432 /** This function is used to store the values used to drive the DAC in order to generate the SCM calibration signal
1410 1433 *
1411 1434 * @param void
1412 1435 *
1413 1436 * @return void
1414 1437 *
1415 1438 * In interleaved mode, one can store more values than in normal mode.
1416 1439 * The data are stored in bunch of 18 bits, 12 bits from one sample and 6 bits from another sample.
1417 1440 * T store 3 values, one need two write operations.
1418 1441 * s1 [ b11 b10 b9 b8 b7 b6 ] s0 [ b11 b10 b9 b8 b7 b6 b5 b3 b2 b1 b0 ]
1419 1442 * s1 [ b5 b4 b3 b2 b1 b0 ] s2 [ b11 b10 b9 b8 b7 b6 b5 b3 b2 b1 b0 ]
1420 1443 *
1421 1444 */
1422 1445
1423 1446 unsigned int k;
1424 1447 float val;
1425 1448 float Ts;
1426 1449 unsigned short data[CAL_NB_PTS_INTER];
1427 1450 unsigned char *dataPtr;
1428 1451
1429 1452 Ts = 1 / CAL_FS_INTER;
1430 1453
1431 1454 time_management_regs->calDataPtr = INIT_CHAR;
1432 1455
1433 1456 // build the signal for the SCM calibration
1434 1457 for (k=0; k<CAL_NB_PTS_INTER; k++)
1435 1458 {
1436 1459 val = sin( 2 * pi * CAL_F0 * k * Ts )
1437 1460 + sin( 2 * pi * CAL_F1 * k * Ts );
1438 1461 data[k] = (unsigned short) ((val * CONST_512) + CONST_2048);
1439 1462 }
1440 1463
1441 1464 // write the signal in interleaved mode
1442 1465 for (k=0; k < STEPS_FOR_STORAGE_INTER; k++)
1443 1466 {
1444 1467 dataPtr = (unsigned char*) &data[ (k * BYTES_FOR_2_SAMPLES) + 2 ];
1445 1468 time_management_regs->calData = ( data[ k * BYTES_FOR_2_SAMPLES ] & CAL_DATA_MASK )
1446 1469 + ( (dataPtr[0] & CAL_DATA_MASK_INTER) << CAL_DATA_SHIFT_INTER);
1447 1470 time_management_regs->calData = ( data[(k * BYTES_FOR_2_SAMPLES) + 1] & CAL_DATA_MASK )
1448 1471 + ( (dataPtr[1] & CAL_DATA_MASK_INTER) << CAL_DATA_SHIFT_INTER);
1449 1472 }
1450 1473 }
1451 1474
1452 1475 void setCalibrationReload( bool state)
1453 1476 {
1454 1477 if (state == true)
1455 1478 {
1456 1479 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_CAL_RELOAD; // [0001 0000]
1457 1480 }
1458 1481 else
1459 1482 {
1460 1483 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_CAL_RELOAD; // [1110 1111]
1461 1484 }
1462 1485 }
1463 1486
1464 1487 void setCalibrationEnable( bool state )
1465 1488 {
1466 1489 // this bit drives the multiplexer
1467 1490 if (state == true)
1468 1491 {
1469 1492 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_CAL_ENABLE; // [0100 0000]
1470 1493 }
1471 1494 else
1472 1495 {
1473 1496 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_CAL_ENABLE; // [1011 1111]
1474 1497 }
1475 1498 }
1476 1499
1477 1500 void setCalibrationInterleaved( bool state )
1478 1501 {
1479 1502 // this bit drives the multiplexer
1480 1503 if (state == true)
1481 1504 {
1482 1505 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl | BIT_SET_INTERLEAVED; // [0010 0000]
1483 1506 }
1484 1507 else
1485 1508 {
1486 1509 time_management_regs->calDACCtrl = time_management_regs->calDACCtrl & MASK_SET_INTERLEAVED; // [1101 1111]
1487 1510 }
1488 1511 }
1489 1512
1490 1513 void setCalibration( bool state )
1491 1514 {
1492 1515 if (state == true)
1493 1516 {
1494 1517 setCalibrationEnable( true );
1495 1518 setCalibrationReload( false );
1496 1519 set_hk_lfr_calib_enable( true );
1497 1520 }
1498 1521 else
1499 1522 {
1500 1523 setCalibrationEnable( false );
1501 1524 setCalibrationReload( true );
1502 1525 set_hk_lfr_calib_enable( false );
1503 1526 }
1504 1527 }
1505 1528
1506 1529 void configureCalibration( bool interleaved )
1507 1530 {
1508 1531 setCalibration( false );
1509 1532 if ( interleaved == true )
1510 1533 {
1511 1534 setCalibrationInterleaved( true );
1512 1535 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1513 1536 setCalibrationDivisor( CAL_F_DIVISOR_INTER ); // => 240 384
1514 1537 setCalibrationDataInterleaved();
1515 1538 }
1516 1539 else
1517 1540 {
1518 1541 setCalibrationPrescaler( 0 ); // 25 MHz => 25 000 000
1519 1542 setCalibrationDivisor( CAL_F_DIVISOR ); // => 160 256 (39 - 1)
1520 1543 setCalibrationData();
1521 1544 }
1522 1545 }
1523 1546
1524 1547 //****************
1525 1548 // CLOSING ACTIONS
1526 1549 void update_last_TC_exe( ccsdsTelecommandPacket_t *TC, unsigned char * time )
1527 1550 {
1528 1551 /** This function is used to update the HK packets statistics after a successful TC execution.
1529 1552 *
1530 1553 * @param TC points to the TC being processed
1531 1554 * @param time is the time used to date the TC execution
1532 1555 *
1533 1556 */
1534 1557
1535 1558 unsigned int val;
1536 1559
1537 1560 housekeeping_packet.hk_lfr_last_exe_tc_id[0] = TC->packetID[0];
1538 1561 housekeeping_packet.hk_lfr_last_exe_tc_id[1] = TC->packetID[1];
1539 1562 housekeeping_packet.hk_lfr_last_exe_tc_type[0] = INIT_CHAR;
1540 1563 housekeeping_packet.hk_lfr_last_exe_tc_type[1] = TC->serviceType;
1541 1564 housekeeping_packet.hk_lfr_last_exe_tc_subtype[0] = INIT_CHAR;
1542 1565 housekeeping_packet.hk_lfr_last_exe_tc_subtype[1] = TC->serviceSubType;
1543 1566 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_0] = time[BYTE_0];
1544 1567 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_1] = time[BYTE_1];
1545 1568 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_2] = time[BYTE_2];
1546 1569 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_3] = time[BYTE_3];
1547 1570 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_4] = time[BYTE_4];
1548 1571 housekeeping_packet.hk_lfr_last_exe_tc_time[BYTE_5] = time[BYTE_5];
1549 1572
1550 1573 val = (housekeeping_packet.hk_lfr_exe_tc_cnt[0] * CONST_256) + housekeeping_packet.hk_lfr_exe_tc_cnt[1];
1551 1574 val++;
1552 1575 housekeeping_packet.hk_lfr_exe_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
1553 1576 housekeeping_packet.hk_lfr_exe_tc_cnt[1] = (unsigned char) (val);
1554 1577 }
1555 1578
1556 1579 void update_last_TC_rej(ccsdsTelecommandPacket_t *TC, unsigned char * time )
1557 1580 {
1558 1581 /** This function is used to update the HK packets statistics after a TC rejection.
1559 1582 *
1560 1583 * @param TC points to the TC being processed
1561 1584 * @param time is the time used to date the TC rejection
1562 1585 *
1563 1586 */
1564 1587
1565 1588 unsigned int val;
1566 1589
1567 1590 housekeeping_packet.hk_lfr_last_rej_tc_id[0] = TC->packetID[0];
1568 1591 housekeeping_packet.hk_lfr_last_rej_tc_id[1] = TC->packetID[1];
1569 1592 housekeeping_packet.hk_lfr_last_rej_tc_type[0] = INIT_CHAR;
1570 1593 housekeeping_packet.hk_lfr_last_rej_tc_type[1] = TC->serviceType;
1571 1594 housekeeping_packet.hk_lfr_last_rej_tc_subtype[0] = INIT_CHAR;
1572 1595 housekeeping_packet.hk_lfr_last_rej_tc_subtype[1] = TC->serviceSubType;
1573 1596 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_0] = time[BYTE_0];
1574 1597 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_1] = time[BYTE_1];
1575 1598 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_2] = time[BYTE_2];
1576 1599 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_3] = time[BYTE_3];
1577 1600 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_4] = time[BYTE_4];
1578 1601 housekeeping_packet.hk_lfr_last_rej_tc_time[BYTE_5] = time[BYTE_5];
1579 1602
1580 1603 val = (housekeeping_packet.hk_lfr_rej_tc_cnt[0] * CONST_256) + housekeeping_packet.hk_lfr_rej_tc_cnt[1];
1581 1604 val++;
1582 1605 housekeeping_packet.hk_lfr_rej_tc_cnt[0] = (unsigned char) (val >> SHIFT_1_BYTE);
1583 1606 housekeeping_packet.hk_lfr_rej_tc_cnt[1] = (unsigned char) (val);
1584 1607 }
1585 1608
1586 1609 void close_action(ccsdsTelecommandPacket_t *TC, int result, rtems_id queue_id )
1587 1610 {
1588 1611 /** This function is the last step of the TC execution workflow.
1589 1612 *
1590 1613 * @param TC points to the TC being processed
1591 1614 * @param result is the result of the TC execution (LFR_SUCCESSFUL / LFR_DEFAULT)
1592 1615 * @param queue_id is the id of the RTEMS message queue used to send TM packets
1593 1616 * @param time is the time used to date the TC execution
1594 1617 *
1595 1618 */
1596 1619
1597 1620 unsigned char requestedMode;
1598 1621
1599 1622 if (result == LFR_SUCCESSFUL)
1600 1623 {
1601 1624 if ( !( (TC->serviceType==TC_TYPE_TIME) & (TC->serviceSubType==TC_SUBTYPE_UPDT_TIME) )
1602 1625 &
1603 1626 !( (TC->serviceType==TC_TYPE_GEN) & (TC->serviceSubType==TC_SUBTYPE_UPDT_INFO))
1604 1627 )
1605 1628 {
1606 1629 send_tm_lfr_tc_exe_success( TC, queue_id );
1607 1630 }
1608 1631 if ( (TC->serviceType == TC_TYPE_GEN) & (TC->serviceSubType == TC_SUBTYPE_ENTER) )
1609 1632 {
1610 1633 //**********************************
1611 1634 // UPDATE THE LFRMODE LOCAL VARIABLE
1612 1635 requestedMode = TC->dataAndCRC[1];
1613 1636 updateLFRCurrentMode( requestedMode );
1614 1637 }
1615 1638 }
1616 1639 else if (result == LFR_EXE_ERROR)
1617 1640 {
1618 1641 send_tm_lfr_tc_exe_error( TC, queue_id );
1619 1642 }
1620 1643 }
1621 1644
1622 1645 //***************************
1623 1646 // Interrupt Service Routines
1624 1647 rtems_isr commutation_isr1( rtems_vector_number vector )
1625 1648 {
1626 1649 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1627 1650 PRINTF("In commutation_isr1 *** Error sending event to DUMB\n")
1628 1651 }
1629 1652 }
1630 1653
1631 1654 rtems_isr commutation_isr2( rtems_vector_number vector )
1632 1655 {
1633 1656 if (rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
1634 1657 PRINTF("In commutation_isr2 *** Error sending event to DUMB\n")
1635 1658 }
1636 1659 }
1637 1660
1638 1661 //****************
1639 1662 // OTHER FUNCTIONS
1640 1663 void updateLFRCurrentMode( unsigned char requestedMode )
1641 1664 {
1642 1665 /** This function updates the value of the global variable lfrCurrentMode.
1643 1666 *
1644 1667 * lfrCurrentMode is a parameter used by several functions to know in which mode LFR is running.
1645 1668 *
1646 1669 */
1647 1670
1648 1671 // update the local value of lfrCurrentMode with the value contained in the housekeeping_packet structure
1649 1672 housekeeping_packet.lfr_status_word[0] = (housekeeping_packet.lfr_status_word[0] & STATUS_WORD_LFR_MODE_MASK)
1650 1673 + (unsigned char) ( requestedMode << STATUS_WORD_LFR_MODE_SHIFT );
1651 1674 lfrCurrentMode = requestedMode;
1652 1675 }
1653 1676
1654 1677 void set_lfr_soft_reset( unsigned char value )
1655 1678 {
1656 1679 if (value == 1)
1657 1680 {
1658 1681 time_management_regs->ctrl = time_management_regs->ctrl | BIT_SOFT_RESET; // [0100]
1659 1682 }
1660 1683 else
1661 1684 {
1662 1685 time_management_regs->ctrl = time_management_regs->ctrl & MASK_SOFT_RESET; // [1011]
1663 1686 }
1664 1687 }
1665 1688
1666 1689 void reset_lfr( void )
1667 1690 {
1668 1691 set_lfr_soft_reset( 1 );
1669 1692
1670 1693 set_lfr_soft_reset( 0 );
1671 1694
1672 1695 set_hk_lfr_sc_potential_flag( true );
1673 1696 }
Show file before | Show file after | Hide comments
@@ -1,2068 +1,2088
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions to load and dump parameters in the LFR registers.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * A group of functions to handle TC related to parameter loading and dumping.\n
7 31 * TC_LFR_LOAD_COMMON_PAR\n
8 32 * TC_LFR_LOAD_NORMAL_PAR\n
9 33 * TC_LFR_LOAD_BURST_PAR\n
10 34 * TC_LFR_LOAD_SBM1_PAR\n
11 35 * TC_LFR_LOAD_SBM2_PAR\n
12 36 *
13 37 */
14 38
15 39 #include "tc_load_dump_parameters.h"
16 40
17 41 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_1 = {0};
18 42 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t kcoefficients_dump_2 = {0};
19 43 ring_node kcoefficient_node_1 = {0};
20 44 ring_node kcoefficient_node_2 = {0};
21 45
22 46 int action_load_common_par(ccsdsTelecommandPacket_t *TC)
23 47 {
24 48 /** This function updates the LFR registers with the incoming common parameters.
25 49 *
26 50 * @param TC points to the TeleCommand packet that is being processed
27 51 *
28 52 *
29 53 */
30 54
31 55 parameter_dump_packet.sy_lfr_common_parameters_spare = TC->dataAndCRC[0];
32 56 parameter_dump_packet.sy_lfr_common_parameters = TC->dataAndCRC[1];
33 57 set_wfp_data_shaping( );
34 58 return LFR_SUCCESSFUL;
35 59 }
36 60
37 61 int action_load_normal_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
38 62 {
39 63 /** This function updates the LFR registers with the incoming normal parameters.
40 64 *
41 65 * @param TC points to the TeleCommand packet that is being processed
42 66 * @param queue_id is the id of the queue which handles TM related to this execution step
43 67 *
44 68 */
45 69
46 70 int result;
47 71 int flag;
48 72 rtems_status_code status;
49 73
50 74 flag = LFR_SUCCESSFUL;
51 75
52 76 if ( (lfrCurrentMode == LFR_MODE_NORMAL) ||
53 77 (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) {
54 78 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
55 79 flag = LFR_DEFAULT;
56 80 }
57 81
58 82 // CHECK THE PARAMETERS SET CONSISTENCY
59 83 if (flag == LFR_SUCCESSFUL)
60 84 {
61 85 flag = check_normal_par_consistency( TC, queue_id );
62 86 }
63 87
64 88 // SET THE PARAMETERS IF THEY ARE CONSISTENT
65 89 if (flag == LFR_SUCCESSFUL)
66 90 {
67 91 result = set_sy_lfr_n_swf_l( TC );
68 92 result = set_sy_lfr_n_swf_p( TC );
69 93 result = set_sy_lfr_n_bp_p0( TC );
70 94 result = set_sy_lfr_n_bp_p1( TC );
71 95 result = set_sy_lfr_n_asm_p( TC );
72 96 result = set_sy_lfr_n_cwf_long_f3( TC );
73 97 }
74 98
75 99 return flag;
76 100 }
77 101
78 102 int action_load_burst_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
79 103 {
80 104 /** This function updates the LFR registers with the incoming burst parameters.
81 105 *
82 106 * @param TC points to the TeleCommand packet that is being processed
83 107 * @param queue_id is the id of the queue which handles TM related to this execution step
84 108 *
85 109 */
86 110
87 111 int flag;
88 112 rtems_status_code status;
89 113 unsigned char sy_lfr_b_bp_p0;
90 114 unsigned char sy_lfr_b_bp_p1;
91 115 float aux;
92 116
93 117 flag = LFR_SUCCESSFUL;
94 118
95 119 if ( lfrCurrentMode == LFR_MODE_BURST ) {
96 120 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
97 121 flag = LFR_DEFAULT;
98 122 }
99 123
100 124 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
101 125 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
102 126
103 127 // sy_lfr_b_bp_p0 shall not be lower than its default value
104 128 if (flag == LFR_SUCCESSFUL)
105 129 {
106 130 if (sy_lfr_b_bp_p0 < DEFAULT_SY_LFR_B_BP_P0 )
107 131 {
108 132 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0 );
109 133 flag = WRONG_APP_DATA;
110 134 }
111 135 }
112 136 // sy_lfr_b_bp_p1 shall not be lower than its default value
113 137 if (flag == LFR_SUCCESSFUL)
114 138 {
115 139 if (sy_lfr_b_bp_p1 < DEFAULT_SY_LFR_B_BP_P1 )
116 140 {
117 141 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P1 + DATAFIELD_OFFSET, sy_lfr_b_bp_p1 );
118 142 flag = WRONG_APP_DATA;
119 143 }
120 144 }
121 145 //****************************************************************
122 146 // check the consistency between sy_lfr_b_bp_p0 and sy_lfr_b_bp_p1
123 147 if (flag == LFR_SUCCESSFUL)
124 148 {
125 149 sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
126 150 sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
127 151 aux = ( (float ) sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0 ) - floor(sy_lfr_b_bp_p1 / sy_lfr_b_bp_p0);
128 152 if (aux > FLOAT_EQUAL_ZERO)
129 153 {
130 154 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_B_BP_P0 + DATAFIELD_OFFSET, sy_lfr_b_bp_p0 );
131 155 flag = LFR_DEFAULT;
132 156 }
133 157 }
134 158
135 159 // SET THE PARAMETERS
136 160 if (flag == LFR_SUCCESSFUL)
137 161 {
138 162 flag = set_sy_lfr_b_bp_p0( TC );
139 163 flag = set_sy_lfr_b_bp_p1( TC );
140 164 }
141 165
142 166 return flag;
143 167 }
144 168
145 169 int action_load_sbm1_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
146 170 {
147 171 /** This function updates the LFR registers with the incoming sbm1 parameters.
148 172 *
149 173 * @param TC points to the TeleCommand packet that is being processed
150 174 * @param queue_id is the id of the queue which handles TM related to this execution step
151 175 *
152 176 */
153 177
154 178 int flag;
155 179 rtems_status_code status;
156 180 unsigned char sy_lfr_s1_bp_p0;
157 181 unsigned char sy_lfr_s1_bp_p1;
158 182 float aux;
159 183
160 184 flag = LFR_SUCCESSFUL;
161 185
162 186 if ( lfrCurrentMode == LFR_MODE_SBM1 ) {
163 187 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
164 188 flag = LFR_DEFAULT;
165 189 }
166 190
167 191 sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
168 192 sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
169 193
170 194 // sy_lfr_s1_bp_p0
171 195 if (flag == LFR_SUCCESSFUL)
172 196 {
173 197 if (sy_lfr_s1_bp_p0 < DEFAULT_SY_LFR_S1_BP_P0 )
174 198 {
175 199 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0 );
176 200 flag = WRONG_APP_DATA;
177 201 }
178 202 }
179 203 // sy_lfr_s1_bp_p1
180 204 if (flag == LFR_SUCCESSFUL)
181 205 {
182 206 if (sy_lfr_s1_bp_p1 < DEFAULT_SY_LFR_S1_BP_P1 )
183 207 {
184 208 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p1 );
185 209 flag = WRONG_APP_DATA;
186 210 }
187 211 }
188 212 //******************************************************************
189 213 // check the consistency between sy_lfr_s1_bp_p0 and sy_lfr_s1_bp_p1
190 214 if (flag == LFR_SUCCESSFUL)
191 215 {
192 216 aux = ( (float ) sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE) )
193 217 - floor(sy_lfr_s1_bp_p1 / (sy_lfr_s1_bp_p0 * S1_BP_P0_SCALE));
194 218 if (aux > FLOAT_EQUAL_ZERO)
195 219 {
196 220 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S1_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s1_bp_p0 );
197 221 flag = LFR_DEFAULT;
198 222 }
199 223 }
200 224
201 225 // SET THE PARAMETERS
202 226 if (flag == LFR_SUCCESSFUL)
203 227 {
204 228 flag = set_sy_lfr_s1_bp_p0( TC );
205 229 flag = set_sy_lfr_s1_bp_p1( TC );
206 230 }
207 231
208 232 return flag;
209 233 }
210 234
211 235 int action_load_sbm2_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
212 236 {
213 237 /** This function updates the LFR registers with the incoming sbm2 parameters.
214 238 *
215 239 * @param TC points to the TeleCommand packet that is being processed
216 240 * @param queue_id is the id of the queue which handles TM related to this execution step
217 241 *
218 242 */
219 243
220 244 int flag;
221 245 rtems_status_code status;
222 246 unsigned char sy_lfr_s2_bp_p0;
223 247 unsigned char sy_lfr_s2_bp_p1;
224 248 float aux;
225 249
226 250 flag = LFR_SUCCESSFUL;
227 251
228 252 if ( lfrCurrentMode == LFR_MODE_SBM2 ) {
229 253 status = send_tm_lfr_tc_exe_not_executable( TC, queue_id );
230 254 flag = LFR_DEFAULT;
231 255 }
232 256
233 257 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
234 258 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
235 259
236 260 // sy_lfr_s2_bp_p0
237 261 if (flag == LFR_SUCCESSFUL)
238 262 {
239 263 if (sy_lfr_s2_bp_p0 < DEFAULT_SY_LFR_S2_BP_P0 )
240 264 {
241 265 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0 );
242 266 flag = WRONG_APP_DATA;
243 267 }
244 268 }
245 269 // sy_lfr_s2_bp_p1
246 270 if (flag == LFR_SUCCESSFUL)
247 271 {
248 272 if (sy_lfr_s2_bp_p1 < DEFAULT_SY_LFR_S2_BP_P1 )
249 273 {
250 274 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P1 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p1 );
251 275 flag = WRONG_APP_DATA;
252 276 }
253 277 }
254 278 //******************************************************************
255 279 // check the consistency between sy_lfr_s2_bp_p0 and sy_lfr_s2_bp_p1
256 280 if (flag == LFR_SUCCESSFUL)
257 281 {
258 282 sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
259 283 sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
260 284 aux = ( (float ) sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0 ) - floor(sy_lfr_s2_bp_p1 / sy_lfr_s2_bp_p0);
261 285 if (aux > FLOAT_EQUAL_ZERO)
262 286 {
263 287 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_S2_BP_P0 + DATAFIELD_OFFSET, sy_lfr_s2_bp_p0 );
264 288 flag = LFR_DEFAULT;
265 289 }
266 290 }
267 291
268 292 // SET THE PARAMETERS
269 293 if (flag == LFR_SUCCESSFUL)
270 294 {
271 295 flag = set_sy_lfr_s2_bp_p0( TC );
272 296 flag = set_sy_lfr_s2_bp_p1( TC );
273 297 }
274 298
275 299 return flag;
276 300 }
277 301
278 302 int action_load_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
279 303 {
280 304 /** This function updates the LFR registers with the incoming sbm2 parameters.
281 305 *
282 306 * @param TC points to the TeleCommand packet that is being processed
283 307 * @param queue_id is the id of the queue which handles TM related to this execution step
284 308 *
285 309 */
286 310
287 311 int flag;
288 312
289 313 flag = LFR_DEFAULT;
290 314
291 315 flag = set_sy_lfr_kcoeff( TC, queue_id );
292 316
293 317 return flag;
294 318 }
295 319
296 320 int action_load_fbins_mask(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
297 321 {
298 322 /** This function updates the LFR registers with the incoming sbm2 parameters.
299 323 *
300 324 * @param TC points to the TeleCommand packet that is being processed
301 325 * @param queue_id is the id of the queue which handles TM related to this execution step
302 326 *
303 327 */
304 328
305 329 int flag;
306 330
307 331 flag = LFR_DEFAULT;
308 332
309 333 flag = set_sy_lfr_fbins( TC );
310 334
311 335 // once the fbins masks have been stored, they have to be merged with the masks which handle the reaction wheels frequencies filtering
312 336 merge_fbins_masks();
313 337
314 338 return flag;
315 339 }
316 340
317 341 int action_load_filter_par(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
318 342 {
319 343 /** This function updates the LFR registers with the incoming sbm2 parameters.
320 344 *
321 345 * @param TC points to the TeleCommand packet that is being processed
322 346 * @param queue_id is the id of the queue which handles TM related to this execution step
323 347 *
324 348 */
325 349
326 350 int flag;
327 351 unsigned char k;
328 352
329 353 flag = LFR_DEFAULT;
330 354 k = INIT_CHAR;
331 355
332 356 flag = check_sy_lfr_filter_parameters( TC, queue_id );
333 357
334 358 if (flag == LFR_SUCCESSFUL)
335 359 {
336 360 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ];
337 361 parameter_dump_packet.sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
338 362 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_0 ];
339 363 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_1 ];
340 364 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_2 ];
341 365 parameter_dump_packet.sy_lfr_pas_filter_tbad[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + BYTE_3 ];
342 366 parameter_dump_packet.sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
343 367 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_0 ];
344 368 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_1 ];
345 369 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_2 ];
346 370 parameter_dump_packet.sy_lfr_pas_filter_shift[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + BYTE_3 ];
347 371 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_0 ];
348 372 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_1 ];
349 373 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_2] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_2 ];
350 374 parameter_dump_packet.sy_lfr_sc_rw_delta_f[BYTE_3] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + BYTE_3 ];
351 375
352 376 //****************************
353 377 // store PAS filter parameters
354 378
355 379 // sy_lfr_pas_filter_enabled
356 380 filterPar.spare_sy_lfr_pas_filter_enabled = parameter_dump_packet.spare_sy_lfr_pas_filter_enabled;
357 381 set_sy_lfr_pas_filter_enabled( parameter_dump_packet.spare_sy_lfr_pas_filter_enabled & BIT_PAS_FILTER_ENABLED );
358 382
359 383 // sy_lfr_pas_filter_modulus
360 384 filterPar.modulus_in_finetime = ((uint64_t) parameter_dump_packet.sy_lfr_pas_filter_modulus) * CONST_65536;
361 385
362 386 // sy_lfr_pas_filter_tbad
363 387 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_tbad,
364 388 parameter_dump_packet.sy_lfr_pas_filter_tbad );
365 389 filterPar.tbad_in_finetime = (uint64_t) (filterPar.sy_lfr_pas_filter_tbad * CONST_65536);
366 390
367 391 // sy_lfr_pas_filter_offset
368 392 filterPar.offset_in_finetime = ((uint64_t) parameter_dump_packet.sy_lfr_pas_filter_offset) * CONST_65536;
369 393
370 394 // sy_lfr_pas_filter_shift
371 395 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_pas_filter_shift,
372 396 parameter_dump_packet.sy_lfr_pas_filter_shift );
373 397 filterPar.shift_in_finetime = (uint64_t) (filterPar.sy_lfr_pas_filter_shift * CONST_65536);
374 398
375 399 //****************************************************
376 400 // store the parameter sy_lfr_sc_rw_delta_f as a float
377 401 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_sc_rw_delta_f,
378 402 parameter_dump_packet.sy_lfr_sc_rw_delta_f );
379 403
380 404 // copy rw.._k.. from the incoming TC to the local parameter_dump_packet
381 405 for (k = 0; k < NB_RW_K_COEFFS * NB_BYTES_PER_RW_K_COEFF; k++)
382 406 {
383 407 parameter_dump_packet.sy_lfr_rw1_k1[k] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_RW1_K1 + k ];
384 408 }
385 409
386 410 //***********************************************
387 411 // store the parameter sy_lfr_rw.._k.. as a float
388 412 // rw1_k
389 413 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k1, parameter_dump_packet.sy_lfr_rw1_k1 );
390 414 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k2, parameter_dump_packet.sy_lfr_rw1_k2 );
391 415 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k3, parameter_dump_packet.sy_lfr_rw1_k3 );
392 416 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw1_k4, parameter_dump_packet.sy_lfr_rw1_k4 );
393 417 // rw2_k
394 418 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k1, parameter_dump_packet.sy_lfr_rw2_k1 );
395 419 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k2, parameter_dump_packet.sy_lfr_rw2_k2 );
396 420 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k3, parameter_dump_packet.sy_lfr_rw2_k3 );
397 421 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw2_k4, parameter_dump_packet.sy_lfr_rw2_k4 );
398 422 // rw3_k
399 423 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k1, parameter_dump_packet.sy_lfr_rw3_k1 );
400 424 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k2, parameter_dump_packet.sy_lfr_rw3_k2 );
401 425 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k3, parameter_dump_packet.sy_lfr_rw3_k3 );
402 426 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw3_k4, parameter_dump_packet.sy_lfr_rw3_k4 );
403 427 // rw4_k
404 428 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k1, parameter_dump_packet.sy_lfr_rw4_k1 );
405 429 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k2, parameter_dump_packet.sy_lfr_rw4_k2 );
406 430 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k3, parameter_dump_packet.sy_lfr_rw4_k3 );
407 431 copyFloatByChar( (unsigned char*) &filterPar.sy_lfr_rw4_k4, parameter_dump_packet.sy_lfr_rw4_k4 );
408 432
409 433 }
410 434
411 435 return flag;
412 436 }
413 437
414 438 int action_dump_kcoefficients(ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time)
415 439 {
416 440 /** This function updates the LFR registers with the incoming sbm2 parameters.
417 441 *
418 442 * @param TC points to the TeleCommand packet that is being processed
419 443 * @param queue_id is the id of the queue which handles TM related to this execution step
420 444 *
421 445 */
422 446
423 447 unsigned int address;
424 448 rtems_status_code status;
425 449 unsigned int freq;
426 450 unsigned int bin;
427 451 unsigned int coeff;
428 452 unsigned char *kCoeffPtr;
429 453 unsigned char *kCoeffDumpPtr;
430 454
431 455 // for each sy_lfr_kcoeff_frequency there is 32 kcoeff
432 456 // F0 => 11 bins
433 457 // F1 => 13 bins
434 458 // F2 => 12 bins
435 459 // 36 bins to dump in two packets (30 bins max per packet)
436 460
437 461 //*********
438 462 // PACKET 1
439 463 // 11 F0 bins, 13 F1 bins and 6 F2 bins
440 464 kcoefficients_dump_1.destinationID = TC->sourceID;
441 465 increment_seq_counter_destination_id_dump( kcoefficients_dump_1.packetSequenceControl, TC->sourceID );
442 466 for( freq = 0;
443 467 freq < NB_BINS_COMPRESSED_SM_F0;
444 468 freq++ )
445 469 {
446 470 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1] = freq;
447 471 bin = freq;
448 // printKCoefficients( freq, bin, k_coeff_intercalib_f0_norm);
449 472 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
450 473 {
451 474 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
452 475 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
453 476 ]; // 2 for the kcoeff_frequency
454 477 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f0_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
455 478 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
456 479 }
457 480 }
458 481 for( freq = NB_BINS_COMPRESSED_SM_F0;
459 482 freq < ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
460 483 freq++ )
461 484 {
462 485 kcoefficients_dump_1.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = freq;
463 486 bin = freq - NB_BINS_COMPRESSED_SM_F0;
464 // printKCoefficients( freq, bin, k_coeff_intercalib_f1_norm);
465 487 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
466 488 {
467 489 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
468 490 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
469 491 ]; // 2 for the kcoeff_frequency
470 492 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f1_norm[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
471 493 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
472 494 }
473 495 }
474 496 for( freq = ( NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 );
475 497 freq < KCOEFF_BLK_NR_PKT1 ;
476 498 freq++ )
477 499 {
478 500 kcoefficients_dump_1.kcoeff_blks[ (freq * KCOEFF_BLK_SIZE) + 1 ] = freq;
479 501 bin = freq - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
480 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
481 502 for ( coeff = 0; coeff <NB_K_COEFF_PER_BIN; coeff++ )
482 503 {
483 504 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_1.kcoeff_blks[
484 505 (freq * KCOEFF_BLK_SIZE) + (coeff * NB_BYTES_PER_FLOAT) + KCOEFF_FREQ
485 506 ]; // 2 for the kcoeff_frequency
486 507 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
487 508 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
488 509 }
489 510 }
490 511 kcoefficients_dump_1.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
491 512 kcoefficients_dump_1.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
492 513 kcoefficients_dump_1.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
493 514 kcoefficients_dump_1.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
494 515 kcoefficients_dump_1.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
495 516 kcoefficients_dump_1.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
496 517 // SEND DATA
497 518 kcoefficient_node_1.status = 1;
498 519 address = (unsigned int) &kcoefficient_node_1;
499 520 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
500 521 if (status != RTEMS_SUCCESSFUL) {
501 522 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 1 , code %d", status)
502 523 }
503 524
504 525 //********
505 526 // PACKET 2
506 527 // 6 F2 bins
507 528 kcoefficients_dump_2.destinationID = TC->sourceID;
508 529 increment_seq_counter_destination_id_dump( kcoefficients_dump_2.packetSequenceControl, TC->sourceID );
509 530 for( freq = 0;
510 531 freq < KCOEFF_BLK_NR_PKT2;
511 532 freq++ )
512 533 {
513 534 kcoefficients_dump_2.kcoeff_blks[ (freq*KCOEFF_BLK_SIZE) + 1 ] = KCOEFF_BLK_NR_PKT1 + freq;
514 535 bin = freq + KCOEFF_BLK_NR_PKT2;
515 // printKCoefficients( freq, bin, k_coeff_intercalib_f2);
516 536 for ( coeff=0; coeff<NB_K_COEFF_PER_BIN; coeff++ )
517 537 {
518 538 kCoeffDumpPtr = (unsigned char*) &kcoefficients_dump_2.kcoeff_blks[
519 539 (freq*KCOEFF_BLK_SIZE) + (coeff*NB_BYTES_PER_FLOAT) + KCOEFF_FREQ ]; // 2 for the kcoeff_frequency
520 540 kCoeffPtr = (unsigned char*) &k_coeff_intercalib_f2[ (bin*NB_K_COEFF_PER_BIN) + coeff ];
521 541 copyFloatByChar( kCoeffDumpPtr, kCoeffPtr );
522 542 }
523 543 }
524 544 kcoefficients_dump_2.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
525 545 kcoefficients_dump_2.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
526 546 kcoefficients_dump_2.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
527 547 kcoefficients_dump_2.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
528 548 kcoefficients_dump_2.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
529 549 kcoefficients_dump_2.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
530 550 // SEND DATA
531 551 kcoefficient_node_2.status = 1;
532 552 address = (unsigned int) &kcoefficient_node_2;
533 553 status = rtems_message_queue_send( queue_id, &address, sizeof( ring_node* ) );
534 554 if (status != RTEMS_SUCCESSFUL) {
535 555 PRINTF1("in action_dump_kcoefficients *** ERR sending packet 2, code %d", status)
536 556 }
537 557
538 558 return status;
539 559 }
540 560
541 561 int action_dump_par( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
542 562 {
543 563 /** This function dumps the LFR parameters by sending the appropriate TM packet to the dedicated RTEMS message queue.
544 564 *
545 565 * @param queue_id is the id of the queue which handles TM related to this execution step.
546 566 *
547 567 * @return RTEMS directive status codes:
548 568 * - RTEMS_SUCCESSFUL - message sent successfully
549 569 * - RTEMS_INVALID_ID - invalid queue id
550 570 * - RTEMS_INVALID_SIZE - invalid message size
551 571 * - RTEMS_INVALID_ADDRESS - buffer is NULL
552 572 * - RTEMS_UNSATISFIED - out of message buffers
553 573 * - RTEMS_TOO_MANY - queue s limit has been reached
554 574 *
555 575 */
556 576
557 577 int status;
558 578
559 579 increment_seq_counter_destination_id_dump( parameter_dump_packet.packetSequenceControl, TC->sourceID );
560 580 parameter_dump_packet.destinationID = TC->sourceID;
561 581
562 582 // UPDATE TIME
563 583 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
564 584 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
565 585 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
566 586 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
567 587 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
568 588 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
569 589 // SEND DATA
570 590 status = rtems_message_queue_send( queue_id, &parameter_dump_packet,
571 591 PACKET_LENGTH_PARAMETER_DUMP + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
572 592 if (status != RTEMS_SUCCESSFUL) {
573 593 PRINTF1("in action_dump *** ERR sending packet, code %d", status)
574 594 }
575 595
576 596 return status;
577 597 }
578 598
579 599 //***********************
580 600 // NORMAL MODE PARAMETERS
581 601
582 602 int check_normal_par_consistency( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
583 603 {
584 604 unsigned char msb;
585 605 unsigned char lsb;
586 606 int flag;
587 607 float aux;
588 608 rtems_status_code status;
589 609
590 610 unsigned int sy_lfr_n_swf_l;
591 611 unsigned int sy_lfr_n_swf_p;
592 612 unsigned int sy_lfr_n_asm_p;
593 613 unsigned char sy_lfr_n_bp_p0;
594 614 unsigned char sy_lfr_n_bp_p1;
595 615 unsigned char sy_lfr_n_cwf_long_f3;
596 616
597 617 flag = LFR_SUCCESSFUL;
598 618
599 619 //***************
600 620 // get parameters
601 621 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
602 622 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
603 623 sy_lfr_n_swf_l = (msb * CONST_256) + lsb;
604 624
605 625 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
606 626 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
607 627 sy_lfr_n_swf_p = (msb * CONST_256) + lsb;
608 628
609 629 msb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
610 630 lsb = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
611 631 sy_lfr_n_asm_p = (msb * CONST_256) + lsb;
612 632
613 633 sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
614 634
615 635 sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
616 636
617 637 sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
618 638
619 639 //******************
620 640 // check consistency
621 641 // sy_lfr_n_swf_l
622 642 if (sy_lfr_n_swf_l != DFLT_SY_LFR_N_SWF_L)
623 643 {
624 644 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_L + DATAFIELD_OFFSET, sy_lfr_n_swf_l );
625 645 flag = WRONG_APP_DATA;
626 646 }
627 647 // sy_lfr_n_swf_p
628 648 if (flag == LFR_SUCCESSFUL)
629 649 {
630 650 if ( sy_lfr_n_swf_p < MIN_SY_LFR_N_SWF_P )
631 651 {
632 652 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_SWF_P + DATAFIELD_OFFSET, sy_lfr_n_swf_p );
633 653 flag = WRONG_APP_DATA;
634 654 }
635 655 }
636 656 // sy_lfr_n_bp_p0
637 657 if (flag == LFR_SUCCESSFUL)
638 658 {
639 659 if (sy_lfr_n_bp_p0 < DFLT_SY_LFR_N_BP_P0)
640 660 {
641 661 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P0 + DATAFIELD_OFFSET, sy_lfr_n_bp_p0 );
642 662 flag = WRONG_APP_DATA;
643 663 }
644 664 }
645 665 // sy_lfr_n_asm_p
646 666 if (flag == LFR_SUCCESSFUL)
647 667 {
648 668 if (sy_lfr_n_asm_p == 0)
649 669 {
650 670 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
651 671 flag = WRONG_APP_DATA;
652 672 }
653 673 }
654 674 // sy_lfr_n_asm_p shall be a whole multiple of sy_lfr_n_bp_p0
655 675 if (flag == LFR_SUCCESSFUL)
656 676 {
657 677 aux = ( (float ) sy_lfr_n_asm_p / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_asm_p / sy_lfr_n_bp_p0);
658 678 if (aux > FLOAT_EQUAL_ZERO)
659 679 {
660 680 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_ASM_P + DATAFIELD_OFFSET, sy_lfr_n_asm_p );
661 681 flag = WRONG_APP_DATA;
662 682 }
663 683 }
664 684 // sy_lfr_n_bp_p1
665 685 if (flag == LFR_SUCCESSFUL)
666 686 {
667 687 if (sy_lfr_n_bp_p1 < DFLT_SY_LFR_N_BP_P1)
668 688 {
669 689 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
670 690 flag = WRONG_APP_DATA;
671 691 }
672 692 }
673 693 // sy_lfr_n_bp_p1 shall be a whole multiple of sy_lfr_n_bp_p0
674 694 if (flag == LFR_SUCCESSFUL)
675 695 {
676 696 aux = ( (float ) sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0 ) - floor(sy_lfr_n_bp_p1 / sy_lfr_n_bp_p0);
677 697 if (aux > FLOAT_EQUAL_ZERO)
678 698 {
679 699 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_N_BP_P1 + DATAFIELD_OFFSET, sy_lfr_n_bp_p1 );
680 700 flag = LFR_DEFAULT;
681 701 }
682 702 }
683 703 // sy_lfr_n_cwf_long_f3
684 704
685 705 return flag;
686 706 }
687 707
688 708 int set_sy_lfr_n_swf_l( ccsdsTelecommandPacket_t *TC )
689 709 {
690 710 /** This function sets the number of points of a snapshot (sy_lfr_n_swf_l).
691 711 *
692 712 * @param TC points to the TeleCommand packet that is being processed
693 713 * @param queue_id is the id of the queue which handles TM related to this execution step
694 714 *
695 715 */
696 716
697 717 int result;
698 718
699 719 result = LFR_SUCCESSFUL;
700 720
701 721 parameter_dump_packet.sy_lfr_n_swf_l[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L ];
702 722 parameter_dump_packet.sy_lfr_n_swf_l[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_L+1 ];
703 723
704 724 return result;
705 725 }
706 726
707 727 int set_sy_lfr_n_swf_p(ccsdsTelecommandPacket_t *TC )
708 728 {
709 729 /** This function sets the time between two snapshots, in s (sy_lfr_n_swf_p).
710 730 *
711 731 * @param TC points to the TeleCommand packet that is being processed
712 732 * @param queue_id is the id of the queue which handles TM related to this execution step
713 733 *
714 734 */
715 735
716 736 int result;
717 737
718 738 result = LFR_SUCCESSFUL;
719 739
720 740 parameter_dump_packet.sy_lfr_n_swf_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P ];
721 741 parameter_dump_packet.sy_lfr_n_swf_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_SWF_P+1 ];
722 742
723 743 return result;
724 744 }
725 745
726 746 int set_sy_lfr_n_asm_p( ccsdsTelecommandPacket_t *TC )
727 747 {
728 748 /** This function sets the time between two full spectral matrices transmission, in s (SY_LFR_N_ASM_P).
729 749 *
730 750 * @param TC points to the TeleCommand packet that is being processed
731 751 * @param queue_id is the id of the queue which handles TM related to this execution step
732 752 *
733 753 */
734 754
735 755 int result;
736 756
737 757 result = LFR_SUCCESSFUL;
738 758
739 759 parameter_dump_packet.sy_lfr_n_asm_p[0] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P ];
740 760 parameter_dump_packet.sy_lfr_n_asm_p[1] = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_ASM_P+1 ];
741 761
742 762 return result;
743 763 }
744 764
745 765 int set_sy_lfr_n_bp_p0( ccsdsTelecommandPacket_t *TC )
746 766 {
747 767 /** This function sets the time between two basic parameter sets, in s (DFLT_SY_LFR_N_BP_P0).
748 768 *
749 769 * @param TC points to the TeleCommand packet that is being processed
750 770 * @param queue_id is the id of the queue which handles TM related to this execution step
751 771 *
752 772 */
753 773
754 774 int status;
755 775
756 776 status = LFR_SUCCESSFUL;
757 777
758 778 parameter_dump_packet.sy_lfr_n_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P0 ];
759 779
760 780 return status;
761 781 }
762 782
763 783 int set_sy_lfr_n_bp_p1(ccsdsTelecommandPacket_t *TC )
764 784 {
765 785 /** This function sets the time between two basic parameter sets (autocorrelation + crosscorrelation), in s (sy_lfr_n_bp_p1).
766 786 *
767 787 * @param TC points to the TeleCommand packet that is being processed
768 788 * @param queue_id is the id of the queue which handles TM related to this execution step
769 789 *
770 790 */
771 791
772 792 int status;
773 793
774 794 status = LFR_SUCCESSFUL;
775 795
776 796 parameter_dump_packet.sy_lfr_n_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_BP_P1 ];
777 797
778 798 return status;
779 799 }
780 800
781 801 int set_sy_lfr_n_cwf_long_f3(ccsdsTelecommandPacket_t *TC )
782 802 {
783 803 /** This function allows to switch from CWF_F3 packets to CWF_LONG_F3 packets.
784 804 *
785 805 * @param TC points to the TeleCommand packet that is being processed
786 806 * @param queue_id is the id of the queue which handles TM related to this execution step
787 807 *
788 808 */
789 809
790 810 int status;
791 811
792 812 status = LFR_SUCCESSFUL;
793 813
794 814 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_N_CWF_LONG_F3 ];
795 815
796 816 return status;
797 817 }
798 818
799 819 //**********************
800 820 // BURST MODE PARAMETERS
801 821
802 822 int set_sy_lfr_b_bp_p0(ccsdsTelecommandPacket_t *TC)
803 823 {
804 824 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P0).
805 825 *
806 826 * @param TC points to the TeleCommand packet that is being processed
807 827 * @param queue_id is the id of the queue which handles TM related to this execution step
808 828 *
809 829 */
810 830
811 831 int status;
812 832
813 833 status = LFR_SUCCESSFUL;
814 834
815 835 parameter_dump_packet.sy_lfr_b_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P0 ];
816 836
817 837 return status;
818 838 }
819 839
820 840 int set_sy_lfr_b_bp_p1( ccsdsTelecommandPacket_t *TC )
821 841 {
822 842 /** This function sets the time between two basic parameter sets, in s (SY_LFR_B_BP_P1).
823 843 *
824 844 * @param TC points to the TeleCommand packet that is being processed
825 845 * @param queue_id is the id of the queue which handles TM related to this execution step
826 846 *
827 847 */
828 848
829 849 int status;
830 850
831 851 status = LFR_SUCCESSFUL;
832 852
833 853 parameter_dump_packet.sy_lfr_b_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_B_BP_P1 ];
834 854
835 855 return status;
836 856 }
837 857
838 858 //*********************
839 859 // SBM1 MODE PARAMETERS
840 860
841 861 int set_sy_lfr_s1_bp_p0( ccsdsTelecommandPacket_t *TC )
842 862 {
843 863 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P0).
844 864 *
845 865 * @param TC points to the TeleCommand packet that is being processed
846 866 * @param queue_id is the id of the queue which handles TM related to this execution step
847 867 *
848 868 */
849 869
850 870 int status;
851 871
852 872 status = LFR_SUCCESSFUL;
853 873
854 874 parameter_dump_packet.sy_lfr_s1_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P0 ];
855 875
856 876 return status;
857 877 }
858 878
859 879 int set_sy_lfr_s1_bp_p1( ccsdsTelecommandPacket_t *TC )
860 880 {
861 881 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S1_BP_P1).
862 882 *
863 883 * @param TC points to the TeleCommand packet that is being processed
864 884 * @param queue_id is the id of the queue which handles TM related to this execution step
865 885 *
866 886 */
867 887
868 888 int status;
869 889
870 890 status = LFR_SUCCESSFUL;
871 891
872 892 parameter_dump_packet.sy_lfr_s1_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S1_BP_P1 ];
873 893
874 894 return status;
875 895 }
876 896
877 897 //*********************
878 898 // SBM2 MODE PARAMETERS
879 899
880 900 int set_sy_lfr_s2_bp_p0( ccsdsTelecommandPacket_t *TC )
881 901 {
882 902 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P0).
883 903 *
884 904 * @param TC points to the TeleCommand packet that is being processed
885 905 * @param queue_id is the id of the queue which handles TM related to this execution step
886 906 *
887 907 */
888 908
889 909 int status;
890 910
891 911 status = LFR_SUCCESSFUL;
892 912
893 913 parameter_dump_packet.sy_lfr_s2_bp_p0 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P0 ];
894 914
895 915 return status;
896 916 }
897 917
898 918 int set_sy_lfr_s2_bp_p1( ccsdsTelecommandPacket_t *TC )
899 919 {
900 920 /** This function sets the time between two basic parameter sets, in s (SY_LFR_S2_BP_P1).
901 921 *
902 922 * @param TC points to the TeleCommand packet that is being processed
903 923 * @param queue_id is the id of the queue which handles TM related to this execution step
904 924 *
905 925 */
906 926
907 927 int status;
908 928
909 929 status = LFR_SUCCESSFUL;
910 930
911 931 parameter_dump_packet.sy_lfr_s2_bp_p1 = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_S2_BP_P1 ];
912 932
913 933 return status;
914 934 }
915 935
916 936 //*******************
917 937 // TC_LFR_UPDATE_INFO
918 938
919 939 unsigned int check_update_info_hk_lfr_mode( unsigned char mode )
920 940 {
921 941 unsigned int status;
922 942
923 943 status = LFR_DEFAULT;
924 944
925 945 if ( (mode == LFR_MODE_STANDBY) || (mode == LFR_MODE_NORMAL)
926 946 || (mode == LFR_MODE_BURST)
927 947 || (mode == LFR_MODE_SBM1) || (mode == LFR_MODE_SBM2))
928 948 {
929 949 status = LFR_SUCCESSFUL;
930 950 }
931 951 else
932 952 {
933 953 status = LFR_DEFAULT;
934 954 }
935 955
936 956 return status;
937 957 }
938 958
939 959 unsigned int check_update_info_hk_tds_mode( unsigned char mode )
940 960 {
941 961 unsigned int status;
942 962
943 963 status = LFR_DEFAULT;
944 964
945 965 if ( (mode == TDS_MODE_STANDBY) || (mode == TDS_MODE_NORMAL)
946 966 || (mode == TDS_MODE_BURST)
947 967 || (mode == TDS_MODE_SBM1) || (mode == TDS_MODE_SBM2)
948 968 || (mode == TDS_MODE_LFM))
949 969 {
950 970 status = LFR_SUCCESSFUL;
951 971 }
952 972 else
953 973 {
954 974 status = LFR_DEFAULT;
955 975 }
956 976
957 977 return status;
958 978 }
959 979
960 980 unsigned int check_update_info_hk_thr_mode( unsigned char mode )
961 981 {
962 982 unsigned int status;
963 983
964 984 status = LFR_DEFAULT;
965 985
966 986 if ( (mode == THR_MODE_STANDBY) || (mode == THR_MODE_NORMAL)
967 987 || (mode == THR_MODE_BURST))
968 988 {
969 989 status = LFR_SUCCESSFUL;
970 990 }
971 991 else
972 992 {
973 993 status = LFR_DEFAULT;
974 994 }
975 995
976 996 return status;
977 997 }
978 998
979 999 void set_hk_lfr_sc_rw_f_flag( unsigned char wheel, unsigned char freq, float value )
980 1000 {
981 1001 unsigned char flag;
982 1002 unsigned char flagPosInByte;
983 1003 unsigned char newFlag;
984 1004 unsigned char flagMask;
985 1005
986 1006 // if the frequency value is not a number, the flag is set to 0 and the frequency RWx_Fy is not filtered
987 1007 if (isnan(value))
988 1008 {
989 1009 flag = FLAG_NAN;
990 1010 }
991 1011 else
992 1012 {
993 1013 flag = FLAG_IAN;
994 1014 }
995 1015
996 1016 switch(wheel)
997 1017 {
998 1018 case WHEEL_1:
999 1019 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
1000 1020 flagMask = ~(1 << flagPosInByte);
1001 1021 newFlag = flag << flagPosInByte;
1002 1022 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
1003 1023 break;
1004 1024 case WHEEL_2:
1005 1025 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1006 1026 flagMask = ~(1 << flagPosInByte);
1007 1027 newFlag = flag << flagPosInByte;
1008 1028 housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags = (housekeeping_packet.hk_lfr_sc_rw1_rw2_f_flags & flagMask) | newFlag;
1009 1029 break;
1010 1030 case WHEEL_3:
1011 1031 flagPosInByte = FLAG_OFFSET_WHEELS_1_3 - freq;
1012 1032 flagMask = ~(1 << flagPosInByte);
1013 1033 newFlag = flag << flagPosInByte;
1014 1034 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1015 1035 break;
1016 1036 case WHEEL_4:
1017 1037 flagPosInByte = FLAG_OFFSET_WHEELS_2_4 - freq;
1018 1038 flagMask = ~(1 << flagPosInByte);
1019 1039 newFlag = flag << flagPosInByte;
1020 1040 housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags = (housekeeping_packet.hk_lfr_sc_rw3_rw4_f_flags & flagMask) | newFlag;
1021 1041 break;
1022 1042 default:
1023 1043 break;
1024 1044 }
1025 1045 }
1026 1046
1027 1047 void set_hk_lfr_sc_rw_f_flags( void )
1028 1048 {
1029 1049 // RW1
1030 1050 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_1, rw_f.cp_rpw_sc_rw1_f1 );
1031 1051 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_2, rw_f.cp_rpw_sc_rw1_f2 );
1032 1052 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_3, rw_f.cp_rpw_sc_rw1_f3 );
1033 1053 set_hk_lfr_sc_rw_f_flag( WHEEL_1, FREQ_4, rw_f.cp_rpw_sc_rw1_f4 );
1034 1054
1035 1055 // RW2
1036 1056 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_1, rw_f.cp_rpw_sc_rw2_f1 );
1037 1057 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_2, rw_f.cp_rpw_sc_rw2_f2 );
1038 1058 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_3, rw_f.cp_rpw_sc_rw2_f3 );
1039 1059 set_hk_lfr_sc_rw_f_flag( WHEEL_2, FREQ_4, rw_f.cp_rpw_sc_rw2_f4 );
1040 1060
1041 1061 // RW3
1042 1062 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_1, rw_f.cp_rpw_sc_rw3_f1 );
1043 1063 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_2, rw_f.cp_rpw_sc_rw3_f2 );
1044 1064 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_3, rw_f.cp_rpw_sc_rw3_f3 );
1045 1065 set_hk_lfr_sc_rw_f_flag( WHEEL_3, FREQ_4, rw_f.cp_rpw_sc_rw3_f4 );
1046 1066
1047 1067 // RW4
1048 1068 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_1, rw_f.cp_rpw_sc_rw4_f1 );
1049 1069 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_2, rw_f.cp_rpw_sc_rw4_f2 );
1050 1070 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_3, rw_f.cp_rpw_sc_rw4_f3 );
1051 1071 set_hk_lfr_sc_rw_f_flag( WHEEL_4, FREQ_4, rw_f.cp_rpw_sc_rw4_f4 );
1052 1072 }
1053 1073
1054 1074 int check_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1055 1075 {
1056 1076 float rw_k;
1057 1077 int ret;
1058 1078
1059 1079 ret = LFR_SUCCESSFUL;
1060 1080 rw_k = INIT_FLOAT;
1061 1081
1062 1082 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->packetID[ offset ] );
1063 1083
1064 1084 *pos = offset;
1065 1085 *value = rw_k;
1066 1086
1067 1087 if (rw_k < MIN_SY_LFR_RW_F)
1068 1088 {
1069 1089 ret = WRONG_APP_DATA;
1070 1090 }
1071 1091
1072 1092 return ret;
1073 1093 }
1074 1094
1075 1095 int check_all_sy_lfr_rw_f( ccsdsTelecommandPacket_t *TC, int *pos, float*value )
1076 1096 {
1077 1097 int ret;
1078 1098
1079 1099 ret = LFR_SUCCESSFUL;
1080 1100
1081 1101 //****
1082 1102 //****
1083 1103 // RW1
1084 1104 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1, pos, value ); // F1
1085 1105 if (ret == LFR_SUCCESSFUL) // F2
1086 1106 {
1087 1107 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2, pos, value );
1088 1108 }
1089 1109 if (ret == LFR_SUCCESSFUL) // F3
1090 1110 {
1091 1111 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3, pos, value );
1092 1112 }
1093 1113 if (ret == LFR_SUCCESSFUL) // F4
1094 1114 {
1095 1115 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4, pos, value );
1096 1116 }
1097 1117
1098 1118 //****
1099 1119 //****
1100 1120 // RW2
1101 1121 if (ret == LFR_SUCCESSFUL) // F1
1102 1122 {
1103 1123 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1, pos, value );
1104 1124 }
1105 1125 if (ret == LFR_SUCCESSFUL) // F2
1106 1126 {
1107 1127 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2, pos, value );
1108 1128 }
1109 1129 if (ret == LFR_SUCCESSFUL) // F3
1110 1130 {
1111 1131 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3, pos, value );
1112 1132 }
1113 1133 if (ret == LFR_SUCCESSFUL) // F4
1114 1134 {
1115 1135 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4, pos, value );
1116 1136 }
1117 1137
1118 1138 //****
1119 1139 //****
1120 1140 // RW3
1121 1141 if (ret == LFR_SUCCESSFUL) // F1
1122 1142 {
1123 1143 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1, pos, value );
1124 1144 }
1125 1145 if (ret == LFR_SUCCESSFUL) // F2
1126 1146 {
1127 1147 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2, pos, value );
1128 1148 }
1129 1149 if (ret == LFR_SUCCESSFUL) // F3
1130 1150 {
1131 1151 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3, pos, value );
1132 1152 }
1133 1153 if (ret == LFR_SUCCESSFUL) // F4
1134 1154 {
1135 1155 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4, pos, value );
1136 1156 }
1137 1157
1138 1158 //****
1139 1159 //****
1140 1160 // RW4
1141 1161 if (ret == LFR_SUCCESSFUL) // F1
1142 1162 {
1143 1163 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1, pos, value );
1144 1164 }
1145 1165 if (ret == LFR_SUCCESSFUL) // F2
1146 1166 {
1147 1167 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2, pos, value );
1148 1168 }
1149 1169 if (ret == LFR_SUCCESSFUL) // F3
1150 1170 {
1151 1171 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3, pos, value );
1152 1172 }
1153 1173 if (ret == LFR_SUCCESSFUL) // F4
1154 1174 {
1155 1175 ret = check_sy_lfr_rw_f( TC, BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4, pos, value );
1156 1176 }
1157 1177
1158 1178 return ret;
1159 1179 }
1160 1180
1161 1181 void getReactionWheelsFrequencies( ccsdsTelecommandPacket_t *TC )
1162 1182 {
1163 1183 /** This function get the reaction wheels frequencies in the incoming TC_LFR_UPDATE_INFO and copy the values locally.
1164 1184 *
1165 1185 * @param TC points to the TeleCommand packet that is being processed
1166 1186 *
1167 1187 */
1168 1188
1169 1189 unsigned char * bytePosPtr; // pointer to the beginning of the incoming TC packet
1170 1190
1171 1191 bytePosPtr = (unsigned char *) &TC->packetID;
1172 1192
1173 1193 // rw1_f
1174 1194 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F1 ] );
1175 1195 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F2 ] );
1176 1196 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F3 ] );
1177 1197 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw1_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW1_F4 ] );
1178 1198
1179 1199 // rw2_f
1180 1200 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F1 ] );
1181 1201 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F2 ] );
1182 1202 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F3 ] );
1183 1203 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw2_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW2_F4 ] );
1184 1204
1185 1205 // rw3_f
1186 1206 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F1 ] );
1187 1207 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F2 ] );
1188 1208 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F3 ] );
1189 1209 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw3_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW3_F4 ] );
1190 1210
1191 1211 // rw4_f
1192 1212 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f1, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F1 ] );
1193 1213 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f2, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F2 ] );
1194 1214 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f3, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F3 ] );
1195 1215 copyFloatByChar( (unsigned char*) &rw_f.cp_rpw_sc_rw4_f4, (unsigned char*) &bytePosPtr[ BYTE_POS_UPDATE_INFO_CP_RPW_SC_RW4_F4 ] );
1196 1216
1197 1217 // test each reaction wheel frequency value. NaN means that the frequency is not filtered
1198 1218
1199 1219 }
1200 1220
1201 1221 void setFBinMask( unsigned char *fbins_mask, float rw_f, unsigned char deltaFreq, float sy_lfr_rw_k )
1202 1222 {
1203 1223 /** This function executes specific actions when a TC_LFR_UPDATE_INFO TeleCommand has been received.
1204 1224 *
1205 1225 * @param fbins_mask
1206 1226 * @param rw_f is the reaction wheel frequency to filter
1207 1227 * @param delta_f is the frequency step between the frequency bins, it depends on the frequency channel
1208 1228 * @param flag [true] filtering enabled [false] filtering disabled
1209 1229 *
1210 1230 * @return void
1211 1231 *
1212 1232 */
1213 1233
1214 1234 float f_RW_min;
1215 1235 float f_RW_MAX;
1216 1236 float fi_min;
1217 1237 float fi_MAX;
1218 1238 float fi;
1219 1239 float deltaBelow;
1220 1240 float deltaAbove;
1221 1241 float freqToFilterOut;
1222 1242 int binBelow;
1223 1243 int binAbove;
1224 1244 int closestBin;
1225 1245 unsigned int whichByte;
1226 1246 int selectedByte;
1227 1247 int bin;
1228 1248 int binToRemove[NB_BINS_TO_REMOVE];
1229 1249 int k;
1230 1250 bool filteringSet;
1231 1251
1232 1252 closestBin = 0;
1233 1253 whichByte = 0;
1234 1254 bin = 0;
1235 1255 filteringSet = false;
1236 1256
1237 1257 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1238 1258 {
1239 1259 binToRemove[k] = -1;
1240 1260 }
1241 1261
1242 1262 if (!isnan(rw_f))
1243 1263 {
1244 1264 // compute the frequency range to filter [ rw_f - delta_f; rw_f + delta_f ]
1245 1265 f_RW_min = rw_f - ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1246 1266 f_RW_MAX = rw_f + ((filterPar.sy_lfr_sc_rw_delta_f) * sy_lfr_rw_k);
1247 1267
1248 1268 freqToFilterOut = f_RW_min;
1249 1269 while ( filteringSet == false )
1250 1270 {
1251 1271 // compute the index of the frequency bin immediately below rw_f
1252 1272 binBelow = (int) ( floor( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1253 1273 deltaBelow = freqToFilterOut - binBelow * deltaFreq;
1254 1274
1255 1275 // compute the index of the frequency bin immediately above rw_f
1256 1276 binAbove = (int) ( ceil( ((double) freqToFilterOut) / ((double) deltaFreq)) );
1257 1277 deltaAbove = binAbove * deltaFreq - freqToFilterOut;
1258 1278
1259 1279 // search the closest bin
1260 1280 if (deltaAbove > deltaBelow)
1261 1281 {
1262 1282 closestBin = binBelow;
1263 1283 }
1264 1284 else
1265 1285 {
1266 1286 closestBin = binAbove;
1267 1287 }
1268 1288
1269 1289 // compute the fi interval [fi - deltaFreq * 0.285, fi + deltaFreq * 0.285]
1270 1290 fi = closestBin * deltaFreq;
1271 1291 fi_min = fi - (deltaFreq * FI_INTERVAL_COEFF);
1272 1292 fi_MAX = fi + (deltaFreq * FI_INTERVAL_COEFF);
1273 1293
1274 1294 //**************************************************************************************
1275 1295 // be careful here, one shall take into account that the bin 0 IS DROPPED in the spectra
1276 1296 // thus, the index 0 in a mask corresponds to the bin 1 of the spectrum
1277 1297 //**************************************************************************************
1278 1298
1279 1299 // 1. IF freqToFilterOut is included in [ fi_min; fi_MAX ]
1280 1300 // => remove f_(i), f_(i-1) and f_(i+1)
1281 1301 if ( ( freqToFilterOut > fi_min ) && ( freqToFilterOut < fi_MAX ) )
1282 1302 {
1283 1303 binToRemove[0] = (closestBin - 1) - 1;
1284 1304 binToRemove[1] = (closestBin) - 1;
1285 1305 binToRemove[2] = (closestBin + 1) - 1;
1286 1306 }
1287 1307 // 2. ELSE
1288 1308 // => remove the two f_(i) which are around f_RW
1289 1309 else
1290 1310 {
1291 1311 binToRemove[0] = (binBelow) - 1;
1292 1312 binToRemove[1] = (binAbove) - 1;
1293 1313 binToRemove[2] = (-1);
1294 1314 }
1295 1315
1296 1316 for (k = 0; k < NB_BINS_TO_REMOVE; k++)
1297 1317 {
1298 1318 bin = binToRemove[k];
1299 1319 if ( (bin >= BIN_MIN) && (bin <= BIN_MAX) )
1300 1320 {
1301 1321 whichByte = (bin >> SHIFT_3_BITS); // division by 8
1302 1322 selectedByte = ( 1 << (bin - (whichByte * BITS_PER_BYTE)) );
1303 1323 fbins_mask[BYTES_PER_MASK - 1 - whichByte] =
1304 1324 fbins_mask[BYTES_PER_MASK - 1 - whichByte] & ((unsigned char) (~selectedByte)); // bytes are ordered MSB first in the packets
1305 1325
1306 1326 }
1307 1327 }
1308 1328
1309 1329 // update freqToFilterOut
1310 1330 if ( freqToFilterOut == f_RW_MAX )
1311 1331 {
1312 1332 filteringSet = true; // end of the loop
1313 1333 }
1314 1334 else
1315 1335 {
1316 1336 freqToFilterOut = freqToFilterOut + deltaFreq;
1317 1337 }
1318 1338
1319 1339 if ( freqToFilterOut > f_RW_MAX)
1320 1340 {
1321 1341 freqToFilterOut = f_RW_MAX;
1322 1342 }
1323 1343 }
1324 1344 }
1325 1345 }
1326 1346
1327 1347 void build_sy_lfr_rw_mask( unsigned int channel )
1328 1348 {
1329 1349 unsigned char local_rw_fbins_mask[BYTES_PER_MASK];
1330 1350 unsigned char *maskPtr;
1331 1351 double deltaF;
1332 1352 unsigned k;
1333 1353
1334 1354 maskPtr = NULL;
1335 1355 deltaF = DELTAF_F2;
1336 1356
1337 1357 switch (channel)
1338 1358 {
1339 1359 case CHANNELF0:
1340 1360 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1341 1361 deltaF = DELTAF_F0;
1342 1362 break;
1343 1363 case CHANNELF1:
1344 1364 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1345 1365 deltaF = DELTAF_F1;
1346 1366 break;
1347 1367 case CHANNELF2:
1348 1368 maskPtr = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1349 1369 deltaF = DELTAF_F2;
1350 1370 break;
1351 1371 default:
1352 1372 break;
1353 1373 }
1354 1374
1355 1375 for (k = 0; k < BYTES_PER_MASK; k++)
1356 1376 {
1357 1377 local_rw_fbins_mask[k] = INT8_ALL_F;
1358 1378 }
1359 1379
1360 1380 // RW1
1361 1381 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f1, deltaF, filterPar.sy_lfr_rw1_k1 );
1362 1382 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f2, deltaF, filterPar.sy_lfr_rw1_k2 );
1363 1383 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f3, deltaF, filterPar.sy_lfr_rw1_k3 );
1364 1384 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw1_f4, deltaF, filterPar.sy_lfr_rw1_k4 );
1365 1385
1366 1386 // RW2
1367 1387 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f1, deltaF, filterPar.sy_lfr_rw2_k1 );
1368 1388 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f2, deltaF, filterPar.sy_lfr_rw2_k2 );
1369 1389 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f3, deltaF, filterPar.sy_lfr_rw2_k3 );
1370 1390 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw2_f4, deltaF, filterPar.sy_lfr_rw2_k4 );
1371 1391
1372 1392 // RW3
1373 1393 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f1, deltaF, filterPar.sy_lfr_rw3_k1 );
1374 1394 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f2, deltaF, filterPar.sy_lfr_rw3_k2 );
1375 1395 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f3, deltaF, filterPar.sy_lfr_rw3_k3 );
1376 1396 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw3_f4, deltaF, filterPar.sy_lfr_rw3_k4 );
1377 1397
1378 1398 // RW4
1379 1399 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f1, deltaF, filterPar.sy_lfr_rw4_k1 );
1380 1400 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f2, deltaF, filterPar.sy_lfr_rw4_k2 );
1381 1401 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f3, deltaF, filterPar.sy_lfr_rw4_k3 );
1382 1402 setFBinMask( local_rw_fbins_mask, rw_f.cp_rpw_sc_rw4_f4, deltaF, filterPar.sy_lfr_rw4_k4 );
1383 1403
1384 1404 // update the value of the fbins related to reaction wheels frequency filtering
1385 1405 if (maskPtr != NULL)
1386 1406 {
1387 1407 for (k = 0; k < BYTES_PER_MASK; k++)
1388 1408 {
1389 1409 maskPtr[k] = local_rw_fbins_mask[k];
1390 1410 }
1391 1411 }
1392 1412 }
1393 1413
1394 1414 void build_sy_lfr_rw_masks( void )
1395 1415 {
1396 1416 build_sy_lfr_rw_mask( CHANNELF0 );
1397 1417 build_sy_lfr_rw_mask( CHANNELF1 );
1398 1418 build_sy_lfr_rw_mask( CHANNELF2 );
1399 1419 }
1400 1420
1401 1421 void merge_fbins_masks( void )
1402 1422 {
1403 1423 unsigned char k;
1404 1424
1405 1425 unsigned char *fbins_f0;
1406 1426 unsigned char *fbins_f1;
1407 1427 unsigned char *fbins_f2;
1408 1428 unsigned char *rw_mask_f0;
1409 1429 unsigned char *rw_mask_f1;
1410 1430 unsigned char *rw_mask_f2;
1411 1431
1412 1432 fbins_f0 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1413 1433 fbins_f1 = parameter_dump_packet.sy_lfr_fbins_f1_word1;
1414 1434 fbins_f2 = parameter_dump_packet.sy_lfr_fbins_f2_word1;
1415 1435 rw_mask_f0 = parameter_dump_packet.sy_lfr_rw_mask_f0_word1;
1416 1436 rw_mask_f1 = parameter_dump_packet.sy_lfr_rw_mask_f1_word1;
1417 1437 rw_mask_f2 = parameter_dump_packet.sy_lfr_rw_mask_f2_word1;
1418 1438
1419 1439 for( k=0; k < BYTES_PER_MASK; k++ )
1420 1440 {
1421 1441 fbins_masks.merged_fbins_mask_f0[k] = fbins_f0[k] & rw_mask_f0[k];
1422 1442 fbins_masks.merged_fbins_mask_f1[k] = fbins_f1[k] & rw_mask_f1[k];
1423 1443 fbins_masks.merged_fbins_mask_f2[k] = fbins_f2[k] & rw_mask_f2[k];
1424 1444 }
1425 1445 }
1426 1446
1427 1447 //***********
1428 1448 // FBINS MASK
1429 1449
1430 1450 int set_sy_lfr_fbins( ccsdsTelecommandPacket_t *TC )
1431 1451 {
1432 1452 int status;
1433 1453 unsigned int k;
1434 1454 unsigned char *fbins_mask_dump;
1435 1455 unsigned char *fbins_mask_TC;
1436 1456
1437 1457 status = LFR_SUCCESSFUL;
1438 1458
1439 1459 fbins_mask_dump = parameter_dump_packet.sy_lfr_fbins_f0_word1;
1440 1460 fbins_mask_TC = TC->dataAndCRC;
1441 1461
1442 1462 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1443 1463 {
1444 1464 fbins_mask_dump[k] = fbins_mask_TC[k];
1445 1465 }
1446 1466
1447 1467 return status;
1448 1468 }
1449 1469
1450 1470 //***************************
1451 1471 // TC_LFR_LOAD_PAS_FILTER_PAR
1452 1472
1453 1473 int check_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int offset, int* pos, float* value )
1454 1474 {
1455 1475 float rw_k;
1456 1476 int ret;
1457 1477
1458 1478 ret = LFR_SUCCESSFUL;
1459 1479 rw_k = INIT_FLOAT;
1460 1480
1461 1481 copyFloatByChar( (unsigned char*) &rw_k, (unsigned char*) &TC->dataAndCRC[ offset ] );
1462 1482
1463 1483 *pos = offset;
1464 1484 *value = rw_k;
1465 1485
1466 1486 if (rw_k < MIN_SY_LFR_RW_F)
1467 1487 {
1468 1488 ret = WRONG_APP_DATA;
1469 1489 }
1470 1490
1471 1491 return ret;
1472 1492 }
1473 1493
1474 1494 int check_all_sy_lfr_rw_k( ccsdsTelecommandPacket_t *TC, int *pos, float *value )
1475 1495 {
1476 1496 int ret;
1477 1497
1478 1498 ret = LFR_SUCCESSFUL;
1479 1499
1480 1500 //****
1481 1501 //****
1482 1502 // RW1
1483 1503 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K1, pos, value ); // K1
1484 1504 if (ret == LFR_SUCCESSFUL) // K2
1485 1505 {
1486 1506 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K2, pos, value );
1487 1507 }
1488 1508 if (ret == LFR_SUCCESSFUL) // K3
1489 1509 {
1490 1510 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K3, pos, value );
1491 1511 }
1492 1512 if (ret == LFR_SUCCESSFUL) // K4
1493 1513 {
1494 1514 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW1_K4, pos, value );
1495 1515 }
1496 1516
1497 1517 //****
1498 1518 //****
1499 1519 // RW2
1500 1520 if (ret == LFR_SUCCESSFUL) // K1
1501 1521 {
1502 1522 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K1, pos, value );
1503 1523 }
1504 1524 if (ret == LFR_SUCCESSFUL) // K2
1505 1525 {
1506 1526 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K2, pos, value );
1507 1527 }
1508 1528 if (ret == LFR_SUCCESSFUL) // K3
1509 1529 {
1510 1530 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K3, pos, value );
1511 1531 }
1512 1532 if (ret == LFR_SUCCESSFUL) // K4
1513 1533 {
1514 1534 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW2_K4, pos, value );
1515 1535 }
1516 1536
1517 1537 //****
1518 1538 //****
1519 1539 // RW3
1520 1540 if (ret == LFR_SUCCESSFUL) // K1
1521 1541 {
1522 1542 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K1, pos, value );
1523 1543 }
1524 1544 if (ret == LFR_SUCCESSFUL) // K2
1525 1545 {
1526 1546 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K2, pos, value );
1527 1547 }
1528 1548 if (ret == LFR_SUCCESSFUL) // K3
1529 1549 {
1530 1550 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K3, pos, value );
1531 1551 }
1532 1552 if (ret == LFR_SUCCESSFUL) // K4
1533 1553 {
1534 1554 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW3_K4, pos, value );
1535 1555 }
1536 1556
1537 1557 //****
1538 1558 //****
1539 1559 // RW4
1540 1560 if (ret == LFR_SUCCESSFUL) // K1
1541 1561 {
1542 1562 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K1, pos, value );
1543 1563 }
1544 1564 if (ret == LFR_SUCCESSFUL) // K2
1545 1565 {
1546 1566 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K2, pos, value );
1547 1567 }
1548 1568 if (ret == LFR_SUCCESSFUL) // K3
1549 1569 {
1550 1570 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K3, pos, value );
1551 1571 }
1552 1572 if (ret == LFR_SUCCESSFUL) // K4
1553 1573 {
1554 1574 ret = check_sy_lfr_rw_k( TC, DATAFIELD_POS_SY_LFR_RW4_K4, pos, value );
1555 1575 }
1556 1576
1557 1577 return ret;
1558 1578 }
1559 1579
1560 1580 int check_sy_lfr_filter_parameters( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
1561 1581 {
1562 1582 int flag;
1563 1583 rtems_status_code status;
1564 1584
1565 1585 unsigned char sy_lfr_pas_filter_enabled;
1566 1586 unsigned char sy_lfr_pas_filter_modulus;
1567 1587 float sy_lfr_pas_filter_tbad;
1568 1588 unsigned char sy_lfr_pas_filter_offset;
1569 1589 float sy_lfr_pas_filter_shift;
1570 1590 float sy_lfr_sc_rw_delta_f;
1571 1591 char *parPtr;
1572 1592 int datafield_pos;
1573 1593 float rw_k;
1574 1594
1575 1595 flag = LFR_SUCCESSFUL;
1576 1596 sy_lfr_pas_filter_tbad = INIT_FLOAT;
1577 1597 sy_lfr_pas_filter_shift = INIT_FLOAT;
1578 1598 sy_lfr_sc_rw_delta_f = INIT_FLOAT;
1579 1599 parPtr = NULL;
1580 1600 datafield_pos = INIT_INT;
1581 1601 rw_k = INIT_FLOAT;
1582 1602
1583 1603 //***************
1584 1604 // get parameters
1585 1605 sy_lfr_pas_filter_enabled = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_ENABLED ] & BIT_PAS_FILTER_ENABLED; // [0000 0001]
1586 1606 sy_lfr_pas_filter_modulus = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS ];
1587 1607 copyFloatByChar(
1588 1608 (unsigned char*) &sy_lfr_pas_filter_tbad,
1589 1609 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD ]
1590 1610 );
1591 1611 sy_lfr_pas_filter_offset = TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET ];
1592 1612 copyFloatByChar(
1593 1613 (unsigned char*) &sy_lfr_pas_filter_shift,
1594 1614 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT ]
1595 1615 );
1596 1616 copyFloatByChar(
1597 1617 (unsigned char*) &sy_lfr_sc_rw_delta_f,
1598 1618 (unsigned char*) &TC->dataAndCRC[ DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F ]
1599 1619 );
1600 1620
1601 1621 //******************
1602 1622 // CHECK CONSISTENCY
1603 1623
1604 1624 //**************************
1605 1625 // sy_lfr_pas_filter_enabled
1606 1626 // nothing to check, value is 0 or 1
1607 1627
1608 1628 //**************************
1609 1629 // sy_lfr_pas_filter_modulus
1610 1630 if ( (sy_lfr_pas_filter_modulus < MIN_PAS_FILTER_MODULUS) || (sy_lfr_pas_filter_modulus > MAX_PAS_FILTER_MODULUS) )
1611 1631 {
1612 1632 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1613 1633 flag = WRONG_APP_DATA;
1614 1634 }
1615 1635
1616 1636 //***********************
1617 1637 // sy_lfr_pas_filter_tbad
1618 1638 if (flag == LFR_SUCCESSFUL)
1619 1639 {
1620 1640 if ( (sy_lfr_pas_filter_tbad < MIN_PAS_FILTER_TBAD) || (sy_lfr_pas_filter_tbad > MAX_PAS_FILTER_TBAD) )
1621 1641 {
1622 1642 parPtr = (char*) &sy_lfr_pas_filter_tbad;
1623 1643 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_TBAD + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1624 1644 flag = WRONG_APP_DATA;
1625 1645 }
1626 1646 }
1627 1647
1628 1648 //*************************
1629 1649 // sy_lfr_pas_filter_offset
1630 1650 if (flag == LFR_SUCCESSFUL)
1631 1651 {
1632 1652 if ( (sy_lfr_pas_filter_offset < MIN_PAS_FILTER_OFFSET) || (sy_lfr_pas_filter_offset > MAX_PAS_FILTER_OFFSET) )
1633 1653 {
1634 1654 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_OFFSET + DATAFIELD_OFFSET, sy_lfr_pas_filter_offset );
1635 1655 flag = WRONG_APP_DATA;
1636 1656 }
1637 1657 }
1638 1658
1639 1659 //************************
1640 1660 // sy_lfr_pas_filter_shift
1641 1661 if (flag == LFR_SUCCESSFUL)
1642 1662 {
1643 1663 if ( (sy_lfr_pas_filter_shift < MIN_PAS_FILTER_SHIFT) || (sy_lfr_pas_filter_shift > MAX_PAS_FILTER_SHIFT) )
1644 1664 {
1645 1665 parPtr = (char*) &sy_lfr_pas_filter_shift;
1646 1666 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_SHIFT + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1647 1667 flag = WRONG_APP_DATA;
1648 1668 }
1649 1669 }
1650 1670
1651 1671 //*************************************
1652 1672 // check global coherency of the values
1653 1673 if (flag == LFR_SUCCESSFUL)
1654 1674 {
1655 1675 if ( (sy_lfr_pas_filter_offset + sy_lfr_pas_filter_shift) >= sy_lfr_pas_filter_modulus )
1656 1676 {
1657 1677 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_PAS_FILTER_MODULUS + DATAFIELD_OFFSET, sy_lfr_pas_filter_modulus );
1658 1678 flag = WRONG_APP_DATA;
1659 1679 }
1660 1680 }
1661 1681
1662 1682 //*********************
1663 1683 // sy_lfr_sc_rw_delta_f
1664 1684 if (flag == LFR_SUCCESSFUL)
1665 1685 {
1666 1686 if ( sy_lfr_sc_rw_delta_f < MIN_SY_LFR_SC_RW_DELTA_F )
1667 1687 {
1668 1688 parPtr = (char*) &sy_lfr_sc_rw_delta_f;
1669 1689 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_SC_RW_DELTA_F + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1670 1690 flag = WRONG_APP_DATA;
1671 1691 }
1672 1692 }
1673 1693
1674 1694 //************
1675 1695 // sy_lfr_rw_k
1676 1696 if (flag == LFR_SUCCESSFUL)
1677 1697 {
1678 1698 flag = check_all_sy_lfr_rw_k( TC, &datafield_pos, &rw_k );
1679 1699 if (flag != LFR_SUCCESSFUL)
1680 1700 {
1681 1701 parPtr = (char*) &sy_lfr_pas_filter_shift;
1682 1702 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, datafield_pos + DATAFIELD_OFFSET, parPtr[FLOAT_LSBYTE] );
1683 1703 }
1684 1704 }
1685 1705
1686 1706 return flag;
1687 1707 }
1688 1708
1689 1709 //**************
1690 1710 // KCOEFFICIENTS
1691 1711 int set_sy_lfr_kcoeff( ccsdsTelecommandPacket_t *TC,rtems_id queue_id )
1692 1712 {
1693 1713 unsigned int kcoeff;
1694 1714 unsigned short sy_lfr_kcoeff_frequency;
1695 1715 unsigned short bin;
1696 1716 float *kcoeffPtr_norm;
1697 1717 float *kcoeffPtr_sbm;
1698 1718 int status;
1699 1719 unsigned char *kcoeffLoadPtr;
1700 1720 unsigned char *kcoeffNormPtr;
1701 1721 unsigned char *kcoeffSbmPtr_a;
1702 1722 unsigned char *kcoeffSbmPtr_b;
1703 1723
1704 1724 sy_lfr_kcoeff_frequency = 0;
1705 1725 bin = 0;
1706 1726 kcoeffPtr_norm = NULL;
1707 1727 kcoeffPtr_sbm = NULL;
1708 1728 status = LFR_SUCCESSFUL;
1709 1729
1710 1730 // copy the value of the frequency byte by byte DO NOT USE A SHORT* POINTER
1711 1731 copyInt16ByChar( (unsigned char*) &sy_lfr_kcoeff_frequency, &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY] );
1712 1732
1713 1733
1714 1734 if ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM )
1715 1735 {
1716 1736 PRINTF1("ERR *** in set_sy_lfr_kcoeff_frequency *** sy_lfr_kcoeff_frequency = %d\n", sy_lfr_kcoeff_frequency)
1717 1737 status = send_tm_lfr_tc_exe_inconsistent( TC, queue_id, DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + DATAFIELD_OFFSET,
1718 1738 TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_FREQUENCY + 1] ); // +1 to get the LSB instead of the MSB
1719 1739 status = LFR_DEFAULT;
1720 1740 }
1721 1741 else
1722 1742 {
1723 1743 if ( ( sy_lfr_kcoeff_frequency >= 0 )
1724 1744 && ( sy_lfr_kcoeff_frequency < NB_BINS_COMPRESSED_SM_F0 ) )
1725 1745 {
1726 1746 kcoeffPtr_norm = k_coeff_intercalib_f0_norm;
1727 1747 kcoeffPtr_sbm = k_coeff_intercalib_f0_sbm;
1728 1748 bin = sy_lfr_kcoeff_frequency;
1729 1749 }
1730 1750 else if ( ( sy_lfr_kcoeff_frequency >= NB_BINS_COMPRESSED_SM_F0 )
1731 1751 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) ) )
1732 1752 {
1733 1753 kcoeffPtr_norm = k_coeff_intercalib_f1_norm;
1734 1754 kcoeffPtr_sbm = k_coeff_intercalib_f1_sbm;
1735 1755 bin = sy_lfr_kcoeff_frequency - NB_BINS_COMPRESSED_SM_F0;
1736 1756 }
1737 1757 else if ( ( sy_lfr_kcoeff_frequency >= (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1) )
1738 1758 && ( sy_lfr_kcoeff_frequency < (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1 + NB_BINS_COMPRESSED_SM_F2) ) )
1739 1759 {
1740 1760 kcoeffPtr_norm = k_coeff_intercalib_f2;
1741 1761 kcoeffPtr_sbm = NULL;
1742 1762 bin = sy_lfr_kcoeff_frequency - (NB_BINS_COMPRESSED_SM_F0 + NB_BINS_COMPRESSED_SM_F1);
1743 1763 }
1744 1764 }
1745 1765
1746 1766 if (kcoeffPtr_norm != NULL ) // update K coefficient for NORMAL data products
1747 1767 {
1748 1768 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1749 1769 {
1750 1770 // destination
1751 1771 kcoeffNormPtr = (unsigned char*) &kcoeffPtr_norm[ (bin * NB_K_COEFF_PER_BIN) + kcoeff ];
1752 1772 // source
1753 1773 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1754 1774 // copy source to destination
1755 1775 copyFloatByChar( kcoeffNormPtr, kcoeffLoadPtr );
1756 1776 }
1757 1777 }
1758 1778
1759 1779 if (kcoeffPtr_sbm != NULL ) // update K coefficient for SBM data products
1760 1780 {
1761 1781 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
1762 1782 {
1763 1783 // destination
1764 1784 kcoeffSbmPtr_a= (unsigned char*) &kcoeffPtr_sbm[ ( (bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ];
1765 1785 kcoeffSbmPtr_b= (unsigned char*) &kcoeffPtr_sbm[ (((bin * NB_K_COEFF_PER_BIN) + kcoeff) * SBM_KCOEFF_PER_NORM_KCOEFF) + 1 ];
1766 1786 // source
1767 1787 kcoeffLoadPtr = (unsigned char*) &TC->dataAndCRC[DATAFIELD_POS_SY_LFR_KCOEFF_1 + (NB_BYTES_PER_FLOAT * kcoeff)];
1768 1788 // copy source to destination
1769 1789 copyFloatByChar( kcoeffSbmPtr_a, kcoeffLoadPtr );
1770 1790 copyFloatByChar( kcoeffSbmPtr_b, kcoeffLoadPtr );
1771 1791 }
1772 1792 }
1773 1793
1774 1794 // print_k_coeff();
1775 1795
1776 1796 return status;
1777 1797 }
1778 1798
1779 1799 void copyFloatByChar( unsigned char *destination, unsigned char *source )
1780 1800 {
1781 1801 destination[BYTE_0] = source[BYTE_0];
1782 1802 destination[BYTE_1] = source[BYTE_1];
1783 1803 destination[BYTE_2] = source[BYTE_2];
1784 1804 destination[BYTE_3] = source[BYTE_3];
1785 1805 }
1786 1806
1787 1807 void copyInt32ByChar( unsigned char *destination, unsigned char *source )
1788 1808 {
1789 1809 destination[BYTE_0] = source[BYTE_0];
1790 1810 destination[BYTE_1] = source[BYTE_1];
1791 1811 destination[BYTE_2] = source[BYTE_2];
1792 1812 destination[BYTE_3] = source[BYTE_3];
1793 1813 }
1794 1814
1795 1815 void copyInt16ByChar( unsigned char *destination, unsigned char *source )
1796 1816 {
1797 1817 destination[BYTE_0] = source[BYTE_0];
1798 1818 destination[BYTE_1] = source[BYTE_1];
1799 1819 }
1800 1820
1801 1821 void floatToChar( float value, unsigned char* ptr)
1802 1822 {
1803 1823 unsigned char* valuePtr;
1804 1824
1805 1825 valuePtr = (unsigned char*) &value;
1806 1826
1807 1827 ptr[BYTE_0] = valuePtr[BYTE_0];
1808 1828 ptr[BYTE_1] = valuePtr[BYTE_1];
1809 1829 ptr[BYTE_2] = valuePtr[BYTE_2];
1810 1830 ptr[BYTE_3] = valuePtr[BYTE_3];
1811 1831 }
1812 1832
1813 1833 //**********
1814 1834 // init dump
1815 1835
1816 1836 void init_parameter_dump( void )
1817 1837 {
1818 1838 /** This function initialize the parameter_dump_packet global variable with default values.
1819 1839 *
1820 1840 */
1821 1841
1822 1842 unsigned int k;
1823 1843
1824 1844 parameter_dump_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
1825 1845 parameter_dump_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
1826 1846 parameter_dump_packet.reserved = CCSDS_RESERVED;
1827 1847 parameter_dump_packet.userApplication = CCSDS_USER_APP;
1828 1848 parameter_dump_packet.packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1829 1849 parameter_dump_packet.packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1830 1850 parameter_dump_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1831 1851 parameter_dump_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1832 1852 parameter_dump_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_PARAMETER_DUMP >> SHIFT_1_BYTE);
1833 1853 parameter_dump_packet.packetLength[1] = (unsigned char) PACKET_LENGTH_PARAMETER_DUMP;
1834 1854 // DATA FIELD HEADER
1835 1855 parameter_dump_packet.spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1836 1856 parameter_dump_packet.serviceType = TM_TYPE_PARAMETER_DUMP;
1837 1857 parameter_dump_packet.serviceSubType = TM_SUBTYPE_PARAMETER_DUMP;
1838 1858 parameter_dump_packet.destinationID = TM_DESTINATION_ID_GROUND;
1839 1859 parameter_dump_packet.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
1840 1860 parameter_dump_packet.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
1841 1861 parameter_dump_packet.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
1842 1862 parameter_dump_packet.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
1843 1863 parameter_dump_packet.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
1844 1864 parameter_dump_packet.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
1845 1865 parameter_dump_packet.sid = SID_PARAMETER_DUMP;
1846 1866
1847 1867 //******************
1848 1868 // COMMON PARAMETERS
1849 1869 parameter_dump_packet.sy_lfr_common_parameters_spare = DEFAULT_SY_LFR_COMMON0;
1850 1870 parameter_dump_packet.sy_lfr_common_parameters = DEFAULT_SY_LFR_COMMON1;
1851 1871
1852 1872 //******************
1853 1873 // NORMAL PARAMETERS
1854 1874 parameter_dump_packet.sy_lfr_n_swf_l[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_L >> SHIFT_1_BYTE);
1855 1875 parameter_dump_packet.sy_lfr_n_swf_l[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_L );
1856 1876 parameter_dump_packet.sy_lfr_n_swf_p[0] = (unsigned char) (DFLT_SY_LFR_N_SWF_P >> SHIFT_1_BYTE);
1857 1877 parameter_dump_packet.sy_lfr_n_swf_p[1] = (unsigned char) (DFLT_SY_LFR_N_SWF_P );
1858 1878 parameter_dump_packet.sy_lfr_n_asm_p[0] = (unsigned char) (DFLT_SY_LFR_N_ASM_P >> SHIFT_1_BYTE);
1859 1879 parameter_dump_packet.sy_lfr_n_asm_p[1] = (unsigned char) (DFLT_SY_LFR_N_ASM_P );
1860 1880 parameter_dump_packet.sy_lfr_n_bp_p0 = (unsigned char) DFLT_SY_LFR_N_BP_P0;
1861 1881 parameter_dump_packet.sy_lfr_n_bp_p1 = (unsigned char) DFLT_SY_LFR_N_BP_P1;
1862 1882 parameter_dump_packet.sy_lfr_n_cwf_long_f3 = (unsigned char) DFLT_SY_LFR_N_CWF_LONG_F3;
1863 1883
1864 1884 //*****************
1865 1885 // BURST PARAMETERS
1866 1886 parameter_dump_packet.sy_lfr_b_bp_p0 = (unsigned char) DEFAULT_SY_LFR_B_BP_P0;
1867 1887 parameter_dump_packet.sy_lfr_b_bp_p1 = (unsigned char) DEFAULT_SY_LFR_B_BP_P1;
1868 1888
1869 1889 //****************
1870 1890 // SBM1 PARAMETERS
1871 1891 parameter_dump_packet.sy_lfr_s1_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P0; // min value is 0.25 s for the period
1872 1892 parameter_dump_packet.sy_lfr_s1_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S1_BP_P1;
1873 1893
1874 1894 //****************
1875 1895 // SBM2 PARAMETERS
1876 1896 parameter_dump_packet.sy_lfr_s2_bp_p0 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P0;
1877 1897 parameter_dump_packet.sy_lfr_s2_bp_p1 = (unsigned char) DEFAULT_SY_LFR_S2_BP_P1;
1878 1898
1879 1899 //************
1880 1900 // FBINS MASKS
1881 1901 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1882 1902 {
1883 1903 parameter_dump_packet.sy_lfr_fbins_f0_word1[k] = INT8_ALL_F;
1884 1904 }
1885 1905
1886 1906 // PAS FILTER PARAMETERS
1887 1907 parameter_dump_packet.pa_rpw_spare8_2 = INIT_CHAR;
1888 1908 parameter_dump_packet.spare_sy_lfr_pas_filter_enabled = INIT_CHAR;
1889 1909 parameter_dump_packet.sy_lfr_pas_filter_modulus = DEFAULT_SY_LFR_PAS_FILTER_MODULUS;
1890 1910 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_TBAD, parameter_dump_packet.sy_lfr_pas_filter_tbad );
1891 1911 parameter_dump_packet.sy_lfr_pas_filter_offset = DEFAULT_SY_LFR_PAS_FILTER_OFFSET;
1892 1912 floatToChar( DEFAULT_SY_LFR_PAS_FILTER_SHIFT, parameter_dump_packet.sy_lfr_pas_filter_shift );
1893 1913 floatToChar( DEFAULT_SY_LFR_SC_RW_DELTA_F, parameter_dump_packet.sy_lfr_sc_rw_delta_f );
1894 1914
1895 1915 // RW1_K
1896 1916 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw1_k1);
1897 1917 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw1_k2);
1898 1918 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw1_k3);
1899 1919 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw1_k4);
1900 1920 // RW2_K
1901 1921 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw2_k1);
1902 1922 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw2_k2);
1903 1923 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw2_k3);
1904 1924 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw2_k4);
1905 1925 // RW3_K
1906 1926 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw3_k1);
1907 1927 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw3_k2);
1908 1928 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw3_k3);
1909 1929 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw3_k4);
1910 1930 // RW4_K
1911 1931 floatToChar( DEFAULT_SY_LFR_RW_K1, parameter_dump_packet.sy_lfr_rw4_k1);
1912 1932 floatToChar( DEFAULT_SY_LFR_RW_K2, parameter_dump_packet.sy_lfr_rw4_k2);
1913 1933 floatToChar( DEFAULT_SY_LFR_RW_K3, parameter_dump_packet.sy_lfr_rw4_k3);
1914 1934 floatToChar( DEFAULT_SY_LFR_RW_K4, parameter_dump_packet.sy_lfr_rw4_k4);
1915 1935
1916 1936 // LFR_RW_MASK
1917 1937 for (k=0; k < BYTES_PER_MASKS_SET; k++)
1918 1938 {
1919 1939 parameter_dump_packet.sy_lfr_rw_mask_f0_word1[k] = INT8_ALL_F;
1920 1940 }
1921 1941
1922 1942 // once the reaction wheels masks have been initialized, they have to be merged with the fbins masks
1923 1943 merge_fbins_masks();
1924 1944 }
1925 1945
1926 1946 void init_kcoefficients_dump( void )
1927 1947 {
1928 1948 init_kcoefficients_dump_packet( &kcoefficients_dump_1, PKTNR_1, KCOEFF_BLK_NR_PKT1 );
1929 1949 init_kcoefficients_dump_packet( &kcoefficients_dump_2, PKTNR_2, KCOEFF_BLK_NR_PKT2 );
1930 1950
1931 1951 kcoefficient_node_1.previous = NULL;
1932 1952 kcoefficient_node_1.next = NULL;
1933 1953 kcoefficient_node_1.sid = TM_CODE_K_DUMP;
1934 1954 kcoefficient_node_1.coarseTime = INIT_CHAR;
1935 1955 kcoefficient_node_1.fineTime = INIT_CHAR;
1936 1956 kcoefficient_node_1.buffer_address = (int) &kcoefficients_dump_1;
1937 1957 kcoefficient_node_1.status = INIT_CHAR;
1938 1958
1939 1959 kcoefficient_node_2.previous = NULL;
1940 1960 kcoefficient_node_2.next = NULL;
1941 1961 kcoefficient_node_2.sid = TM_CODE_K_DUMP;
1942 1962 kcoefficient_node_2.coarseTime = INIT_CHAR;
1943 1963 kcoefficient_node_2.fineTime = INIT_CHAR;
1944 1964 kcoefficient_node_2.buffer_address = (int) &kcoefficients_dump_2;
1945 1965 kcoefficient_node_2.status = INIT_CHAR;
1946 1966 }
1947 1967
1948 1968 void init_kcoefficients_dump_packet( Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump, unsigned char pkt_nr, unsigned char blk_nr )
1949 1969 {
1950 1970 unsigned int k;
1951 1971 unsigned int packetLength;
1952 1972
1953 1973 packetLength =
1954 1974 ((blk_nr * KCOEFF_BLK_SIZE) + BYTE_POS_KCOEFFICIENTS_PARAMETES) - CCSDS_TC_TM_PACKET_OFFSET; // 4 bytes for the CCSDS header
1955 1975
1956 1976 kcoefficients_dump->targetLogicalAddress = CCSDS_DESTINATION_ID;
1957 1977 kcoefficients_dump->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1958 1978 kcoefficients_dump->reserved = CCSDS_RESERVED;
1959 1979 kcoefficients_dump->userApplication = CCSDS_USER_APP;
1960 1980 kcoefficients_dump->packetID[0] = (unsigned char) (APID_TM_PARAMETER_DUMP >> SHIFT_1_BYTE);
1961 1981 kcoefficients_dump->packetID[1] = (unsigned char) APID_TM_PARAMETER_DUMP;
1962 1982 kcoefficients_dump->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1963 1983 kcoefficients_dump->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1964 1984 kcoefficients_dump->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
1965 1985 kcoefficients_dump->packetLength[1] = (unsigned char) packetLength;
1966 1986 // DATA FIELD HEADER
1967 1987 kcoefficients_dump->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
1968 1988 kcoefficients_dump->serviceType = TM_TYPE_K_DUMP;
1969 1989 kcoefficients_dump->serviceSubType = TM_SUBTYPE_K_DUMP;
1970 1990 kcoefficients_dump->destinationID= TM_DESTINATION_ID_GROUND;
1971 1991 kcoefficients_dump->time[BYTE_0] = INIT_CHAR;
1972 1992 kcoefficients_dump->time[BYTE_1] = INIT_CHAR;
1973 1993 kcoefficients_dump->time[BYTE_2] = INIT_CHAR;
1974 1994 kcoefficients_dump->time[BYTE_3] = INIT_CHAR;
1975 1995 kcoefficients_dump->time[BYTE_4] = INIT_CHAR;
1976 1996 kcoefficients_dump->time[BYTE_5] = INIT_CHAR;
1977 1997 kcoefficients_dump->sid = SID_K_DUMP;
1978 1998
1979 1999 kcoefficients_dump->pkt_cnt = KCOEFF_PKTCNT;
1980 2000 kcoefficients_dump->pkt_nr = PKTNR_1;
1981 2001 kcoefficients_dump->blk_nr = blk_nr;
1982 2002
1983 2003 //******************
1984 2004 // SOURCE DATA repeated N times with N in [0 .. PA_LFR_KCOEFF_BLK_NR]
1985 2005 // one blk is 2 + 4 * 32 = 130 bytes, 30 blks max in one packet (30 * 130 = 3900)
1986 2006 for (k=0; k<(KCOEFF_BLK_NR_PKT1 * KCOEFF_BLK_SIZE); k++)
1987 2007 {
1988 2008 kcoefficients_dump->kcoeff_blks[k] = INIT_CHAR;
1989 2009 }
1990 2010 }
1991 2011
1992 2012 void increment_seq_counter_destination_id_dump( unsigned char *packet_sequence_control, unsigned char destination_id )
1993 2013 {
1994 2014 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
1995 2015 *
1996 2016 * @param packet_sequence_control points to the packet sequence control which will be incremented
1997 2017 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
1998 2018 *
1999 2019 * If the destination ID is not known, a dedicated counter is incremented.
2000 2020 *
2001 2021 */
2002 2022
2003 2023 unsigned short sequence_cnt;
2004 2024 unsigned short segmentation_grouping_flag;
2005 2025 unsigned short new_packet_sequence_control;
2006 2026 unsigned char i;
2007 2027
2008 2028 switch (destination_id)
2009 2029 {
2010 2030 case SID_TC_GROUND:
2011 2031 i = GROUND;
2012 2032 break;
2013 2033 case SID_TC_MISSION_TIMELINE:
2014 2034 i = MISSION_TIMELINE;
2015 2035 break;
2016 2036 case SID_TC_TC_SEQUENCES:
2017 2037 i = TC_SEQUENCES;
2018 2038 break;
2019 2039 case SID_TC_RECOVERY_ACTION_CMD:
2020 2040 i = RECOVERY_ACTION_CMD;
2021 2041 break;
2022 2042 case SID_TC_BACKUP_MISSION_TIMELINE:
2023 2043 i = BACKUP_MISSION_TIMELINE;
2024 2044 break;
2025 2045 case SID_TC_DIRECT_CMD:
2026 2046 i = DIRECT_CMD;
2027 2047 break;
2028 2048 case SID_TC_SPARE_GRD_SRC1:
2029 2049 i = SPARE_GRD_SRC1;
2030 2050 break;
2031 2051 case SID_TC_SPARE_GRD_SRC2:
2032 2052 i = SPARE_GRD_SRC2;
2033 2053 break;
2034 2054 case SID_TC_OBCP:
2035 2055 i = OBCP;
2036 2056 break;
2037 2057 case SID_TC_SYSTEM_CONTROL:
2038 2058 i = SYSTEM_CONTROL;
2039 2059 break;
2040 2060 case SID_TC_AOCS:
2041 2061 i = AOCS;
2042 2062 break;
2043 2063 case SID_TC_RPW_INTERNAL:
2044 2064 i = RPW_INTERNAL;
2045 2065 break;
2046 2066 default:
2047 2067 i = GROUND;
2048 2068 break;
2049 2069 }
2050 2070
2051 2071 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
2052 2072 sequence_cnt = sequenceCounters_TM_DUMP[ i ] & SEQ_CNT_MASK;
2053 2073
2054 2074 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
2055 2075
2056 2076 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
2057 2077 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
2058 2078
2059 2079 // increment the sequence counter
2060 2080 if ( sequenceCounters_TM_DUMP[ i ] < SEQ_CNT_MAX )
2061 2081 {
2062 2082 sequenceCounters_TM_DUMP[ i ] = sequenceCounters_TM_DUMP[ i ] + 1;
2063 2083 }
2064 2084 else
2065 2085 {
2066 2086 sequenceCounters_TM_DUMP[ i ] = 0;
2067 2087 }
2068 2088 }
Show file before | Show file after | Hide comments
@@ -1,514 +1,536
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions to send TM packets related to TC parsing and execution.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * A group of functions to send appropriate TM packets after parsing and execution:
7 31 * - TM_LFR_TC_EXE_SUCCESS
8 32 * - TM_LFR_TC_EXE_INCONSISTENT
9 33 * - TM_LFR_TC_EXE_NOT_EXECUTABLE
10 34 * - TM_LFR_TC_EXE_NOT_IMPLEMENTED
11 35 * - TM_LFR_TC_EXE_ERROR
12 36 * - TM_LFR_TC_EXE_CORRUPTED
13 37 *
14 38 */
15 39
16 40 #include "tm_lfr_tc_exe.h"
17 41
18 42 int send_tm_lfr_tc_exe_success( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
19 43 {
20 44 /** This function sends a TM_LFR_TC_EXE_SUCCESS packet in the dedicated RTEMS message queue.
21 45 *
22 46 * @param TC points to the TeleCommand packet that is being processed
23 47 * @param queue_id is the id of the queue which handles TM
24 48 *
25 49 * @return RTEMS directive status code:
26 50 * - RTEMS_SUCCESSFUL - message sent successfully
27 51 * - RTEMS_INVALID_ID - invalid queue id
28 52 * - RTEMS_INVALID_SIZE - invalid message size
29 53 * - RTEMS_INVALID_ADDRESS - buffer is NULL
30 54 * - RTEMS_UNSATISFIED - out of message buffers
31 55 * - RTEMS_TOO_MANY - queue s limit has been reached
32 56 *
33 57 */
34 58
35 59 rtems_status_code status;
36 60 Packet_TM_LFR_TC_EXE_SUCCESS_t TM;
37 61 unsigned char messageSize;
38 62
39 63 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
40 64 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
41 65 TM.reserved = DEFAULT_RESERVED;
42 66 TM.userApplication = CCSDS_USER_APP;
43 67 // PACKET HEADER
44 68 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
45 69 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
46 70 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
47 71 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_SUCCESS >> SHIFT_1_BYTE);
48 72 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_SUCCESS );
49 73 // DATA FIELD HEADER
50 74 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
51 75 TM.serviceType = TM_TYPE_TC_EXE;
52 76 TM.serviceSubType = TM_SUBTYPE_EXE_OK;
53 77 TM.destinationID = TC->sourceID;
54 78 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
55 79 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
56 80 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
57 81 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
58 82 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
59 83 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
60 84 //
61 85 TM.telecommand_pkt_id[0] = TC->packetID[0];
62 86 TM.telecommand_pkt_id[1] = TC->packetID[1];
63 87 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
64 88 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
65 89
66 90 messageSize = PACKET_LENGTH_TC_EXE_SUCCESS + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
67 91
68 92 // SEND DATA
69 93 status = rtems_message_queue_send( queue_id, &TM, messageSize);
70 94 if (status != RTEMS_SUCCESSFUL) {
71 95 PRINTF("in send_tm_lfr_tc_exe_success *** ERR\n")
72 96 }
73 97
74 98 // UPDATE HK FIELDS
75 99 update_last_TC_exe( TC, TM.time );
76 100
77 101 return status;
78 102 }
79 103
80 104 int send_tm_lfr_tc_exe_inconsistent( ccsdsTelecommandPacket_t *TC, rtems_id queue_id,
81 105 unsigned char byte_position, unsigned char rcv_value )
82 106 {
83 107 /** This function sends a TM_LFR_TC_EXE_INCONSISTENT packet in the dedicated RTEMS message queue.
84 108 *
85 109 * @param TC points to the TeleCommand packet that is being processed
86 110 * @param queue_id is the id of the queue which handles TM
87 111 * @param byte_position is the byte position of the MSB of the parameter that has been seen as inconsistent
88 112 * @param rcv_value is the value of the LSB of the parameter that has been detected as inconsistent
89 113 *
90 114 * @return RTEMS directive status code:
91 115 * - RTEMS_SUCCESSFUL - message sent successfully
92 116 * - RTEMS_INVALID_ID - invalid queue id
93 117 * - RTEMS_INVALID_SIZE - invalid message size
94 118 * - RTEMS_INVALID_ADDRESS - buffer is NULL
95 119 * - RTEMS_UNSATISFIED - out of message buffers
96 120 * - RTEMS_TOO_MANY - queue s limit has been reached
97 121 *
98 122 */
99 123
100 124 rtems_status_code status;
101 125 Packet_TM_LFR_TC_EXE_INCONSISTENT_t TM;
102 126 unsigned char messageSize;
103 127
104 128 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
105 129 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
106 130 TM.reserved = DEFAULT_RESERVED;
107 131 TM.userApplication = CCSDS_USER_APP;
108 132 // PACKET HEADER
109 133 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
110 134 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
111 135 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
112 136 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_INCONSISTENT >> SHIFT_1_BYTE);
113 137 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_INCONSISTENT );
114 138 // DATA FIELD HEADER
115 139 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
116 140 TM.serviceType = TM_TYPE_TC_EXE;
117 141 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
118 142 TM.destinationID = TC->sourceID;
119 143 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
120 144 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
121 145 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
122 146 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
123 147 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
124 148 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
125 149 //
126 150 TM.tc_failure_code[0] = (char) (WRONG_APP_DATA >> SHIFT_1_BYTE);
127 151 TM.tc_failure_code[1] = (char) (WRONG_APP_DATA );
128 152 TM.telecommand_pkt_id[0] = TC->packetID[0];
129 153 TM.telecommand_pkt_id[1] = TC->packetID[1];
130 154 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
131 155 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
132 156 TM.tc_service = TC->serviceType; // type of the rejected TC
133 157 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
134 158 TM.byte_position = byte_position;
135 159 TM.rcv_value = (unsigned char) rcv_value;
136 160
137 161 messageSize = PACKET_LENGTH_TC_EXE_INCONSISTENT + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
138 162
139 163 // SEND DATA
140 164 status = rtems_message_queue_send( queue_id, &TM, messageSize);
141 165 if (status != RTEMS_SUCCESSFUL) {
142 166 PRINTF("in send_tm_lfr_tc_exe_inconsistent *** ERR\n")
143 167 }
144 168
145 169 // UPDATE HK FIELDS
146 170 update_last_TC_rej( TC, TM.time );
147 171
148 172 return status;
149 173 }
150 174
151 175 int send_tm_lfr_tc_exe_not_executable( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
152 176 {
153 177 /** This function sends a TM_LFR_TC_EXE_NOT_EXECUTABLE packet in the dedicated RTEMS message queue.
154 178 *
155 179 * @param TC points to the TeleCommand packet that is being processed
156 180 * @param queue_id is the id of the queue which handles TM
157 181 *
158 182 * @return RTEMS directive status code:
159 183 * - RTEMS_SUCCESSFUL - message sent successfully
160 184 * - RTEMS_INVALID_ID - invalid queue id
161 185 * - RTEMS_INVALID_SIZE - invalid message size
162 186 * - RTEMS_INVALID_ADDRESS - buffer is NULL
163 187 * - RTEMS_UNSATISFIED - out of message buffers
164 188 * - RTEMS_TOO_MANY - queue s limit has been reached
165 189 *
166 190 */
167 191
168 192 rtems_status_code status;
169 193 Packet_TM_LFR_TC_EXE_NOT_EXECUTABLE_t TM;
170 194 unsigned char messageSize;
171 195
172 196 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
173 197 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
174 198 TM.reserved = DEFAULT_RESERVED;
175 199 TM.userApplication = CCSDS_USER_APP;
176 200 // PACKET HEADER
177 201 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
178 202 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
179 203 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
180 204 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE >> SHIFT_1_BYTE);
181 205 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE );
182 206 // DATA FIELD HEADER
183 207 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
184 208 TM.serviceType = TM_TYPE_TC_EXE;
185 209 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
186 210 TM.destinationID = TC->sourceID; // default destination id
187 211 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
188 212 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
189 213 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
190 214 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
191 215 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
192 216 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
193 217 //
194 218 TM.tc_failure_code[0] = (char) (TC_NOT_EXE >> SHIFT_1_BYTE);
195 219 TM.tc_failure_code[1] = (char) (TC_NOT_EXE );
196 220 TM.telecommand_pkt_id[0] = TC->packetID[0];
197 221 TM.telecommand_pkt_id[1] = TC->packetID[1];
198 222 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
199 223 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
200 224 TM.tc_service = TC->serviceType; // type of the rejected TC
201 225 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
202 226 TM.lfr_status_word[0] = housekeeping_packet.lfr_status_word[0];
203 227 TM.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1];
204 228
205 229 messageSize = PACKET_LENGTH_TC_EXE_NOT_EXECUTABLE + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
206 230
207 231 // SEND DATA
208 232 status = rtems_message_queue_send( queue_id, &TM, messageSize);
209 233 if (status != RTEMS_SUCCESSFUL) {
210 234 PRINTF("in send_tm_lfr_tc_exe_not_executable *** ERR\n")
211 235 }
212 236
213 237 // UPDATE HK FIELDS
214 238 update_last_TC_rej( TC, TM.time );
215 239
216 240 return status;
217 241 }
218 242
219 243 int send_tm_lfr_tc_exe_not_implemented( ccsdsTelecommandPacket_t *TC, rtems_id queue_id, unsigned char *time )
220 244 {
221 245 /** This function sends a TM_LFR_TC_EXE_NOT_IMPLEMENTED packet in the dedicated RTEMS message queue.
222 246 *
223 247 * @param TC points to the TeleCommand packet that is being processed
224 248 * @param queue_id is the id of the queue which handles TM
225 249 *
226 250 * @return RTEMS directive status code:
227 251 * - RTEMS_SUCCESSFUL - message sent successfully
228 252 * - RTEMS_INVALID_ID - invalid queue id
229 253 * - RTEMS_INVALID_SIZE - invalid message size
230 254 * - RTEMS_INVALID_ADDRESS - buffer is NULL
231 255 * - RTEMS_UNSATISFIED - out of message buffers
232 256 * - RTEMS_TOO_MANY - queue s limit has been reached
233 257 *
234 258 */
235 259
236 260 rtems_status_code status;
237 261 Packet_TM_LFR_TC_EXE_NOT_IMPLEMENTED_t TM;
238 262 unsigned char messageSize;
239 263
240 264 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
241 265 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
242 266 TM.reserved = DEFAULT_RESERVED;
243 267 TM.userApplication = CCSDS_USER_APP;
244 268 // PACKET HEADER
245 269 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
246 270 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
247 271 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
248 272 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED >> SHIFT_1_BYTE);
249 273 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED );
250 274 // DATA FIELD HEADER
251 275 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
252 276 TM.serviceType = TM_TYPE_TC_EXE;
253 277 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
254 278 TM.destinationID = TC->sourceID; // default destination id
255 279 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
256 280 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
257 281 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
258 282 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
259 283 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
260 284 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
261 285 //
262 286 TM.tc_failure_code[0] = (char) (FUNCT_NOT_IMPL >> SHIFT_1_BYTE);
263 287 TM.tc_failure_code[1] = (char) (FUNCT_NOT_IMPL );
264 288 TM.telecommand_pkt_id[0] = TC->packetID[0];
265 289 TM.telecommand_pkt_id[1] = TC->packetID[1];
266 290 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
267 291 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
268 292 TM.tc_service = TC->serviceType; // type of the rejected TC
269 293 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
270 294
271 295 messageSize = PACKET_LENGTH_TC_EXE_NOT_IMPLEMENTED + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
272 296
273 297 // SEND DATA
274 298 status = rtems_message_queue_send( queue_id, &TM, messageSize);
275 299 if (status != RTEMS_SUCCESSFUL) {
276 300 PRINTF("in send_tm_lfr_tc_exe_not_implemented *** ERR\n")
277 301 }
278 302
279 303 // UPDATE HK FIELDS
280 304 update_last_TC_rej( TC, TM.time );
281 305
282 306 return status;
283 307 }
284 308
285 309 int send_tm_lfr_tc_exe_error( ccsdsTelecommandPacket_t *TC, rtems_id queue_id )
286 310 {
287 311 /** This function sends a TM_LFR_TC_EXE_ERROR packet in the dedicated RTEMS message queue.
288 312 *
289 313 * @param TC points to the TeleCommand packet that is being processed
290 314 * @param queue_id is the id of the queue which handles TM
291 315 *
292 316 * @return RTEMS directive status code:
293 317 * - RTEMS_SUCCESSFUL - message sent successfully
294 318 * - RTEMS_INVALID_ID - invalid queue id
295 319 * - RTEMS_INVALID_SIZE - invalid message size
296 320 * - RTEMS_INVALID_ADDRESS - buffer is NULL
297 321 * - RTEMS_UNSATISFIED - out of message buffers
298 322 * - RTEMS_TOO_MANY - queue s limit has been reached
299 323 *
300 324 */
301 325
302 326 rtems_status_code status;
303 327 Packet_TM_LFR_TC_EXE_ERROR_t TM;
304 328 unsigned char messageSize;
305 329
306 330 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
307 331 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
308 332 TM.reserved = DEFAULT_RESERVED;
309 333 TM.userApplication = CCSDS_USER_APP;
310 334 // PACKET HEADER
311 335 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
312 336 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
313 337 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
314 338 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_ERROR >> SHIFT_1_BYTE);
315 339 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_ERROR );
316 340 // DATA FIELD HEADER
317 341 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
318 342 TM.serviceType = TM_TYPE_TC_EXE;
319 343 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
320 344 TM.destinationID = TC->sourceID; // default destination id
321 345 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
322 346 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
323 347 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
324 348 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
325 349 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
326 350 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
327 351 //
328 352 TM.tc_failure_code[0] = (char) (FAIL_DETECTED >> SHIFT_1_BYTE);
329 353 TM.tc_failure_code[1] = (char) (FAIL_DETECTED );
330 354 TM.telecommand_pkt_id[0] = TC->packetID[0];
331 355 TM.telecommand_pkt_id[1] = TC->packetID[1];
332 356 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
333 357 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
334 358 TM.tc_service = TC->serviceType; // type of the rejected TC
335 359 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
336 360
337 361 messageSize = PACKET_LENGTH_TC_EXE_ERROR + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
338 362
339 363 // SEND DATA
340 364 status = rtems_message_queue_send( queue_id, &TM, messageSize);
341 365 if (status != RTEMS_SUCCESSFUL) {
342 366 PRINTF("in send_tm_lfr_tc_exe_error *** ERR\n")
343 367 }
344 368
345 369 // UPDATE HK FIELDS
346 370 update_last_TC_rej( TC, TM.time );
347 371
348 372 return status;
349 373 }
350 374
351 375 int send_tm_lfr_tc_exe_corrupted(ccsdsTelecommandPacket_t *TC, rtems_id queue_id,
352 376 unsigned char *computed_CRC, unsigned char *currentTC_LEN_RCV,
353 377 unsigned char destinationID )
354 378 {
355 379 /** This function sends a TM_LFR_TC_EXE_CORRUPTED packet in the dedicated RTEMS message queue.
356 380 *
357 381 * @param TC points to the TeleCommand packet that is being processed
358 382 * @param queue_id is the id of the queue which handles TM
359 383 * @param computed_CRC points to a buffer of two bytes containing the CRC computed during the parsing of the TeleCommand
360 384 * @param currentTC_LEN_RCV points to a buffer of two bytes containing a packet size field computed on the received data
361 385 *
362 386 * @return RTEMS directive status code:
363 387 * - RTEMS_SUCCESSFUL - message sent successfully
364 388 * - RTEMS_INVALID_ID - invalid queue id
365 389 * - RTEMS_INVALID_SIZE - invalid message size
366 390 * - RTEMS_INVALID_ADDRESS - buffer is NULL
367 391 * - RTEMS_UNSATISFIED - out of message buffers
368 392 * - RTEMS_TOO_MANY - queue s limit has been reached
369 393 *
370 394 */
371 395
372 396 rtems_status_code status;
373 397 Packet_TM_LFR_TC_EXE_CORRUPTED_t TM;
374 398 unsigned char messageSize;
375 399 unsigned int packetLength;
376 400 unsigned int estimatedPacketLength;
377 401 unsigned char *packetDataField;
378 402
379 403 packetLength = (TC->packetLength[0] * CONST_256) + TC->packetLength[1]; // compute the packet length parameter written in the TC
380 404 estimatedPacketLength = (unsigned int) ((currentTC_LEN_RCV[0] * CONST_256) + currentTC_LEN_RCV[1]);
381 405 packetDataField = (unsigned char *) &TC->headerFlag_pusVersion_Ack; // get the beginning of the data field
382 406
383 407 TM.targetLogicalAddress = CCSDS_DESTINATION_ID;
384 408 TM.protocolIdentifier = CCSDS_PROTOCOLE_ID;
385 409 TM.reserved = DEFAULT_RESERVED;
386 410 TM.userApplication = CCSDS_USER_APP;
387 411 // PACKET HEADER
388 412 TM.packetID[0] = (unsigned char) (APID_TM_TC_EXE >> SHIFT_1_BYTE);
389 413 TM.packetID[1] = (unsigned char) (APID_TM_TC_EXE );
390 414 increment_seq_counter_destination_id( TM.packetSequenceControl, TC->sourceID );
391 415 TM.packetLength[0] = (unsigned char) (PACKET_LENGTH_TC_EXE_CORRUPTED >> SHIFT_1_BYTE);
392 416 TM.packetLength[1] = (unsigned char) (PACKET_LENGTH_TC_EXE_CORRUPTED );
393 417 // DATA FIELD HEADER
394 418 TM.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
395 419 TM.serviceType = TM_TYPE_TC_EXE;
396 420 TM.serviceSubType = TM_SUBTYPE_EXE_NOK;
397 421 TM.destinationID = destinationID;
398 422 TM.time[BYTE_0] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_3_BYTES);
399 423 TM.time[BYTE_1] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_2_BYTES);
400 424 TM.time[BYTE_2] = (unsigned char) (time_management_regs->coarse_time >> SHIFT_1_BYTE);
401 425 TM.time[BYTE_3] = (unsigned char) (time_management_regs->coarse_time);
402 426 TM.time[BYTE_4] = (unsigned char) (time_management_regs->fine_time >> SHIFT_1_BYTE);
403 427 TM.time[BYTE_5] = (unsigned char) (time_management_regs->fine_time);
404 428 //
405 429 TM.tc_failure_code[0] = (unsigned char) (CORRUPTED >> SHIFT_1_BYTE);
406 430 TM.tc_failure_code[1] = (unsigned char) (CORRUPTED );
407 431 TM.telecommand_pkt_id[0] = TC->packetID[0];
408 432 TM.telecommand_pkt_id[1] = TC->packetID[1];
409 433 TM.pkt_seq_control[0] = TC->packetSequenceControl[0];
410 434 TM.pkt_seq_control[1] = TC->packetSequenceControl[1];
411 435 TM.tc_service = TC->serviceType; // type of the rejected TC
412 436 TM.tc_subtype = TC->serviceSubType; // subtype of the rejected TC
413 437 TM.pkt_len_rcv_value[0] = TC->packetLength[0];
414 438 TM.pkt_len_rcv_value[1] = TC->packetLength[1];
415 439 TM.pkt_datafieldsize_cnt[0] = currentTC_LEN_RCV[0];
416 440 TM.pkt_datafieldsize_cnt[1] = currentTC_LEN_RCV[1];
417 // TM.rcv_crc[0] = packetDataField[ packetLength - 1 ];
418 // TM.rcv_crc[1] = packetDataField[ packetLength ];
419 441 TM.rcv_crc[0] = packetDataField[ estimatedPacketLength - 1 ];
420 442 TM.rcv_crc[1] = packetDataField[ estimatedPacketLength ];
421 443 TM.computed_crc[0] = computed_CRC[0];
422 444 TM.computed_crc[1] = computed_CRC[1];
423 445
424 446 messageSize = PACKET_LENGTH_TC_EXE_CORRUPTED + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
425 447
426 448 // SEND DATA
427 449 status = rtems_message_queue_send( queue_id, &TM, messageSize);
428 450 if (status != RTEMS_SUCCESSFUL) {
429 451 PRINTF("in send_tm_lfr_tc_exe_error *** ERR\n")
430 452 }
431 453
432 454 // UPDATE HK FIELDS
433 455 update_last_TC_rej( TC, TM.time );
434 456
435 457 return status;
436 458 }
437 459
438 460 void increment_seq_counter_destination_id( unsigned char *packet_sequence_control, unsigned char destination_id )
439 461 {
440 462 /** This function increment the packet sequence control parameter of a TC, depending on its destination ID.
441 463 *
442 464 * @param packet_sequence_control points to the packet sequence control which will be incremented
443 465 * @param destination_id is the destination ID of the TM, there is one counter by destination ID
444 466 *
445 467 * If the destination ID is not known, a dedicated counter is incremented.
446 468 *
447 469 */
448 470
449 471 unsigned short sequence_cnt;
450 472 unsigned short segmentation_grouping_flag;
451 473 unsigned short new_packet_sequence_control;
452 474 unsigned char i;
453 475
454 476 switch (destination_id)
455 477 {
456 478 case SID_TC_GROUND:
457 479 i = GROUND;
458 480 break;
459 481 case SID_TC_MISSION_TIMELINE:
460 482 i = MISSION_TIMELINE;
461 483 break;
462 484 case SID_TC_TC_SEQUENCES:
463 485 i = TC_SEQUENCES;
464 486 break;
465 487 case SID_TC_RECOVERY_ACTION_CMD:
466 488 i = RECOVERY_ACTION_CMD;
467 489 break;
468 490 case SID_TC_BACKUP_MISSION_TIMELINE:
469 491 i = BACKUP_MISSION_TIMELINE;
470 492 break;
471 493 case SID_TC_DIRECT_CMD:
472 494 i = DIRECT_CMD;
473 495 break;
474 496 case SID_TC_SPARE_GRD_SRC1:
475 497 i = SPARE_GRD_SRC1;
476 498 break;
477 499 case SID_TC_SPARE_GRD_SRC2:
478 500 i = SPARE_GRD_SRC2;
479 501 break;
480 502 case SID_TC_OBCP:
481 503 i = OBCP;
482 504 break;
483 505 case SID_TC_SYSTEM_CONTROL:
484 506 i = SYSTEM_CONTROL;
485 507 break;
486 508 case SID_TC_AOCS:
487 509 i = AOCS;
488 510 break;
489 511 case SID_TC_RPW_INTERNAL:
490 512 i = RPW_INTERNAL;
491 513 break;
492 514 default:
493 515 i = GROUND;
494 516 break;
495 517 }
496 518
497 519 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
498 520 sequence_cnt = sequenceCounters_TC_EXE[ i ] & SEQ_CNT_MASK;
499 521
500 522 new_packet_sequence_control = segmentation_grouping_flag | sequence_cnt ;
501 523
502 524 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
503 525 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
504 526
505 527 // increment the sequence counter
506 528 if ( sequenceCounters_TC_EXE[ i ] < SEQ_CNT_MAX )
507 529 {
508 530 sequenceCounters_TC_EXE[ i ] = sequenceCounters_TC_EXE[ i ] + 1;
509 531 }
510 532 else
511 533 {
512 534 sequenceCounters_TC_EXE[ i ] = 0;
513 535 }
514 536 }
Show file before | Show file after | Hide comments
@@ -1,1343 +1,1366
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 /** Functions and tasks related to waveform packet generation.
2 26 *
3 27 * @file
4 28 * @author P. LEROY
5 29 *
6 30 * A group of functions to handle waveforms, in snapshot or continuous format.\n
7 31 *
8 32 */
9 33
10 34 #include "wf_handler.h"
11 35
12 36 //***************
13 37 // waveform rings
14 38 // F0
15 39 ring_node waveform_ring_f0[NB_RING_NODES_F0]= {0};
16 40 ring_node *current_ring_node_f0 = NULL;
17 41 ring_node *ring_node_to_send_swf_f0 = NULL;
18 42 // F1
19 43 ring_node waveform_ring_f1[NB_RING_NODES_F1] = {0};
20 44 ring_node *current_ring_node_f1 = NULL;
21 45 ring_node *ring_node_to_send_swf_f1 = NULL;
22 46 ring_node *ring_node_to_send_cwf_f1 = NULL;
23 47 // F2
24 48 ring_node waveform_ring_f2[NB_RING_NODES_F2] = {0};
25 49 ring_node *current_ring_node_f2 = NULL;
26 50 ring_node *ring_node_to_send_swf_f2 = NULL;
27 51 ring_node *ring_node_to_send_cwf_f2 = NULL;
28 52 // F3
29 53 ring_node waveform_ring_f3[NB_RING_NODES_F3] = {0};
30 54 ring_node *current_ring_node_f3 = NULL;
31 55 ring_node *ring_node_to_send_cwf_f3 = NULL;
32 56 char wf_cont_f3_light[ (NB_SAMPLES_PER_SNAPSHOT) * NB_BYTES_CWF3_LIGHT_BLK ] = {0};
33 57
34 58 bool extractSWF1 = false;
35 59 bool extractSWF2 = false;
36 60 bool swf0_ready_flag_f1 = false;
37 61 bool swf0_ready_flag_f2 = false;
38 62 bool swf1_ready = false;
39 63 bool swf2_ready = false;
40 64
41 65 int swf1_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ] = {0};
42 66 int swf2_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ] = {0};
43 67 ring_node ring_node_swf1_extracted = {0};
44 68 ring_node ring_node_swf2_extracted = {0};
45 69
46 70 typedef enum resynchro_state_t
47 71 {
48 72 MEASURE,
49 73 CORRECTION
50 74 } resynchro_state;
51 75
52 76 //*********************
53 77 // Interrupt SubRoutine
54 78
55 79 ring_node * getRingNodeToSendCWF( unsigned char frequencyChannel)
56 80 {
57 81 ring_node *node;
58 82
59 83 node = NULL;
60 84 switch ( frequencyChannel ) {
61 85 case CHANNELF1:
62 86 node = ring_node_to_send_cwf_f1;
63 87 break;
64 88 case CHANNELF2:
65 89 node = ring_node_to_send_cwf_f2;
66 90 break;
67 91 case CHANNELF3:
68 92 node = ring_node_to_send_cwf_f3;
69 93 break;
70 94 default:
71 95 break;
72 96 }
73 97
74 98 return node;
75 99 }
76 100
77 101 ring_node * getRingNodeToSendSWF( unsigned char frequencyChannel)
78 102 {
79 103 ring_node *node;
80 104
81 105 node = NULL;
82 106 switch ( frequencyChannel ) {
83 107 case CHANNELF0:
84 108 node = ring_node_to_send_swf_f0;
85 109 break;
86 110 case CHANNELF1:
87 111 node = ring_node_to_send_swf_f1;
88 112 break;
89 113 case CHANNELF2:
90 114 node = ring_node_to_send_swf_f2;
91 115 break;
92 116 default:
93 117 break;
94 118 }
95 119
96 120 return node;
97 121 }
98 122
99 123 void reset_extractSWF( void )
100 124 {
101 125 extractSWF1 = false;
102 126 extractSWF2 = false;
103 127 swf0_ready_flag_f1 = false;
104 128 swf0_ready_flag_f2 = false;
105 129 swf1_ready = false;
106 130 swf2_ready = false;
107 131 }
108 132
109 133 inline void waveforms_isr_f3( void )
110 134 {
111 135 rtems_status_code spare_status;
112 136
113 137 if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_BURST) // in BURST the data are used to place v, e1 and e2 in the HK packet
114 138 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) )
115 139 { // in modes other than STANDBY and BURST, send the CWF_F3 data
116 140 //***
117 141 // F3
118 142 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F3) != INIT_CHAR ) { // [1100 0000] check the f3 full bits
119 143 ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
120 144 current_ring_node_f3 = current_ring_node_f3->next;
121 145 if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_0) == BIT_WFP_BUF_F3_0){ // [0100 0000] f3 buffer 0 is full
122 146 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
123 147 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
124 148 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
125 149 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F3_0; // [1000 1000 0100 0000]
126 150 }
127 151 else if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_1) == BIT_WFP_BUF_F3_1){ // [1000 0000] f3 buffer 1 is full
128 152 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
129 153 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
130 154 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
131 155 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F3_1; // [1000 1000 1000 0000]
132 156 }
133 157 if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) {
134 158 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
135 159 }
136 160 }
137 161 }
138 162 }
139 163
140 164 inline void waveforms_isr_burst( void )
141 165 {
142 166 unsigned char status;
143 167 rtems_status_code spare_status;
144 168
145 169 status = (waveform_picker_regs->status & BITS_WFP_STATUS_F2) >> SHIFT_WFP_STATUS_F2; // [0011 0000] get the status bits for f2
146 170
147 171 switch(status)
148 172 {
149 173 case BIT_WFP_BUFFER_0:
150 174 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
151 175 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
152 176 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
153 177 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
154 178 current_ring_node_f2 = current_ring_node_f2->next;
155 179 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
156 180 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
157 181 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
158 182 }
159 183 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000]
160 184 break;
161 185 case BIT_WFP_BUFFER_1:
162 186 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
163 187 ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2;
164 188 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
165 189 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
166 190 current_ring_node_f2 = current_ring_node_f2->next;
167 191 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
168 192 if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) {
169 193 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 );
170 194 }
171 195 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000]
172 196 break;
173 197 default:
174 198 break;
175 199 }
176 200 }
177 201
178 202 inline void waveform_isr_normal_sbm1_sbm2( void )
179 203 {
180 204 rtems_status_code status;
181 205
182 206 //***
183 207 // F0
184 208 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F0) != INIT_CHAR ) // [0000 0011] check the f0 full bits
185 209 {
186 210 swf0_ready_flag_f1 = true;
187 211 swf0_ready_flag_f2 = true;
188 212 ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
189 213 current_ring_node_f0 = current_ring_node_f0->next;
190 214 if ( (waveform_picker_regs->status & BIT_WFP_BUFFER_0) == BIT_WFP_BUFFER_0)
191 215 {
192 216
193 217 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
194 218 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
195 219 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
196 220 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F0_0; // [0001 0001 0000 0001]
197 221 }
198 222 else if ( (waveform_picker_regs->status & BIT_WFP_BUFFER_1) == BIT_WFP_BUFFER_1)
199 223 {
200 224 ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
201 225 ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
202 226 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
203 227 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F0_1; // [0001 0001 0000 0010]
204 228 }
205 229 // send an event to the WFRM task for resynchro activities
206 230 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_SWF_RESYNCH );
231 status = rtems_event_send( Task_id[TASKID_CALI], RTEMS_EVENT_CAL_SWEEP_WAKE );
207 232 }
208 233
209 234 //***
210 235 // F1
211 236 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F1) != INIT_CHAR ) { // [0000 1100] check the f1 full bits
212 237 // (1) change the receiving buffer for the waveform picker
213 238 ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
214 239 current_ring_node_f1 = current_ring_node_f1->next;
215 240 if ( (waveform_picker_regs->status & BIT_WFP_BUF_F1_0) == BIT_WFP_BUF_F1_0)
216 241 {
217 242 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
218 243 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
219 244 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
220 245 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F1_0; // [0010 0010 0000 0100] f1 bits = 0
221 246 }
222 247 else if ( (waveform_picker_regs->status & BIT_WFP_BUF_F1_1) == BIT_WFP_BUF_F1_1)
223 248 {
224 249 ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
225 250 ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
226 251 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
227 252 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F1_1; // [0010 0010 0000 1000] f1 bits = 0
228 253 }
229 254 // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed)
230 255 status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_NORM_S1_S2 );
231 256 }
232 257
233 258 //***
234 259 // F2
235 260 if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F2) != INIT_CHAR ) { // [0011 0000] check the f2 full bit
236 261 // (1) change the receiving buffer for the waveform picker
237 262 ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
238 263 ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2;
239 264 current_ring_node_f2 = current_ring_node_f2->next;
240 265 if ( (waveform_picker_regs->status & BIT_WFP_BUF_F2_0) == BIT_WFP_BUF_F2_0)
241 266 {
242 267 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
243 268 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
244 269 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
245 270 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000]
246 271 }
247 272 else if ( (waveform_picker_regs->status & BIT_WFP_BUF_F2_1) == BIT_WFP_BUF_F2_1)
248 273 {
249 274 ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
250 275 ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
251 276 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
252 277 waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000]
253 278 }
254 279 // (2) send an event for the waveforms transmission
255 280 status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_NORM_S1_S2 );
256 281 }
257 282 }
258 283
259 284 rtems_isr waveforms_isr( rtems_vector_number vector )
260 285 {
261 286 /** This is the interrupt sub routine called by the waveform picker core.
262 287 *
263 288 * This ISR launch different actions depending mainly on two pieces of information:
264 289 * 1. the values read in the registers of the waveform picker.
265 290 * 2. the current LFR mode.
266 291 *
267 292 */
268 293
269 294 // STATUS
270 295 // new error error buffer full
271 296 // 15 14 13 12 11 10 9 8
272 297 // f3 f2 f1 f0 f3 f2 f1 f0
273 298 //
274 299 // ready buffer
275 300 // 7 6 5 4 3 2 1 0
276 301 // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
277 302
278 303 rtems_status_code spare_status;
279 304
280 305 waveforms_isr_f3();
281 306
282 307 //*************************************************
283 308 // copy the status bits in the housekeeping packets
284 309 housekeeping_packet.hk_lfr_vhdl_iir_cal =
285 310 (unsigned char) ((waveform_picker_regs->status & BYTE0_MASK) >> SHIFT_1_BYTE);
286 311
287 312 if ( (waveform_picker_regs->status & BYTE0_MASK) != INIT_CHAR) // [1111 1111 0000 0000] check the error bits
288 313 {
289 314 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_10 );
290 315 }
291 316
292 317 switch(lfrCurrentMode)
293 318 {
294 319 //********
295 320 // STANDBY
296 321 case LFR_MODE_STANDBY:
297 322 break;
298 323 //**************************
299 324 // LFR NORMAL, SBM1 and SBM2
300 325 case LFR_MODE_NORMAL:
301 326 case LFR_MODE_SBM1:
302 327 case LFR_MODE_SBM2:
303 328 waveform_isr_normal_sbm1_sbm2();
304 329 break;
305 330 //******
306 331 // BURST
307 332 case LFR_MODE_BURST:
308 333 waveforms_isr_burst();
309 334 break;
310 335 //********
311 336 // DEFAULT
312 337 default:
313 338 break;
314 339 }
315 340 }
316 341
317 342 //************
318 343 // RTEMS TASKS
319 344
320 345 rtems_task wfrm_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
321 346 {
322 347 /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
323 348 *
324 349 * @param unused is the starting argument of the RTEMS task
325 350 *
326 351 * The following data packets are sent by this task:
327 352 * - TM_LFR_SCIENCE_NORMAL_SWF_F0
328 353 * - TM_LFR_SCIENCE_NORMAL_SWF_F1
329 354 * - TM_LFR_SCIENCE_NORMAL_SWF_F2
330 355 *
331 356 */
332 357
333 358 rtems_event_set event_out;
334 359 rtems_id queue_id;
335 360 rtems_status_code status;
336 361 ring_node *ring_node_swf1_extracted_ptr;
337 362 ring_node *ring_node_swf2_extracted_ptr;
338 363
339 364 event_out = EVENT_SETS_NONE_PENDING;
340 365 queue_id = RTEMS_ID_NONE;
341 366
342 367 ring_node_swf1_extracted_ptr = (ring_node *) &ring_node_swf1_extracted;
343 368 ring_node_swf2_extracted_ptr = (ring_node *) &ring_node_swf2_extracted;
344 369
345 370 status = get_message_queue_id_send( &queue_id );
346 371 if (status != RTEMS_SUCCESSFUL)
347 372 {
348 373 PRINTF1("in WFRM *** ERR get_message_queue_id_send %d\n", status);
349 374 }
350 375
351 376 BOOT_PRINTF("in WFRM ***\n");
352 377
353 378 while(1){
354 379 // wait for an RTEMS_EVENT
355 380 rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_SWF_RESYNCH,
356 381 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
357 382
358 383 if (event_out == RTEMS_EVENT_MODE_NORMAL)
359 384 {
360 385 DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n");
361 386 ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0;
362 387 ring_node_swf1_extracted_ptr->sid = SID_NORM_SWF_F1;
363 388 ring_node_swf2_extracted_ptr->sid = SID_NORM_SWF_F2;
364 389 status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) );
365 390 status = rtems_message_queue_send( queue_id, &ring_node_swf1_extracted_ptr, sizeof( ring_node* ) );
366 391 status = rtems_message_queue_send( queue_id, &ring_node_swf2_extracted_ptr, sizeof( ring_node* ) );
367 392 }
368 393 if (event_out == RTEMS_EVENT_SWF_RESYNCH)
369 394 {
370 395 snapshot_resynchronization( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
371 396 }
372 397 }
373 398 }
374 399
375 400 rtems_task cwf3_task(rtems_task_argument argument) //used with the waveform picker VHDL IP
376 401 {
377 402 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
378 403 *
379 404 * @param unused is the starting argument of the RTEMS task
380 405 *
381 406 * The following data packet is sent by this task:
382 407 * - TM_LFR_SCIENCE_NORMAL_CWF_F3
383 408 *
384 409 */
385 410
386 411 rtems_event_set event_out;
387 412 rtems_id queue_id;
388 413 rtems_status_code status;
389 414 ring_node ring_node_cwf3_light;
390 415 ring_node *ring_node_to_send_cwf;
391 416
392 417 event_out = EVENT_SETS_NONE_PENDING;
393 418 queue_id = RTEMS_ID_NONE;
394 419
395 420 status = get_message_queue_id_send( &queue_id );
396 421 if (status != RTEMS_SUCCESSFUL)
397 422 {
398 423 PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status)
399 424 }
400 425
401 426 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
402 427
403 428 // init the ring_node_cwf3_light structure
404 429 ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light;
405 430 ring_node_cwf3_light.coarseTime = INIT_CHAR;
406 431 ring_node_cwf3_light.fineTime = INIT_CHAR;
407 432 ring_node_cwf3_light.next = NULL;
408 433 ring_node_cwf3_light.previous = NULL;
409 434 ring_node_cwf3_light.sid = SID_NORM_CWF_F3;
410 435 ring_node_cwf3_light.status = INIT_CHAR;
411 436
412 437 BOOT_PRINTF("in CWF3 ***\n");
413 438
414 439 while(1){
415 440 // wait for an RTEMS_EVENT
416 441 rtems_event_receive( RTEMS_EVENT_0,
417 442 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
418 443 if ( (lfrCurrentMode == LFR_MODE_NORMAL)
419 444 || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode==LFR_MODE_SBM2) )
420 445 {
421 446 ring_node_to_send_cwf = getRingNodeToSendCWF( CHANNELF3 );
422 447 if ( (parameter_dump_packet.sy_lfr_n_cwf_long_f3 & BIT_CWF_LONG_F3) == BIT_CWF_LONG_F3)
423 448 {
424 449 PRINTF("send CWF_LONG_F3\n");
425 450 ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3;
426 451 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
427 452 }
428 453 else
429 454 {
430 455 PRINTF("send CWF_F3 (light)\n");
431 456 send_waveform_CWF3_light( ring_node_to_send_cwf, &ring_node_cwf3_light, queue_id );
432 457 }
433 458
434 459 }
435 460 else
436 461 {
437 462 PRINTF1("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode)
438 463 }
439 464 }
440 465 }
441 466
442 467 rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
443 468 {
444 469 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
445 470 *
446 471 * @param unused is the starting argument of the RTEMS task
447 472 *
448 473 * The following data packet is sent by this function:
449 474 * - TM_LFR_SCIENCE_BURST_CWF_F2
450 475 * - TM_LFR_SCIENCE_SBM2_CWF_F2
451 476 *
452 477 */
453 478
454 479 rtems_event_set event_out;
455 480 rtems_id queue_id;
456 481 rtems_status_code status;
457 482 ring_node *ring_node_to_send;
458 483 unsigned long long int acquisitionTimeF0_asLong;
459 484
460 485 event_out = EVENT_SETS_NONE_PENDING;
461 486 queue_id = RTEMS_ID_NONE;
462 487
463 488 acquisitionTimeF0_asLong = INIT_CHAR;
464 489
465 490 status = get_message_queue_id_send( &queue_id );
466 491 if (status != RTEMS_SUCCESSFUL)
467 492 {
468 493 PRINTF1("in CWF2 *** ERR get_message_queue_id_send %d\n", status)
469 494 }
470 495
471 496 BOOT_PRINTF("in CWF2 ***\n");
472 497
473 498 while(1){
474 499 // wait for an RTEMS_EVENT// send the snapshot when built
475 500 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2 );
476 501 rtems_event_receive( RTEMS_EVENT_MODE_NORM_S1_S2 | RTEMS_EVENT_MODE_BURST,
477 502 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
478 503 ring_node_to_send = getRingNodeToSendCWF( CHANNELF2 );
479 504 if (event_out == RTEMS_EVENT_MODE_BURST)
480 505 { // data are sent whatever the transition time
481 506 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
482 507 }
483 508 else if (event_out == RTEMS_EVENT_MODE_NORM_S1_S2)
484 509 {
485 510 if ( lfrCurrentMode == LFR_MODE_SBM2 )
486 511 {
487 512 // data are sent depending on the transition time
488 513 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
489 514 {
490 515 status = rtems_message_queue_send( queue_id, &ring_node_to_send, sizeof( ring_node* ) );
491 516 }
492 517 }
493 518 // launch snapshot extraction if needed
494 519 if (extractSWF2 == true)
495 520 {
496 521 ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
497 522 // extract the snapshot
498 523 build_snapshot_from_ring( ring_node_to_send_swf_f2, CHANNELF2, acquisitionTimeF0_asLong,
499 524 &ring_node_swf2_extracted, swf2_extracted );
500 525 extractSWF2 = false;
501 526 swf2_ready = true; // once the snapshot at f2 is ready the CWF1 task will send an event to WFRM
502 527 }
503 528 if (swf0_ready_flag_f2 == true)
504 529 {
505 530 extractSWF2 = true;
506 531 // record the acquition time of the f0 snapshot to use to build the snapshot at f2
507 532 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
508 533 swf0_ready_flag_f2 = false;
509 534 }
510 535 }
511 536 }
512 537 }
513 538
514 539 rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
515 540 {
516 541 /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
517 542 *
518 543 * @param unused is the starting argument of the RTEMS task
519 544 *
520 545 * The following data packet is sent by this function:
521 546 * - TM_LFR_SCIENCE_SBM1_CWF_F1
522 547 *
523 548 */
524 549
525 550 rtems_event_set event_out;
526 551 rtems_id queue_id;
527 552 rtems_status_code status;
528 553
529 554 ring_node *ring_node_to_send_cwf;
530 555
531 556 event_out = EVENT_SETS_NONE_PENDING;
532 557 queue_id = RTEMS_ID_NONE;
533 558
534 559 status = get_message_queue_id_send( &queue_id );
535 560 if (status != RTEMS_SUCCESSFUL)
536 561 {
537 562 PRINTF1("in CWF1 *** ERR get_message_queue_id_send %d\n", status)
538 563 }
539 564
540 565 BOOT_PRINTF("in CWF1 ***\n");
541 566
542 567 while(1){
543 568 // wait for an RTEMS_EVENT
544 569 rtems_event_receive( RTEMS_EVENT_MODE_NORM_S1_S2,
545 570 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
546 571 ring_node_to_send_cwf = getRingNodeToSendCWF( 1 );
547 572 ring_node_to_send_cwf_f1->sid = SID_SBM1_CWF_F1;
548 573 if (lfrCurrentMode == LFR_MODE_SBM1)
549 574 {
550 575 // data are sent depending on the transition time
551 576 if ( time_management_regs->coarse_time >= lastValidEnterModeTime )
552 577 {
553 578 status = rtems_message_queue_send( queue_id, &ring_node_to_send_cwf, sizeof( ring_node* ) );
554 579 }
555 580 }
556 581 // launch snapshot extraction if needed
557 582 if (extractSWF1 == true)
558 583 {
559 584 ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
560 585 // launch the snapshot extraction
561 586 status = rtems_event_send( Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_NORM_S1_S2 );
562 587 extractSWF1 = false;
563 588 }
564 589 if (swf0_ready_flag_f1 == true)
565 590 {
566 591 extractSWF1 = true;
567 592 swf0_ready_flag_f1 = false; // this step shall be executed only one time
568 593 }
569 594 if ((swf1_ready == true) && (swf2_ready == true)) // swf_f1 is ready after the extraction
570 595 {
571 596 status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL );
572 597 swf1_ready = false;
573 598 swf2_ready = false;
574 599 }
575 600 }
576 601 }
577 602
578 603 rtems_task swbd_task(rtems_task_argument argument)
579 604 {
580 605 /** This RTEMS task is dedicated to the building of snapshots from different continuous waveforms buffers.
581 606 *
582 607 * @param unused is the starting argument of the RTEMS task
583 608 *
584 609 */
585 610
586 611 rtems_event_set event_out;
587 612 unsigned long long int acquisitionTimeF0_asLong;
588 613
589 614 event_out = EVENT_SETS_NONE_PENDING;
590 615 acquisitionTimeF0_asLong = INIT_CHAR;
591 616
592 617 BOOT_PRINTF("in SWBD ***\n")
593 618
594 619 while(1){
595 620 // wait for an RTEMS_EVENT
596 621 rtems_event_receive( RTEMS_EVENT_MODE_NORM_S1_S2,
597 622 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
598 623 if (event_out == RTEMS_EVENT_MODE_NORM_S1_S2)
599 624 {
600 625 acquisitionTimeF0_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime );
601 626 build_snapshot_from_ring( ring_node_to_send_swf_f1, CHANNELF1, acquisitionTimeF0_asLong,
602 627 &ring_node_swf1_extracted, swf1_extracted );
603 628 swf1_ready = true; // the snapshot has been extracted and is ready to be sent
604 629 }
605 630 else
606 631 {
607 632 PRINTF1("in SWBD *** unexpected rtems event received %x\n", (int) event_out)
608 633 }
609 634 }
610 635 }
611 636
612 637 //******************
613 638 // general functions
614 639
615 640 void WFP_init_rings( void )
616 641 {
617 642 // F0 RING
618 643 init_ring( waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER );
619 644 // F1 RING
620 645 init_ring( waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER );
621 646 // F2 RING
622 647 init_ring( waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER );
623 648 // F3 RING
624 649 init_ring( waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER );
625 650
626 651 ring_node_swf1_extracted.buffer_address = (int) swf1_extracted;
627 652 ring_node_swf2_extracted.buffer_address = (int) swf2_extracted;
628 653
629 654 DEBUG_PRINTF1("waveform_ring_f0 @%x\n", (unsigned int) waveform_ring_f0)
630 655 DEBUG_PRINTF1("waveform_ring_f1 @%x\n", (unsigned int) waveform_ring_f1)
631 656 DEBUG_PRINTF1("waveform_ring_f2 @%x\n", (unsigned int) waveform_ring_f2)
632 657 DEBUG_PRINTF1("waveform_ring_f3 @%x\n", (unsigned int) waveform_ring_f3)
633 658 DEBUG_PRINTF1("wf_buffer_f0 @%x\n", (unsigned int) wf_buffer_f0)
634 659 DEBUG_PRINTF1("wf_buffer_f1 @%x\n", (unsigned int) wf_buffer_f1)
635 660 DEBUG_PRINTF1("wf_buffer_f2 @%x\n", (unsigned int) wf_buffer_f2)
636 661 DEBUG_PRINTF1("wf_buffer_f3 @%x\n", (unsigned int) wf_buffer_f3)
637 662
638 663 }
639 664
640 665 void WFP_reset_current_ring_nodes( void )
641 666 {
642 667 current_ring_node_f0 = waveform_ring_f0[0].next;
643 668 current_ring_node_f1 = waveform_ring_f1[0].next;
644 669 current_ring_node_f2 = waveform_ring_f2[0].next;
645 670 current_ring_node_f3 = waveform_ring_f3[0].next;
646 671
647 672 ring_node_to_send_swf_f0 = waveform_ring_f0;
648 673 ring_node_to_send_swf_f1 = waveform_ring_f1;
649 674 ring_node_to_send_swf_f2 = waveform_ring_f2;
650 675
651 676 ring_node_to_send_cwf_f1 = waveform_ring_f1;
652 677 ring_node_to_send_cwf_f2 = waveform_ring_f2;
653 678 ring_node_to_send_cwf_f3 = waveform_ring_f3;
654 679 }
655 680
656 681 int send_waveform_CWF3_light( ring_node *ring_node_to_send, ring_node *ring_node_cwf3_light, rtems_id queue_id )
657 682 {
658 683 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
659 684 *
660 685 * @param waveform points to the buffer containing the data that will be send.
661 686 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
662 687 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
663 688 * contain information to setup the transmission of the data packets.
664 689 *
665 690 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
666 691 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
667 692 *
668 693 */
669 694
670 695 unsigned int i;
671 696 unsigned int j;
672 697 int ret;
673 698 rtems_status_code status;
674 699
675 700 char *sample;
676 701 int *dataPtr;
677 702
678 703 ret = LFR_DEFAULT;
679 704
680 705 dataPtr = (int*) ring_node_to_send->buffer_address;
681 706
682 707 ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
683 708 ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
684 709
685 710 //**********************
686 711 // BUILD CWF3_light DATA
687 712 for ( i=0; i< NB_SAMPLES_PER_SNAPSHOT; i++)
688 713 {
689 714 sample = (char*) &dataPtr[ (i * NB_WORDS_SWF_BLK) ];
690 715 for (j=0; j < CWF_BLK_SIZE; j++)
691 716 {
692 717 wf_cont_f3_light[ (i * NB_BYTES_CWF3_LIGHT_BLK) + j] = sample[ j ];
693 718 }
694 719 }
695 720
696 721 // SEND PACKET
697 722 status = rtems_message_queue_send( queue_id, &ring_node_cwf3_light, sizeof( ring_node* ) );
698 723 if (status != RTEMS_SUCCESSFUL) {
699 724 ret = LFR_DEFAULT;
700 725 }
701 726
702 727 return ret;
703 728 }
704 729
705 730 void compute_acquisition_time( unsigned int coarseTime, unsigned int fineTime,
706 731 unsigned int sid, unsigned char pa_lfr_pkt_nr, unsigned char * acquisitionTime )
707 732 {
708 733 unsigned long long int acquisitionTimeAsLong;
709 734 unsigned char localAcquisitionTime[BYTES_PER_TIME];
710 735 double deltaT;
711 736
712 737 deltaT = INIT_FLOAT;
713 738
714 739 localAcquisitionTime[BYTE_0] = (unsigned char) ( coarseTime >> SHIFT_3_BYTES );
715 740 localAcquisitionTime[BYTE_1] = (unsigned char) ( coarseTime >> SHIFT_2_BYTES );
716 741 localAcquisitionTime[BYTE_2] = (unsigned char) ( coarseTime >> SHIFT_1_BYTE );
717 742 localAcquisitionTime[BYTE_3] = (unsigned char) ( coarseTime );
718 743 localAcquisitionTime[BYTE_4] = (unsigned char) ( fineTime >> SHIFT_1_BYTE );
719 744 localAcquisitionTime[BYTE_5] = (unsigned char) ( fineTime );
720 745
721 746 acquisitionTimeAsLong = ( (unsigned long long int) localAcquisitionTime[BYTE_0] << SHIFT_5_BYTES )
722 747 + ( (unsigned long long int) localAcquisitionTime[BYTE_1] << SHIFT_4_BYTES )
723 748 + ( (unsigned long long int) localAcquisitionTime[BYTE_2] << SHIFT_3_BYTES )
724 749 + ( (unsigned long long int) localAcquisitionTime[BYTE_3] << SHIFT_2_BYTES )
725 750 + ( (unsigned long long int) localAcquisitionTime[BYTE_4] << SHIFT_1_BYTE )
726 751 + ( (unsigned long long int) localAcquisitionTime[BYTE_5] );
727 752
728 753 switch( sid )
729 754 {
730 755 case SID_NORM_SWF_F0:
731 756 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * T0_IN_FINETIME ;
732 757 break;
733 758
734 759 case SID_NORM_SWF_F1:
735 760 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * T1_IN_FINETIME ;
736 761 break;
737 762
738 763 case SID_NORM_SWF_F2:
739 764 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_304 * T2_IN_FINETIME ;
740 765 break;
741 766
742 767 case SID_SBM1_CWF_F1:
743 768 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T1_IN_FINETIME ;
744 769 break;
745 770
746 771 case SID_SBM2_CWF_F2:
747 772 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T2_IN_FINETIME ;
748 773 break;
749 774
750 775 case SID_BURST_CWF_F2:
751 776 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T2_IN_FINETIME ;
752 777 break;
753 778
754 779 case SID_NORM_CWF_F3:
755 780 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF_SHORT_F3 * T3_IN_FINETIME ;
756 781 break;
757 782
758 783 case SID_NORM_CWF_LONG_F3:
759 784 deltaT = ( (double ) (pa_lfr_pkt_nr) ) * BLK_NR_CWF * T3_IN_FINETIME ;
760 785 break;
761 786
762 787 default:
763 788 PRINTF1("in compute_acquisition_time *** ERR unexpected sid %d\n", sid)
764 789 deltaT = 0.;
765 790 break;
766 791 }
767 792
768 793 acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int) deltaT;
769 794 //
770 795 acquisitionTime[BYTE_0] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_5_BYTES);
771 796 acquisitionTime[BYTE_1] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_4_BYTES);
772 797 acquisitionTime[BYTE_2] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_3_BYTES);
773 798 acquisitionTime[BYTE_3] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_2_BYTES);
774 799 acquisitionTime[BYTE_4] = (unsigned char) (acquisitionTimeAsLong >> SHIFT_1_BYTE );
775 800 acquisitionTime[BYTE_5] = (unsigned char) (acquisitionTimeAsLong );
776 801
777 802 }
778 803
779 804 void build_snapshot_from_ring( ring_node *ring_node_to_send,
780 805 unsigned char frequencyChannel,
781 806 unsigned long long int acquisitionTimeF0_asLong,
782 807 ring_node *ring_node_swf_extracted,
783 808 int *swf_extracted)
784 809 {
785 810 unsigned int i;
786 811 unsigned int node;
787 812 unsigned long long int centerTime_asLong;
788 813 unsigned long long int acquisitionTime_asLong;
789 814 unsigned long long int bufferAcquisitionTime_asLong;
790 815 unsigned char *ptr1;
791 816 unsigned char *ptr2;
792 817 unsigned char *timeCharPtr;
793 818 unsigned char nb_ring_nodes;
794 819 unsigned long long int frequency_asLong;
795 820 unsigned long long int nbTicksPerSample_asLong;
796 821 unsigned long long int nbSamplesPart1_asLong;
797 822 unsigned long long int sampleOffset_asLong;
798 823
799 824 unsigned int deltaT_F0;
800 825 unsigned int deltaT_F1;
801 826 unsigned long long int deltaT_F2;
802 827
803 828 deltaT_F0 = DELTAT_F0;
804 829 deltaT_F1 = DELTAT_F1;
805 830 deltaT_F2 = DELTAT_F2;
806 831 sampleOffset_asLong = INIT_CHAR;
807 832
808 833 // (1) get the f0 acquisition time => the value is passed in argument
809 834
810 835 // (2) compute the central reference time
811 836 centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
812 837 acquisitionTime_asLong = centerTime_asLong; //set to default value (Don_Initialisation_P2)
813 838 bufferAcquisitionTime_asLong = centerTime_asLong; //set to default value (Don_Initialisation_P2)
814 839 nbTicksPerSample_asLong = TICKS_PER_T2; //set to default value (Don_Initialisation_P2)
815 840
816 841 // (3) compute the acquisition time of the current snapshot
817 842 switch(frequencyChannel)
818 843 {
819 844 case CHANNELF1: // 1 is for F1 = 4096 Hz
820 845 acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
821 846 nb_ring_nodes = NB_RING_NODES_F1;
822 847 frequency_asLong = FREQ_F1;
823 848 nbTicksPerSample_asLong = TICKS_PER_T1; // 65536 / 4096;
824 849 break;
825 850 case CHANNELF2: // 2 is for F2 = 256 Hz
826 851 acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
827 852 nb_ring_nodes = NB_RING_NODES_F2;
828 853 frequency_asLong = FREQ_F2;
829 854 nbTicksPerSample_asLong = TICKS_PER_T2; // 65536 / 256;
830 855 break;
831 856 default:
832 857 acquisitionTime_asLong = centerTime_asLong;
833 858 nb_ring_nodes = 0;
834 859 frequency_asLong = FREQ_F2;
835 860 nbTicksPerSample_asLong = TICKS_PER_T2;
836 861 break;
837 862 }
838 863
839 864 //*****************************************************************************
840 865 // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
841 866 node = 0;
842 867 while ( node < nb_ring_nodes)
843 868 {
844 //PRINTF1("%d ... ", node);
845 869 bufferAcquisitionTime_asLong = get_acquisition_time( (unsigned char *) &ring_node_to_send->coarseTime );
846 870 if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
847 871 {
848 //PRINTF1("buffer found with acquisition time = %llx\n", bufferAcquisitionTime_asLong);
849 872 node = nb_ring_nodes;
850 873 }
851 874 else
852 875 {
853 876 node = node + 1;
854 877 ring_node_to_send = ring_node_to_send->previous;
855 878 }
856 879 }
857 880
858 881 // (5) compute the number of samples to take in the current buffer
859 882 sampleOffset_asLong = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong ) >> SHIFT_2_BYTES;
860 883 nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
861 //PRINTF2("sampleOffset_asLong = %lld, nbSamplesPart1_asLong = %lld\n", sampleOffset_asLong, nbSamplesPart1_asLong);
862 884
863 885 // (6) compute the final acquisition time
864 886 acquisitionTime_asLong = bufferAcquisitionTime_asLong +
865 887 (sampleOffset_asLong * nbTicksPerSample_asLong);
866 888
867 889 // (7) copy the acquisition time at the beginning of the extrated snapshot
868 890 ptr1 = (unsigned char*) &acquisitionTime_asLong;
869 891 // fine time
870 892 ptr2 = (unsigned char*) &ring_node_swf_extracted->fineTime;
871 893 ptr2[BYTE_2] = ptr1[ BYTE_4 + OFFSET_2_BYTES ];
872 894 ptr2[BYTE_3] = ptr1[ BYTE_5 + OFFSET_2_BYTES ];
873 895 // coarse time
874 896 ptr2 = (unsigned char*) &ring_node_swf_extracted->coarseTime;
875 897 ptr2[BYTE_0] = ptr1[ BYTE_0 + OFFSET_2_BYTES ];
876 898 ptr2[BYTE_1] = ptr1[ BYTE_1 + OFFSET_2_BYTES ];
877 899 ptr2[BYTE_2] = ptr1[ BYTE_2 + OFFSET_2_BYTES ];
878 900 ptr2[BYTE_3] = ptr1[ BYTE_3 + OFFSET_2_BYTES ];
879 901
880 902 // re set the synchronization bit
881 903 timeCharPtr = (unsigned char*) &ring_node_to_send->coarseTime;
882 904 ptr2[0] = ptr2[0] | (timeCharPtr[0] & SYNC_BIT); // [1000 0000]
883 905
884 if ( (nbSamplesPart1_asLong >= NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0) )
906 if ( (nbSamplesPart1_asLong > NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0) )
885 907 {
886 908 nbSamplesPart1_asLong = 0;
887 909 }
888 910 // copy the part 1 of the snapshot in the extracted buffer
889 911 for ( i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++ )
890 912 {
891 913 swf_extracted[i] =
892 914 ((int*) ring_node_to_send->buffer_address)[ i + (sampleOffset_asLong * NB_WORDS_SWF_BLK) ];
893 915 }
894 916 // copy the part 2 of the snapshot in the extracted buffer
895 917 ring_node_to_send = ring_node_to_send->next;
896 918 for ( i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++ )
897 919 {
898 920 swf_extracted[i] =
899 921 ((int*) ring_node_to_send->buffer_address)[ (i-(nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)) ];
900 922 }
901 923 }
902 924
903 925 double computeCorrection( unsigned char *timePtr )
904 926 {
905 927 unsigned long long int acquisitionTime;
906 928 unsigned long long int centerTime;
907 929 unsigned long long int previousTick;
908 930 unsigned long long int nextTick;
909 931 unsigned long long int deltaPreviousTick;
910 932 unsigned long long int deltaNextTick;
911 933 double deltaPrevious_ms;
912 934 double deltaNext_ms;
913 935 double correctionInF2;
914 936
915 937 correctionInF2 = 0; //set to default value (Don_Initialisation_P2)
916 938
917 939 // get acquisition time in fine time ticks
918 940 acquisitionTime = get_acquisition_time( timePtr );
919 941
920 942 // compute center time
921 943 centerTime = acquisitionTime + DELTAT_F0; // (2048. / 24576. / 2.) * 65536. = 2730.667;
922 944 previousTick = centerTime - (centerTime & INT16_ALL_F);
923 945 nextTick = previousTick + TICKS_PER_S;
924 946
925 947 deltaPreviousTick = centerTime - previousTick;
926 948 deltaNextTick = nextTick - centerTime;
927 949
928 950 deltaPrevious_ms = (((double) deltaPreviousTick) / TICKS_PER_S) * MS_PER_S;
929 951 deltaNext_ms = (((double) deltaNextTick) / TICKS_PER_S) * MS_PER_S;
930 952
931 953 PRINTF2(" delta previous = %.3f ms, delta next = %.2f ms\n", deltaPrevious_ms, deltaNext_ms);
932 954
933 955 // which tick is the closest?
934 956 if (deltaPreviousTick > deltaNextTick)
935 957 {
936 958 // the snapshot center is just before the second => increase delta_snapshot
937 959 correctionInF2 = + (deltaNext_ms * FREQ_F2 / MS_PER_S );
938 960 }
939 961 else
940 962 {
941 963 // the snapshot center is just after the second => decrease delta_snapshot
942 964 correctionInF2 = - (deltaPrevious_ms * FREQ_F2 / MS_PER_S );
943 965 }
944 966
945 967 PRINTF1(" correctionInF2 = %.2f\n", correctionInF2);
946 968
947 969 return correctionInF2;
948 970 }
949 971
950 972 void applyCorrection( double correction )
951 973 {
952 974 int correctionInt;
953 975
954 976 correctionInt = 0;
955 977
956 978 if (correction >= 0.)
957 979 {
958 980 if ( (ONE_TICK_CORR_INTERVAL_0_MIN < correction) && (correction < ONE_TICK_CORR_INTERVAL_0_MAX) )
959 981 {
960 982 correctionInt = ONE_TICK_CORR;
961 983 }
962 984 else
963 985 {
964 986 correctionInt = CORR_MULT * floor(correction);
965 987 }
966 988 }
967 989 else
968 990 {
969 991 if ( (ONE_TICK_CORR_INTERVAL_1_MIN < correction) && (correction < ONE_TICK_CORR_INTERVAL_1_MAX) )
970 992 {
971 993 correctionInt = -ONE_TICK_CORR;
972 994 }
973 995 else
974 996 {
975 997 correctionInt = CORR_MULT * ceil(correction);
976 998 }
977 999 }
978 1000 waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + correctionInt;
979 1001 }
980 1002
981 1003 void snapshot_resynchronization( unsigned char *timePtr )
982 1004 {
983 1005 /** This function compute a correction to apply on delta_snapshot.
984 1006 *
985 1007 *
986 1008 * @param timePtr is a pointer to the acquisition time of the snapshot being considered.
987 1009 *
988 1010 * @return void
989 1011 *
990 1012 */
991 1013
992 1014 static double correction = INIT_FLOAT;
993 1015 static resynchro_state state = MEASURE;
994 1016 static unsigned int nbSnapshots = 0;
995 1017
996 1018 int correctionInt;
997 1019
998 1020 correctionInt = 0;
999 1021
1000 1022 switch (state)
1001 1023 {
1002 1024
1003 1025 case MEASURE:
1004 1026 // ********
1005 1027 PRINTF1("MEASURE === %d\n", nbSnapshots);
1006 1028 state = CORRECTION;
1007 1029 correction = computeCorrection( timePtr );
1008 1030 PRINTF1("MEASURE === correction = %.2f\n", correction );
1009 1031 applyCorrection( correction );
1010 1032 PRINTF1("MEASURE === delta_snapshot = %d\n", waveform_picker_regs->delta_snapshot);
1011 1033 //****
1012 1034 break;
1013 1035
1014 1036 case CORRECTION:
1015 1037 //************
1016 1038 PRINTF1("CORRECTION === %d\n", nbSnapshots);
1017 1039 state = MEASURE;
1018 1040 computeCorrection( timePtr );
1019 1041 set_wfp_delta_snapshot();
1020 1042 PRINTF1("CORRECTION === delta_snapshot = %d\n", waveform_picker_regs->delta_snapshot);
1021 1043 //****
1022 1044 break;
1023 1045
1024 1046 default:
1025 1047 break;
1026 1048
1027 1049 }
1028 1050
1029 1051 nbSnapshots++;
1030 1052 }
1031 1053
1032 1054 //**************
1033 1055 // wfp registers
1034 1056 void reset_wfp_burst_enable( void )
1035 1057 {
1036 1058 /** This function resets the waveform picker burst_enable register.
1037 1059 *
1038 1060 * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
1039 1061 *
1040 1062 */
1041 1063
1042 1064 // [1000 000] burst f2, f1, f0 enable f3, f2, f1, f0
1043 1065 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable & RST_BITS_RUN_BURST_EN;
1044 1066 }
1045 1067
1046 1068 void reset_wfp_status( void )
1047 1069 {
1048 1070 /** This function resets the waveform picker status register.
1049 1071 *
1050 1072 * All status bits are set to 0 [new_err full_err full].
1051 1073 *
1052 1074 */
1053 1075
1054 1076 waveform_picker_regs->status = INT16_ALL_F;
1055 1077 }
1056 1078
1057 1079 void reset_wfp_buffer_addresses( void )
1058 1080 {
1059 1081 // F0
1060 1082 waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
1061 1083 waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
1062 1084 // F1
1063 1085 waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
1064 1086 waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
1065 1087 // F2
1066 1088 waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
1067 1089 waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
1068 1090 // F3
1069 1091 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
1070 1092 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
1071 1093 }
1072 1094
1073 1095 void reset_waveform_picker_regs( void )
1074 1096 {
1075 1097 /** This function resets the waveform picker module registers.
1076 1098 *
1077 1099 * The registers affected by this function are located at the following offset addresses:
1078 1100 * - 0x00 data_shaping
1079 1101 * - 0x04 run_burst_enable
1080 1102 * - 0x08 addr_data_f0
1081 1103 * - 0x0C addr_data_f1
1082 1104 * - 0x10 addr_data_f2
1083 1105 * - 0x14 addr_data_f3
1084 1106 * - 0x18 status
1085 1107 * - 0x1C delta_snapshot
1086 1108 * - 0x20 delta_f0
1087 1109 * - 0x24 delta_f0_2
1088 1110 * - 0x28 delta_f1 (obsolet parameter)
1089 1111 * - 0x2c delta_f2
1090 1112 * - 0x30 nb_data_by_buffer
1091 1113 * - 0x34 nb_snapshot_param
1092 1114 * - 0x38 start_date
1093 1115 * - 0x3c nb_word_in_buffer
1094 1116 *
1095 1117 */
1096 1118
1097 1119 set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
1098 1120
1099 1121 reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
1100 1122
1101 1123 reset_wfp_buffer_addresses();
1102 1124
1103 1125 reset_wfp_status(); // 0x18
1104 1126
1105 1127 set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
1106 1128
1107 1129 set_wfp_delta_f0_f0_2(); // 0x20, 0x24
1108 1130
1109 1131 //the parameter delta_f1 [0x28] is not used anymore
1110 1132
1111 1133 set_wfp_delta_f2(); // 0x2c
1112 1134
1113 1135 DEBUG_PRINTF1("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot);
1114 1136 DEBUG_PRINTF1("delta_f0 %x\n", waveform_picker_regs->delta_f0);
1115 1137 DEBUG_PRINTF1("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2);
1116 1138 DEBUG_PRINTF1("delta_f1 %x\n", waveform_picker_regs->delta_f1);
1117 1139 DEBUG_PRINTF1("delta_f2 %x\n", waveform_picker_regs->delta_f2);
1118 1140 // 2688 = 8 * 336
1119 1141 waveform_picker_regs->nb_data_by_buffer = DFLT_WFP_NB_DATA_BY_BUFFER; // 0x30 *** 2688 - 1 => nb samples -1
1120 1142 waveform_picker_regs->snapshot_param = DFLT_WFP_SNAPSHOT_PARAM; // 0x34 *** 2688 => nb samples
1121 1143 waveform_picker_regs->start_date = COARSE_TIME_MASK;
1122 1144 //
1123 1145 // coarse time and fine time registers are not initialized, they are volatile
1124 1146 //
1125 1147 waveform_picker_regs->buffer_length = DFLT_WFP_BUFFER_LENGTH; // buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
1126 1148 }
1127 1149
1128 1150 void set_wfp_data_shaping( void )
1129 1151 {
1130 1152 /** This function sets the data_shaping register of the waveform picker module.
1131 1153 *
1132 1154 * The value is read from one field of the parameter_dump_packet structure:\n
1133 1155 * bw_sp0_sp1_r0_r1
1134 1156 *
1135 1157 */
1136 1158
1137 1159 unsigned char data_shaping;
1138 1160
1139 1161 // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure the register
1140 1162 // waveform picker : [R1 R0 SP1 SP0 BW]
1141 1163
1142 1164 data_shaping = parameter_dump_packet.sy_lfr_common_parameters;
1143 1165
1144 1166 waveform_picker_regs->data_shaping =
1145 1167 ( (data_shaping & BIT_5) >> SHIFT_5_BITS ) // BW
1146 1168 + ( (data_shaping & BIT_4) >> SHIFT_3_BITS ) // SP0
1147 1169 + ( (data_shaping & BIT_3) >> 1 ) // SP1
1148 1170 + ( (data_shaping & BIT_2) << 1 ) // R0
1149 1171 + ( (data_shaping & BIT_1) << SHIFT_3_BITS ) // R1
1150 1172 + ( (data_shaping & BIT_0) << SHIFT_5_BITS ); // R2
1151 1173 }
1152 1174
1153 1175 void set_wfp_burst_enable_register( unsigned char mode )
1154 1176 {
1155 1177 /** This function sets the waveform picker burst_enable register depending on the mode.
1156 1178 *
1157 1179 * @param mode is the LFR mode to launch.
1158 1180 *
1159 1181 * The burst bits shall be before the enable bits.
1160 1182 *
1161 1183 */
1162 1184
1163 1185 // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
1164 1186 // the burst bits shall be set first, before the enable bits
1165 1187 switch(mode) {
1166 1188 case LFR_MODE_NORMAL:
1167 1189 case LFR_MODE_SBM1:
1168 1190 case LFR_MODE_SBM2:
1169 1191 waveform_picker_regs->run_burst_enable = RUN_BURST_ENABLE_SBM2; // [0110 0000] enable f2 and f1 burst
1170 1192 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | BITS_WFP_ENABLE_ALL; // [1111] enable f3 f2 f1 f0
1171 1193 break;
1172 1194 case LFR_MODE_BURST:
1173 1195 waveform_picker_regs->run_burst_enable = RUN_BURST_ENABLE_BURST; // [0100 0000] f2 burst enabled
1174 1196 waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable | BITS_WFP_ENABLE_BURST; // [1100] enable f3 and f2
1175 1197 break;
1176 1198 default:
1177 1199 waveform_picker_regs->run_burst_enable = INIT_CHAR; // [0000 0000] no burst enabled, no waveform enabled
1178 1200 break;
1179 1201 }
1180 1202 }
1181 1203
1182 1204 void set_wfp_delta_snapshot( void )
1183 1205 {
1184 1206 /** This function sets the delta_snapshot register of the waveform picker module.
1185 1207 *
1186 1208 * The value is read from two (unsigned char) of the parameter_dump_packet structure:
1187 1209 * - sy_lfr_n_swf_p[0]
1188 1210 * - sy_lfr_n_swf_p[1]
1189 1211 *
1190 1212 */
1191 1213
1192 1214 unsigned int delta_snapshot;
1193 1215 unsigned int delta_snapshot_in_T2;
1194 1216
1195 1217 delta_snapshot = (parameter_dump_packet.sy_lfr_n_swf_p[0] * CONST_256)
1196 1218 + parameter_dump_packet.sy_lfr_n_swf_p[1];
1197 1219
1198 1220 delta_snapshot_in_T2 = delta_snapshot * FREQ_F2;
1199 1221 waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
1200 1222 }
1201 1223
1202 1224 void set_wfp_delta_f0_f0_2( void )
1203 1225 {
1204 1226 unsigned int delta_snapshot;
1205 1227 unsigned int nb_samples_per_snapshot;
1206 1228 float delta_f0_in_float;
1207 1229
1208 1230 delta_snapshot = waveform_picker_regs->delta_snapshot;
1209 1231 nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_swf_l[1];
1210 1232 delta_f0_in_float = (nb_samples_per_snapshot / 2.) * ( (1. / FREQ_F2) - (1. / FREQ_F0) ) * FREQ_F2;
1211 1233
1212 1234 waveform_picker_regs->delta_f0 = delta_snapshot - floor( delta_f0_in_float );
1213 1235 waveform_picker_regs->delta_f0_2 = DFLT_WFP_DELTA_F0_2;
1214 1236 }
1215 1237
1216 1238 void set_wfp_delta_f1( void )
1217 1239 {
1218 1240 /** Sets the value of the delta_f1 parameter
1219 1241 *
1220 1242 * @param void
1221 1243 *
1222 1244 * @return void
1223 1245 *
1224 1246 * delta_f1 is not used, the snapshots are extracted from CWF_F1 waveforms.
1225 1247 *
1226 1248 */
1227 1249
1228 1250 unsigned int delta_snapshot;
1229 1251 unsigned int nb_samples_per_snapshot;
1230 1252 float delta_f1_in_float;
1231 1253
1232 1254 delta_snapshot = waveform_picker_regs->delta_snapshot;
1233 1255 nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_swf_l[1];
1234 1256 delta_f1_in_float = (nb_samples_per_snapshot / 2.) * ( (1. / FREQ_F2) - (1. / FREQ_F1) ) * FREQ_F2;
1235 1257
1236 1258 waveform_picker_regs->delta_f1 = delta_snapshot - floor( delta_f1_in_float );
1237 1259 }
1238 1260
1239 1261 void set_wfp_delta_f2( void ) // parameter not used, only delta_f0 and delta_f0_2 are used
1240 1262 {
1241 1263 /** Sets the value of the delta_f2 parameter
1242 1264 *
1243 1265 * @param void
1244 1266 *
1245 1267 * @return void
1246 1268 *
1247 1269 * delta_f2 is used only for the first snapshot generation, even when the snapshots are extracted from CWF_F2
1248 1270 * waveforms (see lpp_waveform_snapshot_controler.vhd for details).
1249 1271 *
1250 1272 */
1251 1273
1252 1274 unsigned int delta_snapshot;
1253 1275 unsigned int nb_samples_per_snapshot;
1254 1276
1255 1277 delta_snapshot = waveform_picker_regs->delta_snapshot;
1256 1278 nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256) + parameter_dump_packet.sy_lfr_n_swf_l[1];
1257 1279
1258 1280 waveform_picker_regs->delta_f2 = delta_snapshot - (nb_samples_per_snapshot / 2) - 1;
1259 1281 }
1260 1282
1261 1283 //*****************
1262 1284 // local parameters
1263 1285
1264 1286 void increment_seq_counter_source_id( unsigned char *packet_sequence_control, unsigned int sid )
1265 1287 {
1266 1288 /** This function increments the parameter "sequence_cnt" depending on the sid passed in argument.
1267 1289 *
1268 1290 * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
1269 1291 * @param sid is the source identifier of the packet being updated.
1270 1292 *
1271 1293 * REQ-LFR-SRS-5240 / SSS-CP-FS-590
1272 1294 * The sequence counters shall wrap around from 2^14 to zero.
1273 1295 * The sequence counter shall start at zero at startup.
1274 1296 *
1275 1297 * REQ-LFR-SRS-5239 / SSS-CP-FS-580
1276 1298 * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
1277 1299 *
1278 1300 */
1279 1301
1280 1302 unsigned short *sequence_cnt;
1281 1303 unsigned short segmentation_grouping_flag;
1282 1304 unsigned short new_packet_sequence_control;
1283 1305 rtems_mode initial_mode_set;
1284 1306 rtems_mode current_mode_set;
1285 1307 rtems_status_code status;
1286 1308
1287 1309 initial_mode_set = RTEMS_DEFAULT_MODES;
1288 1310 current_mode_set = RTEMS_DEFAULT_MODES;
1289 1311 sequence_cnt = NULL;
1290 1312
1291 1313 //******************************************
1292 1314 // CHANGE THE MODE OF THE CALLING RTEMS TASK
1293 1315 status = rtems_task_mode( RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set );
1294 1316
1295 1317 if ( (sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
1296 1318 || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3)
1297 1319 || (sid == SID_BURST_CWF_F2)
1298 1320 || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
1299 1321 || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
1300 1322 || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
1301 1323 || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0)
1302 1324 || (sid == SID_BURST_BP1_F1) || (sid == SID_BURST_BP2_F1) )
1303 1325 {
1304 1326 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_NORMAL_BURST;
1305 1327 }
1306 1328 else if ( (sid ==SID_SBM1_CWF_F1) || (sid ==SID_SBM2_CWF_F2)
1307 1329 || (sid == SID_SBM1_BP1_F0) || (sid == SID_SBM1_BP2_F0)
1308 1330 || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
1309 1331 || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1) )
1310 1332 {
1311 1333 sequence_cnt = (unsigned short *) &sequenceCounters_SCIENCE_SBM1_SBM2;
1312 1334 }
1313 1335 else
1314 1336 {
1315 1337 sequence_cnt = (unsigned short *) NULL;
1316 1338 PRINTF1("in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", sid)
1317 1339 }
1318 1340
1319 1341 if (sequence_cnt != NULL)
1320 1342 {
1321 1343 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
1322 1344 *sequence_cnt = (*sequence_cnt) & SEQ_CNT_MASK;
1323 1345
1324 1346 new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt) ;
1325 1347
1326 1348 packet_sequence_control[0] = (unsigned char) (new_packet_sequence_control >> SHIFT_1_BYTE);
1327 1349 packet_sequence_control[1] = (unsigned char) (new_packet_sequence_control );
1328 1350
1329 1351 // increment the sequence counter
1330 1352 if ( *sequence_cnt < SEQ_CNT_MAX)
1331 1353 {
1332 1354 *sequence_cnt = *sequence_cnt + 1;
1333 1355 }
1334 1356 else
1335 1357 {
1336 1358 *sequence_cnt = 0;
1337 1359 }
1338 1360 }
1339 1361
1340 1362 //*************************************
1341 1363 // RESTORE THE MODE OF THE CALLING TASK
1342 1364 status = rtems_task_mode( initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set );
1343 1365 }
1366
Show file before | Show file after | Hide comments
@@ -1,94 +1,117
1 /*------------------------------------------------------------------------------
2 -- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3 -- This file is a part of the LFR FSW
4 -- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5 --
6 -- This program is free software; you can redistribute it and/or modify
7 -- it under the terms of the GNU General Public License as published by
8 -- the Free Software Foundation; either version 2 of the License, or
9 -- (at your option) any later version.
10 --
11 -- This program is distributed in the hope that it will be useful,
12 -- but WITHOUT ANY WARRANTY; without even the implied warranty of
13 -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 -- GNU General Public License for more details.
15 --
16 -- You should have received a copy of the GNU General Public License
17 -- along with this program; if not, write to the Free Software
18 -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19 -------------------------------------------------------------------------------*/
20 /*-- Author : Paul Leroy
21 -- Contact : Alexis Jeandet
22 -- Mail : alexis.jeandet@lpp.polytechnique.fr
23 ----------------------------------------------------------------------------*/
24
1 25 #define NB_VALUES_PER_SM 25
2 26 #define NB_BINS_PER_SM 128
3 27
4 28 #define NB_BINS_COMPRESSED_SM_F0 11
5 29 #define ASM_F0_INDICE_START 17 // 88 bins
6 #define ASM_F0_INDICE_STOP 104 // 2 packets of 44 bins
7 30 #define NB_BINS_TO_AVERAGE_ASM_F0 8
8 31
9 32 void ASM_reorganize_and_divide( float *averaged_spec_mat, float *averaged_spec_mat_reorganized, float divider )
10 33 {
11 34 int frequencyBin;
12 35 int asmComponent;
13 36 unsigned int offsetASM;
14 37 unsigned int offsetASMReorganized;
15 38
16 39 // BUILD DATA
17 40 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
18 41 {
19 42 for( frequencyBin = 0; frequencyBin < NB_BINS_PER_SM; frequencyBin++ )
20 43 {
21 44 offsetASMReorganized =
22 45 frequencyBin * NB_VALUES_PER_SM
23 46 + asmComponent;
24 47 offsetASM =
25 48 asmComponent * NB_BINS_PER_SM
26 49 + frequencyBin;
27 50 averaged_spec_mat_reorganized[offsetASMReorganized ] =
28 51 averaged_spec_mat[ offsetASM ] / divider;
29 52 }
30 53 }
31 54 }
32 55
33 56 void ASM_compress_reorganize_and_divide(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
34 57 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage, unsigned char ASMIndexStart )
35 58 {
36 59 int frequencyBin;
37 60 int asmComponent;
38 61 int offsetASM;
39 62 int offsetCompressed;
40 63 int k;
41 64
42 65 // BUILD DATA
43 66 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
44 67 {
45 68 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
46 69 {
47 70 offsetCompressed = // NO TIME OFFSET
48 71 frequencyBin * NB_VALUES_PER_SM
49 72 + asmComponent;
50 73 offsetASM = // NO TIME OFFSET
51 74 asmComponent * NB_BINS_PER_SM
52 75 + ASMIndexStart
53 76 + frequencyBin * nbBinsToAverage;
54 77 compressed_spec_mat[ offsetCompressed ] = 0;
55 78 for ( k = 0; k < nbBinsToAverage; k++ )
56 79 {
57 80 compressed_spec_mat[offsetCompressed ] =
58 81 ( compressed_spec_mat[ offsetCompressed ]
59 82 + averaged_spec_mat[ offsetASM + k ] );
60 83 }
61 84 compressed_spec_mat[ offsetCompressed ] =
62 85 compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
63 86 }
64 87 }
65 88 }
66 89
67 90 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
68 91 {
69 92 unsigned int i;
70 93 float re;
71 94 float im;
72 95
73 96 for (i=0; i<NB_BINS_PER_SM; i++){
74 97 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 ];
75 98 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 + 1];
76 99 outputASM[ (asmComponent *NB_BINS_PER_SM) + i] = re;
77 100 outputASM[ (asmComponent+1)*NB_BINS_PER_SM + i] = im;
78 101 }
79 102
80 103 }
81 104
82 105 void ASM_patch( float *inputASM, float *outputASM )
83 106 {
84 107 extractReImVectors( inputASM, outputASM, 1); // b1b2
85 108 extractReImVectors( inputASM, outputASM, 3 ); // b1b3
86 109 extractReImVectors( inputASM, outputASM, 5 ); // b1e1
87 110 extractReImVectors( inputASM, outputASM, 7 ); // b1e2
88 111 extractReImVectors( inputASM, outputASM, 10 ); // b2b3
89 112 extractReImVectors( inputASM, outputASM, 12 ); // b2e1
90 113 extractReImVectors( inputASM, outputASM, 14 ); // b2e2
91 114 extractReImVectors( inputASM, outputASM, 17 ); // b3e1
92 115 extractReImVectors( inputASM, outputASM, 19 ); // b3e2
93 116 extractReImVectors( inputASM, outputASM, 22 ); // e1e2
94 117 }
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed, binary diff hidden
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
This diff has been collapsed as it changes many lines, (665 lines changed) Show them Hide them
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
This diff has been collapsed as it changes many lines, (728 lines changed) Show them Hide them
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
This diff has been collapsed as it changes many lines, (696 lines changed) Show them Hide them
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
This diff has been collapsed as it changes many lines, (772 lines changed) Show them Hide them
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
This diff has been collapsed as it changes many lines, (510 lines changed) Show them Hide them
Show file before | Show file after | Hide comments
1 NO CONTENT: file was removed
This diff has been collapsed as it changes many lines, (1323 lines changed) Show them Hide them
General Comments 0
You need to be logged in to leave comments. Login now