Added uart functions

vedderb · Sep 8, 2023 · 7d88898 · 7d88898
1 parent 4e0288d
commit 7d88898
Show file tree

Hide file tree

Showing 4 changed files with 144 additions and 0 deletions.
diff --git 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
@@ -1234,6 +1234,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
@@ -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
@@ -993,6 +993,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,