Skip to content

Commit 2848ec1

Browse files
vedderbvedderb
committed
Merge branch 'dev_fw_5_03' of https://github.com/vedderb/bldc into dev_fw_5_03
2 parents 1aef262 + 1f1e1e9 commit 2848ec1

File tree

5 files changed

+96
-6
lines changed

5 files changed

+96
-6
lines changed

‎appconf/appconf_default.h

+18
Original file line numberDiff line numberDiff line change
@@ -406,6 +406,24 @@
406406
#ifndef APPCONF_BALANCE_TORQUETILT_FILTER
407407
#define APPCONF_BALANCE_TORQUETILT_FILTER 0.95
408408
#endif
409+
#ifndef APPCONF_BALANCE_TURNTILT_POWER
410+
#define APPCONF_BALANCE_TURNTILT_POWER 0
411+
#endif
412+
#ifndef APPCONF_BALANCE_TURNTILT_ANGLE_LIMIT
413+
#define APPCONF_BALANCE_TURNTILT_ANGLE_LIMIT 5
414+
#endif
415+
#ifndef APPCONF_BALANCE_TURNTILT_ANGLE_CUT
416+
#define APPCONF_BALANCE_TURNTILT_ANGLE_CUT 1
417+
#endif
418+
#ifndef APPCONF_BALANCE_TURNTILT_SPEED
419+
#define APPCONF_BALANCE_TURNTILT_SPEED 5
420+
#endif
421+
#ifndef APPCONF_BALANCE_TURNTILT_ERPM_BOOST
422+
#define APPCONF_BALANCE_TURNTILT_ERPM_BOOST 20
423+
#endif
424+
#ifndef APPCONF_BALANCE_TURNTILT_ERPM_BOOST_END
425+
#define APPCONF_BALANCE_TURNTILT_ERPM_BOOST_END 20000
426+
#endif
409427

410428
// PAS app
411429
#ifndef APPCONF_PAS_CTRL_TYPE

‎applications/app_balance.c

+53-5
Original file line numberDiff line numberDiff line change
@@ -73,10 +73,10 @@ static thread_t *app_thread;
7373
// Config values
7474
static volatile balance_config balance_conf;
7575
static volatile imu_config imu_conf;
76-
static float startup_step_size, tiltback_step_size, torquetilt_step_size;
76+
static float startup_step_size, tiltback_step_size, torquetilt_step_size, turntilt_step_size;
7777

7878
// Runtime values read from elsewhere
79-
static float pitch_angle, roll_angle;
79+
static float pitch_angle, roll_angle, abs_roll_angle, abs_roll_angle_sin;
8080
static float gyro[3];
8181
static float duty_cycle, abs_duty_cycle;
8282
static float erpm, abs_erpm, avg_erpm;
@@ -90,7 +90,9 @@ static BalanceState state;
9090
static float proportional, integral, derivative;
9191
static float last_proportional, abs_proportional;
9292
static float pid_value;
93-
static float setpoint, setpoint_target, setpoint_target_interpolated, torquetilt_target, torquetilt_interpolated, torquetilt_filtered_current;
93+
static float setpoint, setpoint_target, setpoint_target_interpolated;
94+
static float torquetilt_filtered_current, torquetilt_target, torquetilt_interpolated;
95+
static float turntilt_target, turntilt_interpolated;
9496
static SetpointAdjustmentType setpointAdjustmentType;
9597
static float yaw_proportional, yaw_integral, yaw_derivative, yaw_last_proportional, yaw_pid_value, yaw_setpoint;
9698
static systime_t current_time, last_time, diff_time;
@@ -110,6 +112,7 @@ void app_balance_configure(balance_config *conf, imu_config *conf2) {
110112
startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
111113
tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz;
112114
torquetilt_step_size = balance_conf.torquetilt_speed / balance_conf.hertz;
115+
turntilt_step_size = balance_conf.turntilt_speed / balance_conf.hertz;
113116
}
114117

115118
void app_balance_start(void) {
@@ -133,6 +136,8 @@ void reset_vars(void){
133136
torquetilt_target = 0;
134137
torquetilt_interpolated = 0;
135138
torquetilt_filtered_current = 0;
139+
turntilt_target = 0;
140+
turntilt_interpolated = 0;
136141
setpointAdjustmentType = CENTERING;
137142
yaw_setpoint = 0;
138143
state = RUNNING;
@@ -295,7 +300,7 @@ void calculate_setpoint_interpolated(void){
295300
}
296301
}
297302

298-
void apply_torque_tilt(void){
303+
void apply_torquetilt(void){
299304
// Filter current (Basic LPF)
300305
torquetilt_filtered_current = ((1-balance_conf.torquetilt_filter) * motor_current) + (balance_conf.torquetilt_filter * torquetilt_filtered_current);
301306
// Wat is this line O_o
@@ -314,6 +319,46 @@ void apply_torque_tilt(void){
314319
setpoint += torquetilt_interpolated;
315320
}
316321

322+
void apply_turntilt(void){
323+
// Calculate desired angle
324+
turntilt_target = abs_roll_angle_sin * balance_conf.turntilt_power;
325+
326+
// Apply cutzone
327+
if(abs_roll_angle < balance_conf.turntilt_angle_cut){
328+
turntilt_target = 0;
329+
}
330+
331+
// Disable at 0 speed otherwise add directionality
332+
if(abs_erpm < 100){
333+
turntilt_target = 0;
334+
}else {
335+
turntilt_target *= SIGN(erpm);
336+
}
337+
338+
// Apply speed scaling
339+
if(balance_conf.turntilt_erpm_boost_end > 0){
340+
if(abs_erpm < balance_conf.turntilt_erpm_boost_end){
341+
turntilt_target *= 1 + ((balance_conf.turntilt_erpm_boost/100.0f) * (abs_erpm / balance_conf.turntilt_erpm_boost_end));
342+
}else{
343+
turntilt_target *= 1 + (balance_conf.turntilt_erpm_boost/100.0f);
344+
}
345+
}
346+
347+
// Limit angle to max angle
348+
turntilt_target = fminf(turntilt_target, balance_conf.turntilt_angle_limit);
349+
350+
// Move towards target limited by max speed
351+
if(fabsf(turntilt_target - turntilt_interpolated) < turntilt_step_size){
352+
turntilt_interpolated = turntilt_target;
353+
}else if (turntilt_target - turntilt_interpolated > 0){
354+
turntilt_interpolated += turntilt_step_size;
355+
}else{
356+
turntilt_interpolated -= turntilt_step_size;
357+
}
358+
setpoint += turntilt_interpolated;
359+
360+
}
361+
317362
float apply_deadzone(float error){
318363
if(balance_conf.deadzone == 0){
319364
return error;
@@ -389,6 +434,8 @@ static THD_FUNCTION(balance_thread, arg) {
389434
// Get the values we want
390435
pitch_angle = imu_get_pitch() * 180.0f / M_PI;
391436
roll_angle = imu_get_roll() * 180.0f / M_PI;
437+
abs_roll_angle = fabsf(roll_angle);
438+
abs_roll_angle_sin = sinf(abs_roll_angle * M_PI / 180.0f);
392439
imu_get_gyro(gyro);
393440
duty_cycle = mc_interface_get_duty_cycle_now();
394441
abs_duty_cycle = fabsf(duty_cycle);
@@ -464,7 +511,8 @@ static THD_FUNCTION(balance_thread, arg) {
464511
calculate_setpoint_target();
465512
calculate_setpoint_interpolated();
466513
setpoint = setpoint_target_interpolated;
467-
apply_torque_tilt();
514+
apply_torquetilt();
515+
apply_turntilt();
468516

469517
// Do PID maths
470518
proportional = setpoint - pitch_angle;

‎confgenerator.c

+18
Original file line numberDiff line numberDiff line change
@@ -305,6 +305,12 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
305305
buffer_append_float32_auto(buffer, conf->app_balance_conf.torquetilt_speed, &ind);
306306
buffer_append_float32_auto(buffer, conf->app_balance_conf.torquetilt_power, &ind);
307307
buffer_append_float32_auto(buffer, conf->app_balance_conf.torquetilt_filter, &ind);
308+
buffer_append_float32_auto(buffer, conf->app_balance_conf.turntilt_power, &ind);
309+
buffer_append_float32_auto(buffer, conf->app_balance_conf.turntilt_angle_limit, &ind);
310+
buffer_append_float32_auto(buffer, conf->app_balance_conf.turntilt_angle_cut, &ind);
311+
buffer_append_float32_auto(buffer, conf->app_balance_conf.turntilt_speed, &ind);
312+
buffer_append_uint16(buffer, conf->app_balance_conf.turntilt_erpm_boost, &ind);
313+
buffer_append_uint16(buffer, conf->app_balance_conf.turntilt_erpm_boost_end, &ind);
308314
buffer[ind++] = conf->app_pas_conf.ctrl_type;
309315
buffer[ind++] = conf->app_pas_conf.sensor_type;
310316
buffer_append_float16(buffer, conf->app_pas_conf.current_scaling, 1000, &ind);
@@ -647,6 +653,12 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
647653
conf->app_balance_conf.torquetilt_speed = buffer_get_float32_auto(buffer, &ind);
648654
conf->app_balance_conf.torquetilt_power = buffer_get_float32_auto(buffer, &ind);
649655
conf->app_balance_conf.torquetilt_filter = buffer_get_float32_auto(buffer, &ind);
656+
conf->app_balance_conf.turntilt_power = buffer_get_float32_auto(buffer, &ind);
657+
conf->app_balance_conf.turntilt_angle_limit = buffer_get_float32_auto(buffer, &ind);
658+
conf->app_balance_conf.turntilt_angle_cut = buffer_get_float32_auto(buffer, &ind);
659+
conf->app_balance_conf.turntilt_speed = buffer_get_float32_auto(buffer, &ind);
660+
conf->app_balance_conf.turntilt_erpm_boost = buffer_get_uint16(buffer, &ind);
661+
conf->app_balance_conf.turntilt_erpm_boost_end = buffer_get_uint16(buffer, &ind);
650662
conf->app_pas_conf.ctrl_type = buffer[ind++];
651663
conf->app_pas_conf.sensor_type = buffer[ind++];
652664
conf->app_pas_conf.current_scaling = buffer_get_float16(buffer, 1000, &ind);
@@ -973,6 +985,12 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
973985
conf->app_balance_conf.torquetilt_speed = APPCONF_BALANCE_TORQUETILT_SPEED;
974986
conf->app_balance_conf.torquetilt_power = APPCONF_BALANCE_TORQUETILT_POWER;
975987
conf->app_balance_conf.torquetilt_filter = APPCONF_BALANCE_TORQUETILT_FILTER;
988+
conf->app_balance_conf.turntilt_power = APPCONF_BALANCE_TURNTILT_POWER;
989+
conf->app_balance_conf.turntilt_angle_limit = APPCONF_BALANCE_TURNTILT_ANGLE_LIMIT;
990+
conf->app_balance_conf.turntilt_angle_cut = APPCONF_BALANCE_TURNTILT_ANGLE_CUT;
991+
conf->app_balance_conf.turntilt_speed = APPCONF_BALANCE_TURNTILT_SPEED;
992+
conf->app_balance_conf.turntilt_erpm_boost = APPCONF_BALANCE_TURNTILT_ERPM_BOOST;
993+
conf->app_balance_conf.turntilt_erpm_boost_end = APPCONF_BALANCE_TURNTILT_ERPM_BOOST_END;
976994
conf->app_pas_conf.ctrl_type = APPCONF_PAS_CTRL_TYPE;
977995
conf->app_pas_conf.sensor_type = APPCONF_PAS_SENSOR_TYPE;
978996
conf->app_pas_conf.current_scaling = APPCONF_PAS_CURRENT_SCALING;

‎confgenerator.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99

1010
// Constants
1111
#define MCCONF_SIGNATURE 2835115562
12-
#define APPCONF_SIGNATURE 1729624904
12+
#define APPCONF_SIGNATURE 3423413835
1313

1414
// Functions
1515
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

‎datatypes.h

+6
Original file line numberDiff line numberDiff line change
@@ -707,6 +707,12 @@ typedef struct {
707707
float torquetilt_speed;
708708
float torquetilt_power;
709709
float torquetilt_filter;
710+
float turntilt_power;
711+
float turntilt_angle_limit;
712+
float turntilt_angle_cut;
713+
float turntilt_speed;
714+
uint16_t turntilt_erpm_boost;
715+
uint16_t turntilt_erpm_boost_end;
710716
} balance_config;
711717

712718
// CAN status modes

0 commit comments

Comments
 (0)