experimental driver for the Tattu 12S 16000mAh Smart Battery on /dev/can0 (#18164)

- This driver is highly specific to this model of Tattu battery and cannot work with other models without code changes. - The driver simply reads CAN frames using the NuttX CAN character device interface and converts the data into the proper format as specified in the Tattu datasheet Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
3 years ago · 4b5a91bf7d
3 changed files with 382 additions and 0 deletions
--- a/src/drivers/tattu_can/CMakeLists.txt
+++ b/src/drivers/tattu_can/CMakeLists.txt
@ -0,0 +1,39 @@
				@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__tattu_can
+	MAIN tattu_can
+	COMPILE_FLAGS
+	SRCS
+		TattuCan.cpp
+	)
--- a/src/drivers/tattu_can/TattuCan.cpp
+++ b/src/drivers/tattu_can/TattuCan.cpp
@ -0,0 +1,237 @@
				@@ -0,0 +1,237 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file TattuCan.cpp
+ * @author Jacob Dahl <dahl.jakejacob@gmail.com>
+ *
+ * Driver for the Tattu 12S 1600mAh Smart Battery connected over CAN.
+ *
+ * This driver simply decodes the CAN frames based on the specification
+ * as provided in the Tattu datasheet DOC 001 REV D, which is highly
+ * specific to the 12S 1600mAh battery. Other models of Tattu batteries
+ * will NOT work with this driver in its current form.
+ *
+ */
+
+#include "TattuCan.hpp"
+
+extern orb_advert_t mavlink_log_pub;
+
+TattuCan::TattuCan() :
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan)
+{
+}
+
+TattuCan::~TattuCan()
+{
+}
+
+void TattuCan::Run()
+{
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
+	}
+
+	if (!_initialized) {
+
+		_fd = ::open("/dev/can0", O_RDWR);
+
+		if (_fd < 0) {
+			PX4_INFO("FAILED TO OPEN /dev/can0");
+			return;
+		}
+
+		_initialized = true;
+	}
+
+	uint8_t data[64] {};
+	CanFrame received_frame{};
+	received_frame.payload = &data;
+
+	Tattu12SBatteryMessage tattu_message = {};
+
+	while (receive(&received_frame) > 0) {
+
+		// Find the start of a transferr
+		if ((received_frame.payload_size == 8) && ((uint8_t *)received_frame.payload)[7] == TAIL_BYTE_START_OF_TRANSFER) {
+		} else {
+			continue;
+		}
+
+		// We have the start of a transfer
+		size_t offset = 5;
+		memcpy(&tattu_message, &(((uint8_t *)received_frame.payload)[2]), offset);
+
+		while (receive(&received_frame) > 0) {
+
+			size_t payload_size = received_frame.payload_size - 1;
+			// TODO: add check to prevent buffer overflow from a corrupt 'payload_size' value
+			// TODO: AND look for TAIL_BYTE_START_OF_TRANSFER to indicate end of transfer. Untested...
+			memcpy(((char *)&tattu_message) + offset, received_frame.payload, payload_size);
+			offset += payload_size;
+		}
+
+		battery_status_s battery_status = {};
+		battery_status.timestamp = hrt_absolute_time();
+		battery_status.connected = true;
+		battery_status.cell_count = 12;
+
+		sprintf(battery_status.serial_number, "%d", tattu_message.manufacturer);
+		battery_status.id = static_cast<uint8_t>(tattu_message.sku);
+
+		battery_status.cycle_count = tattu_message.cycle_life;
+		battery_status.state_of_health = static_cast<uint16_t>(tattu_message.health_status);
+
+		battery_status.voltage_v = static_cast<float>(tattu_message.voltage) / 1000.0f;
+		battery_status.voltage_filtered_v = static_cast<float>(tattu_message.voltage) / 1000.0f;
+		battery_status.current_a = static_cast<float>(tattu_message.current) / 1000.0f;
+		battery_status.current_filtered_a = static_cast<float>(tattu_message.current) / 1000.0f;
+		battery_status.remaining = static_cast<float>(tattu_message.remaining_percent) / 100.0f;
+		battery_status.temperature = static_cast<float>(tattu_message.temperature);
+		battery_status.capacity = tattu_message.standard_capacity;
+		battery_status.voltage_cell_v[0] = static_cast<float>(tattu_message.cell_1_voltage) / 1000.0f;
+		battery_status.voltage_cell_v[1] = static_cast<float>(tattu_message.cell_2_voltage) / 1000.0f;
+		battery_status.voltage_cell_v[2] = static_cast<float>(tattu_message.cell_3_voltage) / 1000.0f;
+		battery_status.voltage_cell_v[3] = static_cast<float>(tattu_message.cell_4_voltage) / 1000.0f;
+		battery_status.voltage_cell_v[4] = static_cast<float>(tattu_message.cell_5_voltage) / 1000.0f;
+		battery_status.voltage_cell_v[5] = static_cast<float>(tattu_message.cell_6_voltage) / 1000.0f;
+		battery_status.voltage_cell_v[6] = static_cast<float>(tattu_message.cell_7_voltage) / 1000.0f;
+		battery_status.voltage_cell_v[7] = static_cast<float>(tattu_message.cell_8_voltage) / 1000.0f;
+		battery_status.voltage_cell_v[8] = static_cast<float>(tattu_message.cell_9_voltage) / 1000.0f;
+		battery_status.voltage_cell_v[9] = static_cast<float>(tattu_message.cell_10_voltage / 1000.0f);
+		battery_status.voltage_cell_v[10] = static_cast<float>(tattu_message.cell_11_voltage) / 1000.0f;
+		battery_status.voltage_cell_v[11] = static_cast<float>(tattu_message.cell_12_voltage) / 1000.0f;
+
+		_battery_status_pub.publish(battery_status);
+	}
+}
+
+int16_t TattuCan::receive(CanFrame *received_frame)
+{
+	if ((_fd < 0) || (received_frame == nullptr)) {
+		PX4_INFO("fd < 0");
+		return -1;
+	}
+
+	struct pollfd fds {};
+
+	fds.fd = _fd;
+
+	fds.events = POLLIN;
+
+	// Jake: this doesn't block even in blocking mode, dunno if we need it
+	::poll(&fds, 1, 0);
+
+	// Only execute this part if can0 is changed.
+	if (fds.revents & POLLIN) {
+
+		// Try to read.
+		struct can_msg_s receive_msg;
+		const ssize_t nbytes = ::read(fds.fd, &receive_msg, sizeof(receive_msg));
+
+		if (nbytes < 0 || (size_t)nbytes < CAN_MSGLEN(0) || (size_t)nbytes > sizeof(receive_msg)) {
+			PX4_INFO("error");
+			return -1;
+
+		} else {
+			received_frame->extended_can_id = receive_msg.cm_hdr.ch_id;
+			received_frame->payload_size = receive_msg.cm_hdr.ch_dlc;
+			memcpy((void *)received_frame->payload, receive_msg.cm_data, receive_msg.cm_hdr.ch_dlc);
+			return nbytes;
+		}
+	}
+
+	return 0;
+}
+
+int TattuCan::start()
+{
+	// There is a race condition at boot that sometimes causes opening of
+	// /dev/can0 to fail. We will delay 0.5s to be safe.
+	uint32_t delay_us = 500000;
+	ScheduleOnInterval(1000000 / SAMPLE_RATE, delay_us);
+	return PX4_OK;
+}
+
+int TattuCan::task_spawn(int argc, char *argv[])
+{
+	TattuCan *instance = new TattuCan();
+
+	if (!instance) {
+		PX4_ERR("driver allocation failed");
+		return PX4_ERROR;
+	}
+
+	_object.store(instance);
+	_task_id = task_id_is_work_queue;
+
+	instance->start();
+	return 0;
+}
+
+int TattuCan::print_usage(const char *reason)
+{
+	if (reason) {
+		printf("%s\n\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Driver for reading data from the Tattu 12S 16000mAh smart battery.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("tattu_can", "system");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+int TattuCan::custom_command(int argc, char *argv[])
+{
+	if (!is_running()) {
+		PX4_INFO("not running");
+		return PX4_ERROR;
+	}
+
+	return print_usage("Unrecognized command.");
+}
+
+extern "C" __EXPORT int tattu_can_main(int argc, char *argv[])
+{
+	return TattuCan::main(argc, argv);
+}
--- a/src/drivers/tattu_can/TattuCan.hpp
+++ b/src/drivers/tattu_can/TattuCan.hpp
@ -0,0 +1,106 @@
				@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <nuttx/can/can.h>
+
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+#include <uORB/Publication.hpp>
+#include <uORB/topics/battery_status.h>
+
+using namespace time_literals;
+
+typedef struct __attribute__((packed))
+{
+	int16_t     manufacturer;
+	uint16_t    sku;
+	uint16_t    voltage;
+	int16_t     current;
+	int16_t     temperature;
+	uint16_t    remaining_percent;
+	uint16_t    cycle_life;
+	int16_t     health_status;
+	uint16_t    cell_1_voltage;
+	uint16_t    cell_2_voltage;
+	uint16_t    cell_3_voltage;
+	uint16_t    cell_4_voltage;
+	uint16_t    cell_5_voltage;
+	uint16_t    cell_6_voltage;
+	uint16_t    cell_7_voltage;
+	uint16_t    cell_8_voltage;
+	uint16_t    cell_9_voltage;
+	uint16_t    cell_10_voltage;
+	uint16_t    cell_11_voltage;
+	uint16_t    cell_12_voltage;
+	uint16_t    standard_capacity;
+	uint16_t    remaining_capacity_mah;
+	uint32_t    error_info;
+} Tattu12SBatteryMessage;
+
+typedef struct {
+	uint64_t timestamp_usec;
+	uint32_t extended_can_id;
+	size_t      payload_size;
+	const void *payload;
+} CanFrame;
+
+class TattuCan : public ModuleBase<TattuCan>, public px4::ScheduledWorkItem
+{
+public:
+	TattuCan();
+
+	virtual ~TattuCan();
+
+	static int print_usage(const char *reason = nullptr);
+	static int custom_command(int argc, char *argv[]);
+
+	static int task_spawn(int argc, char *argv[]);
+
+	int start();
+
+	int16_t receive(CanFrame *received_frame);
+
+private:
+	static constexpr uint32_t SAMPLE_RATE{100}; // samples per second (10ms)
+	static constexpr size_t TAIL_BYTE_START_OF_TRANSFER{128};
+
+	void Run() override;
+
+	int _fd{-1};
+	bool _initialized{false};
+
+	uORB::Publication<battery_status_s> _battery_status_pub{ORB_ID::battery_status};
+};