From 3048273ce4914580e368bb32407112a54e93cabe Mon Sep 17 00:00:00 2001
From: akako <69710421+akakoziqi@users.noreply.github.com>
Date: Sat, 14 Dec 2024 11:09:56 +0800
Subject: [PATCH] change float calc to fixed point calc

---
 Src/main.c | 41 +++++++++++++++++++++--------------------
 1 file changed, 21 insertions(+), 20 deletions(-)

diff --git a/Src/main.c b/Src/main.c
index 8e7485c6..f1480aec 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -346,10 +346,10 @@ char return_to_center = 0;
 uint16_t target_e_com_time = 0;
 int16_t Speed_pid_output;
 char use_speed_control_loop = 0;
-float input_override = 0;
+int32_t input_override = 0;
 int16_t use_current_limit_adjust = 2000;
 char use_current_limit = 0;
-float stall_protection_adjust = 0;
+int32_t stall_protection_adjust = 0;
 
 uint32_t MCU_Id = 0;
 uint32_t REV_Id = 0;
@@ -375,7 +375,7 @@ char cell_count = 0;
 char brushed_direction_set = 0;
 
 uint16_t tenkhzcounter = 0;
-float consumed_current = 0;
+int32_t consumed_current = 0;
 int32_t smoothed_raw_current = 0;
 int16_t actual_current = 0;
 
@@ -553,7 +553,7 @@ uint16_t signaltimeout = 0;
 uint8_t ubAnalogWatchdogStatus = RESET;
 
 
-float doPidCalculations(struct fastPID* pidnow, int actual, int target)
+int32_t doPidCalculations(struct fastPID* pidnow, int actual, int target)
 {
 
     pidnow->error = actual - target;
@@ -1045,21 +1045,21 @@ void setInput()
                         speedPid.error = 0;
                         input_override = 0;
                     } else {
-                        input = (uint16_t)input_override; // speed control pid override
-                        if (input_override > 2047) {
+                        input = (uint16_t)(input_override / 10000); // speed control pid override
+                        if (input > 2047) {
                             input = 2047;
                         }
-                        if (input_override < 48) {
+                        if (input < 48) {
                             input = 48;
                         }
                     }
                 } else {
 
-                    input = (uint16_t)input_override; // speed control pid override
-                    if (input_override > 2047) {
+                    input = (uint16_t)(input_override / 10000); // speed control pid override
+                    if (input > 2047) {
                         input = 2047;
                     }
-                    if (input_override < 48) {
+                    if (input < 48) {
                         input = 48;
                     }
                 }
@@ -1202,7 +1202,7 @@ if (!stepper_sine && armed) {
 
             if (stall_protection_adjust > 0 && input > 47) {
 
-                duty_cycle_setpoint = duty_cycle_setpoint + (uint16_t)stall_protection_adjust;
+                duty_cycle_setpoint = duty_cycle_setpoint + (uint16_t)(stall_protection_adjust/10000);
             }
         }
     }
@@ -1309,18 +1309,18 @@ void tenKhzRoutine()
             if (eepromBuffer.stall_protection && running) { // this boosts throttle as the rpm gets lower, for crawlers
                                                // and rc cars only, do not use for multirotors.
                 stall_protection_adjust += (doPidCalculations(&stallPid, commutation_interval,
-                                               stall_protect_target_interval))/ 10000;
-                if (stall_protection_adjust > 150) {
-                    stall_protection_adjust = 150;
+                                               stall_protect_target_interval));
+                if (stall_protection_adjust > 150 * 10000) {
+                    stall_protection_adjust = 150 * 10000;
                 }
                 if (stall_protection_adjust <= 0) {
                     stall_protection_adjust = 0;
                 }
             }
             if (use_speed_control_loop && running) {
-                input_override += doPidCalculations(&speedPid, e_com_time, target_e_com_time) / 10000;
-                if (input_override > 2047) {
-                    input_override = 2047;
+                input_override += doPidCalculations(&speedPid, e_com_time, target_e_com_time);
+                if (input_override > 2047 * 10000) {
+                    input_override = 2047 * 10000;
                 }
                 if (input_override < 0) {
                     input_override = 0;
@@ -1832,7 +1832,7 @@ if(zero_crosses < 5){
 #endif
 
         if (tenkhzcounter > LOOP_FREQUENCY_HZ) { // 1s sample interval 10000
-            consumed_current = (float)actual_current / 360 + consumed_current;
+            consumed_current += (actual_current << 16) / 360;
             switch (dshot_extended_telemetry) {
 
             case 1:
@@ -1909,7 +1909,7 @@ if(zero_crosses < 5){
         if (send_telemetry) {
 #ifdef USE_SERIAL_TELEMETRY
             makeTelemPackage(degrees_celsius, battery_voltage, actual_current,
-                (uint16_t)consumed_current, e_rpm);
+                (uint16_t)(consumed_current >> 16), e_rpm);
             send_telem_DMA();
             send_telemetry = 0;
 #endif
@@ -1923,7 +1923,8 @@ if(zero_crosses < 5){
 #endif
 #ifdef MCU_GDE23
             ADC_DMA_Callback();
-            converted_degrees = (1.43 - ADC_raw_temp * 3.3 / 4096) * 1000 / 4.3 + 25;
+            // converted_degrees = (1.43 - ADC_raw_temp * 3.3 / 4096) * 1000 / 4.3 + 25;
+            converted_degrees = ((int32_t)(357.5581395348837f * (1 << 16)) - ADC_raw_temp * (int32_t)(0.18736373546511628f * (1 << 16))) >> 16;
             adc_software_trigger_enable(ADC_REGULAR_CHANNEL);
 #endif
 #ifdef ARTERY