/* * Copyright 2004 by Soos, Antal * All rights reserved. Property of Soos, Antal. * Restricted rights to use, duplicate or disclose this code are * granted through contract. */ /***************************************************************************/ /* */ /* H8_Calculus . cpp */ /* */ /* H8_MPC semi off line calculation */ /* */ /***************************************************************************/ /************************************************************************/ #define _in_H*_CALCULUS_CPP_ /************************************************************************/ // DSP 2 <=> TMS320C6713 #include "mmaccfg.h" #include #include "global_var.h" #include "dsk_lib.h" extern "C" {void H8_Calculus() ;} #define PRINT_0x(a,b) print_matrix(a,b) /************************************************************/ /*swiEnablePrephFunc: >>> SWI Priority 1 <<< */ /* Software Interrupt Function configures EDMAC2 and */ /* EDMAC6, enables respective EDMA channels and Timer0 */ /************************************************************/ void H8_Calculus(Void) // <--SWI_post(&H8_Calculations) { static unsigned int iperLED1 = LED1_PERIOD; #if H8_CONTROL /* ON/OFF IF PID CONTROL IS USED */ int i; //DSK6713_LED_on(1); // For the calculation-time measurement //----------------------------------------------------------------------------- // semi-off-line Parameter evaluation : //----------------------------------------------------------------------------- //matrix_alloc(&k, m_s+m_a+m_b+m_c, 1); //temporary matrix Matrix mT; float mT_v[ARMAX_dim+2] ; mT.mtx = mT_v; mT.row = ARMAX_dim; mT.col = 1; mT.dim = ARMAX_dim; // mT_v[0] = SATRT_OF_VECTOR; // mT_v[ARMAX_dim+1] = END_OF_VECTOR; // for(int i=1;i<= ARMAX_dim; i++) mT_v[i] = 0.0; enum algorithms_1 {Nan_1,FIX_1,RLS_1,LMS_1,WCE_1,KF_1,FQR_1,IQR_1,QRL_1}; enum algorithms_1 sel_1; float performanceindex; float tperformanceindex; int new_system; // Select the best adaptive algorithm from the following: //--------------------------------------------------------------------------- performanceindex = pFIX->get_performance_index(); sel_1 = FIX_1; new_system = 0; // No model change is expexted LOG_printf(&trace2,"t= %g",t_n); LOG_printf(&trace2,"FIX pi= %g",performanceindex); if(pRLS->get_stable_coeficients()) { tperformanceindex = pRLS->get_performance_index(); if(performanceindex > tperformanceindex) { performanceindex = tperformanceindex; sel_1 = RLS_1; new_system = 1; // Yes, it will be neded to uptade the comtrl algorithm: } LOG_printf(&trace2,"RLS pi= %g",tperformanceindex); } if( pLMS->get_stable_coeficients()) { tperformanceindex = pLMS->get_performance_index(); if(performanceindex > tperformanceindex) { performanceindex = tperformanceindex; sel_1 = LMS_1; new_system = 1; // Yes, it will be neded to uptade the comtrl algorithm: } LOG_printf(&trace2,"LMS pi= %g",tperformanceindex); } if(pWCE->get_stable_coeficients()) { tperformanceindex = pWCE->get_performance_index(); if(performanceindex > tperformanceindex) { performanceindex = tperformanceindex; sel_1 = WCE_1; new_system = 1; // Yes, it will be neded to uptade the comtrl algorithm: } LOG_printf(&trace2,"WCE pi= %g",tperformanceindex); } if(pKF->get_stable_coeficients()) { tperformanceindex = pKF->get_performance_index(); if(performanceindex > tperformanceindex) { performanceindex = tperformanceindex; sel_1 = KF_1; new_system = 1; // Yes, it will be neded to uptade the comtrl algorithm: } LOG_printf(&trace2,"KF pi= %g",tperformanceindex); } if(pFQR_RLS->get_stable_coeficients()) { tperformanceindex = pFQR_RLS->get_performance_index(); if(performanceindex > tperformanceindex) { performanceindex = tperformanceindex; sel_1 = FQR_1; new_system = 1; // Yes, it will be neded to uptade the comtrl algorithm: } LOG_printf(&trace2,"FQR pi= %g",tperformanceindex); } if(pIQR_RLS->get_stable_coeficients()) { tperformanceindex = pIQR_RLS->get_performance_index(); if(performanceindex > tperformanceindex) { performanceindex = tperformanceindex; sel_1 = IQR_1; new_system = 1; // Yes, it will be neded to uptade the comtrl algorithm: } LOG_printf(&trace2,"IQR pi= %g",tperformanceindex); } if(pQR_LMS->get_stable_coeficients()) { tperformanceindex = pQR_LMS->get_performance_index(); if(performanceindex > tperformanceindex) { performanceindex = tperformanceindex; sel_1 = QRL_1; new_system = 1; // Yes, it will be neded to uptade the comtrl algorithm: } LOG_printf(&trace2,"QRL pi= %g",tperformanceindex); } // The best adaptive algorithm is selected (sel_1), // get the coresponding paramethers: //------------------------------------------------------------------------- // Semi-Real Time update the control paramethers in N*T period once: // H8_MPC control weight calculations only if there are new paramethers //new_system = 1; if(new_system) { LOG_printf(&trace2,"New Sys t= %g",t_n); switch (sel_1) { case FIX_1: //FIX (privious values) LOG_printf(&trace2,"FIX"); break; case RLS_1: pRLS->get_theta(&mT); LOG_printf(&trace2,"RLS"); break; case LMS_1: pLMS->get_theta(&mT); LOG_printf(&trace2,"LMS"); break; case WCE_1: pWCE->get_theta(&mT); LOG_printf(&trace2,"WCE"); break; case KF_1: pKF->get_theta(&mT); LOG_printf(&trace2,"KF"); break; case FQR_1: pFQR_RLS->get_theta(&mT); LOG_printf(&trace2,"FQR"); break; case IQR_1: pIQR_RLS->get_theta(&mT); LOG_printf(&trace2,"IQR"); break; case QRL_1: pQR_LMS->get_theta(&mT); LOG_printf(&trace2,"QRL"); break; default: ;//samting is wrong (privious values) LOG_printf(&trace2,"error in selection"); // I ges, it is same trouble in my code! } //Print the selected paramethers: for(i=1;i<= ARMAX_dim;i++) { LOG_printf(&trace2, "%g", mT_v[i]); } // The best adaptive algorithm paramethers are loded in FIX algoritm pFIX->set_theta(&mT); // update the fix algorithm pH8MPC->update(&mT); tperformanceindex = pH8MPC->get_gama(); LOG_printf(&trace2,"gama = %g",tperformanceindex); //-------------------------------------- // pFIX->get_theta(&mT); // PRINT_0x(&mT,"pFIX"); //-------------------------------------- } //----------------------------------------------------------------------------- // End of semi-off-line Parameter evaluation //----------------------------------------------------------------------------- #endif /* IF H8_CONTROL */ //----------------------------------------------------------------- // If necessery re-sincronize the serial buffer: com_tra[0] = McBSP_MARKER; if(McBSP_MARKER == com_rec[0]) { IRQ_disable(IRQ_EVT_RINT1); /*Disable McBSP interrupt */ com_rec[0] = 0; //McBSP_MARKER+5; // Indicate that the DSP2DSP communication is halty: iperLED1--; if(iperLED1==0) { DSK6713_LED_toggle(1); iperLED1 = LED1_PERIOD; } } else { IRQ_enable(IRQ_EVT_RINT1); // Resinchronize!! DSK6713_LED_on(1); } //----------------------------------------------------------------- //DSK6713_LED_off(1); // For the calculation-time measurement DSK6713_LED_toggle(3); }