From 9a23689456b09ca552608287e17b60f61c79370f Mon Sep 17 00:00:00 2001 From: dr0ps Date: Thu, 16 Sep 2021 22:11:24 +0200 Subject: [PATCH] Added uart functions --- comm/comm_can.h | 1 + comm/commands.c | 135 ++++++++++++++++++++++++++++++++++++++++++++++++ comm/commands.h | 1 + datatypes.h | 7 +++ 4 files changed, 144 insertions(+) diff --git a/comm/comm_can.h b/comm/comm_can.h index 49754f95e..a2f21fe42 100644 --- a/comm/comm_can.h +++ b/comm/comm_can.h @@ -42,6 +42,7 @@ void comm_can_set_current_off_delay(uint8_t controller_id, float current, float void comm_can_set_current_brake(uint8_t controller_id, float current); void comm_can_set_rpm(uint8_t controller_id, float rpm); void comm_can_set_pos(uint8_t controller_id, float pos); +void comm_can_set_handbrake(uint8_t controller_id, float current); void comm_can_set_current_rel(uint8_t controller_id, float current_rel); void comm_can_set_current_rel_off_delay(uint8_t controller_id, float current_rel, float off_delay); void comm_can_set_current_brake_rel(uint8_t controller_id, float current_rel); diff --git a/comm/commands.c b/comm/commands.c index 56937cfed..ebd5d1c02 100644 --- a/comm/commands.c +++ b/comm/commands.c @@ -1232,6 +1232,141 @@ void commands_process_packet(unsigned char *data, unsigned int len, } } break; + case COMM_CAN_SET_DUTY: { + int32_t ind = 0; + float duty = (float)buffer_get_int32(data, &ind); + + for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) { + can_status_msg *msg = comm_can_get_status_msg_index(i); + + if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) { + comm_can_set_duty(msg->id, duty); + } + } + timeout_reset(); + } break; + + case COMM_CAN_SET_CURRENT: { + int32_t ind = 0; + float current = (float)buffer_get_int32(data, &ind); + for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) { + can_status_msg *msg = comm_can_get_status_msg_index(i); + if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) { + comm_can_set_current(msg->id, current / 1000.0); + } + } + timeout_reset(); + } break; + + case COMM_TC_SET_CURRENT: { + int32_t ind = 0; + float current = (float)buffer_get_int32(data, &ind); + float current_out = current; + + + float rpm_local = fabsf(mc_interface_get_rpm()); + float rpm_lowest = rpm_local; + + for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) { + can_status_msg *msg = comm_can_get_status_msg_index(i); + + if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) { + float rpm_tmp = fabsf(msg->rpm); + + if (rpm_tmp < rpm_lowest) { + rpm_lowest = rpm_tmp; + } + } + } + + mc_configuration *mcconf = mempools_alloc_mcconf(); + *mcconf = *mc_interface_get_configuration(); + + for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) { + can_status_msg *msg = comm_can_get_status_msg_index(i); + + if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) { + bool is_braking = (current > 0.0 && msg->duty < 0.0) || (current < 0.0 && msg->duty > 0.0); + + if (!is_braking) { + float rpm_tmp = fabsf(msg->rpm); + + float diff = rpm_tmp - rpm_lowest; + current_out = utils_map(diff, 0.0, 3000.0, current, 0.0); + if (fabsf(current_out) < mcconf->cc_min_current) { + current_out = 0.0; + } + } + + comm_can_set_current(msg->id, current_out / 1000.0); + } + } + + const float duty_now = mc_interface_get_duty_cycle_now(); + + bool is_braking = (current > 0.0 && duty_now < 0.0) || (current < 0.0 && duty_now > 0.0); + + if (!is_braking) { + float diff = rpm_local - rpm_lowest; + current_out = utils_map(diff, 0.0, 3000.0, current, 0.0); + if (fabsf(current_out) < mcconf->cc_min_current) { + current_out = 0.0; + } + } + + mempools_free_mcconf(mcconf); + mc_interface_set_current(current_out / 1000.0); + timeout_reset(); + } break; + + case COMM_CAN_SET_CURRENT_BRAKE: { + int32_t ind = 0; + float current = (float)buffer_get_int32(data, &ind); + for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) { + can_status_msg *msg = comm_can_get_status_msg_index(i); + if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) { + comm_can_set_current_brake(msg->id, current / 1000.0); + } + } + timeout_reset(); + } break; + + case COMM_CAN_SET_RPM: { + int32_t ind = 0; + float rpm = (float)buffer_get_int32(data, &ind); + for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) { + can_status_msg *msg = comm_can_get_status_msg_index(i); + if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) { + comm_can_set_rpm(msg->id, rpm); + } + } + timeout_reset(); + } break; + + case COMM_CAN_SET_POS: { + int32_t ind = 0; + float pos = (float)buffer_get_int32(data, &ind); + for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) { + can_status_msg *msg = comm_can_get_status_msg_index(i); + if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) { + comm_can_set_pos(msg->id, pos); + } + } + timeout_reset(); + } break; + + case COMM_CAN_SET_HANDBRAKE: { + int32_t ind = 0; + float handbrake = buffer_get_float32(data, 1e3, &ind); + for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) { + can_status_msg *msg = comm_can_get_status_msg_index(i); + if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) { + comm_can_set_handbrake(msg->id, handbrake); + } + } + timeout_reset(); + } break; + case COMM_SET_BATTERY_CUT: { int32_t ind = 0; float start = buffer_get_float32(data, 1e3, &ind); diff --git a/comm/commands.h b/comm/commands.h index d07c7baf6..25fd104da 100644 --- a/comm/commands.h +++ b/comm/commands.h @@ -21,6 +21,7 @@ #define COMMANDS_H_ #include "datatypes.h" +#define MAX_CAN_AGE 0.1 // Functions void commands_init(void); diff --git a/datatypes.h b/datatypes.h index e18711b28..46daf6d67 100644 --- a/datatypes.h +++ b/datatypes.h @@ -1005,6 +1005,13 @@ typedef enum { COMM_BM_WRITE_FLASH_LZO = 83, COMM_SET_CURRENT_REL = 84, COMM_CAN_FWD_FRAME = 85, + COMM_CAN_SET_DUTY = 153, + COMM_CAN_SET_CURRENT = 154, + COMM_CAN_SET_CURRENT_BRAKE = 155, + COMM_CAN_SET_RPM = 156, + COMM_CAN_SET_POS = 157, + COMM_CAN_SET_HANDBRAKE = 158, + COMM_TC_SET_CURRENT = 159, COMM_SET_BATTERY_CUT = 86, COMM_SET_BLE_NAME = 87, COMM_SET_BLE_PIN = 88,