AssistCurve.c 46 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244
  1. /**
  2. * @file AssistCurve.c
  3. * @author Zhang, Kai(zhangkai71@midea.com)
  4. * @brief
  5. * @version 0.1
  6. * @date 2021-11-15
  7. *
  8. * @copyright Copyright (c) 2021
  9. *
  10. */
  11. /******************************
  12. *
  13. * Include File
  14. *
  15. ******************************/
  16. #include "string.h"
  17. #include "Syspar.h"
  18. #include "user.h"
  19. #include "AssistCurve.h"
  20. #include "bikebrake.h"
  21. #include "Cadence.h"
  22. #include "flash_master.h"
  23. #include "bikegearsensor.h"
  24. /******************************
  25. *
  26. * Parameter
  27. *
  28. ******************************/
  29. ASS_FSM_STATUS Ass_FSM;
  30. ASS_PER_IN ass_stCalIn = TORQUE_CAL_IN_DEFAULT;
  31. ASS_PER_COEF ass_stCalCoef;
  32. ASS_PER_OUT ass_stCalOut;
  33. ASS_PARA_CONFIGURE ass_stParaCong;
  34. ASS_PARA_SET ass_stParaSet;
  35. ASS_CURLIM_COEF ass_stCurLimCoef = ASS_LIM_DEFAULT;
  36. ASS_CURLIM_OUT ass_stCurLimOut;
  37. ASS_LIMIT_ACCORDING_VOL_COF ass_stCurLimCalBMSCoef;
  38. ASS_LIMIT_ACCORDING_VOL_OUT ass_stCurLimitCalBMSOut;
  39. ASR_SPDPI_IN asr_stTorqSpdPIIn;
  40. ASR_SPDPI_OUT asr_stTorqSpdPIOut;
  41. ASR_SPDPI_COF asr_stTorqSpdPICoef;
  42. ASR_SPDPI_COFIN asr_stTorqSpdPICoefIn;
  43. ASS_TORQ_PI_IN ass_stTorqPIIn;
  44. ASS_TORQ_PI_OUT ass_stTorqPIOut;
  45. SWORD ass_swTorqMafBuf[64];
  46. MAF_IN ass_stTorqMafValue = {0, 32, 0, 0, ass_swTorqMafBuf, 0, FALSE};
  47. SWORD ass_swUqLimMafBuf[64];
  48. MAF_IN ass_stUqLimMafValue = {0, 64, 0, 0, ass_swUqLimMafBuf, 0, FALSE};
  49. static TOR2CURRENT_CAL_COEF ass_Tor2CurCalCoef;
  50. static UWORD StartUpGainArray[5] = START_GAIN_DEFAULT;
  51. static UWORD LinerAssist[5] = ASS_LINER_TORQUE_DEFAULT;
  52. static SWORD ass_pvt_swVoltCnt=0;
  53. static UWORD ass_pvt_uwTorqAccCnt=0,ass_pvt_uwTorqDecCnt=0,ass_pvt_uwSpd2TorqCnt=0;
  54. static UWORD AssCnt1ms;
  55. static UWORD ass_pvt_uwSmoothFlg = 0;
  56. /******************************
  57. *
  58. * Function
  59. *
  60. ******************************/
  61. /**
  62. * @brief Three order polynomial Y = a*X^3 + b*X^2 + c*x +d
  63. *
  64. * @param coef polynomial coefficient a, b, c, d
  65. * @param Value polynomial input value X
  66. * @param Qnum polynomial input Q type
  67. * @return UWORD polynomial output Y
  68. */
  69. static SLONG ass_slPolynomial(const POLY_COEF *coef, const SWORD *value, UWORD Qnum)
  70. {
  71. SLONG out;
  72. SLONG temp_a, temp_b, temp_c;
  73. /* out = a * x ^ 3 + b * x ^ 2 + c * x + d */
  74. temp_a = (SLONG)((((((SQWORD)coef->a * *value >> 12) * *value) >> (SWORD)Qnum) * *value) >> (SWORD)Qnum); // Qx+Q12-Q12+Qx-Qx+Qx-Qx=Qx
  75. temp_b = (SLONG)((((SQWORD)coef->b * *value >> 12) * *value) >> (SWORD)Qnum); // Qx+Q12-Q12+Qx-Qx=Qx
  76. temp_c = (SLONG)((SQWORD)coef->c * *value >> 12); // Qx+Q12-Q12=Qx
  77. out = temp_a + temp_b + temp_c + coef->d;
  78. out = (SLONG)out;
  79. return out;
  80. }
  81. /**
  82. * @brief Y = z*(x-h)^2 + k to Y = a*X^3 + b*X^2 + c*x +d
  83. *
  84. * @param coef original point coefficient z, h, k
  85. * @return POLY_COEF a, b, c, d
  86. */
  87. //static POLY_COEF ass_stPolynomialcenter(ORIG_COEF *coef)
  88. //{
  89. // POLY_COEF out;
  90. // /* a = 0; b = z; c = -2ha; d= ah^2 +k*/
  91. // out.a = (SQWORD)0; // Q12
  92. // out.b = (SQWORD)coef->z; // Q12
  93. // out.c = -(((SQWORD)2 * coef->h * coef->z) >> 12); // Q12
  94. // out.d = (((((SQWORD)coef->z * coef->h) >> 12) * coef->h) >> 12) + (SLONG)coef->k; // Q12
  95. // return out;
  96. //}
  97. /**
  98. * @brief Torque to Current when Id = 0;
  99. * Te = 1.5p*iq*fai -> iq = te/(1.5*p*fai)
  100. * @param coef polynomial coefficient a, b, c, d
  101. * @param Value polynomial input value X
  102. * @param Qnum polynomial input Q type
  103. * @return UWORD polynomial output Y
  104. */
  105. static SWORD ass_swTorq2CurPu(SWORD Tor)
  106. {
  107. SWORD swCurrentPu;
  108. SWORD swMotorTorqueNotPu;
  109. swMotorTorqueNotPu = (SWORD)(((SLONG)Tor * (SWORD)TORQUEBASE / (SWORD)ass_stParaCong.uwMechRationMotor) >> 7); // Q14-Q7 = Q7 0.1Nm Not Pu
  110. swCurrentPu = (SWORD)(((SLONG)swMotorTorqueNotPu * ass_Tor2CurCalCoef.swCalCoefINV) * 10 / IBASE); // Q7+Q7 = Q14; 0.1Nm/0.01A
  111. return swCurrentPu;
  112. }
  113. /**
  114. * @brief
  115. *
  116. * @param
  117. * @return
  118. */
  119. static void ass_voAssistModeSelect(void) // 上电运行一次or助力参数更新后,AssistCoef需要重新计算
  120. {
  121. UWORD TempAssit;
  122. UWORD TempGear, gear;
  123. // if (ass_stParaSet.uwAsssistSelectNum == 1) // OBC:更换成EE参数
  124. // {
  125. // TempAssit = ass_stParaCong.uwAssistSelect1;
  126. // }
  127. // else if (ass_stParaSet.uwAsssistSelectNum == 2)
  128. // {
  129. // TempAssit = ass_stParaCong.uwAssistSelect2;
  130. // }
  131. // else
  132. // {
  133. // TempAssit = ASSISTMOD_SELECT_DEFAULT;
  134. // }
  135. if (ass_stParaCong.uwStartMode == 1) // OBC:更换成EE参数
  136. {
  137. TempAssit = ASSISTMOD_SELECT_DEFAULT;
  138. }
  139. else if (ass_stParaCong.uwStartMode == 2)
  140. {
  141. TempAssit = ass_stParaCong.uwAssistSelect1;
  142. }
  143. else if (ass_stParaCong.uwStartMode == 3)
  144. {
  145. TempAssit = ass_stParaCong.uwAssistSelect2;
  146. }
  147. else
  148. {
  149. TempAssit = ASSISTMOD_SELECT_DEFAULT;
  150. }
  151. SLONG slTorqGainSum =0;
  152. for(UWORD i = 0; i < 4; i++)
  153. {
  154. slTorqGainSum += flash_stPara.slTorqAssGain[0][i];
  155. }
  156. if(slTorqGainSum == 0)
  157. {
  158. SLONG slTorqAssGain[15][4] = TORQUE_ASSIST_DEFAULT;
  159. SLONG slCadAssGain[5][4] = CADENCE_ASSIST_DEFAULT;
  160. UWORD a ;
  161. a =sizeof( slTorqAssGain);
  162. memcpy(&flash_stPara.slTorqAssGain[0], &slTorqAssGain[0], sizeof(slTorqAssGain));
  163. memcpy(&flash_stPara.slCadAssGain[0], &slCadAssGain[0], sizeof(slCadAssGain));
  164. }
  165. for (gear = 0; gear < 5; gear++)
  166. {
  167. TempGear = gear * 3 + ((TempAssit >> (UWORD)(gear << 1)) & 0x0003);
  168. memcpy(&ass_stCalCoef.uwTorqueAssGain[(gear + 1)], &flash_stPara.slTorqAssGain[TempGear], sizeof(POLY_COEF));
  169. }
  170. memcpy(&ass_stCalCoef.uwCadencAsseGain[1], &flash_stPara.slCadAssGain[0], sizeof(flash_stPara.slCadAssGain));
  171. }
  172. /**
  173. * @brief Para from EE Init
  174. *
  175. * @param void
  176. * @return void
  177. */
  178. void ass_voAssitEEInit(void)
  179. {
  180. ass_stParaCong.uwWheelPerimeter = BIKE_WHEEL_PERIMETER; // Q0 0.1CM
  181. ass_stParaCong.uwCadPulsePerCirc = CADENCE_PULSES_PER_CIRC;
  182. ass_stParaCong.uwMechRationMotor = 35; // Q0
  183. ass_stParaCong.uwAssistMaxSpdKmH = BIKE_SPEED_THROTTLE_MAX;
  184. ass_stParaCong.uwThrottleMaxSpdKmH = BIKE_SPEED_WALK_MAX;
  185. ass_stParaCong.uwNmFrontChainring = BIKE_FRONTTEETH_NMB; // front gear
  186. ass_stParaCong.uwNmBackChainring = BIKE_BACKTEETH_NMB; // min number of back gear
  187. ass_stParaCong.uwAssistSelect1 = BIKE_ASSIST_MODE1;
  188. ass_stParaCong.uwAssistSelect2 = BIKE_ASSIST_MODE2;
  189. ass_stParaCong.uwLightVoltage = BIKE_LIGHT_PARA;
  190. ass_stParaCong.swDeltPerimeter = BIKE_WHEEL_SIZE_ADJUST;
  191. ass_stParaCong.uwStartMode = BIKE_START_MODE;
  192. ass_stParaCong.uwAutoPowerOffTime = BIKE_POWER_PARA;
  193. ass_stParaCong.uwThrottleSmooth = BIKE_THROTTLE_SMOOTH;
  194. ass_stParaSet.uwStartupCoef = 8194; // Q12 percentage Min 1-4096 1.5-6144
  195. ass_stParaSet.uwStartupCruiseCoef = 4096; // Q12 percentage Min 1-4096 1-6144
  196. ass_stParaSet.uwAssistStartNm = TORQUE_START_THRESHOLD;
  197. ass_stParaSet.uwAssistStopNm = TORQUE_STOP_THRESHOLD;
  198. ass_stParaSet.uwStartUpGainStep = 25;
  199. ass_stParaSet.uwStartUpCadNm = CADENCE_NUMBERS_PULSES >> 1; // 0.5 circle
  200. ass_stParaSet.uwTorLPFCadNm = CADENCE_NUMBERS_PULSES >> 1; // 0.5 circle
  201. ass_stParaSet.uwSpeedAssistSpdRpm = BIKE_SPD_MOTOR_CONSTANT_COMMAND;
  202. ass_stParaSet.uwSpeedAssistIMaxA = BIKE_SPD_MOTOR_CURRENT_MAX;
  203. ass_stParaSet.uwAssistLimitBikeSpdStart = BIKE_SPEED_IQLIMIT_THRESHOLD1;
  204. ass_stParaSet.uwAssistLimitBikeSpdStop = BIKE_SPEED_IQLIMIT_THRESHOLD2;
  205. ass_stParaSet.uwCadenceWeight = 1229; // Q12 percentage
  206. ass_stParaSet.uwTorWeight = Q12_1 ; // Q12 percentage
  207. ass_stParaSet.uwTorAssAjstGain = 4096; // Q12 percentage
  208. ass_stParaSet.uwCadenceAssAjstGain = 4096; // Q12 percentage
  209. ass_stParaSet.uwAsssistSelectNum = 1;
  210. ass_stParaSet.uwSpdRegion[0] = 8192; // Q15 1500rpm
  211. ass_stParaSet.uwSpdRegion[1] = 16384; // Q15 3000rpm
  212. ass_stParaSet.uwSpdRegion[2] = 21845; // Q15 4000rpm
  213. ass_stParaSet.uwSpdRegionGain[0] = 4094;
  214. ass_stParaSet.uwSpdRegionGain[1] = 4094;
  215. ass_stParaSet.uwSpdRegionGain[2] = 4094;
  216. /* 函数私有变量初始化 */
  217. ass_pvt_swVoltCnt = 0;
  218. ass_pvt_uwTorqAccCnt = 0;
  219. ass_pvt_uwTorqDecCnt = 0;
  220. ass_pvt_uwSpd2TorqCnt = 0;
  221. ass_pvt_uwSmoothFlg = 0;
  222. AssCnt1ms = 0;
  223. }
  224. /**
  225. * @brief Three order polynomial Y = a*X^3 + b*X^2 + c*x +d
  226. *
  227. * @param coef polynomial coefficient a, b, c, d
  228. * @param Value polynomial input value X
  229. * @param Qnum polynomial input Q type
  230. * @return UWORD polynomial output Y
  231. */
  232. LPF_OUT ass_pvt_stCurLpf;
  233. void ass_voAssitCoef(void)
  234. {
  235. /*状态机初始化*/
  236. Ass_FSM = StopAssit;
  237. /*电机限制初始化*/
  238. ass_stParaCong.uwCofCurMaxPu = ((ULONG)cp_stMotorPara.swIpeakMaxA << 14) / IBASE; // Q14
  239. ass_stParaCong.uwMotorPoles = cp_stMotorPara.swMotrPolePairs;
  240. ass_stParaCong.uwCofTorMaxPu = (UWORD)(((ULONG)cp_stMotorPara.swTorMax << 14) / TORQUEBASE); // Q14
  241. ass_stParaCong.uwBikeAssTorMaxPu = ass_stParaCong.uwCofTorMaxPu * ass_stParaCong.uwMechRationMotor; // Q14;
  242. /*速度环参数初始化*/
  243. asr_stTorqSpdPICoefIn.uwUbVt = VBASE;
  244. asr_stTorqSpdPICoefIn.uwIbAp = IBASE;
  245. asr_stTorqSpdPICoefIn.uwFbHz = FBASE;
  246. asr_stTorqSpdPICoefIn.uwFTbsHz = EVENT_1MS_HZ;
  247. asr_stTorqSpdPICoefIn.uwPairs = cp_stMotorPara.swMotrPolePairs;
  248. asr_stTorqSpdPICoefIn.uwMtJm = cp_stMotorPara.swJD;
  249. asr_stTorqSpdPICoefIn.uwMtFlxWb = cp_stMotorPara.swFluxWb;
  250. asr_stTorqSpdPICoefIn.uwMcoef = 5;//cp_stControlPara.swAsrPIM;
  251. asr_stTorqSpdPICoefIn.uwWvcHz = 10;//cp_stControlPara.swAsrPIBandwidth;
  252. asr_stTorqSpdPICoefIn.uwRatioJm = cp_stControlPara.swAsrSpdInerRate;
  253. asr_voSpdPICoef(&asr_stTorqSpdPICoefIn, &asr_stTorqSpdPICoef);
  254. /*电流限幅计算*/
  255. ass_stCurLimCalBMSCoef.uwIqLimitInitAbs = ass_stParaCong.uwCofCurMaxPu; // Q14
  256. ass_stCurLimCalBMSCoef.uwIqLimitStartSoc = 20;
  257. ass_stCurLimCalBMSCoef.uwIqLimitEndSoc = 0;
  258. ass_stCurLimCalBMSCoef.swIqLImitK =
  259. (SWORD)ass_stCurLimCalBMSCoef.uwIqLimitInitAbs / ((SWORD)ass_stCurLimCalBMSCoef.uwIqLimitStartSoc - (SWORD)ass_stCurLimCalBMSCoef.uwIqLimitEndSoc);
  260. /*助力曲线初始化*/
  261. ass_voAssistModeSelect();
  262. /*助力启动阈值初始化*/
  263. ass_stCalCoef.uwAssThreshold = (UWORD)(((ULONG)ass_stParaSet.uwAssistStartNm << 14) / TORQUEBASE); // Q14
  264. ass_stCalCoef.uwAssStopThreshold = (UWORD)(((ULONG)ass_stParaSet.uwAssistStopNm << 14) / TORQUEBASE); // Q14;
  265. /*助力系数初始化*/
  266. ass_stCalCoef.StartFlag = 0;
  267. ass_stCalCoef.swSmoothGain = 0; // Q12
  268. ass_stCalCoef.swSmoothStopGain = 4096; //Q12
  269. ass_stCalCoef.uwStartUpTargetGain = 0; // Q12
  270. ass_stCalCoef.uwStartUpGainAddStep = ass_stParaSet.uwStartUpGainStep; // 25 Q12
  271. /*设置启动到正常助力最少踏频数*/
  272. ass_stCalCoef.uwStartUpTimeCadenceCnt = ass_stParaSet.uwStartUpCadNm;
  273. if (ass_stCalCoef.uwStartUpTimeCadenceCnt < (CADENCE_NUMBERS_PULSES >> 3))
  274. {
  275. ass_stCalCoef.uwStartUpTimeCadenceCnt = CADENCE_NUMBERS_PULSES >> 3;
  276. }
  277. if (ass_stCalCoef.uwStartUpTimeCadenceCnt > CADENCE_NUMBERS_PULSES)
  278. {
  279. ass_stCalCoef.uwStartUpTimeCadenceCnt = CADENCE_NUMBERS_PULSES;
  280. }
  281. /*设置滑动平均滤波踏频数*/
  282. ass_stTorqMafValue.uwLength = ass_stParaSet.uwTorLPFCadNm;
  283. ass_stCalCoef.swCadanceGain = 0;
  284. ass_stCalCoef.uwSwitch1TorqThreshold = ass_stCalCoef.uwAssStopThreshold;//((ULONG)TORQUE_SWITCH1_THRESHOLD << 14) / TORQUEBASE;
  285. ass_stCalCoef.uwSwitch2TorqThreshold = ass_stCalCoef.uwAssThreshold;//((ULONG)TORQUE_SWITCH2_THRESHOLD << 14) / TORQUEBASE;
  286. ass_stCalCoef.ulStartupDeltInv = ((ULONG)1 << 28) / (ass_stCalCoef.uwSwitch2TorqThreshold - ass_stCalCoef.uwSwitch1TorqThreshold); // Q14;
  287. /*初始化计数*/
  288. ass_stCalCoef.uwCadencePeriodCNT = 0;
  289. ass_stCalCoef.swCadanceCNT = 0;
  290. ass_stCalCoef.sw2StopCNT = 0;
  291. ass_stCalCoef.swAss2SpdCNT = 0;
  292. /*配置速度环参数*/
  293. ass_stCalCoef.uwSpeedConstantCommand = (UWORD)(((ULONG)ass_stParaSet.uwSpeedAssistSpdRpm << 15) / ((ULONG)FBASE * 60 / ass_stParaCong.uwMotorPoles));
  294. ass_stCalCoef.swSpdLoopAbsCurMax = ((SWORD)ass_stParaSet.uwSpeedAssistIMaxA << 14) / IBASE;
  295. ass_stCalCoef.swSpeedlimtrpm = -100;
  296. ass_stCalCoef.swBikeSpeedGain = 0;
  297. /*设置电流限幅*/
  298. ass_stCalCoef.uwCurrentMaxPu = ass_stParaCong.uwCofCurMaxPu;
  299. ass_stCalCoef.swCurrentmax_torAssPu = (SWORD)(((SLONG)ass_stCalCoef.uwCurrentMaxPu * (SWORD)ass_stParaSet.uwTorWeight) >> 12); // Q14
  300. ass_stCalCoef.swCurrentmax_cadAssPu = (SWORD)(((SLONG)ass_stCalCoef.uwCurrentMaxPu * (SWORD)ass_stParaSet.uwCadenceWeight )>> 12); // Q14
  301. /*初始化标志*/
  302. ass_stCalCoef.blAssistflag = FALSE;
  303. ass_stCalOut.swTorAssistSum1 = 0;
  304. ass_stCalOut.swTorAssistSum2 = 0;
  305. ass_stCalOut.swTorAss2CurrentTemp = 0;
  306. ass_stCalOut.swCadAss2CurrentTemp = 0;
  307. ass_stCalOut.swTorAssistCurrentTemp = 0;
  308. ass_stCalOut.swTorSpdLoopCurrentTemp = 0;
  309. ass_stCalOut.swTorAssistCurrent = 0;
  310. ass_stCalOut.swSpeedRef = 0;
  311. ass_stCalOut.swCadSpd2MotSpd = 0;
  312. ass_stCurLimCoef.uwLimitGain[0] = 0; // Q10 percentage of max Current
  313. ass_stCurLimCoef.uwLimitGain[1] = 400;
  314. ass_stCurLimCoef.uwLimitGain[2] = 682;
  315. ass_stCurLimCoef.uwLimitGain[3] = 910;
  316. ass_stCurLimCoef.uwLimitGain[4] = 1024;
  317. ass_stCurLimCoef.uwLimitGain[5] = 1024;
  318. ass_stCurLimCoef.uwSpdThresHold = 21845;
  319. /*设置车速限幅*/
  320. ass_stCurLimCoef.uwBikeSpdThresHold1 = (UWORD)(((UQWORD)1000 << 20) * ass_stParaSet.uwAssistLimitBikeSpdStart /
  321. ((UQWORD)36 * (ass_stParaCong.uwWheelPerimeter + (UWORD)ass_stParaCong.swDeltPerimeter) * FBASE)); // Q20 3216 = Q10(3.1415926)
  322. ass_stCurLimCoef.uwBikeSpdThresHold2 = (UWORD)(((UQWORD)1000 << 20) * ass_stParaSet.uwAssistLimitBikeSpdStop /
  323. ((UQWORD)36 * (ass_stParaCong.uwWheelPerimeter + (UWORD)ass_stParaCong.swDeltPerimeter) * FBASE)); // Q20 3216 = Q10(3.1415926)
  324. ass_stCurLimCoef.ulBikeSpdDeltInv = (ULONG)(((UQWORD)1 << 20) / (ass_stCurLimCoef.uwBikeSpdThresHold2 - ass_stCurLimCoef.uwBikeSpdThresHold1)); // Q20;
  325. ass_stCurLimCoef.uwBikeSpdIqLimitK =
  326. (UWORD)((((ULONG)ass_stCurLimCoef.uwBikeSpdThresHold2 - ass_stCurLimCoef.uwBikeSpdThresHold1) << 8) / ass_stParaCong.uwCofCurMaxPu); // Q28-q14 = Q14;
  327. /*设置转矩电流标定系数*/
  328. ass_Tor2CurCalCoef.uwMotorFluxWb = cp_stMotorPara.swFluxWb; // 0.001mWb
  329. ass_Tor2CurCalCoef.uwMotprPolePairs = ass_stParaCong.uwMotorPoles;
  330. ass_Tor2CurCalCoef.swCalCoefINV =
  331. (((SLONG)1 << 7) * 1000 * 1000) /
  332. (((SLONG)3 * ass_Tor2CurCalCoef.uwMotorFluxWb * ass_Tor2CurCalCoef.uwMotprPolePairs) >> 1); // Q7 Not Pu // 1/(1.5p*fai);
  333. mth_voLPFilterCoef(1000000 / 25, EVENT_1MS_HZ, &ass_pvt_stCurLpf.uwKx); //100Hz
  334. ass_pvt_stCurLpf.slY.sl = 0;
  335. }
  336. /**
  337. * @brief Three order polynomial Y = a*X^3 + b*X^2 + c*x +d
  338. *
  339. * @param coef polynomial coefficient a, b, c, d
  340. * @param Value polynomial input value X
  341. * @param Qnum polynomial input Q type
  342. * @return UWORD polynomial output Y
  343. */
  344. //void ass_voAssitTorqPIInit(void)
  345. //{
  346. // ass_stTorqPIOut.slIRefPu = 0;
  347. // ass_stTorqPIOut.swErrZ1Pu = 0;
  348. // ass_stTorqPIOut.swIRefPu = 0;
  349. //}
  350. //
  351. //void ass_voAssitTorqPI(const ASS_TORQ_PI_IN *in, ASS_TORQ_PI_OUT *out)
  352. //{
  353. // SLONG slErrPu, slDeltaErrPu;
  354. // SLONG slIpPu, slIiPu;
  355. // SLONG slImaxPu, slIminPu;
  356. // SQWORD sqIRefPu, sqIpPu;
  357. // UWORD uwKpPu = 5000, uwKitPu = 0; // uwKpPu(Q12), uwKitPu(Q15)
  358. //
  359. //// uwKpPu = ass_stParaSet.uwSpeedAssistIMaxA;
  360. //// uwKitPu = ass_stParaSet.uwStartUpCadNm;
  361. //
  362. // slImaxPu = (SLONG)in->swImaxPu << 15; // Q14+Q15=Q29
  363. // slIminPu = (SLONG)in->swIminPu << 15; // Q14+Q15=Q29
  364. //
  365. // slErrPu = in->swTorqRefPu - in->swTorqFdbPu; // Q14
  366. //
  367. // if (slErrPu > 32767)
  368. // {
  369. // slErrPu = 32767;
  370. // }
  371. // else if (slErrPu < -32768)
  372. // {
  373. // slErrPu = -32768;
  374. // }
  375. // else
  376. // {
  377. // /* Nothing */
  378. // }
  379. // //slDeltaErrPu = slErrPu - out->swErrZ1Pu; // Q14
  380. // slDeltaErrPu = slErrPu;
  381. // if (slDeltaErrPu > 32767)
  382. // {
  383. // slDeltaErrPu = 32767;
  384. // }
  385. // else if (slDeltaErrPu < -32768)
  386. // {
  387. // slDeltaErrPu = -32768;
  388. // }
  389. // else
  390. // {
  391. // /* Nothing */
  392. // }
  393. //
  394. // slIpPu = slDeltaErrPu * uwKpPu; // Q14+Q12=Q26
  395. // sqIpPu = (SQWORD)slIpPu << 3;
  396. //
  397. // slIiPu = slErrPu * uwKitPu; // Q14+Q15=Q29
  398. //
  399. // //sqIRefPu = sqIpPu + (SQWORD)slIiPu + (SQWORD)out->slIRefPu; // Q29
  400. // sqIRefPu = sqIpPu;
  401. //
  402. // if (sqIRefPu > slImaxPu)
  403. // {
  404. // out->slIRefPu = slImaxPu;
  405. // }
  406. // else if (sqIRefPu < slIminPu)
  407. // {
  408. // out->slIRefPu = slIminPu;
  409. // }
  410. // else
  411. // {
  412. // out->slIRefPu = sqIRefPu;
  413. // }
  414. // out->swIRefPu = out->slIRefPu >> 15; // Q29-Q15=Q14
  415. // out->swErrZ1Pu = (SWORD)slErrPu;
  416. //}
  417. SWORD swPreCurrentPu;
  418. SWORD startonce = 0;
  419. UWORD kcoef =4096; //Q10
  420. UWORD uwTempStopCnt,StopCad,swMotorCadspd;
  421. static void AssitCuvApplPerVolt(void) /* parasoft-suppress METRICS-28 "本项目圈复杂度无法更改,后续避免" */
  422. {
  423. SLONG slTeTorAssitTmpPu,slTeTorAssitLinerPu,slTeCadAssitTmpPu;
  424. SWORD swTeTorAssitPu1, swTeTorAssitPu2;
  425. SWORD swTeCadAssitPu1, swTeCadAssitPu2;
  426. SWORD swTmpSpdtoTorqCur;
  427. SLONG slTmpSmoothCur;
  428. SWORD swTorqCmd1, swTorqCmd, swCadCmd;
  429. UWORD uwTorqAccStep = 50,uwTorqDecStep = 80;
  430. SWORD swCurSwitch = 0;
  431. SWORD swTmpVoltPu,swTmpVoltPu2;
  432. SLONG slSpdErr,slTmpVoltLim, slPreSpderror;
  433. SWORD swSpdKpPu = 500; //Q10
  434. UWORD uwVoltAccStep = 1, uwVoltDecStep = 3;
  435. SWORD swSpderror;
  436. // SLONG slTmp_a1, slTmp_b1, slTmp_c1;
  437. /* Select Torq Growth Rate by Bike Gear */
  438. if (ass_stCalIn.uwGearSt == 1)
  439. {
  440. uwTorqAccStep = 50;
  441. }
  442. else if(ass_stCalIn.uwGearSt == 2)
  443. {
  444. uwTorqAccStep = 100;
  445. }
  446. else if(ass_stCalIn.uwGearSt == 3)
  447. {
  448. uwTorqAccStep = 120;
  449. }
  450. else if(ass_stCalIn.uwGearSt == 4)
  451. {
  452. uwTorqAccStep = 150;
  453. }
  454. else if(ass_stCalIn.uwGearSt == 5)
  455. {
  456. uwTorqAccStep = 150;
  457. }
  458. else
  459. {
  460. //do nothing
  461. }
  462. uwTorqDecStep = 80;
  463. AssCnt1ms ++;
  464. if(AssCnt1ms >= 10000)
  465. {
  466. AssCnt1ms = 0;
  467. }
  468. /* Select TorqRef: LPFTorq or MAFTorq */
  469. swTorqCmd1 = (SWORD)(((SLONG)ass_stCalIn.uwtorque * ass_stCalCoef.swTorqFilterGain >> 14) +
  470. ((SLONG)ass_stCalIn.uwtorquelpf * (Q14_1 - ass_stCalCoef.swTorqFilterGain) >> 14)); //转矩指令滤波切换,由低通滤波到踏频相关的滑动平均滤波
  471. swTorqCmd = (SWORD)(((SLONG)swTorqCmd1 * ass_stCalCoef.swSmoothGain) >> 12); //转矩指令斜坡
  472. if (swTorqCmd > ass_stParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
  473. {
  474. swTorqCmd = (SWORD)ass_stParaCong.uwBikeAssTorMaxPu;
  475. }
  476. if(swTorqCmd < ass_stCalCoef.uwSwitch1TorqThreshold )
  477. {
  478. ass_stCalCoef.swCadanceGain = 0; //Q12
  479. }
  480. else if(swTorqCmd >= ass_stCalCoef.uwSwitch1TorqThreshold && swTorqCmd < ass_stCalCoef.uwSwitch2TorqThreshold)
  481. {
  482. ass_stCalCoef.swCadanceGain = (((ULONG)swTorqCmd - (ULONG)ass_stCalCoef.uwSwitch1TorqThreshold) * ass_stCalCoef.ulStartupDeltInv )>>16;
  483. }
  484. else
  485. {
  486. ass_stCalCoef.swCadanceGain = 4096; //Q12
  487. }
  488. /* Assist torque Cal using Assist Curve */
  489. slTeTorAssitTmpPu = (SLONG)(ass_slPolynomial(&ass_stCalCoef.uwTorqueAssGain[ass_stCalIn.uwGearSt], &swTorqCmd, 14)); // Q14 转矩助力曲线
  490. if(ass_stCalIn.uwGearSt == 5)
  491. {
  492. slTeTorAssitLinerPu = (((SLONG)swTorqCmd * LinerAssist[ass_stCalIn.uwGearSt-1] )>> 12) + 273; // Q14 转矩助力曲线线性段
  493. }
  494. else
  495. {
  496. slTeTorAssitLinerPu = (((SLONG)swTorqCmd * LinerAssist[ass_stCalIn.uwGearSt-1] )>> 12) + 273;
  497. }
  498. if (slTeTorAssitTmpPu < slTeTorAssitLinerPu)
  499. {
  500. slTeTorAssitTmpPu = slTeTorAssitLinerPu;
  501. }
  502. else
  503. {
  504. //do nothing;
  505. }
  506. swCadCmd = (SWORD)((((SLONG)ass_stCalIn.uwcadance * ass_stCalCoef.swSmoothGain) >> 12) * 10); // 踏频指令斜坡
  507. slTeCadAssitTmpPu = ((SLONG)(ass_slPolynomial(&ass_stCalCoef.uwCadencAsseGain[ass_stCalIn.uwGearSt], &swCadCmd, 20))) >> 6; // Q20 - Q6 = Q14 //踏频助力曲线
  508. if (slTeTorAssitTmpPu > ass_stParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
  509. {
  510. slTeTorAssitTmpPu = ass_stParaCong.uwBikeAssTorMaxPu;
  511. }
  512. if (slTeCadAssitTmpPu > ass_stParaCong.uwBikeAssTorMaxPu) // 最大转矩限幅
  513. {
  514. slTeCadAssitTmpPu = ass_stParaCong.uwBikeAssTorMaxPu;
  515. }
  516. /* Select Assist Percent of Torq and Candence*/
  517. swTeTorAssitPu1 = (SWORD)(slTeTorAssitTmpPu * (SLONG)ass_stParaSet.uwTorAssAjstGain >> 12); // Q14+Q12-Q12 = Q14; 用户可设置转矩比例
  518. swTeCadAssitPu1 = (SWORD)(slTeCadAssitTmpPu * (SLONG)ass_stParaSet.uwCadenceAssAjstGain >> 12); // Q14+Q12-Q12 = Q14; 用户可设置踏频比例
  519. ass_stCalOut.swTorAssistSum1 = swTeTorAssitPu1 + swTeCadAssitPu1; // Q14
  520. /* Candance Speed to Motor Speed*/
  521. ass_stCalOut.swCadSpd2MotSpd =
  522. (SWORD)(((SLONG)ass_stCalIn.uwcadance * (SWORD)ass_stParaCong.uwMechRationMotor * (SWORD)ass_stParaCong.uwMotorPoles) >> 5); // Q20-Q5= Q15 出力时电机转速计算
  523. ass_stCalCoef.uwCadencePeriodCNT = (UWORD)(TIME_MS2CNT(((ULONG)1000 << 20) / ((ULONG)ass_stCalIn.uwcadance * FBASE))); //一圈踏频时间计数
  524. /* Back EMF Cal */
  525. swTmpVoltPu = (SWORD)((SLONG)ass_stCalOut.swCadSpd2MotSpd * (SLONG)cof_uwFluxPu >> 13);//Q15+Q12-Q13=Q14;
  526. swTmpVoltPu2 = (SWORD)((SLONG)ass_stCalIn.uwSpdFbkAbsPu* (SLONG)cof_uwFluxPu >> 13);//Q15+Q12-Q13=Q14;
  527. if (swTmpVoltPu < swTmpVoltPu2)
  528. {
  529. swTmpVoltPu = swTmpVoltPu2;
  530. }
  531. swMotorCadspd =(SWORD)((SLONG)(ass_stCalIn.uwSpdFbkAbsPu << 15)/(ass_stParaCong.uwMechRationMotor * ass_stParaCong.uwMotorPoles)>>10);
  532. swSpderror = swMotorCadspd - 300;//(SWORD)ass_CalIn.uwcadance;
  533. if(swSpderror < 200)
  534. {
  535. StopCad = 200;
  536. }
  537. else
  538. {
  539. StopCad = swSpderror;
  540. }
  541. ass_stCalCoef.uwStartupGain = ass_stParaSet.uwStartupCoef ; //零速启动助力比计算
  542. ass_stCalCoef.uwStartupCruiseGain = ass_stParaSet.uwStartupCruiseCoef ; //带速启动助力比计算
  543. swPreCurrentPu = (SWORD)(((SLONG)swPreCurrentPu * 990 )>>10);
  544. /* Assist FSM Control */
  545. switch (Ass_FSM)
  546. {
  547. case Prepare:
  548. UWORD tmpKp = 4000;//ass_ParaSet.uwStartUpCadNm ; //Q10
  549. slPreSpderror = (((SLONG)ass_stCalOut.swCadSpd2MotSpd * 800 )>>10) - ass_stCalIn.uwSpdFbkAbsPu; //Q15
  550. swPreCurrentPu = (slPreSpderror * tmpKp )>> 11; //Q14
  551. if(swPreCurrentPu < 0)
  552. {
  553. swPreCurrentPu = 0;
  554. }
  555. if((ass_stCalCoef.swAss2SpdCNT > 3000) || (ass_stCalIn.uwcadance == 0) || ((ass_stCalIn.uwcadance < StopCad) && (ass_stCalIn.uwtorquePer <= ass_stCalCoef.uwAssStopThreshold))) //踏频小于0.1 hz
  556. {
  557. Ass_FSM = StopAssit;
  558. ass_stCalCoef.swAss2SpdCNT = 0;
  559. }
  560. else
  561. {
  562. ass_stCalCoef.swAss2SpdCNT++;
  563. }
  564. if((ass_stCalIn.uwtorquePer > ((ass_stCalCoef.uwAssThreshold * 5) >> 3)) && (ass_stCalIn.uwcadance > 300)
  565. && ((BikeBrake_blGetstate() | bikegearsensor_blBikeGetState()) == FALSE))
  566. {
  567. Ass_FSM = Startup;
  568. ass_stCalCoef.swAss2SpdCNT=0;
  569. startonce = 0;
  570. }
  571. break;
  572. case Startup:
  573. /* 启动系数 */
  574. if(ass_pvt_uwSmoothFlg == 0)
  575. {
  576. ass_stCalCoef.swSmoothGain += ass_stParaSet.uwSpeedAssistIMaxA; ////ass_stCalCoef.uwStartUpGainAddStep;
  577. if(ass_stCalCoef.swSmoothGain >= ass_stParaSet.uwStartupCoef)
  578. {
  579. ass_pvt_uwSmoothFlg = 1;
  580. }
  581. }
  582. else if (ass_pvt_uwSmoothFlg == 1)
  583. {
  584. if(ass_stCalCoef.swSmoothGain >= Q12_1)
  585. {
  586. ass_stCalCoef.swSmoothGain -= (ass_stParaSet.uwSpeedAssistIMaxA >> 1);
  587. }
  588. else
  589. {
  590. ass_stCalCoef.swSmoothGain = Q12_1;
  591. Ass_FSM = TorqueAssit;
  592. ass_stCalCoef.StartFlag=0;
  593. ass_pvt_uwSmoothFlg = 2;
  594. }
  595. }
  596. else
  597. {
  598. // do nothing
  599. }
  600. // swSpdKpPu = 1000; //ass_stParaSet.uwStartUpCadNm;
  601. // slSpdErr = (SLONG)ass_stCalOut.swCadSpd2MotSpd - (SLONG)ass_stCalIn.uwSpdFbkAbsPu;
  602. // if(slSpdErr < 0)
  603. // {
  604. // slSpdErr = 0;
  605. // }
  606. //// ass_stCalCoef.StartFlag = 1;
  607. // /* Open Voltage Limit according SpdErr*/
  608. // if(ass_stCalCoef.StartFlag == 0)
  609. // {
  610. // slTmpVoltLim= ((slSpdErr * swSpdKpPu )>> 11) + swTmpVoltPu;
  611. // if(slTmpVoltLim > scm_swVsDcpLimPu)
  612. // {
  613. // slTmpVoltLim = scm_swVsDcpLimPu;
  614. // }
  615. // else if(slTmpVoltLim <= swTmpVoltPu)
  616. // {
  617. // slTmpVoltLim = swTmpVoltPu;
  618. // }
  619. // else
  620. // {
  621. // //do nothing
  622. // }
  623. // ass_stCalOut.swVoltLimitPu = (SWORD)slTmpVoltLim;
  624. //
  625. // /* 电机与踏频转速差小于阈值启动完成 */
  626. // if(slSpdErr <= 1000)
  627. // {
  628. // ass_stCalCoef.StartFlag = 1;
  629. // }
  630. // /* 根据电流环饱和情况判断启动完成 */
  631. //// if(ABS(scm_swIqRefPu- scm_swIqFdbLpfPu) > 200)
  632. //// {
  633. //// ass_pvt_swVoltCnt++;
  634. //// }
  635. //// else
  636. //// {
  637. //// ass_pvt_swVoltCnt=0;
  638. //// }
  639. //// if(ass_pvt_swVoltCnt > 10)
  640. //// {
  641. //// ass_pvt_swVoltCnt=0;
  642. //// ass_stCalCoef.StartFlag = 1;
  643. //// }
  644. // }
  645. // else if(ass_stCalCoef.StartFlag ==1 )
  646. // {
  647. // if(0 == (AssCnt1ms%5))
  648. // {
  649. // ass_stCalOut.swVoltLimitPu += ass_stCalCoef.uwStartUpGainAddStep;
  650. // if (ass_stCalOut.swVoltLimitPu > scm_swVsDcpLimPu)
  651. // {
  652. // ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
  653. // }
  654. // }
  655. //
  656. // if(slSpdErr <= 100)
  657. // {
  658. // ass_pvt_swVoltCnt++;
  659. // }
  660. // else
  661. // {
  662. // ass_pvt_swVoltCnt--;
  663. // if(ass_pvt_swVoltCnt < 0)
  664. // {
  665. // ass_pvt_swVoltCnt = 0;
  666. // }
  667. // }
  668. // /* Switch to TorqueAssit FSM */
  669. // if(ass_pvt_swVoltCnt > 30)
  670. // {
  671. // Ass_FSM = TorqueAssit;
  672. // ass_stCalCoef.StartFlag=0;
  673. // ass_pvt_swVoltCnt=0;
  674. // }
  675. // }
  676. // else
  677. // {
  678. // //do nothing
  679. // }
  680. /* Switch to ReduceCurrent FSM */
  681. if(((ass_stCalIn.uwcadance < StopCad) && (ass_stCalIn.uwtorquePer <= ass_stCalCoef.uwAssStopThreshold)) || (ass_stCalIn.uwcadance == 0) || (ass_stCalIn.uwGearSt == 0) || (BikeBrake_blGetstate() == TRUE) || (bikegearsensor_blBikeGetState() == TRUE))
  682. {
  683. /* When CandanceFreq=0 or BikeGear=0*/
  684. ass_stCalCoef.swAss2SpdCNT = 0;
  685. Ass_FSM = ReduceCurrent;
  686. }
  687. else if(ass_stCalIn.uwtorquePer <= (ass_stCalCoef.uwAssStopThreshold))
  688. {
  689. ass_stCalCoef.swAss2SpdCNT++;
  690. uwTempStopCnt = ((ULONG)1*100 << 14)/ ass_stCalIn.uwcadance;//((ULONG)1000<<20)/(ass_CalIn.uwcadance * FBASE) ;
  691. if(uwTempStopCnt < 300)
  692. {
  693. uwTempStopCnt = 300;
  694. }
  695. else if(uwTempStopCnt > 2000)
  696. {
  697. uwTempStopCnt = 2000;
  698. }
  699. if(ass_stCalCoef.swAss2SpdCNT > uwTempStopCnt)
  700. {
  701. Ass_FSM = ReduceCurrent;
  702. ass_stCalCoef.swAss2SpdCNT = 0;
  703. ass_stCalCoef.StartFlag=0;
  704. }
  705. }
  706. else
  707. {
  708. ass_stCalCoef.swAss2SpdCNT = 0;
  709. }
  710. break;
  711. case TorqueAssit:
  712. /* 启动系数 */
  713. if(ass_pvt_uwSmoothFlg == 0)
  714. {
  715. ass_stCalCoef.swSmoothGain += ass_stParaSet.uwSpeedAssistIMaxA; ////ass_stCalCoef.uwStartUpGainAddStep;
  716. if(ass_stCalCoef.swSmoothGain >= ass_stParaSet.uwStartupCoef)
  717. {
  718. ass_pvt_uwSmoothFlg = 1;
  719. }
  720. }
  721. else if (ass_pvt_uwSmoothFlg == 1)
  722. {
  723. if(ass_stCalCoef.swSmoothGain >= Q12_1)
  724. {
  725. ass_stCalCoef.swSmoothGain -= (ass_stParaSet.uwSpeedAssistIMaxA >> 1);
  726. }
  727. else
  728. {
  729. ass_stCalCoef.swSmoothGain = Q12_1;
  730. ass_pvt_uwSmoothFlg = 2;
  731. }
  732. }
  733. else
  734. {
  735. // do nothing
  736. }
  737. /* Reduce Voltage Limit When LPFTorq < Switch1TorqThreshold */
  738. if(0 == (AssCnt1ms%5))
  739. {
  740. // if(ass_stCalIn.uwtorque >= ass_stCalCoef.uwSwitch1TorqThreshold)
  741. // {
  742. ass_stCalOut.swVoltLimitPu += ass_stCalCoef.uwStartUpGainAddStep;
  743. if (ass_stCalOut.swVoltLimitPu > scm_swVsDcpLimPu)
  744. {
  745. ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
  746. }
  747. // }
  748. // else if (ass_stCalIn.uwtorque <= ass_stCalCoef.uwSwitch1TorqThreshold)
  749. // {
  750. //// ass_stCalOut.swVoltLimitPu -= ass_stCalCoef.uwSpeedConstantCommand;
  751. //// if (ass_stCalOut.swVoltLimitPu <= (tmpVoltargetPu + ass_stParaSet.uwStartUpCadNm))
  752. //// {
  753. //// ass_stCalOut.swVoltLimitPu = tmpVoltargetPu + ass_stParaSet.uwStartUpCadNm;
  754. //// }
  755. // }
  756. // else
  757. // {
  758. // }
  759. }
  760. /* TorqueRef Select Coef */
  761. ass_stCalCoef.swTorqFilterGain += 4; // Q14 转矩滤波方式切换系数
  762. if (ass_stCalCoef.swTorqFilterGain > Q14_1)
  763. {
  764. ass_stCalCoef.swTorqFilterGain = Q14_1;
  765. }
  766. /* Switch to ReduceCurrent FSM */
  767. if(((ass_stCalIn.uwcadance < StopCad) && (ass_stCalIn.uwtorquePer <= ass_stCalCoef.uwAssStopThreshold)) || (ass_stCalIn.uwcadance == 0) || (ass_stCalIn.uwGearSt == 0) || (BikeBrake_blGetstate() == TRUE) || (bikegearsensor_blBikeGetState() == TRUE))
  768. {
  769. /* When CandanceFreq=0 or BikeGear=0*/
  770. ass_stCalOut.blTorqPIFlg = FALSE;
  771. ass_stCalCoef.swAss2SpdCNT = 0;
  772. Ass_FSM = ReduceCurrent;
  773. ass_stCalCoef.StartFlag = 0;
  774. }
  775. else if(ass_stCalIn.uwtorquePer <= (ass_stCalCoef.uwAssStopThreshold))
  776. {
  777. ass_stCalCoef.swAss2SpdCNT++;
  778. uwTempStopCnt = ((ULONG)1*100 << 14)/ ass_stCalIn.uwcadance;//((ULONG)1000<<20)/(ass_CalIn.uwcadance * FBASE) ;
  779. if(uwTempStopCnt < 300)
  780. {
  781. uwTempStopCnt = 300;
  782. }
  783. else if(uwTempStopCnt > 2000)
  784. {
  785. uwTempStopCnt = 2000;
  786. }
  787. if(ass_stCalCoef.swAss2SpdCNT > uwTempStopCnt)
  788. {
  789. ass_stCalCoef.swAss2SpdCNT = 0;
  790. ass_stCalOut.blTorqPIFlg = FALSE;
  791. Ass_FSM = ReduceCurrent;
  792. ass_stCalCoef.StartFlag = 0;
  793. }
  794. }
  795. else
  796. {
  797. ass_stCalCoef.swAss2SpdCNT = 0;
  798. }
  799. break;
  800. case ReduceCurrent:
  801. /* Switch to StopAssit FSM */
  802. if(ass_stCalCoef.swSmoothStopGain <= 0)
  803. {
  804. ass_pvt_uwSmoothFlg = 0;
  805. ass_stCalCoef.swSmoothGain = 0;
  806. ass_stCalCoef.swSmoothStopGain = 0;
  807. ass_stCalCoef.swTorqFilterGain = 0;
  808. ass_stCalCoef.swCadanceGain = 0;
  809. ass_voMoveAverageFilterClear(&ass_stTorqMafValue);
  810. Ass_FSM = StopAssit;
  811. }
  812. else
  813. {
  814. /* Reduce Curret Coef to Zero*/
  815. ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
  816. ass_stCalCoef.swSmoothGain -= ass_stCalCoef.uwStartUpGainAddStep;
  817. ass_stCalCoef.swSmoothStopGain -= ass_stCalCoef.uwStartUpGainAddStep;
  818. }
  819. /* Switch to Startup FSM */
  820. // if (ass_stCalIn.uwtorquePer > ((ass_stCalCoef.uwAssThreshold * 3)>>3) && ass_stCalIn.uwcadance > 0)
  821. // {
  822. // Ass_FSM = Startup;
  823. // ass_stCalOut.swSpeedRef = ass_stCalIn.uwSpdFbkAbsPu;
  824. // ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
  825. // }
  826. break;
  827. case StopAssit:
  828. if(ass_stCalIn.uwcadance == 0)
  829. {
  830. startonce = 0;
  831. }
  832. ass_stCalOut.swTorSpdLoopCurrentTemp = 0;
  833. /* Switch to Startup FSM */
  834. if ((BikeBrake_blGetstate() == FALSE) && (bikegearsensor_blBikeGetState() == FALSE) && (ass_stCalIn.uwGearSt > 0))
  835. {
  836. if ((ass_stCalIn.uwtorquePer > ass_stCalCoef.uwAssThreshold))
  837. {
  838. if (ass_stCalIn.uwcadance > 0)
  839. {
  840. ass_stCalCoef.sw2StopCNT = 0;
  841. ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
  842. ass_pvt_stCurLpf.slY.sw.hi = 0;
  843. ass_stCalCoef.swSmoothStopGain = Q12_1;
  844. ass_stCalCoef.swSmoothGain = ass_stParaSet.uwStartupCruiseCoef;
  845. Ass_FSM = Startup;
  846. }
  847. }
  848. else
  849. {
  850. if (ass_stCalIn.uwcadance > 300)
  851. {
  852. ass_stCalCoef.sw2StopCNT = 0;
  853. ass_stCalOut.swVoltLimitPu = scm_swVsDcpLimPu;
  854. ass_pvt_stCurLpf.slY.sw.hi = 0;
  855. ass_stCalCoef.swSmoothStopGain = Q12_1;
  856. ass_stCalCoef.swSmoothGain = ass_stParaSet.uwStartupCruiseCoef;
  857. Ass_FSM = Prepare;
  858. startonce = 1;
  859. }
  860. swPreCurrentPu = 0;
  861. }
  862. }
  863. /* Assit Exit */
  864. if (ass_stCalIn.uwcadance == 0 || ass_stCalIn.uwtorquelpf < ass_stCalCoef.uwAssStopThreshold)
  865. {
  866. ass_stCalCoef.sw2StopCNT++;
  867. }
  868. else
  869. {
  870. if (ass_stCalCoef.sw2StopCNT >= 1)
  871. {
  872. ass_stCalCoef.sw2StopCNT--;
  873. }
  874. }
  875. if ((ass_stCalCoef.sw2StopCNT > TIME_MS2CNT(3000)) || (ass_stCalIn.uwGearSt == 0) || (BikeBrake_blGetstate() == TRUE)|| (bikegearsensor_blBikeGetState() == TRUE))// 3s
  876. {
  877. ass_stCalCoef.sw2StopCNT = 0;
  878. ass_stCalCoef.blAssistflag = FALSE;
  879. }
  880. break;
  881. default:
  882. break;
  883. }
  884. /* Bikespeed Limit */
  885. if (ass_stCalIn.uwbikespeed <= ass_stCurLimCoef.uwBikeSpdThresHold1)
  886. {
  887. ass_stCalCoef.swBikeSpeedGain = Q12_1; // Q12
  888. }
  889. else if (ass_stCalIn.uwbikespeed > ass_stCurLimCoef.uwBikeSpdThresHold1 && ass_stCalIn.uwbikespeed <= ass_stCurLimCoef.uwBikeSpdThresHold2)
  890. {
  891. ass_stCalCoef.swBikeSpeedGain =
  892. (SWORD)(Q12_1 - ((((SQWORD)ass_stCalIn.uwbikespeed - (SQWORD)ass_stCurLimCoef.uwBikeSpdThresHold1) * (SQWORD)ass_stCurLimCoef.ulBikeSpdDeltInv) >> 8)); // Q12
  893. uwTorqAccStep = 10;
  894. uwTorqDecStep = 10;
  895. }
  896. else
  897. {
  898. ass_stCalCoef.swBikeSpeedGain = 0;
  899. uwTorqAccStep = 10;
  900. uwTorqDecStep = 10;
  901. }
  902. /* Assist Current Output in each FSM */
  903. switch (Ass_FSM)
  904. {
  905. case Prepare:
  906. ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection * swPreCurrentPu;
  907. break;
  908. case Startup:
  909. swTeTorAssitPu2 = swTeTorAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
  910. swTeCadAssitPu2 = swTeCadAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
  911. ass_stCalOut.swTorAss2CurrentTemp = ass_swTorq2CurPu(swTeTorAssitPu2); // Q14 电流指令计算
  912. ass_stCalOut.swCadAss2CurrentTemp = ass_swTorq2CurPu(swTeCadAssitPu2); // Q14 电流指令计算
  913. if (ass_stCalOut.swTorAss2CurrentTemp > ass_stCalCoef.swCurrentmax_torAssPu)
  914. {
  915. ass_stCalOut.swTorAss2CurrentTemp = ass_stCalCoef.swCurrentmax_torAssPu;
  916. }
  917. if (ass_stCalOut.swCadAss2CurrentTemp > ass_stCalCoef.swCurrentmax_cadAssPu)
  918. {
  919. ass_stCalOut.swCadAss2CurrentTemp = ass_stCalCoef.swCurrentmax_cadAssPu;
  920. }
  921. ass_stCalOut.swTorRefTarget = ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp;
  922. ass_stCalOut.swTorRefEnd = ass_stCalOut.swTorRefTarget;
  923. if(swPreCurrentPu < ass_stCalOut.swTorRefEnd)
  924. {
  925. ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *ass_stCalOut.swTorRefEnd;
  926. }
  927. else
  928. {
  929. ass_stCalOut.swTorAssistCurrentTemp = (ULONG)ass_stCalIn.swDirection * swPreCurrentPu;
  930. }
  931. break;
  932. case TorqueAssit:
  933. swTeTorAssitPu2 = swTeTorAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
  934. swTeCadAssitPu2 = swTeCadAssitPu1 ; // Q14+Q12-Q12+Q12-Q12=Q14
  935. ass_stCalOut.swTorAss2CurrentTemp = ass_swTorq2CurPu(swTeTorAssitPu2); // Q14 电流指令计算
  936. ass_stCalOut.swCadAss2CurrentTemp = ass_swTorq2CurPu(swTeCadAssitPu2); // Q14 电流指令计算
  937. if (ass_stCalOut.swTorAss2CurrentTemp > ass_stCalCoef.swCurrentmax_torAssPu)
  938. {
  939. ass_stCalOut.swTorAss2CurrentTemp = ass_stCalCoef.swCurrentmax_torAssPu;
  940. }
  941. if (ass_stCalOut.swCadAss2CurrentTemp > ass_stCalCoef.swCurrentmax_cadAssPu)
  942. {
  943. ass_stCalOut.swCadAss2CurrentTemp = ass_stCalCoef.swCurrentmax_cadAssPu;
  944. }
  945. #if CURSWITCH
  946. /* Ajust CurrentRef growth and decline rate */
  947. ass_stCalOut.swTorRefTarget = ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp;
  948. if((ass_stCalOut.swTorRefTarget - ass_stCalOut.swTorRefEnd) > 2)
  949. {
  950. ass_pvt_uwTorqAccCnt++;
  951. if(ass_pvt_uwTorqAccCnt >= 2)
  952. {
  953. ass_stCalOut.swTorRefEnd += (SWORD)uwTorqAccStep;
  954. ass_pvt_uwTorqAccCnt = 0;
  955. }
  956. }
  957. else if((ass_stCalOut.swTorRefTarget - ass_stCalOut.swTorRefEnd) < -1)
  958. {
  959. if (ass_stCalIn.uwcadance != ass_stCalIn.uwcadancelast)
  960. {
  961. ass_stCalOut.swTorRefEnd -= (SWORD)uwTorqDecStep;
  962. }
  963. // ass_pvt_uwTorqDecCnt++;
  964. // if(ass_pvt_uwTorqDecCnt >= 10)
  965. // {
  966. // ass_stCalOut.swTorRefEnd += uwTorqAccStep;
  967. // ass_pvt_uwTorqDecCnt = 0;
  968. // }
  969. }
  970. else
  971. {
  972. ass_stCalOut.swTorRefEnd = ass_stCalOut.swTorRefTarget;
  973. }
  974. ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection * ass_stCalOut.swTorRefEnd * ass_stCalCoef.swCadanceGain >> 12;
  975. /* Torq Clzloop Test */
  976. // if(ass_stCalIn.uwtorquelpf <= ass_stCalCoef.uwSwitch1TorqThreshold)
  977. // {
  978. // if(!ass_stCalOut.blTorqPIFlg)
  979. // {
  980. // /* Initial value */
  981. // ass_stTorqPIOut.slIRefPu = 0;
  982. // swCurSwitch = ABS(ass_stCalOut.swTorRefTarget); //ABS(ass_stCalOut.swAssitCurRef);
  983. // ass_stCalOut.blTorqPIFlg = TRUE;
  984. // }
  985. //
  986. // ass_stTorqPIIn.swTorqRefPu = ass_stCalIn.uwtorquelpf ; //torsensor_test_Lpf.slY.sw.hi ; //ass_stCalIn.uwtorque;
  987. // ass_stTorqPIIn.swTorqFdbPu = ass_stCalCoef.uwSwitch1TorqThreshold;
  988. // ass_stTorqPIIn.swImaxPu = 0;
  989. // ass_stTorqPIIn.swIminPu = -swCurSwitch;
  990. // ass_voAssitTorqPI(&ass_stTorqPIIn,&ass_stTorqPIOut);
  991. // ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *(swCurSwitch + ass_stUqLimMafValue.slAverValue);
  992. // }
  993. // else
  994. // {
  995. // ass_stCalOut.blTorqPIFlg = FALSE;
  996. // ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection * ass_stCalOut.swTorRefEnd;
  997. // }
  998. #else
  999. ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *(ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp)*ass_CalCoef.swCadanceGain >> 12;
  1000. #endif
  1001. break;
  1002. case ReduceCurrent:
  1003. swTeTorAssitPu2 = swTeTorAssitPu1; // Q14+Q12-Q12+Q12-Q12=Q14
  1004. swTeCadAssitPu2 = swTeCadAssitPu1; // Q14+Q12-Q12+Q12-Q12=Q14
  1005. ass_stCalOut.swTorAss2CurrentTemp = ass_swTorq2CurPu(swTeTorAssitPu2); // Q14 电流指令计算
  1006. ass_stCalOut.swCadAss2CurrentTemp = ass_swTorq2CurPu(swTeCadAssitPu2); // Q14 电流指令计算
  1007. if (ass_stCalOut.swTorAss2CurrentTemp > ass_stCalCoef.swCurrentmax_torAssPu)
  1008. {
  1009. ass_stCalOut.swTorAss2CurrentTemp = ass_stCalCoef.swCurrentmax_torAssPu;
  1010. }
  1011. if (ass_stCalOut.swCadAss2CurrentTemp > ass_stCalCoef.swCurrentmax_cadAssPu)
  1012. {
  1013. ass_stCalOut.swCadAss2CurrentTemp = ass_stCalCoef.swCurrentmax_cadAssPu;
  1014. }
  1015. ass_stCalOut.swTorAssistCurrentTemp = ass_stCalIn.swDirection *(ass_stCalOut.swTorAss2CurrentTemp + ass_stCalOut.swCadAss2CurrentTemp)*ass_stCalCoef.swCadanceGain >> 12;
  1016. break;
  1017. case StopAssit:
  1018. ass_stCalOut.swTorAssistCurrentTemp = 0;
  1019. ass_stCalOut.swTorRefEnd = 0;
  1020. break;
  1021. default:
  1022. break;
  1023. }
  1024. /* Assist Iqref Output */
  1025. ass_stCalOut.swTorAssistCurrent = ass_stCalOut.swTorAssistCurrentTemp;
  1026. mth_voLPFilter(ass_stCalOut.swTorAssistCurrent, &ass_pvt_stCurLpf);
  1027. /* Bikespeed Limit Coef*/
  1028. ass_stCalOut.swAssitCurRef = (SWORD)((SLONG)ass_pvt_stCurLpf.slY.sw.hi * ass_stCalCoef.swBikeSpeedGain >> 12);
  1029. ass_stCalOut.swAssitCurRef = (SLONG)ass_stCalOut.swAssitCurRef * ass_stCalCoef.swSmoothStopGain >> 12;
  1030. //ass_stCalOut.swAssitCurRef =ass_stCalOut.swTorAssistCurrent;
  1031. }
  1032. /**
  1033. * @brief Three order polynomial Y = a*X^3 + b*X^2 + c*x +d
  1034. *
  1035. * @param coef polynomial coefficient a, b, c, d
  1036. * @param Value polynomial input value X
  1037. * @param Qnum polynomial input Q type
  1038. * @return UWORD polynomial output Y
  1039. */
  1040. static void ass_voAssitCurLim(UWORD gear, UWORD uwBikeSpeedHzPu, UWORD uwCurMaxPu)
  1041. {
  1042. /* Limit the Output Current according to Bike Gear */
  1043. UWORD uwIqLimitTemp1;
  1044. if(gear > 5)
  1045. {
  1046. gear = 5;
  1047. }
  1048. uwIqLimitTemp1 = (UWORD)(((ULONG)ass_stCurLimCoef.uwLimitGain[gear] * uwCurMaxPu) >> 10);
  1049. ass_stCurLimOut.uwIqlimit = uwIqLimitTemp1;
  1050. }
  1051. /**
  1052. * @brief Assist function
  1053. *
  1054. * @param coef polynomial coefficient a, b, c, d
  1055. * @param Value polynomial input value X
  1056. * @param Qnum polynomial input Q type
  1057. * @return UWORD polynomial output Y
  1058. */
  1059. void ass_voAssist(void)
  1060. {
  1061. #ifdef TEST
  1062. ass_stCalIn.uwtorquePer = (UWORD)(((ULONG)10*10 << 14) / TORQUEBASE);
  1063. ass_stCalIn.uwtorque = (UWORD)(((ULONG)10*10 << 14) / TORQUEBASE);
  1064. ass_stCalIn.uwtorquelpf = (UWORD)(((ULONG)10*10 << 14) / TORQUEBASE);
  1065. ass_stCalIn.uwcadancePer = (UWORD)(((ULONG)30 << 20) / cof_uwFbHz / 60);
  1066. ass_stCalIn.uwcadance = (UWORD)(((ULONG)30 << 20) / cof_uwFbHz / 60);
  1067. #endif
  1068. /* Start Assist Jduge */
  1069. if (((ass_stCalIn.uwcadance > 0) || (ass_stCalIn.uwtorquePer > 3000)) && (ass_stCalIn.uwGearSt > 0 && ass_stCalIn.uwGearSt != 0x22))
  1070. {
  1071. ass_stCalCoef.blAssistflag = TRUE;
  1072. }
  1073. if (ass_stCalCoef.blAssistflag == TRUE)
  1074. {
  1075. /* Calculate Iqref Limit */
  1076. ass_voAssitCurLim(ass_stCalIn.uwGearSt, ass_stCalIn.uwbikespeed, ass_stParaCong.uwCofCurMaxPu);
  1077. ass_stCalCoef.uwCurrentMaxPu = (ass_stCurLimOut.uwIqlimit < ass_stCalIn.swFlxIqLimit)
  1078. ? (ass_stCurLimOut.uwIqlimit < ass_stCalIn.swPwrIqLimit ? ass_stCurLimOut.uwIqlimit : ass_stCalIn.swPwrIqLimit)
  1079. : (ass_stCalIn.swFlxIqLimit < ass_stCalIn.swPwrIqLimit ? ass_stCalIn.swFlxIqLimit : ass_stCalIn.swPwrIqLimit);
  1080. ass_stCalCoef.swCurrentmax_torAssPu = (SWORD)(((SLONG)ass_stCalCoef.uwCurrentMaxPu * (SWORD)ass_stParaSet.uwTorWeight) >> 12); // Q14
  1081. ass_stCalCoef.swCurrentmax_cadAssPu = (SWORD)(((SLONG)ass_stCalCoef.uwCurrentMaxPu * (SWORD)ass_stParaSet.uwCadenceWeight) >> 12);
  1082. /* Calculate Assist Current, Iqref*/
  1083. AssitCuvApplPerVolt();
  1084. /* Iqref Limit */
  1085. if (ass_stCalOut.swAssitCurRef > (SWORD)ass_stCalCoef.uwCurrentMaxPu)
  1086. {
  1087. ass_stCalOut.swAssitCurRef = (SWORD)ass_stCalCoef.uwCurrentMaxPu;
  1088. }
  1089. else if(ass_stCalOut.swAssitCurRef < -(SWORD)ass_stCalCoef.uwCurrentMaxPu)
  1090. {
  1091. ass_stCalOut.swAssitCurRef = -(SWORD)ass_stCalCoef.uwCurrentMaxPu;
  1092. }
  1093. else
  1094. {
  1095. //do nothing
  1096. }
  1097. }
  1098. else
  1099. {
  1100. ass_stCalOut.swAssitCurRef = 0;
  1101. }
  1102. }
  1103. /**
  1104. * @brief
  1105. *
  1106. * @param
  1107. * @return
  1108. */
  1109. void ass_voMoveAverageFilter(MAF_IN *in)
  1110. {
  1111. in->slSum -= in->swBuffer[in->uwIndex];
  1112. in->swBuffer[in->uwIndex] = in->swValue;
  1113. in->slSum += (SLONG)in->swValue;
  1114. if (!in->blSecFlag)
  1115. {
  1116. in->slAverValue = in->slSum / ((SLONG)in->uwIndex + (SWORD)1);
  1117. }
  1118. else
  1119. {
  1120. in->slAverValue = in->slSum / (SLONG)in->uwLength;
  1121. }
  1122. in->uwIndex++;
  1123. if (in->uwIndex >= in->uwLength)
  1124. {
  1125. in->blSecFlag = TRUE;
  1126. in->uwIndex = 0;
  1127. }
  1128. }
  1129. void ass_voMoveAverageFilterClear(MAF_IN *in)
  1130. {
  1131. UWORD i;
  1132. in->uwIndex = 0;
  1133. in->slSum = 0;
  1134. in->blSecFlag = FALSE;
  1135. in->slAverValue = 0;
  1136. for (i = 0; i < in->uwLength; i++)
  1137. {
  1138. in->swBuffer[i] = 0;
  1139. }
  1140. }