|
|
@@ -0,0 +1,188 @@
|
|
|
+#include "foc/core/thro_torque.h"
|
|
|
+#include "foc/foc_config.h"
|
|
|
+#include "foc/motor/motor.h"
|
|
|
+#include "foc/motor/motor_param.h"
|
|
|
+#include "foc/core/e_ctrl.h"
|
|
|
+#include "foc/core/PMSM_FOC_Core.h"
|
|
|
+#include "app/nv_storage.h"
|
|
|
+#include "libs/logger.h"
|
|
|
+#include "prot/can_foc_msg.h"
|
|
|
+
|
|
|
+static thro_torque_t _torque;
|
|
|
+static motor_map_t gear_torques[5][5] = {
|
|
|
+ [0] = {{500, 100}, {1200, 100}, {1700, 80}, {2200, 75}, {2800, 50},},
|
|
|
+ [1] = {{800, 130}, {1600, 120}, {2400, 110}, {3200, 80}, {4300, 60},},
|
|
|
+ [2] = {{1200, 150}, {2200, 140}, {3200, 120}, {4200, 80}, {5300, 70},},
|
|
|
+ [3] = {{3000, 200}, {4740, 150}, {5050, 110}, {5200, 85}, {5300, 70},},
|
|
|
+ [4] = {{4500, 200}, {4740, 150}, {5050, 110}, {5200, 85}, {5300, 70},},
|
|
|
+};
|
|
|
+
|
|
|
+void thro_torque_reset(void) {
|
|
|
+ _torque.accl = false;
|
|
|
+ _torque.thro_filted = 0.0f;
|
|
|
+ _torque.thro_ration = _torque.thro_ration_last = 0.0f;
|
|
|
+ _torque.torque_req = _torque.torque_real = 0.0f;
|
|
|
+}
|
|
|
+
|
|
|
+void thro_torque_init(void) {
|
|
|
+ thro_torque_reset();
|
|
|
+ trq2dq_lookup_init();
|
|
|
+ _torque.spd_filted = 0.0f;
|
|
|
+}
|
|
|
+
|
|
|
+static float thro_torque_gear_map(void) {
|
|
|
+ u8 gear = mc_get_gear();
|
|
|
+ if (gear > 4) {
|
|
|
+ gear = 0;
|
|
|
+ }
|
|
|
+ motor_map_t *map = &gear_torques[gear][0];
|
|
|
+ s16 rpm = (s16)_torque.spd_filted;
|
|
|
+ if (rpm <= map[0].rpm) {
|
|
|
+ return (float)map[0].torque;
|
|
|
+ }
|
|
|
+ int map_size = 5;
|
|
|
+ for (int i = 1; i < map_size; i++) {
|
|
|
+ if (rpm <= map[i].rpm) { //线性插值
|
|
|
+ float trq1 = map[i-1].torque;
|
|
|
+ float min_rpm = map[i-1].rpm;
|
|
|
+ float trq2 = map[i].torque;
|
|
|
+ float max_rpm = map[i].rpm;
|
|
|
+ if (trq1 > trq2) {
|
|
|
+ return f_map_inv((float)rpm, min_rpm, max_rpm, trq2, trq1);
|
|
|
+ }else {
|
|
|
+ return f_map((float)rpm, min_rpm, max_rpm, trq1, trq2);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return (float)map[map_size-1].torque;
|
|
|
+}
|
|
|
+
|
|
|
+/* 获取油门开度 */
|
|
|
+static float thro_ration(float f_throttle) {
|
|
|
+ if (f_throttle <= nv_get_foc_params()->n_startThroVol) {
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+ float delta = f_throttle - (nv_get_foc_params()->n_startThroVol);
|
|
|
+ int ration = (delta * 100.0f) / (nv_get_foc_params()->n_endThroVol - nv_get_foc_params()->n_startThroVol);
|
|
|
+ return ((float)ration)/100.0f;
|
|
|
+}
|
|
|
+
|
|
|
+float thro_ration_to_voltage(float r) {
|
|
|
+ if (r == 0) {
|
|
|
+ return 0;
|
|
|
+ }
|
|
|
+ float vol = nv_get_foc_params()->n_startThroVol + r * (nv_get_foc_params()->n_endThroVol - nv_get_foc_params()->n_startThroVol);
|
|
|
+ return fclamp(vol, 0, nv_get_foc_params()->n_endThroVol);
|
|
|
+}
|
|
|
+
|
|
|
+static float thro_torque_for_accelerate(void) {
|
|
|
+ float max_torque = thro_torque_gear_map();
|
|
|
+ float acc_r = 1.0f;
|
|
|
+ if (_torque.thro_ration_last < 1.0f) {
|
|
|
+ acc_r = (_torque.thro_ration - _torque.thro_ration_last)/ (1.0f - _torque.thro_ration_last);
|
|
|
+ }
|
|
|
+ acc_r = fclamp(acc_r, 0, 1.0f);
|
|
|
+ float acc_torque = _torque.torque_real + acc_r * (max_torque - _torque.torque_real);
|
|
|
+
|
|
|
+ return acc_torque;
|
|
|
+}
|
|
|
+
|
|
|
+static float thro_torque_for_decelerate(void) {
|
|
|
+ float dec_r = _torque.thro_ration / _torque.thro_ration_last;
|
|
|
+ dec_r = fclamp(dec_r, 0.0f, 1.0f);
|
|
|
+ return dec_r * _torque.torque_real;
|
|
|
+}
|
|
|
+
|
|
|
+static void thro_torque_request(void) {
|
|
|
+#ifdef CONFIG_CRUISE_ENABLE_ACCL
|
|
|
+ if (mc_is_cruise_enabled() && mc_throttle_released()) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+#endif
|
|
|
+ if (mc_throttle_released() && eCtrl_enable_eBrake(true)) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if (!mc_throttle_released()) {
|
|
|
+ if (_torque.accl) { //加速需求
|
|
|
+ float ref_torque = thro_torque_for_accelerate();
|
|
|
+ float curr_rpm = PMSM_FOC_GetSpeed();
|
|
|
+ if (curr_rpm <= CONFIG_ZERO_SPEED_RPM) {//从静止开始加速
|
|
|
+ float hold_torque = eCtrl_get_FinalTorque();
|
|
|
+ ref_torque = MAX(hold_torque, ref_torque);
|
|
|
+ }
|
|
|
+ _torque.torque_req = ref_torque;
|
|
|
+ }else {
|
|
|
+ float ref_torque = thro_torque_for_decelerate();
|
|
|
+ _torque.torque_req = ref_torque;
|
|
|
+ }
|
|
|
+ PMSM_FOC_Set_Torque(_torque.torque_req);
|
|
|
+ }else if (mc_throttle_released() && eCtrl_get_FinalTorque() != 0){
|
|
|
+ _torque.torque_req = 0.0f;
|
|
|
+ PMSM_FOC_Set_Torque(_torque.torque_req);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+#define THRO_REF_LP_CEOF 0.2f
|
|
|
+static void thro_torque_filter(float f_throttle) {
|
|
|
+ if (mc_throttle_released()){
|
|
|
+ thro_torque_reset();
|
|
|
+ }else {
|
|
|
+ LowPass_Filter(_torque.thro_filted, f_throttle, THRO_REF_LP_CEOF);
|
|
|
+ }
|
|
|
+ float curr_rpm = PMSM_FOC_GetSpeed();
|
|
|
+ if (curr_rpm == 0) {
|
|
|
+ _torque.spd_filted = 0;
|
|
|
+ }else {
|
|
|
+ LowPass_Filter(_torque.spd_filted, curr_rpm, 0.01f);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void thro_torque_process(u8 run_mode, float f_throttle) {
|
|
|
+
|
|
|
+ thro_torque_filter(f_throttle);
|
|
|
+
|
|
|
+ float thro_r = thro_ration(_torque.thro_filted);
|
|
|
+ if (thro_r > _torque.thro_ration) {
|
|
|
+ if (!_torque.accl) {
|
|
|
+ _torque.thro_ration_last = _torque.thro_ration;
|
|
|
+ _torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
|
|
|
+ }
|
|
|
+ _torque.accl = true;
|
|
|
+ }else if (thro_r < _torque.thro_ration) {
|
|
|
+ if (_torque.accl) {
|
|
|
+ _torque.thro_ration_last = _torque.thro_ration + 1e-10f; // add 1e-10f avoid divide zero
|
|
|
+ _torque.torque_real = PMSM_FOC_Get()->in.s_targetTorque;
|
|
|
+ }
|
|
|
+ _torque.accl = false;
|
|
|
+ }
|
|
|
+ _torque.thro_ration = thro_r;
|
|
|
+ if (run_mode == CTRL_MODE_TRQ) {
|
|
|
+ thro_torque_request();
|
|
|
+ }else if (run_mode == CTRL_MODE_SPD) {
|
|
|
+ if (!mc_is_cruise_enabled()) {
|
|
|
+ float speed_Ref = PMSM_FOC_GetSpeedLimit() * _torque.thro_ration;
|
|
|
+ PMSM_FOC_Set_Speed(speed_Ref);
|
|
|
+ }
|
|
|
+ }else if (run_mode == CTRL_MODE_EBRAKE) {
|
|
|
+ eCtrl_reset_Torque(0);
|
|
|
+ if (eCtrl_get_FinalCurrent() < 0.0001f && PMSM_FOC_GetSpeed() < CONFIG_MIN_RPM_EXIT_EBRAKE) {
|
|
|
+ eCtrl_enable_eBrake(false);
|
|
|
+ }
|
|
|
+ if (!mc_throttle_released() || (mc_throttle_released() && (PMSM_FOC_GetSpeed() == 0.0f))) {
|
|
|
+ eCtrl_enable_eBrake(false);
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+/* 定速巡航需要判断是否需要加速 */
|
|
|
+float get_user_request_torque(void) {
|
|
|
+ if (_torque.accl) {
|
|
|
+ return thro_torque_for_accelerate();
|
|
|
+ }
|
|
|
+ return thro_torque_for_decelerate();
|
|
|
+}
|
|
|
+
|
|
|
+void thro_torque_log(void) {
|
|
|
+ sys_debug("accl %d, real %f, req %f\n", _torque.accl, _torque.torque_real, _torque.torque_req);
|
|
|
+ sys_debug("ration %f - %f - %f\n", _torque.thro_ration, _torque.thro_ration_last, thro_torque_for_accelerate());
|
|
|
+}
|
huhui