uavcan_1:Fixed hardfault on stop

Author: davids5

src/drivers/uavcan_v1/Uavcan.cpp

@@ -98,25 +98,28 @@ UavcanNode::~UavcanNode()
 		_task_should_exit.store(true);
 		ScheduleNow();
 
-		unsigned i = 10;
+		unsigned i = 1000;
 
 		do {
-			/* wait 5ms - it should wake every 10ms or so worst-case */
+			/* Wait for it to exit or timeout */
 			usleep(5000);
 
 			if (--i == 0) {
+				PX4_ERR("Failed to Stop Task - reboot needed");
 				break;
 			}
 
 		} while (_instance);
 	}
 
 	delete _can_interface;
+	_can_interface = nullptr;
 
 	perf_free(_cycle_perf);
 	perf_free(_interval_perf);
 
-	//delete _uavcan_heap;
+	delete static_cast<uint8_t *>(_uavcan_heap);
+	_uavcan_heap = nullptr;
 }
 
 int UavcanNode::start(uint32_t node_id, uint32_t bitrate)
@@ -163,7 +166,7 @@ void UavcanNode::Run()
 {
 	pthread_mutex_lock(&_node_mutex);
 
-	if (!_initialized) {
+	if (_instance != nullptr && !_initialized) {
 		init();
 
 		// return early if still not initialized
@@ -216,7 +219,7 @@ void UavcanNode::Run()
 	CanardFrame received_frame{};
 	received_frame.payload = &data;
 
-	while (_can_interface->receive(&received_frame) > 0) {
+	while (!_task_should_exit.load() && _can_interface->receive(&received_frame) > 0) {
 		CanardTransfer receive{};
 		CanardRxSubscription *subscription = NULL;
 		int32_t result = canardRxAccept2(&_canard_instance, &received_frame, 0, &receive, &subscription);
@@ -253,10 +256,14 @@ void UavcanNode::Run()
 
 	perf_end(_cycle_perf);
 
-	if (_task_should_exit.load()) {
-		_can_interface->close();
-
+	if (_instance && _task_should_exit.load()) {
 		ScheduleClear();
+
+		if (_initialized &&  _can_interface != nullptr) {
+			_can_interface->close();
+			_initialized = false;
+		}
+
 		_instance = nullptr;
 	}
 

src/drivers/uavcan_v1/Uavcan.hpp

@@ -148,7 +148,7 @@ class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem
 
 	void *_uavcan_heap{nullptr};
 
-	CanardInterface *const _can_interface;
+	CanardInterface *_can_interface;
 
 	CanardInstance _canard_instance;