/* * optical_bus.h.c * * Created on: 18 авг. 2020 г. * Author: stud */ #include #include #include #include #include #include #include "IQmathLib.h" #include "xp_project.h" OPTICAL_BUS_DATA optical_write_data = OPTICAL_BUS_DATA_DEFAULT; OPTICAL_BUS_DATA optical_read_data = OPTICAL_BUS_DATA_DEFAULT; void parse_task_from_optical_bus(void); #pragma CODE_SECTION(optical_bus_update_data_write,".fast_run"); void optical_bus_update_data_write(void) { optical_write_data.data.cmd.bit.alarm = edrk.summ_errors; optical_write_data.data.cmd.bit.master = edrk.auto_master_slave.local.bits.master; optical_write_data.data.cmd.bit.slave = edrk.auto_master_slave.local.bits.slave; optical_write_data.data.cmd.bit.maybe_master = edrk.auto_master_slave.local.bits.try_master; // optical_write_data.cmd.bit.controlMode = ; // optical_write_data.cmd.bit.err_optbus = // optical_write_data.cmd.bit.master = // optical_write_data.cmd.bit.maybe_master = // optical_write_data.cmd.bit.pwm_status = // if (edrk.Status_Rascepitel_Ok==0) { if (edrk.RunZahvatRascepitel) optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_REQUEST_AND_THIS_OFF;// 10 - запрос на включение, свой выключен else optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_OFF;//00 - можно включать расцепитель, свой выключен } else { if (edrk.Status_Ready.bits.ready_final==0) // схема разобрана { if (edrk.RunUnZahvatRascepitel) optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_REQUEST_AND_THIS_OFF;// 10 - запрос на выключение, свой включен else optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_ON;// 01 - можно включать расцепитель, свой включен } else { if (edrk.you_can_on_rascepitel==0) optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_DISABLE_THIS_ON; // 11 - запрет подключения расцепителя, свой включен else optical_write_data.data.cmd.bit.rascepitel_cmd = CODE_RASCEPITEL_CMD_ENABLE_ON_AND_THIS_ON; // 01 - можно включать расцепитель, свой включен } } if (edrk.Status_Ready.bits.ready7 || (edrk.Status_Ready.bits.ready5 && edrk.Status_Ready.bits.ImitationReady2) ) optical_write_data.data.cmd.bit.ready_cmd = CODE_READY_CMD_READY2; else if (edrk.SumSbor) optical_write_data.data.cmd.bit.ready_cmd = CODE_READY_CMD_READY1TO2; else optical_write_data.data.cmd.bit.ready_cmd = CODE_READY_CMD_READY1; // optical_write_data.cmd.bit.ready_to_go = // optical_write_data.cmd.bit.slave = // // optical_write_data.cmd.bit.start_pwm = optical_write_data.data.cmd.bit.statusQTV = edrk.from_shema_filter.bits.QTV_ON_OFF; // optical_write_data.cmd.bit.stop_pwm = optical_write_data.data.cmd.bit.sync_1_2 = sync_data.local_flag_sync_1_2; optical_write_data.data.cmd.bit.sync_line_detect = !sync_data.timeout_sync_signal; // optical_write_data.data1 = f.Mode == 1 ? f.fzad * 60.0 + 0.5 : // f.Mode == 2 ? f.p_zad / 1000.0 : // 0; // optical_write_data.data2 = rotor.direct_rotor == -1 ? -_IQtoIQ15(rotor.iqFout) : // _IQtoIQ15(rotor.iqFout); // optical_write_data.data3 = _IQtoIQ15(analog.iqIq_zadan); // optical_write_data.data4.bit.controlMode = f.Mode == 2 ? 1 : 0; // optical_write_data.data4.bit.leading_ready = f.Ready2 && (f.Mode == 1 || f.Mode == 2) ? 1 : 0; // optical_write_data.data4.bit.leading_Go = edrk.Go; } #pragma CODE_SECTION(optical_bus_write,".fast_run"); void optical_bus_write(void) { #if(USE_TK_3) // check error write PBUS OPTICAL BUS project.cds_tk[3].optical_bus_check_error_write(&project.cds_tk[3]); // write data to OPTICAL BUS project.cds_tk[3].optical_data_out.buf[0] = optical_write_data.data.pzad_or_wzad; project.cds_tk[3].optical_data_out.buf[1] = optical_write_data.data.angle_pwm; project.cds_tk[3].optical_data_out.buf[2] = optical_write_data.data.iq_zad_i_zad; project.cds_tk[3].optical_data_out.buf[3] = optical_write_data.data.cmd.all; optical_write_data.status = 1; project.cds_tk[3].optical_bus_write_data(&project.cds_tk[3]); #endif } #pragma CODE_SECTION(optical_bus_read_old,".fast_run"); void optical_bus_read_old(void) { // // check error write PBUS OPTICAL BUS // project.cds_tk[3].optical_bus_check_error_write(&project.cds_tk[3]); //// read data from OPTICAL BUS // project.cds_tk[3].read_pbus(&project.cds_tk[3]); // // check error read PBUS OPTICAL BUS // project.cds_tk[3].optical_bus_check_error_read(&project.cds_tk[3]); // // // if (project.cds_tk[3].optical_data_in.new_data_ready) // { // if (project.cds_tk[3].optical_data_in.overfull_new_data) // optical_read_data.overfull_data++; // // optical_read_data.raw.pzad_or_wzad = (int)project.cds_tk[3].optical_data_in.buf[0]; // optical_read_data.raw.angle_pwm = (int)project.cds_tk[3].optical_data_in.buf[1]; // optical_read_data.raw.iq_zad = (int)project.cds_tk[3].optical_data_in.buf[2]; // optical_read_data.raw.cmd.all = project.cds_tk[3].optical_data_in.buf[3]; // // // optical_read_data.data.pzad_or_wzad = optical_read_data.raw.pzad_or_wzad; // optical_read_data.data.angle_pwm = optical_read_data.raw.angle_pwm ; // optical_read_data.data.iq_zad = optical_read_data.raw.iq_zad; // optical_read_data.data.cmd.all = optical_read_data.raw.cmd.all; // // optical_read_data.code_status.bit.ready = 1; // optical_read_data.code_status.bit.recive_error = 0; // optical_read_data.code_status.bit.overfull = 0; // optical_read_data.code_status.bit.timeout = 0; // optical_read_data.code_status.bit.wait = 0; // // project.cds_tk[3].optical_data_in.new_data_ready = 0; // // return (optical_read_data.code_status); // // } // else // { // // if (project.cds_tk[3].optical_data_in.ready == 1) // { // optical_read_data.code_status.bit.ready = 0; // // if (project.cds_tk[3].optical_data_in.raw_local_error) // optical_read_data.code_status.bit.recive_error = 1; // else // optical_read_data.code_status.bit.recive_error = 0; // // optical_read_data.code_status.bit.overfull = 0; // optical_read_data.code_status.bit.timeout = 0; // optical_read_data.code_status.bit.wait = 1; // // return (optical_read_data.code_status); // } // // } // // // if (project.cds_tk[3].optical_data_out.ready == 0) { // // project.cds_tk[3].optical_data_out.error_not_ready_count += 1; // //// optical_read_data.code_status.bit.ready = 0; // optical_read_data.code_status.bit.send_error = 1; //// optical_read_data.code_status.bit.overfull = 0; //// optical_read_data.code_status.bit.timeout = 0; //// optical_read_data.code_status.bit.wait = 0; // //// optical_write_data.status = 2; //// return (optical_read_data.code_status); // } // else // optical_read_data.code_status.bit.send_error = 0; // // // if (project.cds_tk[3].optical_data_in.ready == 0) { // //// f.read_task_from_optical_bus = 0; // project.cds_tk[3].optical_data_in.error_not_ready_count += 1; // // optical_read_data.code_status.bit.ready = 0; // // optical_read_data.code_status.bit.send_error = 0; // //optical_read_data.code_status.bit.recive_error = 1; // optical_read_data.code_status.bit.overfull = 0; // optical_read_data.code_status.bit.timeout = 1; // optical_read_data.code_status.bit.wait = 0; // // optical_read_data.data.pzad_or_wzad = 0; // optical_read_data.data.angle_pwm = 0; // optical_read_data.data.iq_zad = 0; // optical_read_data.data.cmd.all = 0; // // // optical_read_data.status = 2; // // // return (project.cds_tk[3].optical_data_in.raw_local_error); // } // else // { // // // // } // // return (optical_read_data.code_status); // // //// optical_read_data.status = 1; // // // return (project.cds_tk[3].optical_data_in.raw_local_error); // //// if (f.flag_leading == 0 && f.flag_distance && f.Ready1){ //// parse_task_from_optical_bus(); //// } else { //// f.read_task_from_optical_bus = 0; //// } //// rotor.iqFrotFromOptica = _IQ15toIQ(optical_read_data.data2); //Frot //// analog.iqIq_zad_from_optica = _IQ15toIQ(optical_read_data.data3); } #pragma CODE_SECTION(optical_bus_read,".fast_run"); void optical_bus_read(void) { // check error write PBUS OPTICAL BUS // project.cds_tk[3].optical_bus_check_error_write(&project.cds_tk[3]); // read data from OPTICAL BUS #if(USE_TK_3) project.cds_tk[3].read_pbus(&project.cds_tk[3]); #endif return; } #pragma CODE_SECTION(optical_bus_read_clear_count_error,".fast_run"); void optical_bus_read_clear_count_error(void) { #if(USE_TK_3) project.cds_tk[3].optical_data_in.local_count_error = 0; #endif return; } #pragma CODE_SECTION(optical_bus_get_status_and_read,".fast_run"); STATUS_DATA_READ_OPT_BUS optical_bus_get_status_and_read(void) { STATUS_DATA_READ_OPT_BUS status_read; // check error write PBUS OPTICAL BUS // project.cds_tk[3].optical_bus_check_error_write(&project.cds_tk[3]); // read data from OPTICAL BUS // project.cds_tk[3].read_pbus(&project.cds_tk[3]); #if(USE_TK_3) // check error read PBUS OPTICAL BUS project.cds_tk[3].optical_bus_check_error_read(&project.cds_tk[3]); status_read.all = project.cds_tk[3].optical_data_in.status_read.all; project.cds_tk[3].optical_data_in.prev_status_read.all = status_read.all; project.cds_tk[3].optical_data_in.status_read.all = 0; optical_read_data.status = project.cds_tk[3].optical_data_in.ready; if (status_read.bit.new_data_ready) { if (status_read.bit.overfull_new_data) optical_read_data.overfull_data++; optical_read_data.raw.pzad_or_wzad = (int)project.cds_tk[3].optical_data_in.buf[0]; optical_read_data.raw.angle_pwm = (int)project.cds_tk[3].optical_data_in.buf[1]; optical_read_data.raw.iq_zad_i_zad = (int)project.cds_tk[3].optical_data_in.buf[2]; optical_read_data.raw.cmd.all = project.cds_tk[3].optical_data_in.buf[3]; optical_read_data.data.pzad_or_wzad = optical_read_data.raw.pzad_or_wzad; optical_read_data.data.angle_pwm = optical_read_data.raw.angle_pwm ; optical_read_data.data.iq_zad_i_zad = optical_read_data.raw.iq_zad_i_zad; optical_read_data.data.cmd.all = optical_read_data.raw.cmd.all; return (status_read); // мы получили данные } else { // нет данных? // обеуляем в прерывании optical_bus, а не тут! // optical_read_data.data.pzad_or_wzad = 0; // optical_read_data.data.angle_pwm = 0; // optical_read_data.data.iq_zad_i_zad = 0; // optical_read_data.data.cmd.all = 0; } #else status_read.all = 0; #endif return status_read; //нет данных или ошибки } void parse_task_from_optical_bus(void) { // if (optical_read_data.data4.bit.leading_ready == 1) { // f.read_task_from_optical_bus = 1; // f.Mode = optical_read_data.data4.bit.controlMode == 0 ? 1 : 2; // if (f.Mode == 1) { // f.fzad = (float)optical_read_data.data1 / 60.0; // f.iq_fzad = _IQ(f.fzad / NORMA_FROTOR); // limit_mzz_zad_turns(); // } // if (f.Mode == 2) { // f.p_zad = (float)optical_read_data.data1 * 1000.0; // f.iq_p_zad = _IQ(f.p_zad/NORMA_ACP/NORMA_ACP); // // // if (f.iq_p_zad > f.iq_p_limit_zad) { f.iq_p_zad = f.iq_p_limit_zad;} // if (f.iq_p_zad < -f.iq_p_limit_zad) { f.iq_p_zad = -f.iq_p_limit_zad;} // // limit_mzz_zad_power(); // } // if (f.Ready1 && f.Ready2) { // if (((f.Mode == 1 && f.fzad != 0) || (f.Mode == 2 && f.p_zad != 0)) // && (!f.Is_Blocked || (f.Is_Blocked && edrk.Go)) // && (f.rotor_stopped == 0) //// && (faults.faults5.bit.rotor_stopped == 0) // //&& (optical_read_data.data4.bit.leading_Go == 1) // ){ // edrk.Go = 1; // } else { // if (edrk.Go) { // f.p_zad = 0; // f.fzad = 0; // f.iq_fzad = 0; // f.iq_p_zad = 0; // if(f.Mode == 1) // { // if(a.iqk < 1677722 || rotor.iqFout < 48210 //1677722 ~ 0.1 коэффициент 503316 = 3% 838860 = 5% // || (analog.iqIm_1 < 1677721 && analog.iqIm_2 < 1677721)) //1677721 ~ 300A // { // edrk.Go = 0; // f.iq_fzad = 0; // } // // } // if(edrk.Go && f.Mode == 2) // { // if(analog.iqW < 186413) // { // if(a.iqk < 1677722 || //1677722 ~ 0.1 коэффициент // (analog.iqIm_1 < 1677721 && analog.iqIm_2 < 1677721)) //1677721 ~ 300A // { // edrk.Go = 0; // } // } // } // // } else { // f.iq_mzz_zad = _IQ(500.0/NORMA_MZZ); // } // } // } else { // edrk.Go = 0; // } // _IQ15toIQ(optical_read_data.data3); //Iq_zad // } else { // f.read_task_from_optical_bus = 0; // } }