Merge pull request #54 from openppg/refactor-esc

Refactor out ESC functions

.github/workflows/config.yml

@@ -16,7 +16,7 @@ jobs:
         with:
           python-version: 3.x
       - run: pip install cpplint
-      - run: cpplint --linelength 140 --filter=-legal/copyright,-runtime/int,-build/include_subdir --recursive ./inc/ ./lib/ ./src/
+      - run: cpplint --linelength 140 --filter=-legal/copyright,-runtime/int,-build/include_subdir,-readability/casting,-readability/todo --recursive ./inc/ ./lib/ ./src/
   pio-build:
     name: PlatformIO Build
     runs-on: ubuntu-latest

inc/sp140/esc.h

@@ -0,0 +1,30 @@
+#ifndef INC_SP140_ESC_H_
+#define INC_SP140_ESC_H_
+
+#include <Arduino.h>
+#include "sp140/structs.h"
+
+#ifdef M0_PIO
+  #include "../../inc/sp140/m0-config.h"
+#else
+  #include "../../inc/sp140/rp2040-config.h"
+#endif
+
+// ESC communication parameters
+#define ESC_BAUD_RATE         115200
+#define ESC_DATA_V2_SIZE      22
+#define READ_INTERVAL         0
+#define ESC_TIMEOUT           15
+
+void initESC(int escPin);
+void setupESCSerial();
+void setESCThrottle(int throttlePWM);
+void readESCTelemetry();
+void handleESCSerialData(byte buffer[]);
+void prepareESCSerialRead();
+int checkFletcher16(byte byteBuffer[]);
+
+// External declaration of telemetry data structure
+extern STR_ESC_TELEMETRY_140 escTelemetryData;
+
+#endif  // INC_SP140_ESC_H_

inc/sp140/globals.h

@@ -2,22 +2,20 @@
 #ifndef INC_SP140_GLOBALS_H_
 #define INC_SP140_GLOBALS_H_
 
-byte escData[ESC_DATA_SIZE];
-byte escDataV2[ESC_DATA_V2_SIZE];
-unsigned long cruisedAtMillis = 0;
-volatile bool cruising = false;
-int prevPotLvl = 0;
-int cruisedPotVal = 0;
-volatile float throttlePWM = 0;
-bool throttledFlag = true;
+#include "sp140/shared-config.h"
+#include "sp140/structs.h"
 
-float watts = 0;
-float wattHoursUsed = 0;
+extern unsigned long cruisedAtMillis;
+extern volatile bool cruising;
+extern int prevPotLvl;
+extern int cruisedPotVal;
+extern volatile float throttlePWM;
 
-int16_t _amps = 0;
+extern float watts;
+extern float wattHoursUsed;
 
-Servo esc;  // Creating a servo class with name of esc
+extern STR_DEVICE_DATA_140_V1 deviceData;
 
-static STR_DEVICE_DATA_140_V1 deviceData;
+extern STR_ESC_TELEMETRY_140 escTelemetryData;
 
 #endif  // INC_SP140_GLOBALS_H_

inc/sp140/shared-config.h

@@ -24,12 +24,6 @@
 #define ESC_MIN_PWM           1030  // ESC min is 1050
 #define ESC_MAX_PWM           1990  // ESC max 1950
 
-
-#define ESC_BAUD_RATE         115200
-#define ESC_DATA_SIZE         20
-#define ESC_DATA_V2_SIZE      22
-#define READ_INTERVAL         0
-#define ESC_TIMEOUT           15
 #define ENABLE_BUZ            true    // enable buzzer
 #define ENABLE_VIB_LOW_BAT    false   // vibrate if armed and battery voltage sags below min volts. Gets pilot's attention.
 #define POT_MAX_VALUE         4095    // 12 bit ADC //TODO use calibration and store in EEPROM

inc/sp140/structs.h

@@ -73,7 +73,7 @@ typedef struct  {
   // checksum
   int CSUM_HI;
   int CSUM_LO;
-} telem_t;
+} telem_esc_t;
 
 // Tone struct (passed between cores)
 typedef struct {
@@ -87,8 +87,6 @@ struct BatteryVoltagePoint {
 };
 #pragma pack(pop)
 
-static STR_ESC_TELEMETRY_140 telemetryData;
-static telem_t raw_telemdata;
 
 static BatteryVoltagePoint batteryLevels[] = {
   {99.6, 100},  // 99+ volts corresponds to 100%

platformio.ini

@@ -30,7 +30,7 @@ extra_scripts = pre:extra_script.py
 lib_deps =
 	Adafruit GFX [email protected]
 	[email protected]
-	Adafruit DRV2605 Library@1.1.2
+	adafruit/Adafruit DRV2605 Library@1.2.4
 	Adafruit SleepyDog [email protected]
 	Adafruit TinyUSB [email protected]
 	[email protected]
@@ -40,7 +40,7 @@ lib_deps =
 	[email protected]
 	adafruit/Adafruit [email protected]
 	adafruit/Adafruit BMP3XX [email protected]
-	adafruit/Adafruit ST7735 and ST7789 Library@1.6.1
+	adafruit/Adafruit ST7735 and ST7789 Library@1.10.4
 	https://github.com/rlogiacco/[email protected]
 lib_ignore =
 	${extra.lib_ignore}
@@ -71,13 +71,13 @@ lib_deps =
 	[email protected]
 	[email protected] ; deprecated
 	[email protected]
-	adafruit/Adafruit BusIO@1.14.5
+	adafruit/Adafruit BusIO@1.16.1
 	adafruit/Adafruit BMP3XX [email protected]
 	adafruit/Adafruit DRV2605 [email protected]
-	adafruit/Adafruit ST7735 and ST7789 [email protected].3
+	adafruit/Adafruit ST7735 and ST7789 [email protected].4
 	adafruit/Adafruit [email protected]
 	https://github.com/rlogiacco/[email protected]
-	Adafruit GFX [email protected].9
+	Adafruit GFX [email protected].10
 lib_ignore =
 	Adafruit SleepyDog Library
 	${extra.lib_ignore}

src/sp140/esc.cpp

@@ -0,0 +1,208 @@
+#include "sp140/esc.h"
+#include <Servo.h>
+#include "sp140/globals.h"
+#include <CircularBuffer.hpp>
+
+Servo esc;
+extern CircularBuffer<float, 50> voltageBuffer;
+
+STR_ESC_TELEMETRY_140 escTelemetryData;
+static telem_esc_t raw_esc_telemdata;
+
+void initESC(int escPin) {
+  esc.attach(escPin);
+  esc.writeMicroseconds(ESC_DISARMED_PWM);
+  setupESCSerial();
+}
+
+void setupESCSerial() {
+  SerialESC.begin(ESC_BAUD_RATE);
+  SerialESC.setTimeout(ESC_TIMEOUT);
+}
+
+void setESCThrottle(int throttlePWM) {
+  esc.writeMicroseconds(throttlePWM);
+}
+
+void readESCTelemetry() {
+  prepareESCSerialRead();
+  static byte escDataV2[ESC_DATA_V2_SIZE];
+  SerialESC.readBytes(escDataV2, ESC_DATA_V2_SIZE);
+  handleESCSerialData(escDataV2);
+}
+
+void prepareESCSerialRead() {
+  while (SerialESC.available() > 0) {
+    SerialESC.read();
+  }
+}
+
+void handleESCSerialData(byte buffer[]) {
+  // if(sizeof(buffer) != 22) {
+  //     Serial.print("wrong size ");
+  //     Serial.println(sizeof(buffer));
+  //     return; //Ignore malformed packets
+  // }
+
+  if (buffer[20] != 255 || buffer[21] != 255) {
+    // Serial.println("no stop byte");
+
+    return;  // Stop byte of 65535 not received
+  }
+
+  // Check the fletcher checksum
+  int checkFletch = checkFletcher16(buffer);
+
+  // checksum
+  raw_esc_telemdata.CSUM_HI = buffer[19];
+  raw_esc_telemdata.CSUM_LO = buffer[18];
+
+  // TODO: alert if no new data in 3 seconds
+  int checkCalc = (int)(((raw_esc_telemdata.CSUM_HI << 8) + raw_esc_telemdata.CSUM_LO));
+
+  // Checksums do not match
+  if (checkFletch != checkCalc) {
+    return;
+  }
+  // Voltage
+  raw_esc_telemdata.V_HI = buffer[1];
+  raw_esc_telemdata.V_LO = buffer[0];
+
+  float voltage = (raw_esc_telemdata.V_HI << 8 | raw_esc_telemdata.V_LO) / 100.0;
+  escTelemetryData.volts = voltage;  // Voltage
+
+  if (escTelemetryData.volts > BATT_MIN_V) {
+    escTelemetryData.volts += 1.0;  // calibration
+  }
+
+  voltageBuffer.push(escTelemetryData.volts);
+
+  // Temperature
+  raw_esc_telemdata.T_HI = buffer[3];
+  raw_esc_telemdata.T_LO = buffer[2];
+
+  float rawVal = (float)((raw_esc_telemdata.T_HI << 8) + raw_esc_telemdata.T_LO);
+
+  static int SERIESRESISTOR = 10000;
+  static int NOMINAL_RESISTANCE = 10000;
+  static int NOMINAL_TEMPERATURE = 25;
+  static int BCOEFFICIENT = 3455;
+
+  // convert value to resistance
+  float Rntc = (4096 / (float) rawVal) - 1;
+  Rntc = SERIESRESISTOR / Rntc;
+
+  // Get the temperature
+  float temperature = Rntc / (float) NOMINAL_RESISTANCE;  // (R/Ro)
+  temperature = (float) log(temperature);  // ln(R/Ro)
+  temperature /= BCOEFFICIENT;  // 1/B * ln(R/Ro)
+
+  temperature += 1.0 / ((float) NOMINAL_TEMPERATURE + 273.15);  // + (1/To)
+  temperature = 1.0 / temperature;  // Invert
+  temperature -= 273.15;  // convert to Celsius
+
+  // filter bad values
+  if (temperature < 0 || temperature > 200) {
+    escTelemetryData.temperatureC = __FLT_MIN__;
+  } else {
+    temperature = (float) trunc(temperature * 100) / 100;  // 2 decimal places
+    escTelemetryData.temperatureC = temperature;
+  }
+
+  // Current
+  int16_t _amps = 0;
+  _amps = word(buffer[5], buffer[4]);
+  escTelemetryData.amps = _amps / 12.5;
+
+  // Serial.print("amps ");
+  // Serial.print(currentAmpsInput);
+  // Serial.print(" - ");
+
+  watts = escTelemetryData.amps * escTelemetryData.volts;
+
+  // Reserved
+  raw_esc_telemdata.R0_HI = buffer[7];
+  raw_esc_telemdata.R0_LO = buffer[6];
+
+  // eRPM
+  raw_esc_telemdata.RPM0 = buffer[11];
+  raw_esc_telemdata.RPM1 = buffer[10];
+  raw_esc_telemdata.RPM2 = buffer[9];
+  raw_esc_telemdata.RPM3 = buffer[8];
+
+  int poleCount = 62;
+  uint32_t rawERPM = (raw_esc_telemdata.RPM0 << 24) |
+                     (raw_esc_telemdata.RPM1 << 16) |
+                     (raw_esc_telemdata.RPM2 << 8) |
+                     raw_esc_telemdata.RPM3;
+  int currentERPM = static_cast<int>(rawERPM);  // ERPM output
+  int currentRPM = currentERPM / poleCount;  // Real RPM output
+  escTelemetryData.eRPM = currentRPM;
+
+  // Serial.print("RPM ");
+  // Serial.print(currentRPM);
+  // Serial.print(" - ");
+
+  // Input Duty
+  raw_esc_telemdata.DUTYIN_HI = buffer[13];
+  raw_esc_telemdata.DUTYIN_LO = buffer[12];
+
+  int throttleDuty = (int)(((raw_esc_telemdata.DUTYIN_HI << 8) + raw_esc_telemdata.DUTYIN_LO) / 10);
+  escTelemetryData.inPWM = (throttleDuty / 10);  // Input throttle
+
+  // Serial.print("throttle ");
+  // Serial.print(escTelemetryData.inPWM);
+  // Serial.print(" - ");
+
+  // Motor Duty
+  // raw_esc_telemdata.MOTORDUTY_HI = buffer[15];
+  // raw_esc_telemdata.MOTORDUTY_LO = buffer[14];
+
+  // int motorDuty = (int)(((raw_esc_telemdata.MOTORDUTY_HI << 8) + raw_esc_telemdata.MOTORDUTY_LO) / 10);
+  // int currentMotorDuty = (motorDuty / 10);  // Motor duty cycle
+
+  // Reserved
+  // raw_esc_telemdata.R1 = buffer[17];
+
+  /* Status Flags
+  # Bit position in byte indicates flag set, 1 is set, 0 is default
+  # Bit 0: Motor Started, set when motor is running as expected
+  # Bit 1: Motor Saturation Event, set when saturation detected and power is reduced for desync protection
+  # Bit 2: ESC Over temperature event occurring, shut down method as per configuration
+  # Bit 3: ESC Overvoltage event occurring, shut down method as per configuration
+  # Bit 4: ESC Undervoltage event occurring, shut down method as per configuration
+  # Bit 5: Startup error detected, motor stall detected upon trying to start*/
+  raw_esc_telemdata.statusFlag = buffer[16];
+  escTelemetryData.statusFlag = raw_esc_telemdata.statusFlag;
+  // Serial.print("status ");
+  // Serial.print(raw_esc_telemdata.statusFlag, BIN);
+  // Serial.print(" - ");
+  // Serial.println(" ");
+}
+
+// new V2 ESC checking
+int checkFletcher16(byte byteBuffer[]) {
+  int fCCRC16;
+  int i;
+  int c0 = 0;
+  int c1 = 0;
+
+  // Calculate checksum intermediate bytesUInt16
+  for (i = 0; i < 18; i++) {  // Check only first 18 bytes, skip crc bytes
+    c0 = (int)(c0 + ((int)byteBuffer[i])) % 255;
+    c1 = (int)(c1 + c0) % 255;
+  }
+  // Assemble the 16-bit checksum value
+  fCCRC16 = (c1 << 8) | c0;
+  return (int)fCCRC16;
+}
+
+// for debugging
+static void printRawSentence(byte buffer[]) {
+  Serial.print(F("DATA: "));
+  for (int i = 0; i < ESC_DATA_V2_SIZE; i++) {
+    Serial.print(buffer[i], HEX);
+    Serial.print(F(" "));
+  }
+  Serial.println();
+}

src/sp140/globals.cpp

@@ -0,0 +1,39 @@
+/*
+ * globals.cpp
+ *
+ * This file exists to define global variables that are used across multiple
+ * files in the project. It's paired with globals.h, which declares these
+ * variables as extern.
+ *
+ * IMPORTANT NOTE:
+ * Global variables should be avoided whenever possible as they can lead to:
+ * 1. Difficulty in tracking state changes
+ * 2. Increased coupling between different parts of the code
+ * 3. Potential thread-safety issues in multi-threaded environments
+ * 4. Challenges in unit testing
+ *
+ * Instead, consider using:
+ * - Function parameters
+ * - Return values
+ * - Class member variables
+ * - Dependency injection
+ *
+ * If you find yourself needing to add a new global variable, first consider
+ * if there's a way to refactor the code to avoid it. If a global is truly
+ * necessary, add it here and in globals.h, and document why it's needed.
+ */
+
+#include "Arduino.h"
+
+#include "sp140/globals.h"
+
+unsigned long cruisedAtMillis = 0;
+volatile bool cruising = false;
+int prevPotLvl = 0;
+int cruisedPotVal = 0;
+volatile float throttlePWM = 0;
+
+float watts = 0;
+float wattHoursUsed = 0;
+
+STR_DEVICE_DATA_140_V1 deviceData;