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;
}
