Skip to content

Commit 1ab25a4

Browse files
vedderbvedderb
committed
Added position PID offset support and KD_PROC term
1 parent fec05f7 commit 1ab25a4

13 files changed

+178
-75
lines changed

‎CHANGELOG

+2
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,8 @@
2828
* Use fast speed tracker for current controller.
2929
* Disable motor for 5 seconds after flash operations.
3030
* Added kill switch support.
31+
* Added process derivative term to position controller.
32+
* Added position PID-controller angle offset.
3133

3234
=== FW 5.02 ===
3335
* IMU calibration improvement.

‎comm_can.c

+82-69
Original file line numberDiff line numberDiff line change
@@ -99,11 +99,6 @@ static void set_timing(int brp, int ts1, int ts2);
9999
#if CAN_ENABLE
100100
static void send_packet_wrapper(unsigned char *data, unsigned int len);
101101
static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced);
102-
static void send_status1(uint8_t id, bool replace);
103-
static void send_status2(uint8_t id, bool replace);
104-
static void send_status3(uint8_t id, bool replace);
105-
static void send_status4(uint8_t id, bool replace);
106-
static void send_status5(uint8_t id, bool replace);
107102
#endif
108103

109104
// Function pointers
@@ -977,6 +972,17 @@ void comm_can_psw_switch(int id, bool is_on, bool plot) {
977972
buffer, send_index, true);
978973
}
979974

975+
void comm_can_update_pid_pos_offset(int id, float angle_now, bool store) {
976+
int32_t send_index = 0;
977+
uint8_t buffer[8];
978+
979+
buffer_append_float32(buffer, angle_now, 1e4, &send_index);
980+
buffer[send_index++] = store;
981+
982+
comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_UPDATE_PID_POS_OFFSET << 8),
983+
buffer, send_index, true);
984+
}
985+
980986
CANRxFrame *comm_can_get_rx_frame(void) {
981987
#if CAN_ENABLE
982988
chMtxLock(&can_rx_mtx);
@@ -998,6 +1004,55 @@ CANRxFrame *comm_can_get_rx_frame(void) {
9981004
#endif
9991005
}
10001006

1007+
void comm_can_send_status1(uint8_t id, bool replace) {
1008+
int32_t send_index = 0;
1009+
uint8_t buffer[8];
1010+
buffer_append_int32(buffer, (int32_t)mc_interface_get_rpm(), &send_index);
1011+
buffer_append_int16(buffer, (int16_t)(mc_interface_get_tot_current_filtered() * 1e1), &send_index);
1012+
buffer_append_int16(buffer, (int16_t)(mc_interface_get_duty_cycle_now() * 1e3), &send_index);
1013+
comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS << 8),
1014+
buffer, send_index, replace);
1015+
}
1016+
1017+
void comm_can_send_status2(uint8_t id, bool replace) {
1018+
int32_t send_index = 0;
1019+
uint8_t buffer[8];
1020+
buffer_append_int32(buffer, (int32_t)(mc_interface_get_amp_hours(false) * 1e4), &send_index);
1021+
buffer_append_int32(buffer, (int32_t)(mc_interface_get_amp_hours_charged(false) * 1e4), &send_index);
1022+
comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_2 << 8),
1023+
buffer, send_index, replace);
1024+
}
1025+
1026+
void comm_can_send_status3(uint8_t id, bool replace) {
1027+
int32_t send_index = 0;
1028+
uint8_t buffer[8];
1029+
buffer_append_int32(buffer, (int32_t)(mc_interface_get_watt_hours(false) * 1e4), &send_index);
1030+
buffer_append_int32(buffer, (int32_t)(mc_interface_get_watt_hours_charged(false) * 1e4), &send_index);
1031+
comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_3 << 8),
1032+
buffer, send_index, replace);
1033+
}
1034+
1035+
void comm_can_send_status4(uint8_t id, bool replace) {
1036+
int32_t send_index = 0;
1037+
uint8_t buffer[8];
1038+
buffer_append_int16(buffer, (int16_t)(mc_interface_temp_fet_filtered() * 1e1), &send_index);
1039+
buffer_append_int16(buffer, (int16_t)(mc_interface_temp_motor_filtered() * 1e1), &send_index);
1040+
buffer_append_int16(buffer, (int16_t)(mc_interface_get_tot_current_in_filtered() * 1e1), &send_index);
1041+
buffer_append_int16(buffer, (int16_t)(mc_interface_get_pid_pos_now() * 50.0), &send_index);
1042+
comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_4 << 8),
1043+
buffer, send_index, replace);
1044+
}
1045+
1046+
void comm_can_send_status5(uint8_t id, bool replace) {
1047+
int32_t send_index = 0;
1048+
uint8_t buffer[8];
1049+
buffer_append_int32(buffer, mc_interface_get_tachometer_value(false), &send_index);
1050+
buffer_append_int16(buffer, (int16_t)(mc_interface_get_input_voltage_filtered() * 1e1), &send_index);
1051+
buffer_append_int16(buffer, 0, &send_index); // Reserved for now
1052+
comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_5 << 8),
1053+
buffer, send_index, replace);
1054+
}
1055+
10011056
#if CAN_ENABLE
10021057
static THD_FUNCTION(cancom_read_thread, arg) {
10031058
(void)arg;
@@ -1108,11 +1163,11 @@ static THD_FUNCTION(cancom_status_internal_thread, arg) {
11081163
mc_interface_select_motor_thread(2);
11091164

11101165
for (;;) {
1111-
send_status1(utils_second_motor_id(), true);
1112-
send_status2(utils_second_motor_id(), true);
1113-
send_status3(utils_second_motor_id(), true);
1114-
send_status4(utils_second_motor_id(), true);
1115-
send_status5(utils_second_motor_id(), true);
1166+
comm_can_send_status1(utils_second_motor_id(), true);
1167+
comm_can_send_status2(utils_second_motor_id(), true);
1168+
comm_can_send_status3(utils_second_motor_id(), true);
1169+
comm_can_send_status4(utils_second_motor_id(), true);
1170+
comm_can_send_status5(utils_second_motor_id(), true);
11161171
chThdSleepMilliseconds(2);
11171172
}
11181173
}
@@ -1132,10 +1187,10 @@ static THD_FUNCTION(cancom_status_thread, arg) {
11321187
conf->send_can_status == CAN_STATUS_1_2_3_4 ||
11331188
conf->send_can_status == CAN_STATUS_1_2_3_4_5) {
11341189
mc_interface_select_motor_thread(1);
1135-
send_status1(conf->controller_id, false);
1190+
comm_can_send_status1(conf->controller_id, false);
11361191
#ifdef HW_HAS_DUAL_MOTORS
11371192
mc_interface_select_motor_thread(2);
1138-
send_status1(utils_second_motor_id(), false);
1193+
comm_can_send_status1(utils_second_motor_id(), false);
11391194
#endif
11401195
}
11411196

@@ -1144,40 +1199,40 @@ static THD_FUNCTION(cancom_status_thread, arg) {
11441199
conf->send_can_status == CAN_STATUS_1_2_3_4 ||
11451200
conf->send_can_status == CAN_STATUS_1_2_3_4_5) {
11461201
mc_interface_select_motor_thread(1);
1147-
send_status2(conf->controller_id, false);
1202+
comm_can_send_status2(conf->controller_id, false);
11481203
#ifdef HW_HAS_DUAL_MOTORS
11491204
mc_interface_select_motor_thread(2);
1150-
send_status2(utils_second_motor_id(), false);
1205+
comm_can_send_status2(utils_second_motor_id(), false);
11511206
#endif
11521207
}
11531208

11541209
if (conf->send_can_status == CAN_STATUS_1_2_3 ||
11551210
conf->send_can_status == CAN_STATUS_1_2_3_4 ||
11561211
conf->send_can_status == CAN_STATUS_1_2_3_4_5) {
11571212
mc_interface_select_motor_thread(1);
1158-
send_status3(conf->controller_id, false);
1213+
comm_can_send_status3(conf->controller_id, false);
11591214
#ifdef HW_HAS_DUAL_MOTORS
11601215
mc_interface_select_motor_thread(2);
1161-
send_status3(utils_second_motor_id(), false);
1216+
comm_can_send_status3(utils_second_motor_id(), false);
11621217
#endif
11631218
}
11641219

11651220
if (conf->send_can_status == CAN_STATUS_1_2_3_4 ||
11661221
conf->send_can_status == CAN_STATUS_1_2_3_4_5) {
11671222
mc_interface_select_motor_thread(1);
1168-
send_status4(conf->controller_id, false);
1223+
comm_can_send_status4(conf->controller_id, false);
11691224
#ifdef HW_HAS_DUAL_MOTORS
11701225
mc_interface_select_motor_thread(2);
1171-
send_status4(utils_second_motor_id(), false);
1226+
comm_can_send_status4(utils_second_motor_id(), false);
11721227
#endif
11731228
}
11741229

11751230
if (conf->send_can_status == CAN_STATUS_1_2_3_4_5) {
11761231
mc_interface_select_motor_thread(1);
1177-
send_status5(conf->controller_id, false);
1232+
comm_can_send_status5(conf->controller_id, false);
11781233
#ifdef HW_HAS_DUAL_MOTORS
11791234
mc_interface_select_motor_thread(2);
1180-
send_status5(utils_second_motor_id(), false);
1235+
comm_can_send_status5(utils_second_motor_id(), false);
11811236
#endif
11821237
}
11831238
}
@@ -1550,6 +1605,13 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced)
15501605
#endif
15511606
} break;
15521607

1608+
case CAN_PACKET_UPDATE_PID_POS_OFFSET: {
1609+
ind = 0;
1610+
float angle_now = buffer_get_float32(data8, 1e4, &ind);
1611+
bool store = data8[ind++];
1612+
mc_interface_update_pid_pos_offset(angle_now, store);
1613+
} break;
1614+
15531615
default:
15541616
break;
15551617
}
@@ -1710,55 +1772,6 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced)
17101772
#endif
17111773
}
17121774

1713-
static void send_status1(uint8_t id, bool replace) {
1714-
int32_t send_index = 0;
1715-
uint8_t buffer[8];
1716-
buffer_append_int32(buffer, (int32_t)mc_interface_get_rpm(), &send_index);
1717-
buffer_append_int16(buffer, (int16_t)(mc_interface_get_tot_current_filtered() * 1e1), &send_index);
1718-
buffer_append_int16(buffer, (int16_t)(mc_interface_get_duty_cycle_now() * 1e3), &send_index);
1719-
comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS << 8),
1720-
buffer, send_index, replace);
1721-
}
1722-
1723-
static void send_status2(uint8_t id, bool replace) {
1724-
int32_t send_index = 0;
1725-
uint8_t buffer[8];
1726-
buffer_append_int32(buffer, (int32_t)(mc_interface_get_amp_hours(false) * 1e4), &send_index);
1727-
buffer_append_int32(buffer, (int32_t)(mc_interface_get_amp_hours_charged(false) * 1e4), &send_index);
1728-
comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_2 << 8),
1729-
buffer, send_index, replace);
1730-
}
1731-
1732-
static void send_status3(uint8_t id, bool replace) {
1733-
int32_t send_index = 0;
1734-
uint8_t buffer[8];
1735-
buffer_append_int32(buffer, (int32_t)(mc_interface_get_watt_hours(false) * 1e4), &send_index);
1736-
buffer_append_int32(buffer, (int32_t)(mc_interface_get_watt_hours_charged(false) * 1e4), &send_index);
1737-
comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_3 << 8),
1738-
buffer, send_index, replace);
1739-
}
1740-
1741-
static void send_status4(uint8_t id, bool replace) {
1742-
int32_t send_index = 0;
1743-
uint8_t buffer[8];
1744-
buffer_append_int16(buffer, (int16_t)(mc_interface_temp_fet_filtered() * 1e1), &send_index);
1745-
buffer_append_int16(buffer, (int16_t)(mc_interface_temp_motor_filtered() * 1e1), &send_index);
1746-
buffer_append_int16(buffer, (int16_t)(mc_interface_get_tot_current_in_filtered() * 1e1), &send_index);
1747-
buffer_append_int16(buffer, (int16_t)(mc_interface_get_pid_pos_now() * 50.0), &send_index);
1748-
comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_4 << 8),
1749-
buffer, send_index, replace);
1750-
}
1751-
1752-
static void send_status5(uint8_t id, bool replace) {
1753-
int32_t send_index = 0;
1754-
uint8_t buffer[8];
1755-
buffer_append_int32(buffer, mc_interface_get_tachometer_value(false), &send_index);
1756-
buffer_append_int16(buffer, (int16_t)(mc_interface_get_input_voltage_filtered() * 1e1), &send_index);
1757-
buffer_append_int16(buffer, 0, &send_index); // Reserved for now
1758-
comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_5 << 8),
1759-
buffer, send_index, replace);
1760-
}
1761-
17621775
#endif
17631776

17641777
/**

‎comm_can.h

+7
Original file line numberDiff line numberDiff line change
@@ -81,7 +81,14 @@ void comm_can_io_board_set_output_pwm(int id, int channel, float duty);
8181
psw_status *comm_can_get_psw_status_index(int index);
8282
psw_status *comm_can_get_psw_status_id(int id);
8383
void comm_can_psw_switch(int id, bool is_on, bool plot);
84+
void comm_can_update_pid_pos_offset(int id, float angle_now, bool store);
8485

8586
CANRxFrame *comm_can_get_rx_frame(void);
8687

88+
void comm_can_send_status1(uint8_t id, bool replace);
89+
void comm_can_send_status2(uint8_t id, bool replace);
90+
void comm_can_send_status3(uint8_t id, bool replace);
91+
void comm_can_send_status4(uint8_t id, bool replace);
92+
void comm_can_send_status5(uint8_t id, bool replace);
93+
8794
#endif /* COMM_CAN_H_ */

‎commands.c

+4
Original file line numberDiff line numberDiff line change
@@ -194,6 +194,10 @@ void commands_process_packet(unsigned char *data, unsigned int len,
194194
reply_func = commands_send_packet;
195195
}
196196

197+
if (!send_func_can_fwd) {
198+
send_func_can_fwd = reply_func;
199+
}
200+
197201
switch (packet_id) {
198202
case COMM_FW_VERSION: {
199203
int32_t ind = 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 42
27+
#define FW_TEST_VERSION_NUMBER 44
2828

2929
#include "datatypes.h"
3030

‎confgenerator.c

+6
Original file line numberDiff line numberDiff line change
@@ -143,9 +143,11 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *
143143
buffer_append_float32_auto(buffer, conf->p_pid_kp, &ind);
144144
buffer_append_float32_auto(buffer, conf->p_pid_ki, &ind);
145145
buffer_append_float32_auto(buffer, conf->p_pid_kd, &ind);
146+
buffer_append_float32_auto(buffer, conf->p_pid_kd_proc, &ind);
146147
buffer_append_float32_auto(buffer, conf->p_pid_kd_filter, &ind);
147148
buffer_append_float32_auto(buffer, conf->p_pid_ang_div, &ind);
148149
buffer_append_float16(buffer, conf->p_pid_gain_dec_angle, 10, &ind);
150+
buffer_append_float32_auto(buffer, conf->p_pid_offset, &ind);
149151
buffer_append_float32_auto(buffer, conf->cc_startup_boost_duty, &ind);
150152
buffer_append_float32_auto(buffer, conf->cc_min_current, &ind);
151153
buffer_append_float32_auto(buffer, conf->cc_gain, &ind);
@@ -503,9 +505,11 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c
503505
conf->p_pid_kp = buffer_get_float32_auto(buffer, &ind);
504506
conf->p_pid_ki = buffer_get_float32_auto(buffer, &ind);
505507
conf->p_pid_kd = buffer_get_float32_auto(buffer, &ind);
508+
conf->p_pid_kd_proc = buffer_get_float32_auto(buffer, &ind);
506509
conf->p_pid_kd_filter = buffer_get_float32_auto(buffer, &ind);
507510
conf->p_pid_ang_div = buffer_get_float32_auto(buffer, &ind);
508511
conf->p_pid_gain_dec_angle = buffer_get_float16(buffer, 10, &ind);
512+
conf->p_pid_offset = buffer_get_float32_auto(buffer, &ind);
509513
conf->cc_startup_boost_duty = buffer_get_float32_auto(buffer, &ind);
510514
conf->cc_min_current = buffer_get_float32_auto(buffer, &ind);
511515
conf->cc_gain = buffer_get_float32_auto(buffer, &ind);
@@ -859,9 +863,11 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) {
859863
conf->p_pid_kp = MCCONF_P_PID_KP;
860864
conf->p_pid_ki = MCCONF_P_PID_KI;
861865
conf->p_pid_kd = MCCONF_P_PID_KD;
866+
conf->p_pid_kd_proc = MCCONF_P_PID_KD_PROC;
862867
conf->p_pid_kd_filter = MCCONF_P_PID_KD_FILTER;
863868
conf->p_pid_ang_div = MCCONF_P_PID_ANG_DIV;
864869
conf->p_pid_gain_dec_angle = MCCONF_P_PID_GAIN_DEC_ANGLE;
870+
conf->p_pid_offset = MCCONF_P_PID_OFFSET;
865871
conf->cc_startup_boost_duty = MCCONF_CC_STARTUP_BOOST_DUTY;
866872
conf->cc_min_current = MCCONF_CC_MIN_CURRENT;
867873
conf->cc_gain = MCCONF_CC_GAIN;

‎confgenerator.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
#include <stdbool.h>
99

1010
// Constants
11-
#define MCCONF_SIGNATURE 838668798
11+
#define MCCONF_SIGNATURE 2236691136
1212
#define APPCONF_SIGNATURE 2302012416
1313

1414
// Functions

‎datatypes.h

+3
Original file line numberDiff line numberDiff line change
@@ -422,9 +422,11 @@ typedef struct {
422422
float p_pid_kp;
423423
float p_pid_ki;
424424
float p_pid_kd;
425+
float p_pid_kd_proc;
425426
float p_pid_kd_filter;
426427
float p_pid_ang_div;
427428
float p_pid_gain_dec_angle;
429+
float p_pid_offset;
428430

429431
// Current controller
430432
float cc_startup_boost_duty;
@@ -1057,6 +1059,7 @@ typedef enum {
10571059
CAN_PACKET_BMS_HW_DATA_5,
10581060
CAN_PACKET_BMS_AH_WH_CHG_TOTAL,
10591061
CAN_PACKET_BMS_AH_WH_DIS_TOTAL,
1062+
CAN_PACKET_UPDATE_PID_POS_OFFSET,
10601063
CAN_PACKET_MAKE_ENUM_32_BITS = 0xFFFFFFFF
10611064
} CAN_PACKET_ID;
10621065

‎mc_interface.c

+21
Original file line numberDiff line numberDiff line change
@@ -655,6 +655,7 @@ void mc_interface_set_pid_pos(float pos) {
655655

656656
motor_now()->m_position_set = pos;
657657

658+
pos += motor_now()->m_conf.p_pid_offset;
658659
pos *= DIR_MULT;
659660
utils_norm_angle(&pos);
660661

@@ -1359,11 +1360,31 @@ float mc_interface_get_pid_pos_now(void) {
13591360
}
13601361

13611362
ret *= DIR_MULT;
1363+
ret -= motor_now()->m_conf.p_pid_offset;
13621364
utils_norm_angle(&ret);
13631365

13641366
return ret;
13651367
}
13661368

1369+
/**
1370+
* Update the offset such that the current angle becomes angle_now
1371+
*/
1372+
void mc_interface_update_pid_pos_offset(float angle_now, bool store) {
1373+
mc_configuration *mcconf = mempools_alloc_mcconf();
1374+
*mcconf = *mc_interface_get_configuration();
1375+
1376+
mcconf->p_pid_offset += mc_interface_get_pid_pos_now() - angle_now;
1377+
utils_norm_angle(&mcconf->p_pid_offset);
1378+
1379+
if (store) {
1380+
conf_general_store_mc_configuration(mcconf, mc_interface_get_motor_thread() == 2);
1381+
}
1382+
1383+
mc_interface_set_configuration(mcconf);
1384+
1385+
mempools_free_mcconf(mcconf);
1386+
}
1387+
13671388
float mc_interface_get_last_sample_adc_isr_duration(void) {
13681389
return m_last_adc_duration_sample;
13691390
}

‎mc_interface.h

+1
Original file line numberDiff line numberDiff line change
@@ -80,6 +80,7 @@ float mc_interface_read_reset_avg_vd(void);
8080
float mc_interface_read_reset_avg_vq(void);
8181
float mc_interface_get_pid_pos_set(void);
8282
float mc_interface_get_pid_pos_now(void);
83+
void mc_interface_update_pid_pos_offset(float angle_now, bool store);
8384
float mc_interface_get_last_sample_adc_isr_duration(void);
8485
void mc_interface_sample_print_data(debug_sampling_mode mode, uint16_t len, uint8_t decimation);
8586
float mc_interface_temp_fet_filtered(void);

0 commit comments

Comments
 (0)