Adding PT Calib

platformio.ini

@@ -38,6 +38,8 @@ monitor_speed = 115200
 upload_speed = 921600
 build_src_filter = -<*> +<Common.h> +<EspComms.*> +<EReg/*>
 build_flags = -DFUEL -DIP_ADDRESS_END=62 -DFW_COMMIT=\"1234567\" -DEREG
+monitor_echo = yes
+monitor_filters = send_on_enter, colorize, time
 
 [env:LOX Tank EReg]
 platform = espressif32
@@ -47,6 +49,8 @@ monitor_speed = 115200
 upload_speed = 921600
 build_src_filter = -<*> +<Common.h> +<EspComms.*> +<EReg/*>
 build_flags = -DLOX -DIP_ADDRESS_END=61 -DFW_COMMIT=\"1234567\" -DEREG
+monitor_echo = yes
+monitor_filters = send_on_enter, colorize, time
 
 
 

src/EReg/Config.h

@@ -42,7 +42,7 @@ namespace Config {
     const unsigned long rampDuration = 500UL * 1000UL; // time in microseconds
 
     // Pressurization Parameters
-    const unsigned long pressurizationRampDuration = 30 * 1000UL * 1000UL;
+    const unsigned long pressurizationRampDuration = 60 * 1000UL * 1000UL;
     const float pressurizationCutoff = pressureSetpoint * 0.99;
     const unsigned long tankPidStart = 0; // time in microseconds
     const unsigned long tankPidFull = 1 * 1000UL * 1000UL; // time in microseconds

src/EReg/Ducers.cpp

@@ -3,6 +3,30 @@
 namespace Ducers {
 
     //set on call of HAL::readAllDucers(). 
+    bool persistentCal = true;
+
+    float data[4];
+    float offset[4];
+    float point1[4];
+    float multiplier[4];
+
+    // //0
+    // //float _upstreamPT1_offset = 0;
+    // //float _upstreamPT1_multiplier = 1;
+
+    // //1
+    // float _downstreamPT1_offset = 0;
+    // float _downstreamPT1_multiplier = 1;
+
+    // //2
+    // float _upstreamPT2_offset = 0;
+    // float _upstreamPT2_multiplier = 1;
+
+
+    // //3
+    // float _downstreamPT2_offset = 0;
+    // float _downstreamPT2_multiplier = 1;
+
     float _upstreamPT1;
     float _downstreamPT1;
     float _upstreamPT2;
@@ -30,6 +54,103 @@ namespace Ducers {
         upstreamPT2Buff->insert(millis(), upstreamPT2);
     }
 
+    void cal1Channel(uint8_t channel, float inputvalue){
+        float value;
+
+        if(channel == 0){
+            value = readRawPressurantPT1();
+        }
+        else if(channel == 1){
+            value = readRawTankPT1();
+        }
+        else if(channel == 2){
+            value = readRawPressurantPT2();
+        }
+        else if(channel == 3){
+            value = readRawTankPT2();
+        }
+
+        point1[channel] = inputvalue;
+        offset[channel] = inputvalue - value + offset[channel];
+        Serial.println("calibrated channel offset" + String(channel) + " to " + String(offset[channel]));
+        if (persistentCal){
+            EEPROM.begin(8*sizeof(float));
+            EEPROM.put(channel*sizeof(float),offset[channel]);
+            EEPROM.end();
+        }
+    }
+
+    void cal2Channel(uint8_t channel, float inputvalue){
+        float value;
+
+        if(channel == 0){
+            value = readRawPressurantPT1();
+        }
+        else if(channel == 1){
+            value = readRawTankPT1();
+        }
+        else if(channel == 2){
+            value = readRawPressurantPT2();
+        }
+        else if(channel == 3){
+            value = readRawTankPT2();
+        }
+
+
+        multiplier[channel] = (inputvalue - point1[channel])/(value/multiplier[channel] - point1[channel]);
+        offset[channel] += inputvalue/multiplier[channel] - value;
+        Serial.println("calibrated channel multiplier" + String(channel) + " to " + String(multiplier[channel]));
+        if (persistentCal){
+            EEPROM.begin(8*sizeof(float));
+            EEPROM.put((channel)*sizeof(float),offset[channel]);
+            EEPROM.put((channel+4)*sizeof(float),multiplier[channel]);
+            EEPROM.end();
+        }
+    }
+
+    void onCal1Command(Comms::Packet packet, uint8_t ip){
+        uint8_t channel = Comms::packetGetUint8(&packet, 0);
+        float value = Comms::packetGetFloat(&packet, 1);
+        cal1Channel(channel, value);
+        return;
+    }
+
+    void onCal2Command(Comms::Packet packet, uint8_t ip){
+        uint8_t channel = Comms::packetGetUint8(&packet, 0);
+        float value = Comms::packetGetFloat(&packet, 1);
+        cal2Channel(channel, value);
+        return;
+    }
+
+    void sendCal(Comms::Packet packet, uint8_t ip){
+        Comms::Packet response = {.id=102, .len =0};
+        for (int i = 0; i < 4; i++){
+            Comms::packetAddFloat(&response, offset[i]);
+            Comms::packetAddFloat(&response, multiplier[i]);
+            Serial.println(" " + String(offset[i]) + " " + String(multiplier[i]));
+        }
+        Comms::emitPacketToGS(&response);
+        return;
+    }
+
+    void resetCal(Comms::Packet packet, uint8_t ip){
+        uint8_t channel = Comms::packetGetUint8(&packet, 0);
+
+        offset[channel] = 0;
+        multiplier[channel] = 1;
+
+        Serial.println("reset channel " + String(channel));
+
+        if (persistentCal){
+            EEPROM.begin(8*sizeof(float));
+            EEPROM.put(channel*sizeof(float),offset[channel]);
+            EEPROM.put((channel+4)*sizeof(float),multiplier[channel]);
+            EEPROM.end();
+        }
+        return;
+    }
+
+
     void initPTs() {
             upstreamPT1Buff = new Buffer(Config::PTFilterBufferSize);
             downstreamPT1Buff = new Buffer(Config::PTFilterBufferSize);
@@ -39,9 +160,48 @@ namespace Ducers {
             downstreamPT1Buff->clear();
             upstreamPT2Buff->clear();
             downstreamPT2Buff->clear();
+
+            Comms::registerCallback(100, onCal1Command);
+            Comms::registerCallback(101, onCal2Command);
+            Comms::registerCallback(102, sendCal);
+            Comms::registerCallback(103, resetCal);
+
+
+            if (persistentCal){
+            EEPROM.begin(8*sizeof(float));
+            for (int i = 0; i < 4; i++){
+                EEPROM.get(i*sizeof(float),offset[i]);
+                if (isnan(offset[i])){
+                    offset[i] = 0;
+                }
+            }
+            for (int i = 0; i < 4; i++){
+                EEPROM.get((i+4)*sizeof(float),multiplier[i]);
+                if (isnan(multiplier[i]) || multiplier[i] < 0.01){
+                    multiplier[i] = 1;
+                }
+            }
+            EEPROM.end();
+        } else {
+            for (int i = 0; i < 4; i++){
+                offset[i] = 0;
+            }
+            for (int i = 0; i < 4; i++){
+                multiplier[i] = 1;
+            }
+        }
+
+        for (int i = 0; i < 4; i ++){
+            Serial.println("channel " + String(i) + " offset value: " + String(offset[i]));
+        }
+
+        for (int i = 0; i < 4; i ++){
+            Serial.println("channel " + String(i) + " multiplier value: " + String(multiplier[i]));
+        }
     }
 
 
+
     float interpolate1000(double rawValue) {
         return ((rawValue - 0.5) * 250) + 12.5;
     }
@@ -50,45 +210,46 @@ namespace Ducers {
         return (rawValue * 1000 * 1.0042) + 5; //1.0042 from the voltage divider - 5647ohm and 5600ohm
     }
 
+    float readRawTankPT1() {
+        return multiplier[1] * (interpolate1000(_downstreamPT1) + offset[1]);
+    }
+    float readRawTankPT2() {
+        return multiplier[3] * (interpolate1000(_downstreamPT2) + offset[3]);
+    }
+    float readRawPressurantPT1() {
+        return multiplier[0] * (interpolate5000(_upstreamPT1) + offset[0]);
+    }
+    float readRawPressurantPT2() {
+        return multiplier[2] * (interpolate5000(_upstreamPT2) + offset[2]);
+    }
+
     float readPressurantPT1() {
-        return max((float)1, interpolate5000(_upstreamPT1));
+        return max((float)1, readRawPressurantPT1());
     }
     float readPressurantPT2() {
-        return max((float)1, interpolate5000(_upstreamPT2));
+        return max((float)1, readRawPressurantPT2());
     }
 
     float readTankPT1() {
-        return max((float)1, interpolate1000(_downstreamPT1));
+        return max((float)1, readRawTankPT1());
     }
 
     float readTankPT2() {
-        return max((float)1, interpolate1000(_downstreamPT2));
-    }
-
-    float readRawTankPT1() {
-        return interpolate1000(_downstreamPT1);
-    }
-    float readRawTankPT2() {
-        return interpolate1000(_downstreamPT2);
-    }
-    float readRawPressurantPT1() {
-        return interpolate5000(_upstreamPT1);
-    }
-    float readRawPressurantPT2() {
-        return interpolate5000(_upstreamPT2);
+        return max((float)1, readRawTankPT2());
     }
+
 
     float readFilteredTankPT1() {
-        return (float) downstreamPT1Buff->getFiltered();
+        return (float) multiplier[1] * (interpolate1000(downstreamPT1Buff->getFiltered()) + offset[1]);
     }
     float readFilteredTankPT2() {
-        return (float) downstreamPT2Buff->getFiltered();
+        return (float) multiplier[3] * (interpolate1000(downstreamPT2Buff->getFiltered()) + offset[3]);
     }
     float readFilteredPressurantPT1() {
-        return (float) upstreamPT1Buff->getFiltered();
+        return (float) multiplier[0] * (interpolate5000(upstreamPT1Buff->getFiltered()) + offset[0]);
     }
     float readFilteredPressurantPT2() {
-        return (float) upstreamPT2Buff->getFiltered();
+        return (float) multiplier[2] * (interpolate5000(upstreamPT2Buff->getFiltered()) + offset[2]);
     }
 
     /**

src/EReg/Ducers.h

@@ -2,6 +2,7 @@
 #include "HAL.h"
 #include "Util.h"
 #include "data_buff.h"
+#include "EEPROM.h"
 
 namespace Ducers {
 

src/EReg/ElectronicRegulator.cpp

@@ -42,7 +42,7 @@ void flow(Comms::Packet packet, uint8_t ip) {
         Serial.printf("bad flow packet len %d\n", packet.len);
         return;
     }
-    uint32_t flowLength = packetGetUint32(&packet, 1);
+    uint32_t flowLength = packetGetUint32(&packet, 1) * 1000;
     if ((flowLength < 1 * 1000 * 1000) || (flowLength > 70 * 1000 * 1000)) {
         Serial.printf("bad flow duration %d\n", flowLength);
         return;

src/EReg/Packets.cpp

@@ -21,13 +21,18 @@ namespace Packets {
     uint8_t ac1_ip = 11;
     uint8_t ac2_ip = 12;
     uint8_t ACTUATE_IP = 100;
+    uint32_t lastTelemetry = 0;
 
 
     void sendTelemetry(
-        float upstreamPressure1,
-        float upstreamPressure2,
-        float downstreamPressure1,
-        float downstreamPressure2,
+        float filteredUpstreamPressure1,
+        float filteredUpstreamPressure2,
+        float filteredDownstreamPressure1,
+        float filteredDownstreamPressure2,
+        float rawUpstreamPressure1,
+        float rawUpstreamPressure2,
+        float rawDownstreamPressure1,
+        float rawDownstreamPressure2,
         float encoderAngle,
         float angleSetpoint,
         float pressureSetpoint,
@@ -37,11 +42,21 @@ namespace Packets {
         float pressureControlD
     ) {
 
-        Comms::Packet packet = {.id = TELEMETRY_ID};
-        Comms::packetAddFloat(&packet, upstreamPressure1);
-        Comms::packetAddFloat(&packet, upstreamPressure2);
-        Comms::packetAddFloat(&packet, downstreamPressure1);
-        Comms::packetAddFloat(&packet, downstreamPressure2);
+        //pressure data
+        Comms::Packet packet = {.id = PT_TELEMETRY_ID, .len=0};
+        Comms::packetAddFloat(&packet, filteredUpstreamPressure1);
+        Comms::packetAddFloat(&packet, filteredUpstreamPressure2);
+        Comms::packetAddFloat(&packet, filteredDownstreamPressure1);
+        Comms::packetAddFloat(&packet, filteredDownstreamPressure2);
+        Comms::packetAddFloat(&packet, rawUpstreamPressure1);
+        Comms::packetAddFloat(&packet, rawUpstreamPressure2);
+        Comms::packetAddFloat(&packet, rawDownstreamPressure1);
+        Comms::packetAddFloat(&packet, rawDownstreamPressure2);
+        Comms::emitPacket(&packet);
+
+        //misc data
+        packet.id = MISC_TELEMETRY_ID;
+        packet.len = 0;
         Comms::packetAddFloat(&packet, encoderAngle);
         Comms::packetAddFloat(&packet, angleSetpoint);
         Comms::packetAddFloat(&packet, pressureSetpoint);
@@ -50,9 +65,20 @@ namespace Packets {
         Comms::packetAddFloat(&packet, pressureControlI);
         Comms::packetAddFloat(&packet, pressureControlD);
         Comms::emitPacket(&packet);
+
         sendTemperatures();
         sendPhaseCurrents();
         sendLimitSwitches();
+
+        if (millis() - lastTelemetry > 2000) {
+            lastTelemetry = millis();
+            Serial.println("Upstream Pressure 1: filtered" + String(filteredUpstreamPressure1) + ", raw" + String(rawUpstreamPressure1));
+            Serial.println("Upstream Pressure 2: filtered" + String(filteredUpstreamPressure2) + ", raw" + String(rawUpstreamPressure2));
+            Serial.println("Downstream Pressure 1: filtered" + String(filteredDownstreamPressure1) + ", raw" + String(rawDownstreamPressure1));
+            Serial.println("Downstream Pressure 2: filtered" + String(filteredDownstreamPressure2) + ", raw" + String(rawDownstreamPressure2));
+            Serial.println("Encoder Angle: " + String(encoderAngle) + ", Setpoint: " + String(angleSetpoint) + ", Pressure Setpoint: " + String(pressureSetpoint) + ", Motor Power: " + String(motorPower));
+            Serial.println("Pressure Control P: " + String(pressureControlP) + ", I: " + String(pressureControlI) + ", D: " + String(pressureControlD));
+        }
         // Serial.printf("packet sent\n");
     }