Skip to content

Commit e29cf12

Browse files
BobSaidHiBobSaidHi
committed
Power + load data improvements
1 parent 844f66d commit e29cf12

8 files changed

Lines changed: 69 additions & 24 deletions

File tree

‎include/INA260.hpp‎

Lines changed: 42 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,8 @@
99

1010
namespace INA260 {
1111
constexpr uint_fast16_t m_TO_BASE = 1000;
12+
constexpr uint_fast16_t u_TO_m = 1000;
13+
constexpr uint_fast32_t u_TO_BASE = m_TO_BASE * u_TO_m;
1214
static constexpr int_fast8_t MIN_VOLTAGE_V = -32;
1315
static constexpr int_fast16_t MIN_VOLTAGE_mV = MIN_VOLTAGE_V * m_TO_BASE;
1416
static constexpr int_fast8_t MAX_VOLTAGE_V = 32;
@@ -27,8 +29,18 @@ namespace INA260 {
2729
Adafruit_INA260 powerSensor;
2830

2931
std::atomic<int_fast16_t> voltage_mV = 0;
32+
int_fast16_t prevVoltage_mV = 0;
33+
std::atomic<int_fast16_t> dVoltage_mVPS = 0;
3034
std::atomic<int_fast16_t> current_mA = 0;
35+
int_fast16_t prevCurrent_mA = 0;
36+
std::atomic<int_fast16_t> dCurrent_mAPS = 0;
3137
std::atomic<int_fast32_t> power_mW = 0;
38+
int_fast32_t prevPower_mW = 0;
39+
std::atomic<int_fast32_t> dPower_mWPS = 0;
40+
41+
namespace detail {
42+
unsigned long lastUpdateTime_us = 0;
43+
}
3244

3345
bool begin(uint8_t i2c_addr, INA260_AveragingCount avgCount,
3446
INA260_ConversionTime convTime) {
@@ -50,35 +62,47 @@ namespace INA260 {
5062
bool updateReadings() {
5163
bool result = true;
5264

65+
unsigned long currentTime_us = micros();
66+
unsigned long dTime_us = currentTime_us - detail::lastUpdateTime_us;
67+
5368
auto vTemp_mV = (uint_fast32_t)powerSensor.readBusVoltage();
5469
if ((vTemp_mV > MAX_VOLTAGE_mV) ||
5570
(vTemp_mV < PSENSOR::MIN_VOLTAGE_mV)) {
5671
ESP_LOGE("TAG", "V reading out of bounds: %u mV", vTemp_mV);
5772
result &= false;
5873
} // Else: Valid reading
74+
prevVoltage_mV = voltage_mV;
5975
voltage_mV = vTemp_mV;
76+
int_fast16_t dVoltage_mV = voltage_mV - prevVoltage_mV;
77+
dVoltage_mVPS = dVoltage_mV * u_TO_BASE / dTime_us;
6078

6179
auto iTemp = (uint_fast32_t)powerSensor.readCurrent();
6280
if ((iTemp > MAX_CURRENT_mA) || (iTemp < MIN_CURRENT_mA)) {
6381
ESP_LOGE("TAG", "C reading out of bounds: %d mA", iTemp);
6482
result &= false;
6583
} // Else: Valid reading
84+
prevCurrent_mA = current_mA;
6685
current_mA = iTemp;
86+
int_fast16_t dCurrent_mA = current_mA - prevCurrent_mA;
87+
dCurrent_mAPS = dCurrent_mA * u_TO_BASE / dTime_us;
6788

6889
auto pTemp = (uint_fast32_t)powerSensor.readPower();
6990
if ((pTemp > (MAX_VOLTAGE_mV * MAX_CURRENT_A)) ||
7091
(pTemp < (PSENSOR::MIN_VOLTAGE_mV * MIN_CURRENT_A))) {
7192
ESP_LOGE("TAG", "P out of bounds: %d mW", pTemp);
7293
result &= false;
7394
} // Else: Valid reading
95+
prevPower_mW = power_mW;
7496
power_mW = pTemp;
97+
int_fast16_t dPower_mW = power_mW - prevPower_mW;
98+
dPower_mWPS = dPower_mW * u_TO_BASE / dTime_us;
7599

76100
return result;
77101
}
78102

79103
// TODO - improve this and null terminator may not be needed
80104
static constexpr uint_fast8_t LOG_STRING_SIZE =
81-
3 + 6 + 5 + 6 + 5 + 5 + 5 + 1;
105+
3 + ((6 + 5) * 3) + ((8 + 5) * 3) + 1;
82106
/**
83107
* @brief Get at string that describes the current state of the PID instance
84108
* @returns the current state of the PID instance as a string
@@ -95,14 +119,23 @@ namespace INA260 {
95119
* https://stackoverflow.com/questions/12346487/what-do-each-memory-order-mean
96120
* @see https://en.cppreference.com/cpp/atomic/memory_order
97121
*/
98-
etl::to_string(voltage_mV.load(std::memory_order::relaxed), logString,
99-
decFormatA, true); // 5 chars
100-
logString.append(", mA: "); // 6 chars
101-
etl::to_string(current_mA.load(std::memory_order::relaxed), logString,
102-
decFormatA, true); // 5 chars
103-
logString.append(", W: "); // 5 chars
104-
etl::to_string(power_mW.load(std::memory_order::relaxed) / m_TO_BASE,
105-
logString, decFormatA, true); // 5 chars
122+
etl::to_string(voltage_mV.load(), logString, decFormatA,
123+
true); // 5 chars
124+
logString.append(", mA: "); // 6 chars
125+
etl::to_string(current_mA.load(), logString, decFormatA,
126+
true); // 5 chars
127+
logString.append(", mW: "); // 6 chars
128+
etl::to_string(power_mW.load() / m_TO_BASE, logString, decFormatA,
129+
true); // 5 chars
130+
logString.append(", mV/s: "); // 8 chars
131+
etl::to_string(dVoltage_mVPS.load(), logString, decFormatA,
132+
true); // 5 chars
133+
logString.append(", mA/s: "); // 8 chars
134+
etl::to_string(dCurrent_mAPS.load(), logString, decFormatA,
135+
true); // 5 chars
136+
logString.append(", mW/s: "); // 8 chars
137+
etl::to_string(dPower_mWPS.load(), logString, decFormatA,
138+
true); // 5 chars
106139

107140
return logString;
108141
}

‎include/LoadConfig.hpp‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -93,7 +93,7 @@ namespace RUN {
9393
TI_POLL_SENSORS_mS = 2, // CONFIG - 2 ms (500 Hz)
9494
TI_ADJUST_LOAD_mS = 10, // CONFIG - 10 ms (100 Hz)
9595
TI_RECV_ms = 100, // CONFIG - 100 ms (10 Hz)
96-
TI_SEND_ms = 100, // CONFIG - 10 ms (100 Hz) // FIXME
96+
TI_SEND_ms = 100, // CONFIG - 10 ms (100 Hz) // FIXME
9797
TI_CFG_ms = 1000, // CONFIG - 1000 ms (1 Hz)
9898
TI_TELNET_ms = 500, // CONFIG - 500 ms (2 Hz)
9999
TI_OTA_ms = 1000, // CONFIG - 1000 ms (1 Hz)
@@ -102,7 +102,8 @@ namespace RUN {
102102
10, // CONFIG - Multiplier for task delay on failure
103103
FAIL_BACKOFF_MULTIPLIER =
104104
2 // CONFIG - Multiplier for backoff factor on subsequent failures
105-
// TODO: Consider one or two fixed, precalculated delays instead or add max backoff
105+
// TODO: Consider one or two fixed, precalculated delays instead or add
106+
// max backoff
106107
};
107108

108109
// todo: What was this for?
@@ -135,6 +136,9 @@ namespace PSENSOR {
135136
constexpr int_fast32_t MIN_POWER_mW = MIN_VOLTAGE_mV * MIN_CURRENT_mA;
136137
// CONFIG
137138
constexpr int_fast32_t MAX_POWER_mW = MAX_VOLTAGE_mV * MAX_CURRENT_mA;
139+
140+
static constexpr uint_fast16_t LOAD_SHED_I_THRESHOLD_mA =
141+
500; // CONFIG // todo
138142
} // namespace PSENSOR
139143

140144
// MARK: Load

‎include/LoadContainer.hpp‎

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
#include <cstdint>
44

5+
#include "2026Core/TurbinePacket/TurbinePacket.hpp"
56
#include "INA260.hpp"
67
#include "LoadConfig.hpp"
78
#include "LoadTasks.hpp"
@@ -44,16 +45,20 @@ class LoadContainer {
4445
: loadDevice(loadDevice), loadComms(loadComms) {}
4546
~LoadContainer() = default;
4647

47-
inline bool getSafetyFlag() const { return (safetyFlag); }
48+
inline ESTOP_TYPE_FAST getSafetyFlag() const { return (safetyFlag); }
4849
inline bool isPowerPositive() const { return (INA260::current_mA > 0); }
4950
inline bool isSteadyRPM() const {
5051
return false;
5152
} // todo - steady power is actually more important
5253
inline bool isTargetRPMExceeded() const { return false; } // todo
5354

5455
inline void updateSafetyFlag(bool safetyFlag) {
55-
this->safetyFlag =
56-
(digitalRead(UM_PROS3::ESTOP_PIN) == LOW) || (!isPowerPositive());
56+
if (digitalRead(UM_PROS3::ESTOP_PIN) == LOW) {
57+
this->safetyFlag = ESTOP_TYPE_FAST::BUTTON;
58+
} else if (abs(INA260::current_mA) <
59+
PSENSOR::LOAD_SHED_I_THRESHOLD_mA) {
60+
this->safetyFlag = ESTOP_TYPE_FAST::NONE;
61+
}
5762
}
5863
// inline void updatePowerPositive(bool powerPositive) {
5964
// this->powerPositive = powerPositive;
@@ -65,7 +70,7 @@ class LoadContainer {
6570

6671
private:
6772
MCP23008T &loadDevice;
68-
bool safetyFlag = false; // todo - make atomic?
73+
ESTOP_TYPE_FAST safetyFlag = ESTOP_TYPE_FAST::NONE; // todo - make atomic?
6974
LoadComms &loadComms;
7075
// bool powerPositive = false;
7176
int_fast16_t currentRPM = 0; // todo

‎include/LoadFSM.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -59,7 +59,7 @@ class LoadFSM {
5959
UPDATE_RESULT updateState() {
6060
// Check safety task / E-Stop conditions
6161
if ((currentState != FSMCommon::States::sESTOP) &&
62-
load.getSafetyFlag()) {
62+
(load.getSafetyFlag() != ESTOP_TYPE_FAST::NONE)) {
6363
// * -> sESTOP
6464
currentState = FSMCommon::States::sESTOP;
6565

@@ -69,7 +69,7 @@ class LoadFSM {
6969

7070
return UPDATE_RESULT::STATE_CHANGED;
7171
} else if ((currentState == FSMCommon::States::sESTOP) &&
72-
load.getSafetyFlag()) {
72+
(load.getSafetyFlag() != ESTOP_TYPE_FAST::NONE)) {
7373
// Nothing to do
7474
return UPDATE_RESULT::NO_CHANGE;
7575
} // else: ~safetyTask

‎lib/2026-Controls_Core‎

‎lib/LoadComms/LoadComms.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -112,8 +112,8 @@ void LoadComms::onDataRecv_(const esp_now_recv_info_t *recv_info, const uint8_t
112112

113113
const uint8_t *mac = recv_info->src_addr;
114114

115-
Serial.printf("Packet received from: %02X:%02X:%02X:%02X:%02X:%02X\n",
116-
mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
115+
// Serial.printf("Packet received from: %02X:%02X:%02X:%02X:%02X:%02X\n",
116+
// mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
117117

118118
if (len == sizeof(NacellePacket)) {
119119
// memcpy(&s_instance->incomingPacket_, data, sizeof(NacellePacket));
@@ -132,9 +132,9 @@ void LoadComms::onDataRecv_(const esp_now_recv_info_t *recv_info, const uint8_t
132132
}
133133
}
134134

135-
bool LoadComms::sendLoadboxData(uint8_t estop) {
135+
bool LoadComms::sendLoadboxData(int16_t d_mVPS, int16_t current_mA, int16_t dIPS, ESTOP_TYPE_NET safety) {
136136
// if (now - lastSendTime_ >= LOAD_COMMS_SEND_PERIOD_MS) {
137-
makeLoadboxPacket(outgoingPacket_, estop);
137+
makeLoadboxPacket(outgoingPacket_, d_mVPS, current_mA, dIPS, safety);
138138
esp_err_t result = esp_now_send(NACELLE_MAC, (uint8_t *)&outgoingPacket_, sizeof(outgoingPacket_));
139139
if(result == ESP_OK) {
140140
lastSendTime_ = millis();

‎lib/LoadComms/LoadComms.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -62,7 +62,7 @@ class LoadComms {
6262
* @brief Send load box data to nacelle.
6363
* @param safety Safety value to send.
6464
*/
65-
bool sendLoadboxData(uint8_t safety);
65+
bool sendLoadboxData(int16_t d_mVPS, int16_t current_mA, int16_t dIPS, ESTOP_TYPE_NET safety);
6666

6767
/**
6868
* @brief Process communication - call in main loop.

‎src/main.cpp‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -416,7 +416,10 @@ vTaskSendData([[maybe_unused]] void *pvParameters) { // NOSONAR
416416
RUN::TASK_INTERVALS::FAIL_BACKOFF_BASE_FACTOR;
417417

418418
uint32_t delay_ms = 0;
419-
if (loadComms.sendLoadboxData((uint8_t)(load.getSafetyFlag()))) {
419+
if (loadComms.sendLoadboxData(
420+
INA260::dVoltage_mVPS, INA260::current_mA,
421+
INA260::dCurrent_mAPS,
422+
static_cast<ESTOP_TYPE_NET>(load.getSafetyFlag()))) {
420423
backoffFactor = RUN::TASK_INTERVALS::FAIL_BACKOFF_BASE_FACTOR;
421424
delay_ms = RUN::TASK_INTERVALS::TI_SEND_ms;
422425
} else {

0 commit comments

Comments
 (0)