#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <string.h>
#include <ncurses.h>
#include <time.h>
#include <sys/time.h>
#include <pthread.h>
#include <math.h>
#include "calibration.h"
#include "communication/communication.h"
#include "datalogger.h"
#include "ui.h"
#include "main2.h"

Include dependency graph for main2.c:

Macros
#define	TASK1_PERIOD_US 20000

#define	TASK2_PERIOD_US 100000

Functions
void	f_timer_task_1 (union sigval sigval)

void	f_timer_task_2 (union sigval sigval)

int	main (void)

int	periodic_task_1 (void)

int	periodic_task_2 (void)

void	f_timer_task_1 (union sigval sigval)

void	f_timer_task_2 (union sigval sigval)

void	timer_start_task_1 (void)

void	timer_start_task_2 (void)

void	timer_stop_task_1 (void)

void	timer_stop_task_2 (void)

void	timer_function_task_1 (void)

void	timer_function_task_2 (void)

int	reset_timer (void)

int	get_time (double time_control_task_s, double Ts_control_task_s, double mean_time_control_task_s, double t0_control_task_s)
	Review of this function: More...

void	exit_program (void)
	Internal function to end program. More...

Variables
unsigned char	quittask =0

int	acquire =0

int	total = 0

int	failure = 0

timer_t	timer_task_1

volatile double	t_task_1_global = 0.0

volatile double	T_task_1_exec_global = 0.0

volatile double	T_task_1_mean_global = 0.0

volatile double	T_task_1_min_global = 0.0

volatile double	T_task_1_max_global = 0.0

volatile int	task_1_period_us = TASK1_PERIOD_US

volatile int	flag_task_1_firstexecution = 1

volatile double	t0 = 0.0

timer_t	timer_task_2

volatile double	t_task_2_global = 0.0

volatile double	T_task_2_exec_global = 0.0

volatile double	T_task_2_mean_global = 0.0

volatile double	T_task_2_min_global = 0.0

volatile double	T_task_2_max_global = 0.0

volatile int	task_2_period_us = TASK2_PERIOD_US

volatile int	flag_task_2_firstexecution = 1

unsigned int	telemetry_mode = UI_NCURSES_MODE

int	buff_i =0

short int	buff [3][3][3]

IMU_PARAM_STRUCT	imu_param

SPI_PARAM_STRUCT	spi_param

IMU_DATA_STRUCT	imu_data

EFF_DATA_STRUCT	eff_data

MRA_DATA_STRUCT	mra_data

ENC_DATA_STRUCT	enc_data

Macro Definition Documentation

#define TASK1_PERIOD_US 20000

Definition at line 31 of file main2.c.

Referenced by main().

#define TASK2_PERIOD_US 100000

Definition at line 32 of file main2.c.

Referenced by main().

Function Documentation

void exit_program ( void )

Internal function to end program.

Definition at line 554 of file main2.c.

References quittask.

Referenced by ui_update().

 {
         quittask = 1;
         return;
 }

Here is the caller graph for this function:

void f_timer_task_1 ( union sigval sigval )

Definition at line 330 of file main2.c.

References timer_function_task_1().

Referenced by timer_start_task_1().

 {
     sigval.sival_int = 0; // Remove warning
     timer_function_task_1();
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void f_timer_task_1 ( union sigval sigval )

Definition at line 330 of file main2.c.

References timer_function_task_1().

Referenced by timer_start_task_1().

 {
     sigval.sival_int = 0; // Remove warning
     timer_function_task_1();
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void f_timer_task_2 ( union sigval sigval )

Definition at line 336 of file main2.c.

References timer_function_task_2().

Referenced by timer_start_task_2().

 {
     sigval.sival_int = 0; // Remove warning
     timer_function_task_2();
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void f_timer_task_2 ( union sigval sigval )

Definition at line 336 of file main2.c.

References timer_function_task_2().

Referenced by timer_start_task_2().

 {
     sigval.sival_int = 0; // Remove warning
     timer_function_task_2();
 }

Here is the call graph for this function:

Here is the caller graph for this function:

int get_time	(	double *	time_control_task_s,
		double *	Ts_control_task_s,
		double *	mean_time_control_task_s,
		double *	t0_control_task_s
	)

Review of this function:

Definition at line 545 of file main2.c.

References TASK_S::period_us, SUCCESS, t0, TASK_S::t_global, TASK_S::T_mean_global, t_task_1_global, T_task_1_mean_global, and task_1_period_us.

 {
     *time_control_task_s = t_task_1_global;
     *Ts_control_task_s = task_1_period_us/1e6;
     *mean_time_control_task_s = T_task_1_mean_global;
     *t0_control_task_s = t0;
     return SUCCESS;
 }

int main ( void )

Definition at line 88 of file main2.c.

 {
     int status = 0;
     int n = 0;
     int return_value = SUCCESS;    
  
 // Parameters for IMU and SPI
 
   imu_param.acc.full_res=1;
   imu_param.acc.rate=100;
   imu_param.acc.range=16;
   imu_param.gyr.rate=100;
   imu_param.gyr.lpf_bw=188;
   imu_param.gyr.clk_source='Z';
   imu_param.gyr.act="XYZ";
   imu_param.mag.rate=75;
   imu_param.mag.range=0;
   imu_param.mag.samples_avg=8;
   imu_param.mag.meas_mode=0;
   imu_param.mag.op_mode=0;
   
   spi_param.mode=0;
   spi_param.speed=375000;
   spi_param.cs=0;
   
    // printf("\ngpio186 inicialmente = %d\n",gpio_read(GPIO_S0));
   //printf("gpio10 inicialmente = %d\n",gpio_read(GPIO_SHDN));
   
 // Initialization
 
   if( devices_init(&imu_param, &spi_param, &mra_data)==FAILURE )
   {
     perror("Unsuccesful devices initialization");
     return -1;
   }
   if( datalogger_init()!=SUCCESS )
   {
     perror("Unsuccesful datalogger initialization");
     return -1;
   }
   if( ui_init() != SUCCESS)
   {
     perror("Unsuccesful user interface initialization");
     return -1;
   }
   // enc_zero_set(enc_data)!=SUCCESS ? perror("Unsuccesfull encoder set zero "); return -1;
 
   //gpio_write(GPIO_S0,1);
   //mra_data.Out_0=2275;
   //actuate(spi_param.spi_dev,&mra_data);
  
   //return_value=FAILURE;
   
     // Main Loop
     if(return_value != FAILURE)
     {
         //datalogger_start();
         timer_start_task_1();
         usleep(0.5*task_1_period_us);
         timer_start_task_2();
         usleep(5*task_1_period_us);
         
         while(quittask == 0)
         {
             #if ANU_COMPILE_FOR_OVERO
             #else
             //if(datalogger_status() == DATALOGGER_RUNNING) datalogger_update_IPC();
             #endif
 
             
             if(++n>100)
             {
               //printf("n = %d\n",n);
                 if(datalogger_status() == DATALOGGER_RUNNING) datalogger_write_file();
                 n = 0;
             }
             usleep(5*task_1_period_us);
         }
         datalogger_stop();
         timer_stop_task_1();
         usleep(TASK1_PERIOD_US);
         timer_stop_task_2();
         usleep(TASK2_PERIOD_US);
     }
 
     status = datalogger_close();
     if(status != DATALOGGER_SUCCESS)
     {
         return_value = FAILURE;
     }
 
     status = ui_close();
     if(status != SUCCESS)
     {
         return_value = FAILURE;
     }
     
     devices_close(&imu_param, &spi_param, &mra_data);
 
     return return_value;
 }

Here is the call graph for this function:

int periodic_task_1 ( void )

Definition at line 190 of file main2.c.

References actuate(), calibrate_all(), DATALOGGER_NOT_RUNNING, DATALOGGER_RUNNING, datalogger_set_Ts(), datalogger_status(), datalogger_update(), failure, IMU_PARAM_STRUCT::i2c_dev, read_all_data(), reset_timer(), SPI_PARAM_STRUCT::spi_dev, SUCCESS, t0, T_task_1_exec_global, t_task_1_global, T_task_2_exec_global, task_1_period_us, total, and MRA_DATA_STRUCT::v_ctl.

Referenced by timer_function_task_1().

 {
     //static int previous_calibration_status = CALIBRATION_NOT_RUNNING;
     //int current_calibration_status; // Used to detect lowering edge
     static int previous_datalogger_status = DATALOGGER_NOT_RUNNING;
     int current_datalogger_status; // Used to detect rising edge
     char user =0;
    /* int buff_i=0; //i of buffer
     int i, j, k, equal=0;
     short int buff[3][3][3]; //{acc,gyr,mag}{x,y,z}{i=1,2,3}
 */
 
     // Main communication/control thread
 
     //Acquire
     total++;
     if( read_all_data(imu_param.i2c_dev, spi_param.spi_dev, &imu_data, &eff_data, &mra_data, &enc_data) != SUCCESS)
       failure++;
         
     // Calibrate and Estimate
     calibrate_all(&imu_data);
     //calibration_calibrate_all(&pwm_read_data, &scp1000_data, &battery_data, &gps_data, &imu_data, &pitot_data, &sonar_data, &calibration_local_coordinate_system_data, &calibration_altimeter_data, &calibration_local_fields_data, &gps_measure, &imu_measure, &magnetometer_measure);
     //estimation_update_state_estimate(&estimation_data, &gps_measure, &imu_measure, &magnetometer_measure, &sonar_measure, &calibration_local_fields_data, task_1_period_us/(double)1e6);
 
     // Control
     mra_data.v_ctl=1275+(uint8_t)(800*cosf(t_task_1_global*1000));
     //control_main_task(t_task_1_global, &pwm_write_data, &pwm_read_data, &control_data, &estimation_data);
 
     // Actuate
     actuate(spi_param.spi_dev, &mra_data);
     //protocol_send_actuation_data(&pwm_write_data);
 
     // Log
     /*user=getch();
     switch(user){
       case 'e': 
         exit_program();
         break;
       case 'a':
         acquire=1;
         break;
       case 's':
         acquire=0;
         break;
       default:
         break;
     }*/
     current_datalogger_status = datalogger_status();
     if( (current_datalogger_status == DATALOGGER_RUNNING))
     {
         if(previous_datalogger_status == DATALOGGER_NOT_RUNNING) // Rising edge
         {
             datalogger_set_Ts(task_1_period_us/1e6);
             reset_timer();
         }
         datalogger_update(t_task_1_global, T_task_1_exec_global, T_task_2_exec_global, t0, &imu_data, &eff_data, &mra_data /*&imu_measure, &magnetometer_measure, &estimation_data, &control_data*/);
   //printf("\n    %d\t|",i);
  /* printw("%d\t|",eff_data.F.x);
     printw("\t%d\t%d\t%d\t|",imu_data.acc.x,imu_data.acc.y,imu_data.acc.z);
     printw("\t%d\t%d\t%d\t|",imu_data.mag.x,imu_data.mag.y,imu_data.mag.z);
     printw("\t%d\t%d\t%d\t%d",imu_data.gyr.x,imu_data.gyr.y,imu_data.gyr.z, imu_data.temp);
     printw("\t%d\t%d",eff_data.new_data,imu_data.new_data);
     printw("\t%d\t%d",mra_data.Out_0, mra_data.Read_0);
     printw("\n");
     refresh();*/
     }
     previous_datalogger_status = current_datalogger_status;
 
 /*    current_calibration_status = calibration_get_status();
     if(current_calibration_status == CALIBRATION_RUNNING)
     {
         calibration_imu_add_sample_bias_estimate(&imu_data);
         calibration_altimeter_add_sample_initial_pressure(&scp1000_data, &calibration_altimeter_data);
     }
     else // Calibration not running
     {
         if(previous_calibration_status == CALIBRATION_RUNNING) // Lowering edge
         {
             calibration_imu_finish_bias_estimate(&imu_data);
             calibration_altimeter_finish_initial_pressure_estimate(&calibration_altimeter_data);
         }
     }
     previous_calibration_status = current_calibration_status;
 */
     return SUCCESS;
 }

Here is the call graph for this function:

Here is the caller graph for this function:

int periodic_task_2 ( void )

Definition at line 277 of file main2.c.

References IMU_DATA_STRUCT::acc, buff, buff_i, devices_init(), failure, IMU_DATA_STRUCT::gyr, IMU_PARAM_STRUCT::i2c_dev, IMU_DATA_STRUCT::mag, SPI_PARAM_STRUCT::spi_dev, SUCCESS, telemetry_mode, total, UI_NCURSES_MODE, ui_update(), DATA_XYZ::x, DATA_XYZ::y, and DATA_XYZ::z.

Referenced by timer_function_task_2().

 {
 
     int i, j, k, equal=0;
 
     // UI thread
     if(telemetry_mode == UI_NCURSES_MODE)
     {
         ui_update(&imu_data, &eff_data, &mra_data,&enc_data, total, failure);
     }
                 
             buff_i=buff_i%5;
         buff[0][0][buff_i]=imu_data.acc.x;
         buff[0][1][buff_i]=imu_data.acc.y;
         buff[0][2][buff_i]=imu_data.acc.z;
         buff[1][0][buff_i]=imu_data.gyr.x;
         buff[1][0][buff_i]=imu_data.gyr.x;
         buff[1][1][buff_i]=imu_data.gyr.y;
         buff[1][2][buff_i]=imu_data.gyr.z;
         buff[2][0][buff_i]=imu_data.mag.x;
         buff[2][1][buff_i]=imu_data.mag.y;
         buff[2][2][buff_i]=imu_data.mag.z;
         buff_i++;
         
         for( i=0; i<3; i++)
         {
           
           for( j=0; j<3; j++)
             for( k=1; k<3; k++)
             {
               if( buff[i][j][k]==buff[i][j][0] )
                 equal=1;
               else
                 equal=0;
             }
           if( equal==1)
             i=3;
         }
         if( equal==1 )
         {
           close(imu_param.i2c_dev);
           close(spi_param.spi_dev);
           devices_init(&imu_param, &spi_param, &mra_data);
           equal=0;
         }
     /*if(telemetry_mode == UI_MAVLINK_MODE)
     {
         mavlink_module_update(t_task_1_global, t0, &battery_data, &gps_data, &imu_data, &pitot_data, &pwm_read_data, &pwm_write_data, &scp1000_data, &sonar_data, &gps_measure, &imu_measure, &magnetometer_measure, &estimation_data, &control_data);
     }*/
 
     return SUCCESS;
 }

Here is the call graph for this function:

Here is the caller graph for this function:

int reset_timer ( void )

Definition at line 539 of file main2.c.

References SUCCESS, t0, TASK_S::t_global, and t_task_1_global.

Referenced by periodic_task_1().

 {
     t0 = t_task_1_global;
     return SUCCESS;
 }

Here is the caller graph for this function:

void timer_function_task_1 ( void )

Definition at line 426 of file main2.c.

References flag_task_1_firstexecution, periodic_task_1(), status, t0, T_task_1_exec_global, t_task_1_global, T_task_1_max_global, T_task_1_mean_global, and T_task_1_min_global.

Referenced by f_timer_task_1().

 {
     int status = 0;
     static struct timeval timereset;
     static struct timeval time;
     static struct timeval time_exec_start;
     static struct timeval time_exec_end;
     static double T_task,t_task,t_task_previous;
     static double T_task_mean,T_task_min,T_task_max,T_task_exec;
 
     gettimeofday(&time_exec_start, NULL);
 
     // Time calculation
     if(flag_task_1_firstexecution)
     {
         gettimeofday(&timereset, NULL);
         t_task = 0.0;
         T_task_mean = 0.0;
         T_task_min = 0.0;
         T_task_max = 0.0;
         T_task_exec = 0.0;
     }
 
     gettimeofday(&time, NULL);
     t_task = ((time.tv_sec - timereset.tv_sec) + (time.tv_usec - timereset.tv_usec)*1e-6);
     T_task = t_task - t_task_previous;
     t_task_previous = t_task;
 
     if(flag_task_1_firstexecution)
     {
         T_task_mean = T_task;
         T_task_min  = 1e200;
         T_task_max  = 0.0;
         t0 = t_task;
     }
     else
     {
         T_task_mean = 0.05*T_task + 0.95*T_task_mean;
         if(T_task < T_task_min) T_task_min  = T_task;
         if(T_task > T_task_max) T_task_max  = T_task;
     }
 
     // Run the thread
     status = periodic_task_1();
 
     gettimeofday(&time_exec_end, NULL);
     T_task_exec = ((time_exec_end.tv_sec - time_exec_start.tv_sec) + (time_exec_end.tv_usec - time_exec_start.tv_usec)*1e-6);
 
     t_task_1_global = t_task;
     T_task_1_mean_global = T_task_mean;
     T_task_1_min_global = T_task_min;
     T_task_1_max_global = T_task_max;
     T_task_1_exec_global = T_task_exec;
 
     flag_task_1_firstexecution = 0;
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void timer_function_task_2 ( void )

Definition at line 483 of file main2.c.

References flag_task_2_firstexecution, periodic_task_2(), status, T_task_2_exec_global, t_task_2_global, T_task_2_max_global, T_task_2_mean_global, and T_task_2_min_global.

Referenced by f_timer_task_2().

 {
     int status = 0;
     static struct timeval timereset;
     static struct timeval time;
     static struct timeval time_exec_start;
     static struct timeval time_exec_end;
     static double T_task,t_task,t_task_previous;
     static double T_task_mean,T_task_min,T_task_max,T_task_exec;
 
     gettimeofday(&time_exec_start, NULL);
 
     // Time calculation
     if(flag_task_2_firstexecution)
     {
         gettimeofday(&timereset, NULL);
         t_task = 0.0;
         T_task_mean = 0.0;
         T_task_min = 0.0;
         T_task_max = 0.0;
         T_task_exec = 0.0;
     }
 
     gettimeofday(&time, NULL);
     t_task = ((time.tv_sec - timereset.tv_sec) + (time.tv_usec - timereset.tv_usec)*1e-6);
     T_task = t_task - t_task_previous;
     t_task_previous = t_task;
 
     if(flag_task_2_firstexecution)
     {
         T_task_mean = T_task;
         T_task_min  = 1e200;
         T_task_max  = 0.0;
     }
     else
     {
         T_task_mean = 0.05*T_task + 0.95*T_task_mean;
         if(T_task < T_task_min) T_task_min  = T_task;
         if(T_task > T_task_max) T_task_max  = T_task;
     }
 
     // Run the thread
     status = periodic_task_2();
 
     gettimeofday(&time_exec_end, NULL);
     T_task_exec = ((time_exec_end.tv_sec - time_exec_start.tv_sec) + (time_exec_end.tv_usec - time_exec_start.tv_usec)*1e-6);
 
     t_task_2_global = t_task;
     T_task_2_mean_global = T_task_mean;
     T_task_2_min_global = T_task_min;
     T_task_2_max_global = T_task_max;
     T_task_2_exec_global = T_task_exec;
 
     flag_task_2_firstexecution = 0;
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void timer_start_task_1 ( void )

Definition at line 342 of file main2.c.

References f_timer_task_1(), flag_task_1_firstexecution, task_1_period_us, and timer_task_1.

Referenced by main().

 {
     struct itimerspec itimer;
     struct sigevent sigev;
     int erno = 0;
 
     flag_task_1_firstexecution = 1;
 
     itimer.it_interval.tv_sec=0;
     itimer.it_interval.tv_nsec=task_1_period_us * 1000;
     itimer.it_value=itimer.it_interval;
 
     memset(&sigev, 0, sizeof (struct sigevent));
 
     sigev.sigev_value.sival_int = 0;
     sigev.sigev_notify = SIGEV_THREAD;
     sigev.sigev_notify_attributes = NULL;
     sigev.sigev_notify_function = f_timer_task_1;
 
     if (timer_create(CLOCK_REALTIME, &sigev, &timer_task_1) < 0)
     {
         fprintf(stderr, "[%d]: %s\n", __LINE__, strerror(erno));
         exit(erno);
     }
 
     if(timer_settime(timer_task_1, 0, &itimer, NULL) < 0)
     {
         fprintf(stderr, "[%d]: %s\n", __LINE__, strerror(erno));
         exit(erno);
     }
 
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void timer_start_task_2 ( void )

Definition at line 375 of file main2.c.

References f_timer_task_2(), flag_task_2_firstexecution, task_2_period_us, and timer_task_2.

Referenced by main().

 {
     struct itimerspec itimer;
     struct sigevent sigev;
     int erno = 0;
 
     flag_task_2_firstexecution = 1;
 
     itimer.it_interval.tv_sec=0;
     itimer.it_interval.tv_nsec=task_2_period_us * 1000;
     itimer.it_value=itimer.it_interval;
 
     memset (&sigev, 0, sizeof (struct sigevent));
     sigev.sigev_value.sival_int = 0;
     sigev.sigev_notify = SIGEV_THREAD;
     sigev.sigev_notify_attributes = NULL;
     sigev.sigev_notify_function = f_timer_task_2;
 
     if(timer_create(CLOCK_REALTIME, &sigev, &timer_task_2) < 0)
     {
         fprintf(stderr, "[%d]: %s\n", __LINE__, strerror(erno));
         exit(erno);
     }
 
     if(timer_settime(timer_task_2, 0, &itimer, NULL) < 0)
     {
         fprintf(stderr, "[%d]: %s\n", __LINE__, strerror(erno));
         exit(erno);
     }
 }

Here is the call graph for this function:

Here is the caller graph for this function:

void timer_stop_task_1 ( void )

Definition at line 406 of file main2.c.

References timer_task_1.

Referenced by main().

 {
     int erno = 0;
     if(timer_delete(timer_task_1) < 0)
     {
         fprintf(stderr, "[%d]: %s\n", __LINE__, strerror(erno));
         exit(erno);
     }
 }

Here is the caller graph for this function:

void timer_stop_task_2 ( void )

Definition at line 416 of file main2.c.

References timer_task_2.

Referenced by main().

 {
     int erno = 0;
     if(timer_delete(timer_task_2) < 0)
     {
         fprintf(stderr, "[%d]: %s\n", __LINE__, strerror(erno));
         exit(erno);
     }
 }