int main(int argc, char** argv)
{
#ifdef RAM_ONLY
if (mlockall(MCL_FUTURE) == -1)
{
std::cerr << "Failed to lock memory to RAM" << std::endl;
}
#endif
float husbands_2[HUSBANDS_ROWS][HUSBANDS_COL];
float husbands_3[HUSBANDS_ROWS][HUSBANDS_COL];
float husbands_4[HUSBANDS_ROWS][HUSBANDS_COL];
float husbands_5[HUSBANDS_ROWS][HUSBANDS_COL];
float wives[WIVES_ROWS][WIVES_COL];
float wage_moments[WAGE_MOM_ROWS][WAGE_MOM_COL];
float marr_fer_moments[MARR_MOM_ROWS][MARR_MOM_COL];
float emp_moments[EMP_MOM_ROWS][EMP_MOM_COL];
float general_moments[GEN_MOM_ROWS][GEN_MOM_COL];
float* epsilon_b_arr = new float[DRAW_B*T_MAX*SCHOOL_GROUPS*3];
float* epsilon_f_arr = new float[DRAW_F*T_MAX*SCHOOL_GROUPS*3];
float* h_draws_b_arr = new float[DRAW_B*T_MAX*SCHOOL_GROUPS*4];
float* w_draws_b_arr = new float[DRAW_B*T_MAX*SCHOOL_GROUPS*6];
uint16_t* w_ability_draw_arr = new uint16_t[DRAW_F*SCHOOL_GROUPS];
float* h_draws_arr = new float[DRAW_F*T_MAX*SCHOOL_GROUPS*5];
float* w_draws_arr = new float[DRAW_F*T_MAX*SCHOOL_GROUPS*3];
float param[INIT_PARAM_ROWS];
if (estimation_init(param, husbands_2, husbands_3, husbands_4, husbands_5, wives, wage_moments, marr_fer_moments, emp_moments, general_moments,
epsilon_b_arr, epsilon_f_arr, h_draws_b_arr, w_draws_b_arr, w_ability_draw_arr, h_draws_arr, w_draws_arr))
{
// TODO read run_mode from input
// TODO add "usage" function
RunMode run_mode = TEST_MODE;
switch (run_mode)
{
case TEST_MODE:
{
std::cout << "Run in test mode..." << std::endl;
float value = estimation_f(param, husbands_2, husbands_3, husbands_4, husbands_5, wives, wage_moments, marr_fer_moments,
emp_moments, general_moments, epsilon_b_arr, epsilon_f_arr, h_draws_b_arr, w_draws_b_arr, w_ability_draw_arr, h_draws_arr, w_draws_arr);
std::cout << "Value of estimation is: " << value << std::endl;
}
break;
default:
break;
}
}
delete[] epsilon_b_arr;
delete[] epsilon_f_arr;
delete[] h_draws_b_arr;
delete[] w_draws_b_arr;
delete[] w_ability_draw_arr;
delete[] h_draws_arr;
delete[] w_draws_arr;
return 0;
}
int threads_linux_init(void)
{
int status = 0;
int n = 0;
int return_value = THREADS_LINUX_SUCCESS;
// Init data
status = sensors_init(&battery_data, &gps_data, &imu_data, &pitot_data, &pwm_read_data, &pwm_write_data, &scp1000_data, &sonar_data);
if(status != SENSORS_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = calibration_init(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
if(status != CALIBRATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = gps_init(&gps_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = imu_init(&imu_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = magnetometer_init(&magnetometer_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = sonar_init(&sonar_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = estimation_init(ESTIMATION_DO_NOT_ESTIMATE_ACCEL_BIAS, &estimation_data);
if(status != ESTIMATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = control_init(&control_data);
if(status != CONTROL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = protocol_init();
if(status != PROTOCOL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = ui_init();
if(status != UI_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
if(return_value != THREADS_LINUX_FAILURE)
{
status = datalogger_init();
if(status != DATALOGGER_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
}
// Main Loop
if(return_value != THREADS_LINUX_FAILURE)
{
threads_linux_timer_start_task_1();
usleep(0.5*task_1_period_us);
threads_linux_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)
{
if(datalogger_status() == DATALOGGER_RUNNING) datalogger_write_file();
n = 0;
}
usleep(5*task_1_period_us);
}
threads_linux_timer_stop_task_1();
usleep(TASK1_PERIOD_US);
threads_linux_timer_stop_task_2();
usleep(TASK2_PERIOD_US);
}
status = datalogger_close();
if(status != DATALOGGER_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = ui_close();
if(status != UI_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = protocol_close();
if(status != PROTOCOL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = control_close();
if(status != CONTROL_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = estimation_close(&estimation_data);
if(status != ESTIMATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = magnetometer_close(&magnetometer_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = imu_close(&imu_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = gps_close(&gps_measure);
if(status != 1)
{
return_value = THREADS_LINUX_FAILURE;
}
status = calibration_close(&calibration_local_coordinate_system_data, &calibration_local_fields_data, &calibration_altimeter_data);
if(status != CALIBRATION_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
status = sensors_close();
if(status != SENSORS_SUCCESS)
{
return_value = THREADS_LINUX_FAILURE;
}
return return_value;
}
