Skip to content

Commit 75b84f1

Browse files
vedderbvedderb
committed
Added kill switch support
1 parent 5742d1b commit 75b84f1

16 files changed

+142
-41
lines changed

‎CHANGELOG

+1
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
* Dynamic QML-script write support.
2828
* Use fast speed tracker for current controller.
2929
* Disable motor for 5 seconds after flash operations.
30+
* Added kill switch support.
3031

3132
=== FW 5.02 ===
3233
* IMU calibration improvement.

‎appconf/appconf_default.h

+3
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,9 @@
4545
#ifndef APPCONF_SERVO_OUT_ENABLE
4646
#define APPCONF_SERVO_OUT_ENABLE false
4747
#endif
48+
#ifndef APPCONF_KILL_SW_MODE
49+
#define APPCONF_KILL_SW_MODE KILL_SW_MODE_DISABLED
50+
#endif
4851
#ifndef APPCONF_SEND_CAN_STATUS_RATE_HZ
4952
#define APPCONF_SEND_CAN_STATUS_RATE_HZ 50
5053
#endif

‎commands.c

+17-1
Original file line numberDiff line numberDiff line change
@@ -426,6 +426,12 @@ void commands_process_packet(unsigned char *data, unsigned int len,
426426
if (mask & ((uint32_t)1 << 20)) {
427427
buffer_append_float32(send_buffer, mc_interface_read_reset_avg_vq(), 1e3, &ind);
428428
}
429+
if (mask & ((uint32_t)1 << 21)) {
430+
uint8_t status = 0;
431+
status |= timeout_has_timeout();
432+
status |= timeout_kill_sw_active() << 1;
433+
send_buffer[ind++] = status;
434+
}
429435

430436
reply_func(send_buffer, ind);
431437
chMtxUnlock(&send_buffer_mutex);
@@ -565,7 +571,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
565571

566572
conf_general_store_app_configuration(appconf);
567573
app_set_configuration(appconf);
568-
timeout_configure(appconf->timeout_msec, appconf->timeout_brake_current);
574+
timeout_configure(appconf->timeout_msec, appconf->timeout_brake_current, appconf->kill_sw_mode);
569575
chThdSleepMilliseconds(200);
570576

571577
int32_t ind = 0;
@@ -1122,6 +1128,16 @@ void commands_process_packet(unsigned char *data, unsigned int len,
11221128
buffer_append_float32_auto(send_buffer, q[3], &ind);
11231129
}
11241130

1131+
if (mask & ((uint32_t)1 << 16)) {
1132+
uint8_t current_controller_id = app_get_configuration()->controller_id;
1133+
#ifdef HW_HAS_DUAL_MOTORS
1134+
if (mc_interface_get_motor_thread() == 2) {
1135+
current_controller_id = utils_second_motor_id();
1136+
}
1137+
#endif
1138+
send_buffer[ind++] = current_controller_id;
1139+
}
1140+
11251141
reply_func(send_buffer, ind);
11261142
} break;
11271143

‎conf_general.c

+18-13
Original file line numberDiff line numberDiff line change
@@ -531,8 +531,9 @@ bool conf_general_detect_motor_param(float current, float min_rpm, float low_dut
531531
// Disable timeout
532532
systime_t tout = timeout_get_timeout_msec();
533533
float tout_c = timeout_get_brake_current();
534+
KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
534535
timeout_reset();
535-
timeout_configure(60000, 0.0);
536+
timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
536537

537538
mc_interface_lock();
538539

@@ -597,7 +598,7 @@ bool conf_general_detect_motor_param(float current, float min_rpm, float low_dut
597598

598599
if (!started) {
599600
mc_interface_set_current(0.0);
600-
timeout_configure(tout, tout_c);
601+
timeout_configure(tout, tout_c, tout_ksw);
601602
mc_interface_set_configuration(mcconf_old);
602603
mc_interface_unlock();
603604
mempools_free_mcconf(mcconf);
@@ -689,7 +690,7 @@ bool conf_general_detect_motor_param(float current, float min_rpm, float low_dut
689690

690691
// Restore settings
691692
mc_interface_set_configuration(mcconf_old);
692-
timeout_configure(tout, tout_c);
693+
timeout_configure(tout, tout_c, tout_ksw);
693694

694695
mc_interface_unlock();
695696

@@ -762,8 +763,9 @@ bool conf_general_measure_flux_linkage(float current, float duty,
762763
// Disable timeout
763764
systime_t tout = timeout_get_timeout_msec();
764765
float tout_c = timeout_get_brake_current();
766+
KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
765767
timeout_reset();
766-
timeout_configure(60000, 0.0);
768+
timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
767769

768770
mc_interface_lock();
769771

@@ -838,7 +840,7 @@ bool conf_general_measure_flux_linkage(float current, float duty,
838840

839841
if (!started) {
840842
mc_interface_set_current(0.0);
841-
timeout_configure(tout, tout_c);
843+
timeout_configure(tout, tout_c, tout_ksw);
842844
mc_interface_set_configuration(mcconf);
843845
mc_interface_unlock();
844846
mempools_free_mcconf(mcconf);
@@ -861,7 +863,7 @@ bool conf_general_measure_flux_linkage(float current, float duty,
861863
chThdSleepMilliseconds(1.0);
862864
}
863865

864-
timeout_configure(tout, tout_c);
866+
timeout_configure(tout, tout_c, tout_ksw);
865867
mc_interface_set_configuration(mcconf_old);
866868
mc_interface_unlock();
867869
mc_interface_set_current(0.0);
@@ -978,8 +980,9 @@ bool conf_general_measure_flux_linkage_openloop(float current, float duty,
978980
// Disable timeout
979981
systime_t tout = timeout_get_timeout_msec();
980982
float tout_c = timeout_get_brake_current();
983+
KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
981984
timeout_reset();
982-
timeout_configure(60000, 0.0);
985+
timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
983986

984987
mc_interface_lock();
985988

@@ -1100,7 +1103,7 @@ bool conf_general_measure_flux_linkage_openloop(float current, float duty,
11001103
result = true;
11011104
}
11021105

1103-
timeout_configure(tout, tout_c);
1106+
timeout_configure(tout, tout_c, tout_ksw);
11041107
mc_interface_unlock();
11051108
mc_interface_release_motor();
11061109
mc_interface_wait_for_motor_release(1.0);
@@ -1166,8 +1169,9 @@ int conf_general_autodetect_apply_sensors_foc(float current,
11661169
// Disable timeout
11671170
systime_t tout = timeout_get_timeout_msec();
11681171
float tout_c = timeout_get_brake_current();
1172+
KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
11691173
timeout_reset();
1170-
timeout_configure(60000, 0.0);
1174+
timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
11711175

11721176
mc_interface_lock();
11731177

@@ -1242,7 +1246,7 @@ int conf_general_autodetect_apply_sensors_foc(float current,
12421246
res = true;
12431247
}
12441248

1245-
timeout_configure(tout, tout_c);
1249+
timeout_configure(tout, tout_c, tout_ksw);
12461250
mc_interface_unlock();
12471251
mc_interface_release_motor();
12481252
mc_interface_wait_for_motor_release(1.0);
@@ -1531,8 +1535,9 @@ int conf_general_detect_apply_all_foc(float max_power_loss,
15311535
// Disable timeout
15321536
systime_t tout = timeout_get_timeout_msec();
15331537
float tout_c = timeout_get_brake_current();
1538+
KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
15341539
timeout_reset();
1535-
timeout_configure(60000, 0.0);
1540+
timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
15361541

15371542
mc_interface_lock();
15381543

@@ -1565,7 +1570,7 @@ int conf_general_detect_apply_all_foc(float max_power_loss,
15651570
#endif
15661571

15671572
if (!res_r_l_imax_m1 || !res_r_l_imax_m2) {
1568-
timeout_configure(tout, tout_c);
1573+
timeout_configure(tout, tout_c, tout_ksw);
15691574
mc_interface_unlock();
15701575
mc_interface_release_motor();
15711576
mc_interface_wait_for_motor_release(1.0);
@@ -1690,7 +1695,7 @@ int conf_general_detect_apply_all_foc(float max_power_loss,
16901695
result = -10;
16911696
}
16921697

1693-
timeout_configure(tout, tout_c);
1698+
timeout_configure(tout, tout_c, tout_ksw);
16941699
mc_interface_lock_override_once();
16951700
mc_interface_release_motor();
16961701
mc_interface_wait_for_motor_release(1.0);

‎conf_general.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@
2424
#define FW_VERSION_MAJOR 5
2525
#define FW_VERSION_MINOR 03
2626
// Set to 0 for building a release and iterate during beta test builds
27-
#define FW_TEST_VERSION_NUMBER 38
27+
#define FW_TEST_VERSION_NUMBER 39
2828

2929
#include "datatypes.h"
3030

‎confgenerator.c

+3
Original file line numberDiff line numberDiff line change
@@ -200,6 +200,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
200200
buffer[ind++] = (uint8_t)conf->uavcan_esc_index;
201201
buffer[ind++] = conf->uavcan_raw_mode;
202202
buffer[ind++] = conf->servo_out_enable;
203+
buffer[ind++] = conf->kill_sw_mode;
203204
buffer[ind++] = conf->app_to_use;
204205
buffer[ind++] = conf->app_ppm_conf.ctrl_type;
205206
buffer_append_float32_auto(buffer, conf->app_ppm_conf.pid_max_erpm, &ind);
@@ -560,6 +561,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
560561
conf->uavcan_esc_index = buffer[ind++];
561562
conf->uavcan_raw_mode = buffer[ind++];
562563
conf->servo_out_enable = buffer[ind++];
564+
conf->kill_sw_mode = buffer[ind++];
563565
conf->app_to_use = buffer[ind++];
564566
conf->app_ppm_conf.ctrl_type = buffer[ind++];
565567
conf->app_ppm_conf.pid_max_erpm = buffer_get_float32_auto(buffer, &ind);
@@ -904,6 +906,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
904906
conf->uavcan_esc_index = APPCONF_UAVCAN_ESC_INDEX;
905907
conf->uavcan_raw_mode = APPCONF_UAVCAN_RAW_MODE;
906908
conf->servo_out_enable = APPCONF_SERVO_OUT_ENABLE;
909+
conf->kill_sw_mode = APPCONF_KILL_SW_MODE;
907910
conf->app_to_use = APPCONF_APP_TO_USE;
908911
conf->app_ppm_conf.ctrl_type = APPCONF_PPM_CTRL_TYPE;
909912
conf->app_ppm_conf.pid_max_erpm = APPCONF_PPM_PID_MAX_ERPM;

‎confgenerator.h

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

1010
// Constants
1111
#define MCCONF_SIGNATURE 4213619100
12-
#define APPCONF_SIGNATURE 470228522
12+
#define APPCONF_SIGNATURE 2268062315
1313

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

‎datatypes.h

+9
Original file line numberDiff line numberDiff line change
@@ -797,6 +797,14 @@ typedef enum {
797797
UAVCAN_RAW_MODE_DUTY
798798
} UAVCAN_RAW_MODE;
799799

800+
typedef enum {
801+
KILL_SW_MODE_DISABLED = 0,
802+
KILL_SW_MODE_PPM_LOW,
803+
KILL_SW_MODE_PPM_HIGH,
804+
KILL_SW_MODE_ADC2_LOW,
805+
KILL_SW_MODE_ADC2_HIGH
806+
} KILL_SW_MODE;
807+
800808
typedef struct {
801809
// Settings
802810
uint8_t controller_id;
@@ -809,6 +817,7 @@ typedef struct {
809817
bool permanent_uart_enabled;
810818
SHUTDOWN_MODE shutdown_mode;
811819
bool servo_out_enable;
820+
KILL_SW_MODE kill_sw_mode;
812821

813822
// CAN modes
814823
CAN_MODE can_mode;

‎flash_helper.c

+4-8
Original file line numberDiff line numberDiff line change
@@ -120,8 +120,7 @@ uint16_t flash_helper_erase_new_app(uint32_t new_app_size) {
120120

121121
new_app_size += flash_addr[NEW_APP_BASE];
122122

123-
mc_interface_unlock();
124-
mc_interface_release_motor();
123+
mc_interface_release_motor_override();
125124
mc_interface_ignore_input_both(5000);
126125

127126
if (!mc_interface_wait_for_motor_release(3.0)) {
@@ -214,8 +213,7 @@ uint16_t flash_helper_qmlui_flags(void) {
214213
void flash_helper_jump_to_bootloader(void) {
215214
typedef void (*pFunction)(void);
216215

217-
mc_interface_unlock();
218-
mc_interface_release_motor();
216+
mc_interface_release_motor_override();
219217
usbDisconnectBus(&USBD1);
220218
usbStop(&USBD1);
221219

@@ -370,8 +368,7 @@ static uint16_t erase_sector(uint32_t sector) {
370368
FLASH_ClearFlag(FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR |
371369
FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
372370

373-
mc_interface_unlock();
374-
mc_interface_release_motor();
371+
mc_interface_release_motor_override();
375372
mc_interface_ignore_input_both(5000);
376373

377374
if (!mc_interface_wait_for_motor_release(3.0)) {
@@ -404,8 +401,7 @@ static uint16_t write_data(uint32_t base, uint8_t *data, uint32_t len) {
404401
FLASH_ClearFlag(FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR |
405402
FLASH_FLAG_PGPERR | FLASH_FLAG_PGSERR);
406403

407-
mc_interface_unlock();
408-
mc_interface_release_motor();
404+
mc_interface_release_motor_override();
409405
mc_interface_ignore_input_both(5000);
410406

411407
if (!mc_interface_wait_for_motor_release(3.0)) {

‎main.c

+1-1
Original file line numberDiff line numberDiff line change
@@ -264,7 +264,7 @@ int main(void) {
264264
chThdCreateStatic(flash_integrity_check_thread_wa, sizeof(flash_integrity_check_thread_wa), LOWPRIO, flash_integrity_check_thread, NULL);
265265

266266
timeout_init();
267-
timeout_configure(appconf->timeout_msec, appconf->timeout_brake_current);
267+
timeout_configure(appconf->timeout_msec, appconf->timeout_brake_current, appconf->kill_sw_mode);
268268

269269
mempools_free_appconf(appconf);
270270

‎mc_interface.c

+16
Original file line numberDiff line numberDiff line change
@@ -805,6 +805,22 @@ void mc_interface_release_motor(void) {
805805
mc_interface_set_current(0.0);
806806
}
807807

808+
void mc_interface_release_motor_override(void) {
809+
switch (motor_now()->m_conf.motor_type) {
810+
case MOTOR_TYPE_BLDC:
811+
case MOTOR_TYPE_DC:
812+
mcpwm_set_current(0.0);
813+
break;
814+
815+
case MOTOR_TYPE_FOC:
816+
mcpwm_foc_set_current(0.0);
817+
break;
818+
819+
default:
820+
break;
821+
}
822+
}
823+
808824
bool mc_interface_wait_for_motor_release(float timeout) {
809825
systime_t time_start = chVTGetSystemTimeX();
810826
bool res = false;

‎mc_interface.h

+1
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@ void mc_interface_set_handbrake_rel(float val);
5252
int mc_interface_set_tachometer_value(int steps);
5353
void mc_interface_brake_now(void);
5454
void mc_interface_release_motor(void);
55+
void mc_interface_release_motor_override(void);
5556
bool mc_interface_wait_for_motor_release(float timeout);
5657
float mc_interface_get_duty_cycle_set(void);
5758
float mc_interface_get_duty_cycle_now(void);

0 commit comments

Comments
 (0)