fsw_processing.c
800 lines
| 27.9 KiB
| text/x-c
|
CLexer
|
r126 | /** Functions related to data processing. | |
* | |||
* @file | |||
* @author P. LEROY | |||
* | |||
* These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation. | |||
* | |||
*/ | |||
#include "fsw_processing.h" | |||
#include "fsw_processing_globals.c" | |||
|
r197 | #include "fsw_init.h" | |
|
r126 | ||
unsigned int nb_sm_f0; | |||
unsigned int nb_sm_f0_aux_f1; | |||
unsigned int nb_sm_f1; | |||
unsigned int nb_sm_f0_aux_f2; | |||
|
r259 | typedef enum restartState_t | |
{ | |||
WAIT_FOR_F2, | |||
WAIT_FOR_F1, | |||
WAIT_FOR_F0 | |||
} restartState; | |||
|
r126 | //************************ | |
// spectral matrices rings | |||
|
r173 | ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ]; | |
ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ]; | |||
ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ]; | |||
ring_node *current_ring_node_sm_f0; | |||
ring_node *current_ring_node_sm_f1; | |||
ring_node *current_ring_node_sm_f2; | |||
ring_node *ring_node_for_averaging_sm_f0; | |||
ring_node *ring_node_for_averaging_sm_f1; | |||
ring_node *ring_node_for_averaging_sm_f2; | |||
|
r126 | ||
|
r179 | // | |
ring_node * getRingNodeForAveraging( unsigned char frequencyChannel) | |||
{ | |||
ring_node *node; | |||
node = NULL; | |||
switch ( frequencyChannel ) { | |||
|
r318 | case CHANNELF0: | |
|
r179 | node = ring_node_for_averaging_sm_f0; | |
break; | |||
|
r318 | case CHANNELF1: | |
|
r179 | node = ring_node_for_averaging_sm_f1; | |
break; | |||
|
r318 | case CHANNELF2: | |
|
r179 | node = ring_node_for_averaging_sm_f2; | |
break; | |||
default: | |||
break; | |||
} | |||
return node; | |||
} | |||
|
r126 | //*********************************************************** | |
// Interrupt Service Routine for spectral matrices processing | |||
|
r265 | void spectral_matrices_isr_f0( int statusReg ) | |
|
r126 | { | |
|
r139 | unsigned char status; | |
|
r179 | rtems_status_code status_code; | |
|
r180 | ring_node *full_ring_node; | |
|
r139 | ||
|
r318 | status = (unsigned char) (statusReg & BITS_STATUS_F0); // [0011] get the status_ready_matrix_f0_x bits | |
|
r147 | ||
|
r139 | switch(status) | |
|
r135 | { | |
|
r139 | case 0: | |
break; | |||
|
r318 | case BIT_READY_0_1: | |
|
r179 | // UNEXPECTED VALUE | |
|
r318 | spectral_matrix_regs->status = BIT_READY_0_1; // [0011] | |
|
r179 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 ); | |
|
r139 | break; | |
|
r318 | case BIT_READY_0: | |
|
r180 | full_ring_node = current_ring_node_sm_f0->previous; | |
|
r181 | full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time; | |
full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time; | |||
|
r139 | current_ring_node_sm_f0 = current_ring_node_sm_f0->next; | |
spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address; | |||
|
r179 | // if there are enough ring nodes ready, wake up an AVFx task | |
nb_sm_f0 = nb_sm_f0 + 1; | |||
|
r318 | if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1) | |
|
r179 | { | |
|
r180 | ring_node_for_averaging_sm_f0 = full_ring_node; | |
|
r179 | if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) | |
{ | |||
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); | |||
} | |||
nb_sm_f0 = 0; | |||
} | |||
|
r318 | spectral_matrix_regs->status = BIT_READY_0; // [0000 0001] | |
|
r139 | break; | |
|
r318 | case BIT_READY_1: | |
|
r180 | full_ring_node = current_ring_node_sm_f0->previous; | |
|
r181 | full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time; | |
full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time; | |||
|
r139 | current_ring_node_sm_f0 = current_ring_node_sm_f0->next; | |
|
r180 | spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address; | |
|
r179 | // if there are enough ring nodes ready, wake up an AVFx task | |
nb_sm_f0 = nb_sm_f0 + 1; | |||
|
r318 | if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1) | |
|
r179 | { | |
|
r180 | ring_node_for_averaging_sm_f0 = full_ring_node; | |
|
r179 | if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) | |
{ | |||
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); | |||
} | |||
nb_sm_f0 = 0; | |||
} | |||
|
r318 | spectral_matrix_regs->status = BIT_READY_1; // [0000 0010] | |
|
r147 | break; | |
|
r319 | default: | |
break; | |||
|
r147 | } | |
} | |||
|
r265 | void spectral_matrices_isr_f1( int statusReg ) | |
|
r147 | { | |
|
r179 | rtems_status_code status_code; | |
|
r147 | unsigned char status; | |
|
r180 | ring_node *full_ring_node; | |
|
r147 | ||
|
r318 | status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS); // [1100] get the status_ready_matrix_f1_x bits | |
|
r147 | ||
switch(status) | |||
{ | |||
case 0: | |||
break; | |||
|
r318 | case BIT_READY_0_1: | |
|
r150 | // UNEXPECTED VALUE | |
|
r318 | spectral_matrix_regs->status = BITS_STATUS_F1; // [1100] | |
|
r166 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 ); | |
|
r147 | break; | |
|
r318 | case BIT_READY_0: | |
|
r180 | full_ring_node = current_ring_node_sm_f1->previous; | |
|
r181 | full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time; | |
full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time; | |||
|
r147 | current_ring_node_sm_f1 = current_ring_node_sm_f1->next; | |
spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address; | |||
|
r179 | // if there are enough ring nodes ready, wake up an AVFx task | |
nb_sm_f1 = nb_sm_f1 + 1; | |||
|
r318 | if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1) | |
|
r179 | { | |
|
r180 | ring_node_for_averaging_sm_f1 = full_ring_node; | |
|
r179 | if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) | |
{ | |||
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); | |||
} | |||
nb_sm_f1 = 0; | |||
} | |||
|
r318 | spectral_matrix_regs->status = BIT_STATUS_F1_0; // [0000 0100] | |
|
r147 | break; | |
|
r318 | case BIT_READY_1: | |
|
r180 | full_ring_node = current_ring_node_sm_f1->previous; | |
|
r181 | full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time; | |
full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time; | |||
|
r147 | current_ring_node_sm_f1 = current_ring_node_sm_f1->next; | |
spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address; | |||
|
r179 | // if there are enough ring nodes ready, wake up an AVFx task | |
nb_sm_f1 = nb_sm_f1 + 1; | |||
|
r318 | if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1) | |
|
r179 | { | |
|
r180 | ring_node_for_averaging_sm_f1 = full_ring_node; | |
|
r179 | if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) | |
{ | |||
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); | |||
} | |||
nb_sm_f1 = 0; | |||
} | |||
|
r318 | spectral_matrix_regs->status = BIT_STATUS_F1_1; // [1000 0000] | |
|
r147 | break; | |
|
r319 | default: | |
break; | |||
|
r147 | } | |
} | |||
|
r265 | void spectral_matrices_isr_f2( int statusReg ) | |
|
r147 | { | |
unsigned char status; | |||
|
r166 | rtems_status_code status_code; | |
|
r147 | ||
|
r318 | status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS); // [0011 0000] get the status_ready_matrix_f2_x bits | |
|
r147 | ||
switch(status) | |||
{ | |||
case 0: | |||
|
r149 | break; | |
|
r318 | case BIT_READY_0_1: | |
|
r150 | // UNEXPECTED VALUE | |
|
r318 | spectral_matrix_regs->status = BITS_STATUS_F2; // [0011 0000] | |
|
r166 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 ); | |
|
r147 | break; | |
|
r318 | case BIT_READY_0: | |
|
r179 | ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous; | |
current_ring_node_sm_f2 = current_ring_node_sm_f2->next; | |||
|
r150 | ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time; | |
ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time; | |||
|
r147 | spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address; | |
|
r318 | spectral_matrix_regs->status = BIT_STATUS_F2_0; // [0001 0000] | |
|
r149 | if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) | |
{ | |||
|
r166 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); | |
|
r149 | } | |
|
r147 | break; | |
|
r318 | case BIT_READY_1: | |
|
r179 | ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous; | |
current_ring_node_sm_f2 = current_ring_node_sm_f2->next; | |||
|
r150 | ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time; | |
ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time; | |||
|
r147 | spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address; | |
|
r318 | spectral_matrix_regs->status = BIT_STATUS_F2_1; // [0010 0000] | |
|
r149 | if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) | |
{ | |||
|
r166 | status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 ); | |
|
r149 | } | |
|
r139 | break; | |
|
r319 | default: | |
break; | |||
|
r135 | } | |
|
r147 | } | |
|
r265 | void spectral_matrix_isr_error_handler( int statusReg ) | |
|
r147 | { | |
|
r265 | // STATUS REGISTER | |
// input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0) | |||
// 10 9 8 | |||
|
r273 | // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0 | |
|
r265 | // 7 6 5 4 3 2 1 0 | |
|
r273 | // [bad_component_err] not defined in the last version of the VHDL code | |
|
r265 | ||
|
r179 | rtems_status_code status_code; | |
|
r166 | ||
|
r265 | //*************************************************** | |
// the ASM status register is copied in the HK packet | |||
|
r318 | housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS); // [0111 1000 0000] | |
|
r265 | ||
|
r318 | if (statusReg & BITS_SM_ERR) // [0111 1100 0000] | |
|
r179 | { | |
status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 ); | |||
} | |||
|
r174 | ||
|
r318 | spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR; | |
|
r265 | ||
|
r147 | } | |
|
r126 | ||
|
r147 | rtems_isr spectral_matrices_isr( rtems_vector_number vector ) | |
{ | |||
// STATUS REGISTER | |||
// input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0) | |||
// 10 9 8 | |||
// buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0 | |||
// 7 6 5 4 3 2 1 0 | |||
|
r135 | ||
|
r265 | int statusReg; | |
|
r147 | ||
|
r259 | static restartState state = WAIT_FOR_F2; | |
|
r182 | statusReg = spectral_matrix_regs->status; | |
|
r259 | if (thisIsAnASMRestart == 0) | |
{ // this is not a restart sequence, process incoming matrices normally | |||
spectral_matrices_isr_f0( statusReg ); | |||
spectral_matrices_isr_f1( statusReg ); | |||
|
r135 | ||
|
r259 | spectral_matrices_isr_f2( statusReg ); | |
} | |||
else | |||
{ // a restart sequence has to be launched | |||
switch (state) { | |||
case WAIT_FOR_F2: | |||
|
r318 | if ((statusReg & BITS_STATUS_F2) != INIT_CHAR) // [0011 0000] check the status_ready_matrix_f2_x bits | |
|
r259 | { | |
state = WAIT_FOR_F1; | |||
} | |||
break; | |||
case WAIT_FOR_F1: | |||
|
r318 | if ((statusReg & BITS_STATUS_F1) != INIT_CHAR) // [0000 1100] check the status_ready_matrix_f1_x bits | |
|
r259 | { | |
state = WAIT_FOR_F0; | |||
} | |||
break; | |||
case WAIT_FOR_F0: | |||
|
r318 | if ((statusReg & BITS_STATUS_F0) != INIT_CHAR) // [0000 0011] check the status_ready_matrix_f0_x bits | |
|
r259 | { | |
state = WAIT_FOR_F2; | |||
thisIsAnASMRestart = 0; | |||
} | |||
break; | |||
default: | |||
break; | |||
} | |||
reset_sm_status(); | |||
} | |||
|
r182 | ||
spectral_matrix_isr_error_handler( statusReg ); | |||
|
r259 | ||
|
r126 | } | |
//****************** | |||
// Spectral Matrices | |||
void reset_nb_sm( void ) | |||
{ | |||
nb_sm_f0 = 0; | |||
nb_sm_f0_aux_f1 = 0; | |||
nb_sm_f0_aux_f2 = 0; | |||
nb_sm_f1 = 0; | |||
} | |||
void SM_init_rings( void ) | |||
{ | |||
|
r173 | init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM ); | |
init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM ); | |||
init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM ); | |||
DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0) | |||
DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1) | |||
DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2) | |||
DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0) | |||
DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1) | |||
DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2) | |||
|
r126 | } | |
void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes ) | |||
{ | |||
unsigned char i; | |||
ring[ nbNodes - 1 ].next | |||
= (ring_node_asm*) &ring[ 0 ]; | |||
for(i=0; i<nbNodes-1; i++) | |||
{ | |||
ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ]; | |||
} | |||
} | |||
void SM_reset_current_ring_nodes( void ) | |||
{ | |||
|
r139 | current_ring_node_sm_f0 = sm_ring_f0[0].next; | |
current_ring_node_sm_f1 = sm_ring_f1[0].next; | |||
current_ring_node_sm_f2 = sm_ring_f2[0].next; | |||
|
r126 | ||
|
r180 | ring_node_for_averaging_sm_f0 = NULL; | |
ring_node_for_averaging_sm_f1 = NULL; | |||
ring_node_for_averaging_sm_f2 = NULL; | |||
|
r126 | } | |
//***************** | |||
// Basic Parameters | |||
|
r181 | void BP_init_header( bp_packet *packet, | |
|
r126 | unsigned int apid, unsigned char sid, | |
unsigned int packetLength, unsigned char blkNr ) | |||
{ | |||
|
r181 | packet->targetLogicalAddress = CCSDS_DESTINATION_ID; | |
packet->protocolIdentifier = CCSDS_PROTOCOLE_ID; | |||
|
r318 | packet->reserved = INIT_CHAR; | |
|
r181 | packet->userApplication = CCSDS_USER_APP; | |
|
r318 | packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE); | |
|
r181 | packet->packetID[1] = (unsigned char) (apid); | |
packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; | |||
|
r318 | packet->packetSequenceControl[1] = INIT_CHAR; | |
packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE); | |||
|
r181 | packet->packetLength[1] = (unsigned char) (packetLength); | |
|
r126 | // DATA FIELD HEADER | |
|
r318 | packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2; | |
|
r181 | packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type | |
|
r192 | packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype | |
|
r181 | packet->destinationID = TM_DESTINATION_ID_GROUND; | |
|
r318 | packet->time[BYTE_0] = INIT_CHAR; | |
packet->time[BYTE_1] = INIT_CHAR; | |||
packet->time[BYTE_2] = INIT_CHAR; | |||
packet->time[BYTE_3] = INIT_CHAR; | |||
packet->time[BYTE_4] = INIT_CHAR; | |||
packet->time[BYTE_5] = INIT_CHAR; | |||
|
r126 | // AUXILIARY DATA HEADER | |
|
r181 | packet->sid = sid; | |
|
r318 | packet->pa_bia_status_info = INIT_CHAR; | |
packet->sy_lfr_common_parameters_spare = INIT_CHAR; | |||
packet->sy_lfr_common_parameters = INIT_CHAR; | |||
packet->acquisitionTime[BYTE_0] = INIT_CHAR; | |||
packet->acquisitionTime[BYTE_1] = INIT_CHAR; | |||
packet->acquisitionTime[BYTE_2] = INIT_CHAR; | |||
packet->acquisitionTime[BYTE_3] = INIT_CHAR; | |||
packet->acquisitionTime[BYTE_4] = INIT_CHAR; | |||
packet->acquisitionTime[BYTE_5] = INIT_CHAR; | |||
packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR; // BLK_NR MSB | |||
|
r181 | packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB | |
|
r126 | } | |
|
r181 | void BP_init_header_with_spare( bp_packet_with_spare *packet, | |
|
r126 | unsigned int apid, unsigned char sid, | |
unsigned int packetLength , unsigned char blkNr) | |||
{ | |||
|
r181 | packet->targetLogicalAddress = CCSDS_DESTINATION_ID; | |
packet->protocolIdentifier = CCSDS_PROTOCOLE_ID; | |||
|
r318 | packet->reserved = INIT_CHAR; | |
|
r181 | packet->userApplication = CCSDS_USER_APP; | |
|
r318 | packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE); | |
|
r181 | packet->packetID[1] = (unsigned char) (apid); | |
packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; | |||
|