int clsyncmgr(clsyncmgr_t *ctx_p) { errno = 0; ctx_p->pthread_root = pthread_self(); debug(3, "starting"); clsyncmgr_switch_state(ctx_p, STATE_STARTING); sighandler_run(ctx_p); ctx_p->sock_unix_ht_path = g_hash_table_new_full(g_str_hash, g_str_equal, free, 0); ctx_p->sock_unix_ht_label = g_hash_table_new_full(g_str_hash, g_str_equal, free, 0); if ((errno=dynamic_foreach(&ctx_p->socketpaths, clsyncmgr_socketpath_connect, ctx_p))) error("Got error while connecting to sockets"); if (!errno) if (!ctx_p->flags[FL_EXECUTE]) terminal_run(ctx_p); /* if (!errno) control_run(ctx_p); */ if (!errno) { clsyncmgr_switch_state(ctx_p, STATE_RUNNING); if (ctx_p->flags[FL_EXECUTE]) { control_execute(ctx_p, ctx_p->execute); } else { while (ctx_p->state == STATE_RUNNING) { clsyncmgr_idle(ctx_p); sleep(1); break; } } clsyncmgr_switch_state(ctx_p, STATE_CLEANUP); } debug(3, "cleanup"); if (!ctx_p->flags[FL_EXECUTE]) terminal_cleanup(ctx_p); // control_cleanup(ctx_p); sighandler_stop(ctx_p); clsyncmgr_watchdir_remove_all(ctx_p); g_hash_table_destroy(ctx_p->sock_unix_ht_path); g_hash_table_destroy(ctx_p->sock_unix_ht_label); clsyncmgr_switch_state(ctx_p, STATE_EXIT); debug(3, "finished"); return 0; }
int main() { pedal_node_state = pedal_state_neutral; LCD_Start(); CAN_invertor_init(); ADC_SAR_Start(); ADC_SAR_StartConvert(); EEPROM_Start(); //isr_Start(); //Timer_Start(); CAN_timer_Start(); CAN_Init(); CAN_Start(); isr_start_StartEx(&isr_start_handler); Start_Reset_Write(1); /* source of interrupt (reset) */ isr_start_ClearPending(); isr_neutral_StartEx(&isr_neutral_handler); Neutral_Reset_Write(1); isr_neutral_ClearPending(); isr_calibration_StartEx(&isr_calibration_handler); CyGlobalIntEnable; //enable global interrupts //Initialize terminal terminal_init(); monitor_init(); pedal_restore_calibration_data(); //set min and max values pedal_set_CAN(); //Setup tunnel from pedal control to CAN pedal_set_monitor(); //Setup tunnel from pedal control to USB Monitor // Initialize global variables EEPROM_ERROR_LED_Write(0); should_calibrate = false; // terminal_registerCommand("newCmd", &newCmdRout); sendNMT(NMT_command_startRemoteNode); CyDelay(1000); pedal_node_state = pedal_state_driving; for(;;){ pedal_fetch_data(); } for(;;) { CyDelay(50); terminal_run(); // Refresh terminal if (pedal_node_state == pedal_state_neutral) { if (should_calibrate) { pedal_node_state = pedal_state_calibrating; } else if (should_turn_to_drive) { pedal_node_state = pedal_state_driving; //Start sending can message for invertor CAN_invertor_resume(); } } else if (pedal_node_state == pedal_state_driving) { if (should_turn_to_neutral) { pedal_node_state = pedal_state_neutral; //Stop sending messages for invertor CAN_invertor_pause(); } } //Clear all flags after handling should_calibrate = false; should_turn_to_drive = false; should_turn_to_neutral = false; uint8_t out_of_range_flag; double brake_percent = 0, throttle_percent = 0; double brake_percent_diff = 0, throttle_percent_diff = 0; uint8_t torque_plausible_flag; uint8_t brake_plausible_flag; pedal_fetch_data(); //Update ADC readings CAN_invertor_update_pedal_state(pedal_node_state); monitor_update_vechicle_state(pedal_node_state); //Update vecicle state monitor_status_update_vehicle_state(pedal_node_state); switch (pedal_node_state) { case pedal_state_neutral: LCD_ClearDisplay(); LCD_Position(0,0); LCD_PrintString("NEUTRAL"); break; case pedal_state_driving: LCD_ClearDisplay(); LCD_Position(0,0); LCD_PrintString("DRIVING"); //out_of_range_flag = pedal_get_out_of_range_flag(); if (out_of_range_flag != 0) { pedal_node_state = pedal_state_out_of_range; break; } torque_plausible_flag = pedal_is_pedal_reading_matched(&brake_percent_diff, &throttle_percent_diff); if (torque_plausible_flag != 0) { pedal_node_state = pedal_state_discrepency; break; } brake_plausible_flag = pedal_is_brake_plausible(&brake_percent, &throttle_percent); if (brake_plausible_flag != 0) { pedal_node_state = pedal_state_implausible; break; } break; case pedal_state_calibrating: //clock_StopBlock(); //stop clock to disable interrupt pedal_calibrate(); LCD_ClearDisplay(); //isr_ClearPending(); //clock_Start(); // isr_calibration_Enable(); pedal_node_state = pedal_state_neutral; break; case pedal_state_out_of_range: LCD_ClearDisplay(); LCD_Position(0,0); LCD_PrintString("Pedal out of"); LCD_Position(1,0); LCD_PrintString("range"); out_of_range_flag = pedal_get_out_of_range_flag(); if (out_of_range_flag == 0) { pedal_node_state = pedal_state_driving; } break; case pedal_state_discrepency: LCD_ClearDisplay(); LCD_Position(0,0); LCD_PrintString("Pedal discrepency"); torque_plausible_flag = pedal_is_pedal_reading_matched(&brake_percent_diff, &throttle_percent_diff); if (torque_plausible_flag == 0) { pedal_node_state = pedal_state_driving; } break; case pedal_state_implausible: LCD_ClearDisplay(); LCD_Position(0,0); LCD_PrintString("Pedal implausible"); brake_plausible_flag = pedal_is_brake_plausible(&brake_percent, &throttle_percent); if (throttle_percent < PEDAL_BRAKE_IMPLAUSIBLE_EXIT_THROTTLE_PERCENT) { pedal_node_state = pedal_state_driving; } break; } // CyDelay(100); } return 0; }