Merge pull request #41 from CarbonixUAV/feature/OV3-809-hv-pro-esc-co…

…de-changes OV3-809 fixed code as per suggestion
tridge · Jun 28, 2023 · bcb646f · bcb646f
2 parents e7e9075 + da799c6
commit bcb646f
Show file tree

Hide file tree

Showing 4 changed files with 47 additions and 37 deletions.
diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt
@@ -1,3 +1,9 @@
+Release Ottano Carbopilot V4.3.14 28th Jun 2023
+----------------------------------
+
+This release includes: 
+ - OV3-809: AP_Periph: APDHVPRo ESC fixed code as per suggestion
+
 Release Ottano Carbopilot V4.3.13 28th Jun 2023
 ----------------------------------
 

diff --git a/ArduPlane/version.h b/ArduPlane/version.h
@@ -6,7 +6,7 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "Ottano Carbopilot V4.3.13"
+#define THISFIRMWARE "Ottano Carbopilot V4.3.14"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 4,2,1,FIRMWARE_VERSION_TYPE_OFFICIAL

diff --git a/Tools/AP_Periph/AP_ESC_APDHVPro.cpp b/Tools/AP_Periph/AP_ESC_APDHVPro.cpp
@@ -13,9 +13,7 @@ AP_ESC_APDHVPro::AP_ESC_APDHVPro(void){
 void AP_ESC_APDHVPro::init(AP_HAL::UARTDriver *_uart) {
 
     port = _uart;
-    port->begin(57600);
-
-    hal.serial(1)->begin(115200, 128, 256);
+    port->begin(115200, 128, 256);
 }
 
 bool AP_ESC_APDHVPro::update() {
@@ -29,14 +27,16 @@ bool AP_ESC_APDHVPro::update() {
     if (size >= ESC_PACKET_SIZE) {
 
         if (size > ESC_PACKET_SIZE * 2) {
+            port->discard_input();
         }
         else {
             int bytes_read = port->read(max_buffer, size);
 
             if (read_ESC_telemetry_data(bytes_read)) {
-                last_read_ms = AP_HAL::native_millis();
-
                 status = parse_ESC_telemetry_data();
+                if (status) {
+                    last_read_ms = AP_HAL::native_millis();
+                } 
             }
         }
     }
@@ -62,45 +62,49 @@ bool AP_ESC_APDHVPro::read_ESC_telemetry_data(uint32_t bytes_read) {
 
 bool AP_ESC_APDHVPro::parse_ESC_telemetry_data() {
     bool status = false;
+    float temperature = 0.0;
 
     int check_fletch = check_flectcher16();
+    decoded.checksum = (uint16_t)((raw_buffer[19] << 8) | raw_buffer[18]);
 
-    //decoded.voltage = (float)(((raw_buffer[1] << 8) | raw_buffer[0])/100.0);
-    decoded.voltage = (float)(((((raw_buffer[1] << 8) | raw_buffer[0])/100.0) - HVPRO_CALIB_INTCPT)/HVPRO_CALIB_SLOPE);
-
-    float rntc = (TEMPERATURE_MAX_RESOLUTION / (float)((raw_buffer[3] << 8) | raw_buffer[2])) - 1;
+    // Currently, checksum code is not mandated
+    if (check_fletch == (int)decoded.checksum) {
+        status = true;
 
-    rntc = SERIESRESISTOR / rntc;
+        decoded.voltage = (float)(((((raw_buffer[1] << 8) | raw_buffer[0]) / 100.0) - HVPRO_CALIB_INTCPT) / HVPRO_CALIB_SLOPE);
 
-    float temperature = rntc / (float)NOMINAL_RESISTANCE;
-    temperature = (float)logF(temperature);
-    temperature /= BCOEFFICIENT;
+        float temp = (float)((raw_buffer[3] << 8) | raw_buffer[2]);
+        if (temp > 0)
+        {
+            float rntc = (TEMPERATURE_MAX_RESOLUTION / temp) - 1;
 
-    temperature += (float)1.0 / ((float)NOMINAL_TEMPERATURE + (float)273.15);
-    temperature = (float)1.0 / temperature;
-    temperature -= (float)273.15;
+            rntc = SERIESRESISTOR / rntc;
 
-    // filter bad values
-    if (temperature < 0 || temperature > 200){
-        temperature = 0;
-    }
+            temperature = rntc / (float)NOMINAL_RESISTANCE;
+            temperature = (float)logF(temperature);
+            temperature /= BCOEFFICIENT;
 
-    decoded.temperature = ((float)trunc(temperature * 100) / 100) + 273.15;  // Temperature
-
-    float temp_current = (short)((raw_buffer[5] << 8) | raw_buffer[4]);
-    temp_current /= CURRENT_COEFFICIENT;
-    decoded.bus_current = (temp_current);
-    decoded.reserved1 = (uint16_t)((raw_buffer[7] << 8) | raw_buffer[6]);
-    decoded.rpm = ((uint32_t)((raw_buffer[11] << 24) | (raw_buffer[10] << 16) | (raw_buffer[9] << 8) | raw_buffer[8])) / POLEPAIRS;
-    decoded.input_duty = (uint16_t)((raw_buffer[13] << 8) | raw_buffer[12]) / 10;
-    decoded.motor_duty = (uint16_t)((raw_buffer[15] << 8) | raw_buffer[14]) / 10;
-    decoded.status = raw_buffer[16];
-    decoded.reserved2 = raw_buffer[17];
-    decoded.checksum = (uint16_t)((raw_buffer[19] << 8) | raw_buffer[18]);
+            temperature += (float)1.0 / ((float)NOMINAL_TEMPERATURE + (float)273.15);
+            temperature = (temperature > 0) ? (float)1.0 / temperature : 0;
+            temperature -= (float)273.15;
+        }
 
-    // Currently, checksum code is not mandated
-    if (check_fletch == (int)decoded.checksum) { 
-        status = true;
+        // filter bad values
+        if (temperature < 0 || temperature > 200) {
+            temperature = 0;
+        }
+
+        decoded.temperature = ((float)trunc(temperature * 100) / 100) + 273.15; // Temperature
+
+        float temp_current = (short)((raw_buffer[5] << 8) | raw_buffer[4]);
+        temp_current /= CURRENT_COEFFICIENT;
+        decoded.bus_current = (temp_current);
+        decoded.reserved1 = (uint16_t)((raw_buffer[7] << 8) | raw_buffer[6]);
+        decoded.rpm = ((uint32_t)((raw_buffer[11] << 24) | (raw_buffer[10] << 16) | (raw_buffer[9] << 8) | raw_buffer[8])) / POLEPAIRS;
+        decoded.input_duty = (uint16_t)((raw_buffer[13] << 8) | raw_buffer[12]) / 10;
+        decoded.motor_duty = (uint16_t)((raw_buffer[15] << 8) | raw_buffer[14]) / 10;
+        decoded.status = raw_buffer[16];
+        decoded.reserved2 = raw_buffer[17];
     }
 
     return status;

diff --git a/Tools/AP_Periph/version.h b/Tools/AP_Periph/version.h
@@ -3,7 +3,7 @@
 #ifndef __AP_PERIPH_FW_VERSION__
 #define __AP_PERIPH_FW_VERSION__
 
-#define THISFIRMWARE "Ottano Carbopilot V4.3.13"
+#define THISFIRMWARE "Ottano Carbopilot V4.3.14"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 1,3,0,FIRMWARE_VERSION_TYPE_DEV