fsw_spool.c
276 lines
| 10.4 KiB
| text/x-c
|
CLexer
/ src / fsw_spool.c
paul
|
r165 | /** Functions and tasks related to waveform packet generation. | ||
* | ||||
* @file | ||||
* @author P. LEROY | ||||
* | ||||
* A group of functions to handle waveforms, in snapshot or continuous format.\n | ||||
* | ||||
*/ | ||||
#include "fsw_spool.h" | ||||
//********************* | ||||
// Interrupt SubRoutine | ||||
void spool_waveforms( void ) | ||||
{ | ||||
/** This is the interrupt sub routine called by the waveform picker core. | ||||
* | ||||
* This ISR launch different actions depending mainly on two pieces of information: | ||||
* 1. the values read in the registers of the waveform picker. | ||||
* 2. the current LFR mode. | ||||
* | ||||
*/ | ||||
rtems_status_code status; | ||||
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 | ||||
|| (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) | ||||
{ // in modes other than STANDBY and BURST, send the CWF_F3 data | ||||
if ((waveform_picker_regs->status & 0x08) == 0x08){ // [1000] f3 is full | ||||
// (1) change the receiving buffer for the waveform picker | ||||
ring_node_to_send_cwf_f3 = current_ring_node_f3; | ||||
current_ring_node_f3 = current_ring_node_f3->next; | ||||
waveform_picker_regs->addr_data_f3 = current_ring_node_f3->buffer_address; | ||||
// (2) send an event for the waveforms transmission | ||||
if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) { | ||||
rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_2 ); | ||||
} | ||||
rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_2); | ||||
waveform_picker_regs->status = waveform_picker_regs->status & 0xfffff777; // reset f3 bits to 0, [1111 0111 0111 0111] | ||||
} | ||||
} | ||||
switch(lfrCurrentMode) | ||||
{ | ||||
//******** | ||||
// STANDBY | ||||
case(LFR_MODE_STANDBY): | ||||
break; | ||||
//****** | ||||
// NORMAL | ||||
case(LFR_MODE_NORMAL): | ||||
if ( (waveform_picker_regs->status & 0xff8) != 0x00) // [1000] check the error bits | ||||
{ | ||||
rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_2 ); | ||||
} | ||||
if ( (waveform_picker_regs->status & 0x07) == 0x07) // [0111] check the f2, f1, f0 full bits | ||||
{ | ||||
// change F0 ring node | ||||
ring_node_to_send_swf_f0 = current_ring_node_f0; | ||||
current_ring_node_f0 = current_ring_node_f0->next; | ||||
waveform_picker_regs->addr_data_f0 = current_ring_node_f0->buffer_address; | ||||
// change F1 ring node | ||||
ring_node_to_send_swf_f1 = current_ring_node_f1; | ||||
current_ring_node_f1 = current_ring_node_f1->next; | ||||
waveform_picker_regs->addr_data_f1 = current_ring_node_f1->buffer_address; | ||||
// change F2 ring node | ||||
ring_node_to_send_swf_f2 = current_ring_node_f2; | ||||
current_ring_node_f2 = current_ring_node_f2->next; | ||||
waveform_picker_regs->addr_data_f2 = current_ring_node_f2->buffer_address; | ||||
// | ||||
if (rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL ) != RTEMS_SUCCESSFUL) | ||||
{ | ||||
rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_2 ); | ||||
} | ||||
waveform_picker_regs->status = waveform_picker_regs->status & 0xfffff888; // [1000 1000 1000] | ||||
} | ||||
break; | ||||
//****** | ||||
// BURST | ||||
case(LFR_MODE_BURST): | ||||
if ( (waveform_picker_regs->status & 0x04) == 0x04 ){ // [0100] check the f2 full bit | ||||
// (1) change the receiving buffer for the waveform picker | ||||
ring_node_to_send_cwf_f2 = current_ring_node_f2; | ||||
current_ring_node_f2 = current_ring_node_f2->next; | ||||
waveform_picker_regs->addr_data_f2 = current_ring_node_f2->buffer_address; | ||||
// (2) send an event for the waveforms transmission | ||||
if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) { | ||||
rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_2 ); | ||||
} | ||||
waveform_picker_regs->status = waveform_picker_regs->status & 0xfffffbbb; // [1111 1011 1011 1011] f2 bit = 0 | ||||
} | ||||
break; | ||||
//***** | ||||
// SBM1 | ||||
case(LFR_MODE_SBM1): | ||||
if ( (waveform_picker_regs->status & 0x02) == 0x02 ) { // [0010] check the f1 full bit | ||||
// (1) change the receiving buffer for the waveform picker | ||||
ring_node_to_send_cwf_f1 = current_ring_node_f1; | ||||
current_ring_node_f1 = current_ring_node_f1->next; | ||||
waveform_picker_regs->addr_data_f1 = current_ring_node_f1->buffer_address; | ||||
// (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed) | ||||
status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_SBM1 ); | ||||
waveform_picker_regs->status = waveform_picker_regs->status & 0xfffffddd; // [1111 1101 1101 1101] f1 bits = 0 | ||||
} | ||||
if ( (waveform_picker_regs->status & 0x01) == 0x01 ) { // [0001] check the f0 full bit | ||||
swf_f0_ready = true; | ||||
waveform_picker_regs->status = waveform_picker_regs->status & 0xfffffeee; // [1111 1110 1110 1110] f0 bits = 0 | ||||
} | ||||
if ( (waveform_picker_regs->status & 0x04) == 0x04 ) { // [0100] check the f2 full bit | ||||
swf_f2_ready = true; | ||||
waveform_picker_regs->status = waveform_picker_regs->status & 0xfffffbbb; // [1111 1011 1011 1011] f2 bits = 0 | ||||
} | ||||
break; | ||||
//***** | ||||
// SBM2 | ||||
case(LFR_MODE_SBM2): | ||||
if ( (waveform_picker_regs->status & 0x04) == 0x04 ){ // [0100] check the f2 full bit | ||||
// (1) change the receiving buffer for the waveform picker | ||||
ring_node_to_send_cwf_f2 = current_ring_node_f2; | ||||
current_ring_node_f2 = current_ring_node_f2->next; | ||||
waveform_picker_regs->addr_data_f2 = current_ring_node_f2->buffer_address; | ||||
// (2) send an event for the waveforms transmission | ||||
status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_SBM2 ); | ||||
waveform_picker_regs->status = waveform_picker_regs->status & 0xfffffbbb; // [1111 1011 1011 1011] f2 bit = 0 | ||||
} | ||||
if ( (waveform_picker_regs->status & 0x01) == 0x01 ) { // [0001] check the f0 full bit | ||||
swf_f0_ready = true; | ||||
waveform_picker_regs->status = waveform_picker_regs->status & 0xfffffeee; // [1111 1110 1110 1110] f0 bits = 0 | ||||
} | ||||
if ( (waveform_picker_regs->status & 0x02) == 0x02 ) { // [0010] check the f1 full bit | ||||
swf_f1_ready = true; | ||||
waveform_picker_regs->status = waveform_picker_regs->status & 0xfffffddd; // [1111 1101 1101 1101] f1, f0 bits = 0 | ||||
} | ||||
break; | ||||
//******** | ||||
// DEFAULT | ||||
default: | ||||
break; | ||||
} | ||||
} | ||||
void spool_waveforms_alt( void ) | ||||
{ | ||||
// WFRM | ||||
if (wake_up_task_wfrm == true) | ||||
{ | ||||
rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL ); | ||||
wake_up_task_wfrm = false; | ||||
} | ||||
// CWF_F1 | ||||
if (wake_up_task_cwf_f1 == true) | ||||
{ | ||||
rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_SBM1 ); | ||||
wake_up_task_cwf_f1 = false; | ||||
} | ||||
// CWF_F2 BURST | ||||
if (wake_up_task_cwf_f2_burst == true) | ||||
{ | ||||
rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ); | ||||
wake_up_task_cwf_f2_burst = false; | ||||
} | ||||
// CWF_F2 SBM2 | ||||
if (wake_up_task_cwf_f2_sbm2 == true) | ||||
{ | ||||
rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_SBM2 ); | ||||
wake_up_task_cwf_f2_sbm2 = false; | ||||
} | ||||
// CWF_F3 | ||||
if (wake_up_task_cwf_f3 == true) | ||||
{ | ||||
rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ); | ||||
wake_up_task_cwf_f3 = false; | ||||
} | ||||
} | ||||
void spool_spectral_matrices( void ) | ||||
{ | ||||
// 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 | ||||
unsigned char status_f0; | ||||
unsigned char status_f1; | ||||
unsigned char status_f2; | ||||
static unsigned int counter = 0; | ||||
rtems_interrupt_level level; | ||||
rtems_interrupt_disable( level ); | ||||
status_f0 = spectral_matrix_regs->status & 0x03; // [0011] get the status_ready_matrix_f0_x bits | ||||
status_f1 = (spectral_matrix_regs->status & 0x0c) >> 2; // [0011] get the status_ready_matrix_f0_x bits | ||||
status_f2 = (spectral_matrix_regs->status & 0x30) >> 4; // [0011] get the status_ready_matrix_f0_x bits | ||||
// if ( status_f0 == 0x03) | ||||
// { | ||||
// printf("%d \n", counter); | ||||
// } | ||||
if ( status_f1 == 0x03) | ||||
{ | ||||
printf("f1 %d \n", counter); | ||||
} | ||||
if ( status_f2 == 0x03) | ||||
{ | ||||
printf("f2 %d \n", counter); | ||||
} | ||||
spectral_matrices_isr_f0(); | ||||
spectral_matrices_isr_f1(); | ||||
spectral_matrices_isr_f2(); | ||||
spectral_matrix_isr_error_handler(); | ||||
rtems_interrupt_enable( level ); | ||||
counter = counter + 1; | ||||
} | ||||
//************ | ||||
// RTEMS TASKS | ||||
rtems_task spoo_task(rtems_task_argument argument) | ||||
{ | ||||
rtems_status_code status; | ||||
BOOT_PRINTF("in SPOOL ***\n") | ||||
if (rtems_rate_monotonic_ident( name_spool_rate_monotonic, &spool_period_id) != RTEMS_SUCCESSFUL) { | ||||
status = rtems_rate_monotonic_create( name_spool_rate_monotonic, &spool_period_id ); | ||||
if( status != RTEMS_SUCCESSFUL ) { | ||||
PRINTF1( "in SPOO *** rtems_rate_monotonic_create failed with status %d\n", status ) | ||||
} | ||||
} | ||||
status = rtems_rate_monotonic_cancel( spool_period_id ); | ||||
if( status != RTEMS_SUCCESSFUL ) | ||||
{ | ||||
PRINTF1( "ERR *** in SPOOL *** rtems_rate_monotonic_cancel(spool_period_id) ***code: %d\n", status ) | ||||
} | ||||
else | ||||
{ | ||||
DEBUG_PRINTF("OK *** in SPOOL *** rtems_rate_monotonic_cancel(spool_period_id)\n") | ||||
} | ||||
while(1){ // launch the rate monotonic task | ||||
// status = rtems_rate_monotonic_period( spool_period_id, SPOOL_TIMEOUT_in_ticks ); | ||||
rtems_task_wake_after( SPOOL_TIMEOUT_in_ticks / 2 ); | ||||
spool_waveforms_alt(); | ||||
spool_spectral_matrices(); | ||||
// if ( status != RTEMS_SUCCESSFUL ) | ||||
// { | ||||
// PRINTF1( "in SPOOL *** ERR period: %d\n", status); | ||||
// } | ||||
// else | ||||
// { | ||||
// spool_waveforms(); | ||||
// spool_spectral_matrices(); | ||||
// } | ||||
} | ||||
PRINTF("in SPOOL *** deleting task\n") | ||||
status = rtems_task_delete( RTEMS_SELF ); // should not return | ||||
printf( "rtems_task_delete returned with status of %d.\n", status ); | ||||
return; | ||||
} | ||||