void RSI_KEYBOARD(){
unsigned char key = inb(0x60);
unsigned char mask = 0b10000000;
unsigned char make = key & mask;
char c = '0';
if(make == 0) {
mask = 0b01111111;
c = char_map[key & mask];
if(!(c>=0x20 && c<=0x7E))
c = 'C';
cbWrite(&keybuffer, &c);
//MIRAR
if (!(cbIsFull(&keybuffer) && list_empty(&keyboardqueue))) {
if(!list_empty(&keyboardqueue)) {
struct list_head * to_unblock;
struct task_struct * pcb_to_unlock;
to_unblock = list_first(&keyboardqueue);
pcb_to_unlock = list_head_to_task_struct(to_unblock);
if ((cbIsFull(&keybuffer) || pcb_to_unlock->characters_left <= cbCount(&keybuffer))) {
list_del(to_unblock);
list_add_tail(to_unblock, &readyqueue);
}
}
}
}
else
c = ' ';
printc_xy(0x3f, 0x15, c);
}
void CAN_requestHandler(void)
{
int count = 0;
int16_t norm;
//float F_T_units[6];
uint8_t forceIssue[6];
uint8_t torqueIssue[6];
if(cbIsFull(&cb)) GaugeVectorConversion(/*&F_T_units[0]*/);
/* Debug section BEGIN */
/*if(DEBUG_count < 250)
DEBUG_count++;
else
errFlag = 3;*/
/* Debug section END */
for(count = 0; count < 6; count += 2)
{
norm = (int16_t)(F_T_units[count / 2] / 10000);
forceIssue[count] = (uint8_t)(norm >> 8);
forceIssue[count + 1] = (uint8_t)(norm);
norm = (int16_t)(F_T_units[3 + count / 2] / 10000);
torqueIssue[count] = (uint8_t)(norm >> 8);
torqueIssue[count + 1] = (uint8_t)(norm);
}
CanTxMsgStructure.ExtId = (CanTxMsgStructure.ExtId & 0x1FFF80FF) | \
(CanRxMsgStructure.ExtId & 0x7F00);
switch (CanRxMsgStructure.ExtId & 0xFF)
{
case 1:
for(count = 0; count < 6; count++)
CanTxMsgStructure.Data[count] = forceIssue[count];
CAN_Transmit(CAN1, &CanTxMsgStructure);
break;
case 2:
for(count = 0; count < 6; count++)
CanTxMsgStructure.Data[count] = torqueIssue[count];
CAN_Transmit(CAN1, &CanTxMsgStructure);
break;
case 3:
for(count = 0; count < 6; count++)
CanTxMsgStructure.Data[count] = forceIssue[count];
CAN_Transmit(CAN1, &CanTxMsgStructure);
for(count = 0; count < 6; count++)
CanTxMsgStructure.Data[count] = torqueIssue[count];
CAN_Transmit(CAN1, &CanTxMsgStructure);
break;
default:
for(count = 0; count < 6; count++)
CanTxMsgStructure.Data[count] = 0;
CAN_Transmit(CAN1, &CanTxMsgStructure);
}
}
int main() {
CircularBuffer cb; /* */
ElemType elem = {0};
int testBufferSize = 10; /* arbitrary size */
cbInit(&cb, testBufferSize);
/* Fill buffer with test elements 3 times */
for (elem.value = 0; elem.value < 3 * testBufferSize; ++ elem.value)
{
if(! cbIsFull(&cb)) { /* if not full */
cbWrite(&cb, &elem);
}
}
/* Remove and print all elements */
while (!cbIsEmpty(&cb)) {
cbRead(&cb, &elem);
printf("%d\n", elem.value);
}
cbFree(&cb);
return(0);
}
int main() {
CircularBuffer cb;
int *value;
value = (int *)0; // cast must be
printf("[ ] %d %0x\n", (int)value,(int)value); // cast must be
cbInit(&cb);
int i;
for(i=0;i<20;++i)
{
value = (int *)i;
printf("[T] %d %0x\n", (int)value,(int)value); // cast must be
if(! cbIsFull(&cb)) { /* if not full */
cbWrite(&cb, (int)value);
}
}
while(!cbIsEmpty(&cb))
{
cbRead(&cb, (int*)&value);
printf("[P] %d %0x\n", (int)value,(int)value); // cast must be
}
return(0);
}
int main(int argc, char *argv[]){
write_uart_error_ptr = &write_uart_error; //initialize the function pointer to write error
write_udp_error_ptr = &write_udp_error;
write_decode_error_ptr = &write_decode_error;
write_log_error_ptr = &write_log_error;
//parse arguments
if(argc == 4){
//first argument is always name of program or empty string
connection.server_ip=argv[1];
connection.port_number_lisa_to_pc=atoi(argv[2]);
connection.port_number_pc_to_lisa=atoi(argv[3]);
}else{
printf("wrong parameters: server ip - send port number - receive port number\n");
exit(EXIT_FAILURE);
}
//init log (mount sd card if necessary)
int err = init_log();
LOG_err_handler(err,write_log_error_ptr);
if(err != LOG_ERR_NONE){
exit(EXIT_FAILURE); //mounting SD card failed
}
enable_ptp();
#if LOGGING > 0
//init circular data log buffers
cbInit(cb_read_lisa, CBSIZE);
cbInit(cb_write_lisa, CBSIZE);
cbInit(cb_read_ground, CBSIZE);
cbInit(cb_write_ground, CBSIZE);
#endif
err = serial_port_setup();
UART_err_handler(err,write_uart_error_ptr);
if(err != UART_ERR_NONE){
exit(EXIT_FAILURE);
}
//thread variables
pthread_t thread_lisa_to_pc,thread_data_logging_lisa,thread_data_logging_ground;
//create a second thread which executes lisa_to_pc
if(pthread_create(&thread_lisa_to_pc, NULL, lisa_to_pc,NULL)) {
error_write(FILENAME,"error creating lisa thread");
exit(EXIT_FAILURE);
}
#if LOGGING > 0
//create a third thread which executes data_logging_lisa
if(pthread_create(&thread_data_logging_lisa, NULL, data_logging_lisa,NULL)) {
error_write(FILENAME,"error creating lisa logging thread");
exit(EXIT_FAILURE);
}
//create a fourth thread which executes data_logging_groundstation
if(pthread_create(&thread_data_logging_ground, NULL, data_logging_groundstation,NULL)) {
error_write(FILENAME,"error creating groundstation logging thread");
exit(EXIT_FAILURE);
}
#endif
/*-------------------------START OF FIRST THREAD: PC TO LISA------------------------*/
static UDP udp_server;
uint8_t input_stream[OUTPUT_BUFFER];
ElemType cb_elem = {0};
//init the data decode pointers
init_decoding();
UDP_err_handler(openUDPServerSocket(&udp_server,connection.port_number_pc_to_lisa,UDP_SOCKET_TIMEOUT),write_udp_error_ptr);
while(1){
//1. retreive UDP data form PC from ethernet port.
err=receiveUDPServerData(&udp_server,(void *)&input_stream,sizeof(input_stream)); //blocking !!!
UDP_err_handler(err,write_udp_error_ptr);
if(err==UDP_ERR_NONE){
#if LOGGING > 0
if(!cbIsFull(cb_write_ground)){
memcpy (&cb_elem.value, &input_stream, sizeof(input_stream));
cbWrite(cb_write_ground, &cb_elem);
}else{
if(reading_flag_ground==0){
switch_cb_ground_pointers();
}else{
printf("GROUND WRITE WAS NOT READY \n");
}
}
#endif
int new_length = strip_timestamp(input_stream); //lisa expects a package without a timestamp
UART_err_handler(serial_port_write(input_stream,new_length),write_uart_error_ptr);
}
}
UART_err_handler(serial_port_close(),write_uart_error_ptr);
UDP_err_handler(closeUDPServerSocket(&udp_server),write_udp_error_ptr);
/*------------------------END OF FIRST THREAD------------------------*/
//wait for the second thread to finish
if(pthread_join(thread_lisa_to_pc, NULL)) {
error_write(FILENAME,"error joining thread_lisa_to_pc");
}
#if LOGGING > 0
//wait for the third thread to finish
if(pthread_join(thread_data_logging_lisa, NULL)) {
error_write(FILENAME,"error joining thread_data_logging_lisa");
}
//wait for the fourth thread to finish
if(pthread_join(thread_data_logging_ground, NULL)) {
error_write(FILENAME,"error joining thread_data_logging_ground");
}
//free circular buffers
cbFree(cb_read_lisa);
cbFree(cb_write_lisa);
cbFree(cb_read_ground);
cbFree(cb_write_ground);
#endif
return 0;
}
/************************************
* FUNCTIONS
* **********************************/
static void *lisa_to_pc(void *arg){
/*-------------------------START OF SECOND THREAD: LISA TO PC------------------------*/
static UDP udp_client;
int message_length;
ElemType cb_elem = {0};
uint8_t input_buffer[INPUT_BUFFER_SIZE];
UDP_err_handler(openUDPClientSocket(&udp_client,connection.server_ip,connection.port_number_lisa_to_pc,UDP_SOCKET_TIMEOUT),write_udp_error_ptr);
while(1)
{
message_length = serial_input_get_lisa_data(input_buffer); //blocking !!!
if(message_length > 0){
//to test latency from lisa pull pin high when airspeed package arrives
/*if(serial_input.buffer[3]==AIRSPEED_ETS){
if (test==0){
system("echo 1 > /sys/class/gpio/gpio60/value");
test=1;
}else{
system("echo 0 > /sys/class/gpio/gpio60/value");
test=0;
}
}*/
//add timestamp
message_length=add_timestamp(input_buffer);
//send data to eth port using UDP
UDP_err_handler(sendUDPClientData(&udp_client,input_buffer,message_length),write_udp_error_ptr);
#if LOGGING > 0
//write the data to circual buffer for log thread
if(!cbIsFull(cb_write_lisa)){
memcpy (&cb_elem.value, input_buffer, message_length);
cbWrite(cb_write_lisa, &cb_elem);
}else{
if(reading_flag_lisa==0){
switch_cb_lisa_pointers();
//printf("switching lisa pointers\n");
}else{
printf("LISA WRITE WAS NOT READY \n");
exit(1); //FOR DEBUGGING
}
}
#endif
}else{
//send error message to server: not receiving data on uart port
printf("error on uart, see log...\n"); //FOR DEBUGGING
UART_err_handler(message_length,write_uart_error_ptr);
}
}
UART_err_handler(serial_port_close(),write_uart_error_ptr);
UDP_err_handler(closeUDPClientSocket(&udp_client),write_udp_error_ptr);
return NULL;
/*------------------------END OF SECOND THREAD------------------------*/
}
