Skip to content

Commit c16963a

Browse files
BobSaidHiBobSaidHi
committed
Formatted LoadComms
1 parent a0d2d3b commit c16963a

2 files changed

Lines changed: 219 additions & 210 deletions

File tree

‎lib/LoadComms/LoadComms.cpp‎

Lines changed: 135 additions & 130 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,8 @@
1313

1414
// Initialization of static members
1515
QueueHandle_t LoadComms::priorityDataQueue = nullptr;
16-
std::atomic<uint_fast32_t> LoadComms::txEvents = 0; // DONE: check against last years code
16+
std::atomic<uint_fast32_t> LoadComms::txEvents =
17+
0; // DONE: check against last years code
1718
std::atomic<uint_fast32_t> LoadComms::bytesSent = 0;
1819
std::atomic<uint_fast32_t> LoadComms::bytesNotSent = 0;
1920
std::atomic<uint_fast32_t> LoadComms::rxEvents = 0;
@@ -30,17 +31,15 @@ const uint8_t NACELLE_MAC[] = {0x30, 0xED, 0xA0, 0xE0, 0x6B, 0x78};
3031
static LoadComms *s_instance = nullptr;
3132

3233
LoadComms::LoadComms()
33-
: lastSendTime_(0),
34-
lastRxTime_(0),
35-
linkAlive_(false) //,
36-
// nacelleRPM_(0.0f)
37-
{
38-
s_instance = this;
39-
priorityDataQueue = xQueueCreate(1, sizeof(NacellePacket));
34+
: lastSendTime_(0), lastRxTime_(0), linkAlive_(false) //,
35+
// nacelleRPM_(0.0f)
36+
{
37+
s_instance = this;
38+
priorityDataQueue = xQueueCreate(1, sizeof(NacellePacket));
4039
}
4140

4241
bool LoadComms::begin() {
43-
if (!WiFi.mode(WIFI_STA)) {
42+
if (!WiFi.mode(WIFI_STA)) {
4443
ESP_LOGE(TAG, "Failed to set WiFi mode");
4544
return false;
4645
}
@@ -52,16 +51,15 @@ bool LoadComms::begin() {
5251

5352
etl::array<uint8_t, 6> MACAddress = {0};
5453
esp_err_t opStatus = esp_wifi_get_mac(WIFI_IF_STA, MACAddress.data());
55-
if(opStatus != ESP_OK) {
54+
if (opStatus != ESP_OK) {
5655
ESP_LOGE(TAG, "Failed to get MAC address: %d", opStatus);
5756
return false;
5857
} else {
5958
ESP_LOGI(TAG, "Device MAC: %02X:%02X:%02X:%02X:%02X:%02X",
60-
MACAddress[0], MACAddress[1], MACAddress[2],
61-
MACAddress[3], MACAddress[4], MACAddress[5]);
59+
MACAddress[0], MACAddress[1], MACAddress[2], MACAddress[3],
60+
MACAddress[4], MACAddress[5]);
6261
}
6362

64-
6563
if (!WiFi.STA.bandwidth(WIFI_BW_HT20)) {
6664
ESP_LOGE(TAG, "Failed to set WiFi bandwidth");
6765
return false;
@@ -72,158 +70,165 @@ bool LoadComms::begin() {
7270
return false;
7371
}
7472

75-
if (esp_now_init() != ESP_OK) {
76-
ESP_LOGE(TAG, "ESP-NOW init failed");
77-
return false;
78-
}
73+
if (esp_now_init() != ESP_OK) {
74+
ESP_LOGE(TAG, "ESP-NOW init failed");
75+
return false;
76+
}
7977

80-
if(esp_now_register_send_cb(onDataSent_) != ESP_OK) {
81-
ESP_LOGE(TAG, "Failed to register tx cb");
82-
return false;
83-
}
78+
if (esp_now_register_send_cb(onDataSent_) != ESP_OK) {
79+
ESP_LOGE(TAG, "Failed to register tx cb");
80+
return false;
81+
}
8482

85-
if(esp_now_register_recv_cb(onDataRecv_) != ESP_OK) {
86-
ESP_LOGE(TAG, "Failed to register rx cb");
87-
return false;
88-
}
83+
if (esp_now_register_recv_cb(onDataRecv_) != ESP_OK) {
84+
ESP_LOGE(TAG, "Failed to register rx cb");
85+
return false;
86+
}
8987

90-
if( setupPeer_() != ESP_OK) {
91-
// Logging already handled
92-
return false;
93-
}
88+
if (setupPeer_() != ESP_OK) {
89+
// Logging already handled
90+
return false;
91+
}
9492

95-
Serial.println("Loadbox ready");
96-
return true;
93+
Serial.println("Loadbox ready");
94+
return true;
9795
}
9896

9997
esp_err_t LoadComms::setupPeer_() {
100-
esp_now_peer_info_t peerInfo = {};
101-
memcpy(peerInfo.peer_addr, NACELLE_MAC, 6);
102-
peerInfo.channel = 0;
103-
peerInfo.encrypt = false;
104-
105-
esp_err_t result = esp_now_add_peer(&peerInfo);
106-
if (result != ESP_OK) {
107-
ESP_LOGE(TAG, "Failed to add peer: %d at %02x:%02x:%02x:%02x:%02x:%02x", result,
108-
NACELLE_MAC[0], NACELLE_MAC[1], NACELLE_MAC[2],
109-
NACELLE_MAC[3], NACELLE_MAC[4], NACELLE_MAC[5]);
110-
} else{
111-
ESP_LOGI(TAG, "Peer added: %02X:%02X:%02X:%02X:%02X:%02X",
112-
NACELLE_MAC[0], NACELLE_MAC[1], NACELLE_MAC[2],
113-
NACELLE_MAC[3], NACELLE_MAC[4], NACELLE_MAC[5]);
114-
}
115-
116-
return result;
98+
esp_now_peer_info_t peerInfo = {};
99+
memcpy(peerInfo.peer_addr, NACELLE_MAC, 6);
100+
peerInfo.channel = 0;
101+
peerInfo.encrypt = false;
102+
103+
esp_err_t result = esp_now_add_peer(&peerInfo);
104+
if (result != ESP_OK) {
105+
ESP_LOGE(TAG, "Failed to add peer: %d at %02x:%02x:%02x:%02x:%02x:%02x",
106+
result, NACELLE_MAC[0], NACELLE_MAC[1], NACELLE_MAC[2],
107+
NACELLE_MAC[3], NACELLE_MAC[4], NACELLE_MAC[5]);
108+
} else {
109+
ESP_LOGI(TAG, "Peer added: %02X:%02X:%02X:%02X:%02X:%02X",
110+
NACELLE_MAC[0], NACELLE_MAC[1], NACELLE_MAC[2], NACELLE_MAC[3],
111+
NACELLE_MAC[4], NACELLE_MAC[5]);
112+
}
113+
114+
return result;
117115
}
118116

119117
etl::string<LoadComms::LOG_STRING_SIZE> LoadComms::getLogString() const {
120-
etl::string<LOG_STRING_SIZE> logString(TAG); // 3 chars
121-
logString.append(": TxE: "); // 7 chars
122-
123-
etl::format_spec decFormatA;
124-
decFormatA.width(6).fill('0'); // [6 chars, expecting up to 30 mins * (1/(2ms)) = 900,000 events]
125-
/**
126-
* @details I don't think we need strong guarantees on logging data
127-
* @see
128-
* https://stackoverflow.com/questions/12346487/what-do-each-memory-order-mean
129-
* @see https://en.cppreference.com/cpp/atomic/memory_order
130-
*/
131-
etl::to_string(txEvents.load(std::memory_order_relaxed), logString, decFormatA, true); // 6 chars
132-
logString.append(", TxBS: "); // 8 chars
133-
134-
etl::format_spec decFormatB;
135-
decFormatB.width(7).fill('0'); // [7 chars]
136-
etl::to_string(bytesSent.load(std::memory_order_relaxed), logString, decFormatB, true); // 7 chars
137-
logString.append(", TxBF: "); // 8 chars
138-
etl::to_string(bytesNotSent.load(std::memory_order_relaxed), logString, decFormatB, true); // 7 chars
139-
140-
logString.append(", RxE: "); // 7 chars
141-
etl::to_string(rxEvents.load(std::memory_order_relaxed), logString, decFormatA, true); // 6 chars
142-
logString.append(", RxBS: "); // 8 chars
143-
etl::to_string(bytesReceived.load(std::memory_order_relaxed), logString, decFormatB, true); // 7 chars
144-
145-
return logString;
146-
}
118+
etl::string<LOG_STRING_SIZE> logString(TAG); // 3 chars
119+
logString.append(": TxE: "); // 7 chars
120+
121+
etl::format_spec decFormatA;
122+
decFormatA.width(6).fill(
123+
'0'); // [6 chars, expecting up to 30 mins * (1/(2ms)) = 900,000 events]
124+
/**
125+
* @details I don't think we need strong guarantees on logging data
126+
* @see
127+
* https://stackoverflow.com/questions/12346487/what-do-each-memory-order-mean
128+
* @see https://en.cppreference.com/cpp/atomic/memory_order
129+
*/
130+
etl::to_string(txEvents.load(std::memory_order_relaxed), logString,
131+
decFormatA, true); // 6 chars
132+
logString.append(", TxBS: "); // 8 chars
133+
134+
etl::format_spec decFormatB;
135+
decFormatB.width(7).fill('0'); // [7 chars]
136+
etl::to_string(bytesSent.load(std::memory_order_relaxed), logString,
137+
decFormatB, true); // 7 chars
138+
logString.append(", TxBF: "); // 8 chars
139+
etl::to_string(bytesNotSent.load(std::memory_order_relaxed), logString,
140+
decFormatB, true); // 7 chars
141+
142+
logString.append(", RxE: "); // 7 chars
143+
etl::to_string(rxEvents.load(std::memory_order_relaxed), logString,
144+
decFormatA, true); // 6 chars
145+
logString.append(", RxBS: "); // 8 chars
146+
etl::to_string(bytesReceived.load(std::memory_order_relaxed), logString,
147+
decFormatB, true); // 7 chars
148+
149+
return logString;
150+
}
147151

148152
LoadComms::LogData LoadComms::getLogData() const {
149153
return LogData{txEvents, bytesSent, bytesNotSent, rxEvents, bytesReceived};
150154
}
151155

152-
void LoadComms::onDataSent_(const wifi_tx_info_t *tx_info, esp_now_send_status_t status) {
153-
// (void)tx_info;
154-
// Serial.print("Send status: ");
155-
// Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Success" : "Fail");
156-
// This is check elsewhere
157-
txEvents++;
158-
if(status == ESP_NOW_SEND_SUCCESS) {
159-
bytesSent += tx_info->data_len;
156+
void LoadComms::onDataSent_(const wifi_tx_info_t *tx_info,
157+
esp_now_send_status_t status) {
158+
// (void)tx_info;
159+
// Serial.print("Send status: ");
160+
// Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Success" : "Fail");
161+
// This is check elsewhere
162+
txEvents++;
163+
if (status == ESP_NOW_SEND_SUCCESS) {
164+
bytesSent += tx_info->data_len;
160165
} else {
161-
bytesNotSent += tx_info->data_len;
166+
bytesNotSent += tx_info->data_len;
162167
}
163-
164168
}
165169

166-
void LoadComms::onDataRecv_(const esp_now_recv_info_t *recv_info, const uint8_t *data, int len) {
167-
rxEvents++;
168-
bytesReceived += len;
170+
void LoadComms::onDataRecv_(const esp_now_recv_info_t *recv_info,
171+
const uint8_t *data, int len) {
172+
rxEvents++;
173+
bytesReceived += len;
169174

170-
if (s_instance == nullptr) {
171-
ESP_LOGE(TAG, "Rx CB: Invalid instance");
172-
return;
173-
}
175+
if (s_instance == nullptr) {
176+
ESP_LOGE(TAG, "Rx CB: Invalid instance");
177+
return;
178+
}
174179

175-
const uint8_t *mac = recv_info->src_addr;
180+
const uint8_t *mac = recv_info->src_addr;
176181

177-
// Serial.printf("Packet received from: %02X:%02X:%02X:%02X:%02X:%02X\n",
178-
// mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
182+
// Serial.printf("Packet received from: %02X:%02X:%02X:%02X:%02X:%02X\n",
183+
// mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
179184

180-
if (len == sizeof(NacellePacket)) {
181-
// memcpy(&s_instance->incomingPacket_, data, sizeof(NacellePacket));
185+
if (len == sizeof(NacellePacket)) {
186+
// memcpy(&s_instance->incomingPacket_, data, sizeof(NacellePacket));
182187

183-
s_instance->lastRxTime_ = millis();
184-
s_instance->linkAlive_ = true;
188+
s_instance->lastRxTime_ = millis();
189+
s_instance->linkAlive_ = true;
185190

186-
// s_instance->nacelleRPM_ = s_instance->incomingPacket_.rpm;
191+
// s_instance->nacelleRPM_ = s_instance->incomingPacket_.rpm;
187192

188-
// Serial.println("Received NacellePacket:");
189-
// printNacellePacket(s_instance->incomingPacket_, Serial);
193+
// Serial.println("Received NacellePacket:");
194+
// printNacellePacket(s_instance->incomingPacket_, Serial);
190195

191-
(void)xQueueOverwrite(priorityDataQueue, data); // Allegedly cannot fail
192-
} else {
193-
ESP_LOGE(TAG, "Rx invalid len: %d", len);
194-
}
196+
(void)xQueueOverwrite(priorityDataQueue, data); // Allegedly cannot fail
197+
} else {
198+
ESP_LOGE(TAG, "Rx invalid len: %d", len);
199+
}
195200
}
196201

197-
bool LoadComms::sendLoadboxData(int16_t d_mVPS, int16_t current_mA, int16_t dIPS, ESTOP_TYPE_NET safety) {
198-
// if (now - lastSendTime_ >= LOAD_COMMS_SEND_PERIOD_MS) {
199-
makeLoadboxPacket(outgoingPacket_, d_mVPS, current_mA, dIPS, safety);
200-
esp_err_t result = esp_now_send(NACELLE_MAC, (uint8_t *)&outgoingPacket_, sizeof(outgoingPacket_));
201-
if(result == ESP_OK) {
202-
lastSendTime_ = millis();
203-
linkAlive_ = true;
204-
} else {
205-
linkAlive_ = false;
206-
ESP_LOGE(TAG, "Tx fail w/ %d", result);
207-
}
208-
209-
// }
210-
// if (now - lastRxTime_ > LOAD_COMMS_TIMEOUT_MS) {
211-
// linkAlive_ = false;
212-
// }
213-
214-
// if (!linkAlive_) {
215-
// Serial.println("WARNING: nacelle comms timeout");
216-
// }
217-
return linkAlive_;
202+
bool LoadComms::sendLoadboxData(int16_t d_mVPS, int16_t current_mA,
203+
int16_t dIPS, ESTOP_TYPE_NET safety) {
204+
// if (now - lastSendTime_ >= LOAD_COMMS_SEND_PERIOD_MS) {
205+
makeLoadboxPacket(outgoingPacket_, d_mVPS, current_mA, dIPS, safety);
206+
esp_err_t result = esp_now_send(NACELLE_MAC, (uint8_t *)&outgoingPacket_,
207+
sizeof(outgoingPacket_));
208+
if (result == ESP_OK) {
209+
lastSendTime_ = millis();
210+
linkAlive_ = true;
211+
} else {
212+
linkAlive_ = false;
213+
ESP_LOGE(TAG, "Tx fail w/ %d", result);
214+
}
215+
216+
// }
217+
// if (now - lastRxTime_ > LOAD_COMMS_TIMEOUT_MS) {
218+
// linkAlive_ = false;
219+
// }
220+
221+
// if (!linkAlive_) {
222+
// Serial.println("WARNING: nacelle comms timeout");
223+
// }
224+
return linkAlive_;
218225
}
219226

220227
// void LoadComms::process() {
221228
// // This method can be used for future expansion (e.g., periodic checks)
222229
// }
223230

224-
bool LoadComms::isLinkAlive() const {
225-
return linkAlive_;
226-
}
231+
bool LoadComms::isLinkAlive() const { return linkAlive_; }
227232

228233
// float LoadComms::getNacelleRPM() const {
229234
// return nacelleRPM_;

0 commit comments

Comments
 (0)