Skip to content

Commit 5dd5b9c

Browse files
JensOgorekfalkoschindler
JensOgorek
and
falkoschindler
authored
Adding recovery to can and canopen_motor (#137)
* adding diagnose and recovery to can * remove uart slow waiting * remove workaround from settings * remove uart driver include * cleanup * removing diagnose (get_status does the same without analysing) * add reset to documentation * updated recovery for canopen motors * rollback some changes * remove trying for stop (was there for debugging) * code review --------- Co-authored-by: Falko Schindler <[email protected]>
1 parent 12712c7 commit 5dd5b9c

File tree

6 files changed

+154
-38
lines changed

6 files changed

+154
-38
lines changed

‎docs/module_reference.md‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -221,6 +221,7 @@ The CAN module allows communicating with peripherals on the specified CAN bus.
221221
| `can.start()` | Start the driver | |
222222
| `can.stop()` | Stop the driver | |
223223
| `can.recover()` | Recover the driver | |
224+
| `can.reset()` | Reset the driver | |
224225

225226
The method `get_status()` prints the following information:
226227

@@ -238,6 +239,9 @@ The method `get_status()` prints the following information:
238239
After creating a CAN module, the driver is started automatically.
239240
The `start()` and `stop()` methods are primarily for debugging purposes.
240241

242+
The `recover()` method can be used to recover the driver from a "BUS_OFF" state.
243+
In contrast, the `reset()` method will stop the driver, try to recover it and then start it again.
244+
241245
## Serial interface
242246

243247
The serial module allows communicating with peripherals via the specified connection.

‎main/modules/can.cpp‎

Lines changed: 104 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
#include "can.h"
22
#include "../utils/string_utils.h"
3+
#include "../utils/timing.h"
34
#include "../utils/uart.h"
45
#include "driver/twai.h"
56
#include <stdexcept>
@@ -93,7 +94,26 @@ void Can::step() {
9394

9495
bool Can::receive() {
9596
twai_message_t message;
96-
if (twai_receive(&message, pdMS_TO_TICKS(0)) != ESP_OK) {
97+
const esp_err_t result = twai_receive(&message, pdMS_TO_TICKS(0));
98+
if (result == ESP_ERR_TIMEOUT) {
99+
// no message available
100+
return false;
101+
}
102+
103+
if (result != ESP_OK) {
104+
// reset if bus is off
105+
twai_status_info_t status_info;
106+
if (twai_get_status_info(&status_info) == ESP_OK) {
107+
if (status_info.state == TWAI_STATE_BUS_OFF) {
108+
try {
109+
this->reset_can_bus();
110+
} catch (const std::exception &e) {
111+
echo("CAN recovery failed: %s", e.what());
112+
}
113+
}
114+
} else {
115+
echo("CAN receive error: %d (could not get status info)", result);
116+
}
97117
return false;
98118
}
99119

@@ -126,27 +146,18 @@ void Can::send(const uint32_t id, const uint8_t data[8], const bool rtr, uint8_t
126146
for (int i = 0; i < dlc; ++i) {
127147
message.data[i] = data[i];
128148
}
149+
129150
if (twai_transmit(&message, pdMS_TO_TICKS(0)) != ESP_OK) {
130-
twai_status_info_t status_info;
151+
try {
152+
echo("CAN send failed, attempting bus reset...");
153+
const_cast<Can *>(this)->reset_can_bus();
131154

132-
if (twai_get_status_info(&status_info) != ESP_OK) {
133-
throw std::runtime_error("could not get twai status");
134-
}
135-
if (status_info.state == TWAI_STATE_BUS_OFF) {
136-
if (twai_initiate_recovery() != ESP_OK) {
137-
throw std::runtime_error("could not initiate recovery");
155+
if (twai_transmit(&message, pdMS_TO_TICKS(0)) != ESP_OK) {
156+
throw std::runtime_error("could not send CAN message even after bus reset");
138157
}
139-
vTaskDelay(pdMS_TO_TICKS(100)); // Wait for recovery to start
158+
} catch (const std::exception &e) {
159+
throw std::runtime_error(std::string("Failed to send CAN message: ") + e.what());
140160
}
141-
if (status_info.state != TWAI_STATE_STOPPED) {
142-
if (twai_stop() != ESP_OK) {
143-
throw std::runtime_error("could not stop twai driver");
144-
}
145-
}
146-
if (twai_start() != ESP_OK) {
147-
throw std::runtime_error("could not restart twai driver");
148-
}
149-
throw std::runtime_error("could not send CAN message");
150161
}
151162
}
152163

@@ -185,18 +196,26 @@ void Can::call(const std::string method_name, const std::vector<ConstExpression_
185196
} else if (method_name == "start") {
186197
Module::expect(arguments, 0);
187198
if (twai_start() != ESP_OK) {
188-
throw std::runtime_error("could not start twai driver");
199+
throw std::runtime_error("could not start TWAI driver");
189200
}
190201
} else if (method_name == "stop") {
191202
Module::expect(arguments, 0);
192203
if (twai_stop() != ESP_OK) {
193-
throw std::runtime_error("could not stop twai driver");
204+
throw std::runtime_error("could not stop TWAI driver");
194205
}
195206
} else if (method_name == "recover") {
196207
Module::expect(arguments, 0);
197208
if (twai_initiate_recovery() != ESP_OK) {
198209
throw std::runtime_error("could not initiate recovery");
199210
}
211+
} else if (method_name == "reset") {
212+
Module::expect(arguments, 0);
213+
try {
214+
this->reset_can_bus();
215+
} catch (const std::exception &e) {
216+
echo("Error during CAN reset: %s", e.what());
217+
throw;
218+
}
200219
} else {
201220
Module::call(method_name, arguments);
202221
}
@@ -208,3 +227,68 @@ void Can::subscribe(const uint32_t id, const Module_ptr module) {
208227
}
209228
this->subscribers[id] = module;
210229
}
230+
231+
void Can::reset_can_bus() {
232+
twai_status_info_t status_info;
233+
234+
if (twai_get_status_info(&status_info) != ESP_OK) {
235+
throw std::runtime_error("could not get TWAI status");
236+
}
237+
238+
echo("CAN bus state before reset: %s",
239+
status_info.state == TWAI_STATE_STOPPED ? "STOPPED"
240+
: status_info.state == TWAI_STATE_RUNNING ? "RUNNING"
241+
: status_info.state == TWAI_STATE_BUS_OFF ? "BUS_OFF"
242+
: status_info.state == TWAI_STATE_RECOVERING ? "RECOVERING"
243+
: "UNKNOWN");
244+
245+
if (status_info.state != TWAI_STATE_STOPPED) {
246+
if (twai_stop() != ESP_OK) {
247+
throw std::runtime_error("could not stop TWAI driver");
248+
}
249+
if (twai_get_status_info(&status_info) != ESP_OK || status_info.state != TWAI_STATE_STOPPED) {
250+
throw std::runtime_error("TWAI driver didn't stop properly");
251+
}
252+
}
253+
254+
if (status_info.state == TWAI_STATE_BUS_OFF) {
255+
if (twai_initiate_recovery() != ESP_OK) {
256+
throw std::runtime_error("could not initiate recovery");
257+
}
258+
259+
const unsigned long start_time = millis();
260+
const unsigned long timeout_ms = 500;
261+
262+
while (true) {
263+
if (twai_get_status_info(&status_info) != ESP_OK) {
264+
throw std::runtime_error("failed to get status during recovery");
265+
}
266+
267+
if (status_info.state != TWAI_STATE_RECOVERING) {
268+
echo("Recovery completed, state: %s",
269+
status_info.state == TWAI_STATE_STOPPED ? "STOPPED"
270+
: status_info.state == TWAI_STATE_RUNNING ? "RUNNING"
271+
: status_info.state == TWAI_STATE_BUS_OFF ? "BUS_OFF"
272+
: "UNKNOWN");
273+
break;
274+
}
275+
276+
if (millis_since(start_time) > timeout_ms) {
277+
throw std::runtime_error("recovery timeout");
278+
}
279+
280+
delay(20);
281+
}
282+
}
283+
284+
echo("Starting TWAI driver...");
285+
if (twai_start() != ESP_OK) {
286+
throw std::runtime_error("could not start TWAI driver");
287+
}
288+
289+
if (twai_get_status_info(&status_info) != ESP_OK || status_info.state != TWAI_STATE_RUNNING) {
290+
throw std::runtime_error("TWAI driver didn't start properly");
291+
}
292+
293+
echo("CAN bus reset successful, state: RUNNING");
294+
}

‎main/modules/can.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,4 +24,5 @@ class Can : public Module {
2424
const uint8_t d4, const uint8_t d5, const uint8_t d6, const uint8_t d7,
2525
const bool rtr = false) const;
2626
void subscribe(const uint32_t id, const Module_ptr module);
27+
void reset_can_bus();
2728
};

‎main/modules/canopen_motor.cpp‎

Lines changed: 44 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -84,8 +84,13 @@ void CanOpenMotor::wait_for_sdo_writes(uint32_t timeout_ms) {
8484
uint32_t cycles = timeout_ms / ms_per_sleep;
8585

8686
for (uint32_t i = 0; i < cycles; ++i) {
87-
while (this->can->receive())
88-
;
87+
try {
88+
while (this->can->receive())
89+
;
90+
} catch (const std::exception &e) {
91+
echo("CAN receive error during SDO write wait: %s", e.what());
92+
}
93+
8994
delay(ms_per_sleep);
9095

9196
if (this->properties[PROP_PENDING_WRITES]->integer_value == 0) {
@@ -220,15 +225,40 @@ void CanOpenMotor::transition_operational() {
220225
this->can->send(0, data, false, sizeof(data));
221226
}
222227

228+
bool CanOpenMotor::send_sdo_with_retry(uint32_t cob_id, const uint8_t *data) {
229+
const int max_attempts = 3;
230+
231+
for (int attempt = 0; attempt < max_attempts; attempt++) {
232+
try {
233+
this->properties[PROP_PENDING_WRITES]->integer_value++;
234+
this->can->send(cob_id, data);
235+
wait_for_sdo_writes(100);
236+
return true;
237+
} catch (const std::exception &e) {
238+
this->properties[PROP_PENDING_WRITES]->integer_value--;
239+
if (attempt < max_attempts - 1) {
240+
try {
241+
this->can->reset_can_bus();
242+
delay(50);
243+
} catch (const std::exception &e) {
244+
echo("CAN recovery failed: %s", e.what());
245+
}
246+
}
247+
}
248+
}
249+
250+
return false;
251+
}
252+
223253
void CanOpenMotor::write_od_u8(uint16_t index, uint8_t sub, uint8_t value) {
224254
uint8_t data[8];
225255
data[0] = sdo_write_u8_header;
226256
marshal_index(index, sub, &data[1]);
227257
marshal_unsigned(value, &data[4]);
228258

229-
can->send(wrap_cob_id(COB_SDO_CLIENT2SERVER, node_id), data);
230-
this->properties[PROP_PENDING_WRITES]->integer_value++;
231-
wait_for_sdo_writes(100);
259+
if (!send_sdo_with_retry(wrap_cob_id(COB_SDO_CLIENT2SERVER, node_id), data)) {
260+
throw std::runtime_error("Failed to send SDO write command after multiple retries");
261+
}
232262
}
233263

234264
void CanOpenMotor::write_od_u16(uint16_t index, uint8_t sub, uint16_t value) {
@@ -237,9 +267,9 @@ void CanOpenMotor::write_od_u16(uint16_t index, uint8_t sub, uint16_t value) {
237267
marshal_index(index, sub, &data[1]);
238268
marshal_unsigned(value, &data[4]);
239269

240-
can->send(wrap_cob_id(COB_SDO_CLIENT2SERVER, node_id), data);
241-
this->properties[PROP_PENDING_WRITES]->integer_value++;
242-
wait_for_sdo_writes(100);
270+
if (!send_sdo_with_retry(wrap_cob_id(COB_SDO_CLIENT2SERVER, node_id), data)) {
271+
throw std::runtime_error("Failed to send SDO write command after multiple retries");
272+
}
243273
}
244274

245275
void CanOpenMotor::write_od_u32(uint16_t index, uint8_t sub, uint32_t value) {
@@ -248,9 +278,9 @@ void CanOpenMotor::write_od_u32(uint16_t index, uint8_t sub, uint32_t value) {
248278
marshal_index(index, sub, &data[1]);
249279
marshal_unsigned(value, &data[4]);
250280

251-
can->send(wrap_cob_id(COB_SDO_CLIENT2SERVER, node_id), data);
252-
this->properties[PROP_PENDING_WRITES]->integer_value++;
253-
wait_for_sdo_writes(100);
281+
if (!send_sdo_with_retry(wrap_cob_id(COB_SDO_CLIENT2SERVER, node_id), data)) {
282+
throw std::runtime_error("Failed to send SDO write command after multiple retries");
283+
}
254284
}
255285

256286
void CanOpenMotor::write_od_i32(uint16_t index, uint8_t sub, int32_t value) {
@@ -259,9 +289,9 @@ void CanOpenMotor::write_od_i32(uint16_t index, uint8_t sub, int32_t value) {
259289
marshal_index(index, sub, &data[1]);
260290
marshal_i32(value, &data[4]);
261291

262-
can->send(wrap_cob_id(COB_SDO_CLIENT2SERVER, node_id), data);
263-
this->properties[PROP_PENDING_WRITES]->integer_value++;
264-
wait_for_sdo_writes(100);
292+
if (!send_sdo_with_retry(wrap_cob_id(COB_SDO_CLIENT2SERVER, node_id), data)) {
293+
throw std::runtime_error("Failed to send SDO write command after multiple retries");
294+
}
265295
}
266296

267297
void CanOpenMotor::sdo_read(uint16_t index, uint8_t sub) {

‎main/modules/canopen_motor.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@ class CanOpenMotor : public Module, public std::enable_shared_from_this<CanOpenM
4646
uint16_t build_ctrl_word(bool new_set_point);
4747

4848
void wait_for_sdo_writes(uint32_t timeout_ms);
49+
bool send_sdo_with_retry(uint32_t cob_id, const uint8_t *data);
4950
void enter_position_mode(int velocity);
5051
void enter_velocity_mode(int velocity);
5152

‎sdkconfig.defaults‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,3 @@ CONFIG_ESP_COREDUMP_ENABLE_TO_FLASH=y
3333
CONFIG_ESP_COREDUMP_FLASH_SAVE=y
3434
CONFIG_ESP_COREDUMP_MAX_TASKS_NUM=16
3535
CONFIG_ESP_COREDUMP_FLASH_PARTITION="coredump"
36-
37-
# This is a workaround for a bug in the ESP-IDF TWAI driver
38-
# See https://github.com/zauberzeug/lizard/issues/126 for more details.
39-
CONFIG_TWAI_ERRATA_FIX_TX_INTR_LOST=n

0 commit comments

Comments
 (0)