static hal_result_t s_cmd_set_matrix_rc(uint8_t *data_in, uint8_t *data_out, uint8_t *len_data_out) // CAN_CMD_SET_MATRIX_RC: //set i,j value of transform. matrix:
{
int16_t sender_status;
if( (data_in[1] >= AN_CHANNEL_NUM) || (data_in[2] >= AN_CHANNEL_NUM) )
{
return(hal_res_NOK_wrongparam);
}
parser_data.ee_data_is_saved = 0;
sender_status = sender_is_running();
sender_stop();
CFG_6SG_EEDATA.tr_matrix[data_in[1]][data_in[2]] = data_in[3]<<8 | data_in[4];
if(sender_status)
{
sender_start();
}
*len_data_out = 0;
return(hal_res_OK);
}
static hal_result_t s_cmd_set_can_data_rate(uint8_t *data_in, uint8_t *data_out, uint8_t *len_data_out) // CAN_CMD_SET_CANDATARATE: // set datarate for transmission in milliseconds
{
uint16_t sender_status;
if( (data_in[1] < 1) || (data_in[1] > 210) ) //Todo: rivedere tempo massimo
{
return(hal_res_NOK_wrongparam);
}
parser_data.ee_data_is_saved = 0;
sender_status = sender_is_running();
sender_stop();
CFG_6SG_EEDATA.can_msg_datarate = data_in[1];
#ifdef _TIMER_HW_
sender_config_datarate();
#endif
if(sender_status)
{
sender_start();
}
*len_data_out = 0;
return(hal_res_OK);
}
static hal_result_t s_cmd_set_curr_tare(uint8_t *data_in, uint8_t *data_out, uint8_t *len_data_out) // CAN_CMD_SET_CURR_TARE
{
hal_result_t res = hal_res_OK;
int16_t sender_status;
adc_state_t adc_st;
//******************************************************************************
//********************* IMPORTANTE***********************************************
//ferma la spediziobne di messaggi perchè la current tare è coinvolta nei calcoli
sender_status = sender_is_running();
sender_stop();
switch(data_in[1])
{
case 0:
{
calc_current_tare_init();
} break;
case 1:
{
/* In this case I must stop adc because new tare values depend on adc values.*/
adc_st = adc_is_running();
adc_stop();
calc_current_tare_reset();
if(adc_st_started == adc_st)
{
adc_start();
}
} break;
case 2:
{
res = calc_current_tare_set_value(data_in[2], (data_in[3]<<8 | data_in[4]));
} break;
#warning -> acemor: nei default si devono mettere i break.
default:
{
res = hal_res_NOK_wrongparam;
} break;
}
if(sender_status)
{
sender_start();
}
if(hal_res_OK != res)
{
return(hal_res_NOK_wrongparam);
}
*len_data_out = 0;
return(res);
}
static hal_result_t s_cmd_set_tx_mode(uint8_t *data_in, uint8_t *data_out, uint8_t *len_data_out) // CAN_CMD_SET_TXMODE: // set continuous or on demand tx 0x205 len 2 data 7 0/1
{
sender_stop();
adc_stop();
switch(data_in[1])
{
case 0://transmit calibrated data
{
CFG_6SG_BEHAV.tx_outMsg_mode = tx_outMsg_torqueData_on;
s_tx_mode_check();
adc_start();
sender_start();
} break;
case 1: //do acquisition, but do not transmit
{
CFG_6SG_BEHAV.tx_outMsg_mode = tx_outMsg_readOnly;
adc_start();
} break;
case 2: //debug mode
{
CFG_6SG_BEHAV.tx_outMsg_mode = tx_outMsg_off;
} break;
case 3: //TODO: transmit not calibrated data
{
CFG_6SG_BEHAV.tx_outMsg_mode = tx_outMsg_uncalibData_on;
sendAmsg = 1;
sendBmsg = 1;
adc_start();
sender_start();
} break;
case 4: //TODO: transmit calibrated and not calibrated
{
} break;
}
*len_data_out = 0;
return(hal_res_OK);
}
int allocator_start_senders(struct allocator *allocator, unsigned bitrate,
size_t psize)
{
struct le *le;
double tbps = allocator->num_allocations * bitrate;
unsigned ptime;
int err = 0;
ptime = calculate_ptime(bitrate, psize);
re_printf("starting traffic generators:"
" psize=%zu, ptime=%u (total target bitrate is %H)\n",
psize, ptime, print_bitrate, &tbps);
tmr_start(&allocator->tmr_ui, 1, tmr_ui_handler, allocator);
for (le = allocator->allocl.head; le; le = le->next) {
struct allocation *alloc = le->data;
if (alloc->sender) {
re_fprintf(stderr, "sender already started\n");
return EALREADY;
}
err = sender_alloc(&alloc->sender, alloc,
allocator->session_cookie,
alloc->ix, bitrate, ptime, psize);
if (err)
return err;
err = sender_start(alloc->sender);
if (err) {
re_fprintf(stderr, "could not start sender (%m)", err);
return err;
}
}
/* start sending timer/thread */
tmr_start(&allocator->tmr_pace, PACING_INTERVAL_MS,
tmr_pace_handler, allocator);
return 0;
}
static hal_result_t s_cmd_set_calib_tare(uint8_t *data_in, uint8_t *data_out, uint8_t *len_data_out) // CAN_CMD_SET_CALIB_TARE:
{
uint8_t i;
int16_t ch_values[AN_CHANNEL_NUM];
hal_result_t res = hal_res_OK;
int16_t sender_status;
adc_state_t adc_st;
sender_status = sender_is_running();
sender_stop();
switch(data_in[1])
{
case 0:
{
memset( CFG_6SG_EEDATA.calibration_tare, 0, (sizeof(uint16_t)*AN_CHANNEL_NUM) );
} break;
case 1:
{
adc_st =adc_is_running();
adc_stop();
adc_get_data(ch_values);
;
if(adc_st_started == adc_st)
{
adc_start();
}
for (i=0; i<AN_CHANNEL_NUM; i++)
{
CFG_6SG_EEDATA.calibration_tare[i]= -(ch_values[i]);
}
} break;
case 2:
{
if(data_in[2] >= AN_CHANNEL_NUM)
{
res= hal_res_NOK_wrongparam;
break;
}
CFG_6SG_EEDATA.calibration_tare[data_in[2]] = (data_in[3]<<8) | data_in[4];
} break;
default:
{
res= hal_res_NOK_wrongparam;
} break;
}
if(sender_status)
{
sender_start();
}
if(hal_res_OK != res)
{
parser_data.ee_data_is_saved = 0;
}
*len_data_out = 0;
return(res);
}
int send_ymodem_protocol(char *filename, int mode)
{
int result;
int packetno = 0;
int attempts;
int crc_type;
char *buffer;
FILE *fileptr;
long int curlength = 0;
long int maxlength;
int blocklength;
int not_got_sensible;
int not_sent;
int ymodem_block0 = 1;
int lastpacket = 0;
#ifdef DEBUG
char s[80];
#endif
#ifdef DEBUG
print_str_cr_to("Started protocol",0);
#endif
result = sender_start(&crc_type);
if ((result == -1) || (result == CAN_CHR))
{
massive_cancel();
return (-1);
}
#ifdef DEBUG
print_str_cr_to("Received start",0);
#endif
if (*filename)
{
if (proto_open_file(filename,&fileptr,"rb"))
{
massive_cancel();
return (-1);
}
} else fileptr = 0;
#ifdef DEBUG
print_str_cr_to("opened file",0);
#endif
buffer = g_malloc(MAX_PROTO_BUFFER_SIZE,"SNDYMDM");
if (!buffer)
{
massive_cancel();
if (fileptr) g_fclose(fileptr);
return (-1);
}
if (fileptr) maxlength = 128 + proto_file_length(fileptr);
else maxlength = 128;
while (!lastpacket)
{
if (curlength >= maxlength) lastpacket = 1;
if (!lastpacket)
{
if (ymodem_block0)
{
create_ymodem_block0(buffer,filename,maxlength-128);
} else proto_read_buffer(fileptr,buffer,1024,&blocklength);
}
not_sent = 1;
attempts = 0;
while ((not_sent) && (attempts++ < 10))
{
empty_inbuffer(tswitch);
#ifdef DEBUG
if (lastpacket) print_str_cr_to("Last packet",0);
else
{
sprintf(s,"Sending packet %03d",packetno);
print_str_cr_to(s,0);
}
#endif
if (lastpacket) send_short(EOT_CHR);
else
{
if (ymodem_block0)
{
send_packet(buffer,128,packetno,crc_type);
if (!(*filename)) lastpacket = 1;
}
else
send_packet(buffer,1024,packetno,crc_type);
}
not_got_sensible = 1;
while (not_got_sensible)
{
result = time_char();
#ifdef DEBUG
sprintf(s,"Received char %03d",result);
print_str_cr_to(s,0);
#endif
switch (result)
{
case (-1): not_got_sensible = 0;
break;
case ACK_CHR: not_sent = 0;
not_got_sensible = 0;
break;
case NAK_CHR: not_got_sensible = 0;
break;
case CAN_CHR: if (check_cancel())
{
massive_cancel();
if (fileptr) g_fclose(fileptr);
g_free(buffer);
return (-1);
}
not_got_sensible = 0;
break;
}
}
}
if (not_sent)
{
massive_cancel();
g_free(buffer);
if (fileptr) g_fclose(fileptr);
return (-1);
}
if (!lastpacket)
{
if (ymodem_block0)
{
curlength += 128;
ymodem_block0 = 0;
} else curlength += 1024;
packetno++;
}
}
if (fileptr) g_fclose(fileptr);
g_free(buffer);
return 0;
}
void send_xmodem_protocol(char *filename, int length)
{
int result;
int packetno = 1;
int attempts;
int crc_type;
char *buffer;
FILE *fileptr;
long int curlength = 0;
long int maxlength;
int blocklength;
int not_got_sensible;
int not_sent;
#ifdef DEBUG
char s[80];
#endif
#ifdef DEBUG
print_str_cr_to("Started protocol",0);
#endif
result = sender_start(&crc_type);
if ((result == -1) || (result == CAN_CHR))
{
massive_cancel();
return;
}
#ifdef DEBUG
print_str_cr_to("Received start",0);
#endif
if (proto_open_file(filename,&fileptr,"rb"))
{
massive_cancel();
return;
}
#ifdef DEBUG
print_str_cr_to("opened file",0);
#endif
buffer = g_malloc(MAX_PROTO_BUFFER_SIZE,"SNDXMDM");
if (!buffer)
{
massive_cancel();
g_fclose(fileptr);
return;
}
maxlength = proto_file_length(fileptr);
while (curlength < maxlength)
{
proto_read_buffer(fileptr,buffer,length,&blocklength);
not_sent = 1;
attempts = 0;
while ((not_sent) && (attempts++ < 10))
{
empty_inbuffer(tswitch);
#ifdef DEBUG
sprintf(s,"Sending packet %03d",packetno);
print_str_cr_to(s,0);
#endif
send_packet(buffer,length,packetno,crc_type);
not_got_sensible = 1;
while (not_got_sensible)
{
result = time_char();
#ifdef DEBUG
sprintf(s,"Received char %03d",result);
print_str_cr_to(s,0);
#endif
switch (result)
{
case (-1): not_got_sensible = 0;
break;
case ACK_CHR: not_sent = 0;
not_got_sensible = 0;
break;
case NAK_CHR: not_got_sensible = 0;
break;
case CAN_CHR: if (check_cancel())
{
massive_cancel();
g_fclose(fileptr);
g_free(buffer);
return;
}
not_got_sensible = 0;
break;
}
}
}
if (not_sent)
{
massive_cancel();
g_free(fileptr);
g_fclose(fileptr);
return;
}
curlength += length;
packetno++;
}
send_short(EOT_CHR);
delay(10);
send_short(EOT_CHR);
g_fclose(fileptr);
g_free(fileptr);
}
/*
* The main method of the Cluster Membership Monitor
*/
int
main( int argc,
char *argv[] )
{
int c;
cmm_nodeid_t node_id = CMM_INVALID_NODE_ID;
char *filename = NULL;
cmm_error_t err;
cm_trace_level_t trace_level = CM_TRACE_LEVEL_NOTICE;
fifo_t *lobby_to_sender, *stack_to_sender;
int pipe_stack[2];
struct sigaction action;
/* Read and verify the input arguments */
while ( (c=getopt(argc, argv, "n:f:d")) != EOF ) {
switch (c) {
case 'n' :
node_id = atoi( argv[optind-1] );
break;
case 'f' :
filename = strdup(optarg);
if (!filename) {
cm_trace(CM_TRACE_LEVEL_ERROR,
"strdup failed" );
exit(1);
}
break;
case 'd' :
trace_level = CM_TRACE_LEVEL_DEBUG;
break;
}
}
if ( node_id == CMM_INVALID_NODE_ID ) {
usage( argv[0],
"The node id paramater has to be specified" );
}
if ( filename == NULL ) {
usage( argv[0],
"The configuration file parameter has to be specified" );
}
daemonize();
cm_openlog("CMM", trace_level);
/* Block the SIGPIPE signals */
action.sa_handler = SIG_IGN;
if (sigaction(SIGPIPE, &action, NULL)) {
cm_trace(CM_TRACE_LEVEL_ERROR,
"sigaction failed");
cm_closelog();
return(1);
}
cm_trace(CM_TRACE_LEVEL_NOTICE,
"The local node id is %d",
node_id );
cm_trace(CM_TRACE_LEVEL_DEBUG,
"The configuration file is %s",
filename );
err = cnt_init( filename, node_id );
if (err != CMM_OK) {
cm_trace(CM_TRACE_LEVEL_ERROR,
"cnt_init failed [%d]",
err );
return(1);
}
cnt_print();
if ( pipe(pipe_stack) ) {
cm_trace(CM_TRACE_LEVEL_ERROR,
"pipe failed" );
return(1);
}
cm_trace(CM_TRACE_LEVEL_DEBUG,
"stack pipe has been created (%d %d)",
pipe_stack[0], pipe_stack[1] );
stack_to_sender = fifo_init(NULL);
if (!stack_to_sender) {
cm_trace(CM_TRACE_LEVEL_ERROR,
"fifo_init failed" );
return(1);
}
lobby_to_sender = fifo_init(stack_to_sender);
if (!lobby_to_sender) {
cm_trace(CM_TRACE_LEVEL_ERROR,
"fifo_init failed" );
return(1);
}
/* start the lobby server thread to listen to incoming messages from
* the predecessor node
*/
err = lobby_start( RING_PORT,
pipe_stack[1],
lobby_to_sender );
if (err != CMM_OK) {
cm_trace(CM_TRACE_LEVEL_ERROR,
"lobby_start failed [%d]",
err );
return(1);
}
/* start the sender server thread to establish connection with the
* successor node and send messages to it from the lobby_to_sender and
* the stack_to_sender fifos
*/
err = sender_start( lobby_to_sender,
stack_to_sender );
if (err != CMM_OK) {
cm_trace(CM_TRACE_LEVEL_ERROR,
"sender_start failed [%d]",
err );
return(1);
}
/* Start the api server */
err = api_init();
if (err != CMM_OK) {
cm_trace(CM_TRACE_LEVEL_ERROR,
"api_init failed [%d]",
err );
return(1);
}
/* err = heartbeat_start(); */
/* if (err != CMM_OK) { */
/* cm_trace(CM_TRACE_LEVEL_ERROR, */
/* "heartbeat init failed [%d]", err); */
/* return (1); */
/* } */
stack_start( pipe_stack[0], stack_to_sender );
cm_trace(CM_TRACE_LEVEL_ERROR,
"The stack exited !");
cm_closelog();
return(0);
}