void run_app() {
/*
* Builder for the application
*/
Repository* repo = repository_init("Files/cheltuieli.txt");
Validator* valid = validator_init();
Controller* ctr = controller_init(repo, valid);
Console* cons = console_init(ctr);
console_run(cons);
console_destroy(cons);
controller_destroy(ctr);
validator_destroy(valid);
repository_destroy(repo);
}
static void
primwatch_finalize(
primwatch_t *primwatch)
{
if (primwatch->controller) {
controller_destroy(primwatch->controller);
}
if (primwatch->watcher) {
watcher_destroy(primwatch->watcher);
}
if (primwatch->config_manager) {
config_manager_destroy(primwatch->config_manager);
}
if (primwatch->event_base) {
event_base_free(primwatch->event_base);
}
}
static void cleanup(void)
{
LOG_WARNING("Terminating");
controller_destroy();
if(tunnel_driver)
driver_dns_destroy(tunnel_driver);
if(group)
select_group_destroy(group);
if(system_dns)
safe_free(system_dns);
print_memory();
}
int control_loop_run( void )
{
int first_time = 1;
next_period = get_utime( ) + us_period;
while( running )
{
if( send_motor_signals( ) )
{
break;
}
if( javiator_port_get_data( &javiator_data ) )
{
fprintf( stderr, "ERROR: connection to JAviator broken\n" );
heli_state = HELI_STATE_SHUTDOWN;
perform_shut_down( );
break;
}
if( check_terminal_connection( ) )
{
heli_state = HELI_STATE_SHUTDOWN;
if( first_time )
{
fprintf( stderr, "ERROR: connection to Terminal broken\n" );
first_time = 0;
}
}
else
{
first_time = 1;
}
get_command_data( );
get_control_params( );
prepare_sensor_data( );
switch( heli_state )
{
case HELI_STATE_GROUND:
perform_ground_actions( );
break;
case HELI_STATE_FLYING:
compute_motor_signals( );
break;
case HELI_STATE_SHUTDOWN:
perform_shut_down( );
break;
default:
fprintf( stderr, "ERROR: invalid altitude mode %d\n", heli_state );
}
send_report_to_terminal( );
wait_for_next_period( );
}
iir_lp_filter_destroy( &iir_acc_x );
iir_lp_filter_destroy( &iir_acc_y );
iir_lp_filter_destroy( &iir_acc_z );
iir_lp_filter_destroy( &iir_cmd_roll );
iir_lp_filter_destroy( &iir_cmd_pitch );
iir_lp_filter_destroy( &iir_cmd_yaw );
iir_lp_filter_destroy( &iir_cmd_z );
median_filter_destroy( &med_bat_level );
position_ekf_destroy ( &ekf_pos_z );
controller_destroy ( &ctrl_roll );
controller_destroy ( &ctrl_pitch );
controller_destroy ( &ctrl_yaw );
controller_destroy ( &ctrl_z );
return( 0 );
}
int control_loop_run( void )
{
int first_time = 1;
long long start, end;
long long loop_start;
uint32_t rem_time;
next_period = get_utime( ) + us_period;
while( running )
{
trace_data.value_3 = (int16_t)( period * 1000 );
trace_data.value_4 = (int16_t)( heli_state );
/* start of STAT_TO_JAV */
start = get_utime( );
loop_start = start;
rem_time = (uint32_t)( start % MAX_TIME );
trace_data.value_5 = (int16_t)( rem_time >> 16 );
trace_data.value_6 = (int16_t)( rem_time );
if( send_motor_signals( ) )
{
break;
}
end = get_utime( );
rem_time = (uint32_t)( end % MAX_TIME );
trace_data.value_7 = (int16_t)( rem_time >> 16 );
trace_data.value_8 = (int16_t)( rem_time );
calc_stats( end - start, STAT_TO_JAV );
/* start of STAT_FROM_JAV */
start = get_utime( );
if( javiator_port_get_data( &javiator_data ) )
{
fprintf( stderr, "ERROR: connection to JAviator broken\n" );
heli_state = HELI_STATE_SHUTDOWN;
perform_shut_down( );
break;
}
if( ubisense_enabled )
{
if( (new_data_x = new_data_y = ubisense_port_is_new_data( )) )
{
ubisense_port_get_data( &sensor_data );
}
}
end = get_utime( );
rem_time = (uint32_t)( end % MAX_TIME );
trace_data.value_9 = (int16_t)( rem_time >> 16 );
trace_data.value_10 = (int16_t)( rem_time );
calc_stats( end - start, STAT_FROM_JAV );
/* start of STAT_FROM_TERM */
start = get_utime( );
if( check_terminal_connection( ) )
{
heli_state = HELI_STATE_SHUTDOWN;
if( first_time )
{
fprintf( stderr, "ERROR: connection to Terminal broken\n" );
first_time = 0;
}
}
else
{
first_time = 1;
}
get_command_data( );
get_control_params( );
end = get_utime( );
rem_time = (uint32_t)( end % MAX_TIME );
trace_data.value_11 = (int16_t)( rem_time >> 16 );
trace_data.value_12 = (int16_t)( rem_time );
calc_stats( end - start, STAT_FROM_TERM );
/* start of STAT_CONTROL */
start = get_utime( );
prepare_sensor_data( );
switch( heli_state )
{
case HELI_STATE_GROUND:
perform_ground_actions( );
break;
case HELI_STATE_FLYING:
compute_motor_signals( );
break;
case HELI_STATE_SHUTDOWN:
perform_shut_down( );
break;
default:
fprintf( stderr, "ERROR: invalid altitude mode %d\n", heli_state );
}
end = get_utime( );
rem_time = (uint32_t)( end % MAX_TIME );
trace_data.value_13 = (int16_t)( rem_time >> 16 );
trace_data.value_14 = (int16_t)( rem_time );
calc_stats( end - start, STAT_CONTROL );
/* start of STAT_TO_TERM */
start = get_utime( );
send_report_to_terminal( );
send_trace_data_to_terminal( );
end = get_utime( );
rem_time = (uint32_t)( end % MAX_TIME );
trace_data.value_15 = (int16_t)( rem_time >> 16 );
trace_data.value_16 = (int16_t)( rem_time );
calc_stats( end - start, STAT_TO_TERM );
calc_stats( end - loop_start, STAT_ALL );
/* start of STAT_SLEEP */
start = get_utime( );
wait_for_next_period( );
end = get_utime( );
calc_stats( end - start, STAT_SLEEP );
if( ++loop_count < 0 )
{
fprintf( stderr, "WARNING: stats overrun\n" );
memset( stats, 0, sizeof( stats ) );
loop_count = 0;
}
}
outlier_filter_destroy( &cof_out_x );
outlier_filter_destroy( &cof_out_y );
iir_lp_filter_destroy ( &iir_acc_x );
iir_lp_filter_destroy ( &iir_acc_y );
iir_lp_filter_destroy ( &iir_acc_z );
iir_lp_filter_destroy ( &iir_cmd_roll );
iir_lp_filter_destroy ( &iir_cmd_pitch );
iir_lp_filter_destroy ( &iir_cmd_yaw );
iir_lp_filter_destroy ( &iir_cmd_z );
average_filter_destroy( &avg_bmu_maps );
median_filter_destroy ( &med_bmu_temp );
median_filter_destroy ( &med_bmu_batt );
attitude_ekf_destroy ( &ekf_att_roll );
attitude_ekf_destroy ( &ekf_att_pitch );
attitude_ekf_destroy ( &ekf_att_yaw );
position_ekf_destroy ( &ekf_pos_x );
position_ekf_destroy ( &ekf_pos_y );
position_ekf_destroy ( &ekf_pos_z );
controller_destroy ( &ctrl_roll );
controller_destroy ( &ctrl_pitch );
controller_destroy ( &ctrl_yaw );
controller_destroy ( &ctrl_x );
controller_destroy ( &ctrl_y );
controller_destroy ( &ctrl_z );
print_stats( );
return( 0 );
}
