From dc51a0ca7d117a61b90e3940bede8adaf0adee7d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Luk=C3=A1=C5=A1=20Hr=C3=A1zk=C3=BD?= Date: Mon, 1 Jul 2024 20:24:55 +0200 Subject: [PATCH] Add Motor Additional Info config to package interface --- lispBM/c_libs/vesc_c_if.h | 9 +++++++ lispBM/lispif_c_lib.c | 49 ++++++++++++++++++++++++++++++--------- 2 files changed, 47 insertions(+), 11 deletions(-) diff --git a/lispBM/c_libs/vesc_c_if.h b/lispBM/c_libs/vesc_c_if.h index 07e6bee4..2024d453 100755 --- a/lispBM/c_libs/vesc_c_if.h +++ b/lispBM/c_libs/vesc_c_if.h @@ -287,6 +287,15 @@ typedef enum { CFG_PARAM_IMU_gyro_offset_z, CFG_PARAM_app_shutdown_mode, + + // Motor Additional Info + CFG_PARAM_si_motor_poles, + CFG_PARAM_si_gear_ratio, + CFG_PARAM_si_wheel_diameter, + CFG_PARAM_si_battery_type, + CFG_PARAM_si_battery_cells, + CFG_PARAM_si_battery_ah, + CFG_PARAM_si_motor_nl_current, } CFG_PARAM; typedef struct { diff --git a/lispBM/lispif_c_lib.c b/lispBM/lispif_c_lib.c index a6ff30b9..56daa036 100644 --- a/lispBM/lispif_c_lib.c +++ b/lispBM/lispif_c_lib.c @@ -458,6 +458,11 @@ static float lib_get_cfg_float(CFG_PARAM p) { case CFG_PARAM_IMU_gyro_offset_y: res = appconf->imu_conf.gyro_offsets[1]; break; case CFG_PARAM_IMU_gyro_offset_z: res = appconf->imu_conf.gyro_offsets[2]; break; + case CFG_PARAM_si_gear_ratio: res = mcconf->si_gear_ratio; break; + case CFG_PARAM_si_wheel_diameter: res = mcconf->si_wheel_diameter; break; + case CFG_PARAM_si_battery_ah: res = mcconf->si_battery_ah; break; + case CFG_PARAM_si_motor_nl_current: res = mcconf->si_motor_nl_current; break; + default: break; } @@ -467,14 +472,19 @@ static float lib_get_cfg_float(CFG_PARAM p) { static int lib_get_cfg_int(CFG_PARAM p) { int res = 0.0; - const app_configuration *conf = app_get_configuration(); + const volatile mc_configuration *mcconf = mc_interface_get_configuration(); + const app_configuration *appconf = app_get_configuration(); switch (p) { - case CFG_PARAM_app_can_mode: res = conf->can_mode; break; - case CFG_PARAM_app_can_baud_rate: res = conf->can_baud_rate; break; - case CFG_PARAM_IMU_ahrs_mode: res = conf->imu_conf.mode; break; - case CFG_PARAM_IMU_sample_rate: res = conf->imu_conf.sample_rate_hz; break; - case CFG_PARAM_app_shutdown_mode: res = conf->shutdown_mode; break; + case CFG_PARAM_app_can_mode: res = appconf->can_mode; break; + case CFG_PARAM_app_can_baud_rate: res = appconf->can_baud_rate; break; + case CFG_PARAM_IMU_ahrs_mode: res = appconf->imu_conf.mode; break; + case CFG_PARAM_IMU_sample_rate: res = appconf->imu_conf.sample_rate_hz; break; + case CFG_PARAM_app_shutdown_mode: res = appconf->shutdown_mode; break; + + case CFG_PARAM_si_motor_poles: res = mcconf->si_motor_poles; break; + case CFG_PARAM_si_battery_type: res = mcconf->si_battery_type; break; + case CFG_PARAM_si_battery_cells: res = mcconf->si_battery_cells; break; default: break; } @@ -530,6 +540,11 @@ static bool lib_set_cfg_float(CFG_PARAM p, float value) { case CFG_PARAM_IMU_gyro_offset_x: appconf->imu_conf.gyro_offsets[0] = value; changed_app = 1; res = true; break; case CFG_PARAM_IMU_gyro_offset_y: appconf->imu_conf.gyro_offsets[1] = value; changed_app = 1; res = true; break; case CFG_PARAM_IMU_gyro_offset_z: appconf->imu_conf.gyro_offsets[2] = value; changed_app = 1; res = true; break; + + case CFG_PARAM_si_gear_ratio: mcconf->si_gear_ratio = value; changed_mc = 1; res = true; break; + case CFG_PARAM_si_wheel_diameter: mcconf->si_wheel_diameter = value; changed_mc = 1; res = true; break; + case CFG_PARAM_si_battery_ah: mcconf->si_battery_ah = value; changed_mc = 1; res = true; break; + case CFG_PARAM_si_motor_nl_current: mcconf->si_motor_nl_current = value; changed_mc = 1; res = true; break; default: break; } @@ -549,19 +564,31 @@ static bool lib_set_cfg_float(CFG_PARAM p, float value) { static bool lib_set_cfg_int(CFG_PARAM p, int value) { bool res = false; + mc_configuration *mcconf = (mc_configuration*)mc_interface_get_configuration(); + int changed_mc = 0; + app_configuration *appconf = mempools_alloc_appconf(); *appconf = *app_get_configuration(); + int changed_app = 0; switch (p) { - case CFG_PARAM_app_can_mode: appconf->can_mode = value; res = true; break; - case CFG_PARAM_app_can_baud_rate: appconf->can_baud_rate = value; res = true; break; - case CFG_PARAM_IMU_ahrs_mode: appconf->imu_conf.mode = value; res = true; break; - case CFG_PARAM_IMU_sample_rate: appconf->imu_conf.sample_rate_hz = value; res = true; break; + case CFG_PARAM_app_can_mode: appconf->can_mode = value; changed_app = 1; res = true; break; + case CFG_PARAM_app_can_baud_rate: appconf->can_baud_rate = value; changed_app = 1; res = true; break; + case CFG_PARAM_IMU_ahrs_mode: appconf->imu_conf.mode = value; changed_app = 1; res = true; break; + case CFG_PARAM_IMU_sample_rate: appconf->imu_conf.sample_rate_hz = value; changed_app = 1; res = true; break; case CFG_PARAM_app_shutdown_mode: appconf->shutdown_mode = value; changed_app = 1; res = true; break; + + case CFG_PARAM_si_motor_poles: mcconf->si_motor_poles = value; changed_mc = 1; res = true; break; + case CFG_PARAM_si_battery_type: mcconf->si_battery_type = value; changed_mc = 1; res = true; break; + case CFG_PARAM_si_battery_cells: mcconf->si_battery_cells = value; changed_mc = 1; res = true; break; default: break; } - if (res) { + if (changed_mc > 0) { + commands_apply_mcconf_hw_limits(mcconf); + } + + if (changed_app > 0) { app_set_configuration(appconf); }