Battery: Add Tattu CAN driver

libraries/AP_Arming/AP_Arming.cpp

@@ -1076,6 +1076,7 @@ bool AP_Arming::can_checks(bool report)
                 case AP_CANManager::Driver_Type_USD1:
                 case AP_CANManager::Driver_Type_None:
                 case AP_CANManager::Driver_Type_Scripting:
+                case AP_CANManager::Driver_Type_Tattu:
                 case AP_CANManager::Driver_Type_Benewake:
                     break;
             }

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -15,6 +15,7 @@
 #include "AP_BattMonitor_Generator.h"
 #include "AP_BattMonitor_INA2xx.h"
 #include "AP_BattMonitor_LTC2946.h"
+#include "AP_BattMonitor_TattuCAN.h"
 #include "AP_BattMonitor_Torqeedo.h"
 
 #include <AP_HAL/AP_HAL.h>
@@ -293,6 +294,11 @@ AP_BattMonitor::init()
                 drivers[instance] = new AP_BattMonitor_LTC2946(*this, state[instance], _params[instance]);
                 break;
 #endif
+#if AP_BATT_MONITOR_TATTU_ENABLED
+            case Type::TattuCAN:
+                drivers[instance] = new AP_BattMonitor_TattuCAN(*this, state[instance], _params[instance]);
+                break;
+#endif
 #if HAL_TORQEEDO_ENABLED
             case Type::Torqeedo:
                 drivers[instance] = new AP_BattMonitor_Torqeedo(*this, state[instance], _params[instance]);

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -48,6 +48,7 @@ class AP_BattMonitor_Generator;
 class AP_BattMonitor_INA2XX;
 class AP_BattMonitor_LTC2946;
 class AP_BattMonitor_Torqeedo;
+class AP_BattMonitor_TattuCAN;
 
 class AP_BattMonitor
 {
@@ -65,8 +66,8 @@ class AP_BattMonitor
     friend class AP_BattMonitor_Generator;
     friend class AP_BattMonitor_INA2XX;
     friend class AP_BattMonitor_LTC2946;
-
     friend class AP_BattMonitor_Torqeedo;
+    friend class AP_BattMonitor_TattuCAN;
 
 public:
 
@@ -101,6 +102,7 @@ class AP_BattMonitor
         INA2XX                     = 21,
         LTC2946                    = 22,
         Torqeedo                   = 23,
+        TattuCAN                   = 24,
     };
 
     FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);

libraries/AP_BattMonitor/AP_BattMonitor_TattuCAN.cpp

@@ -0,0 +1,158 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Author: Tom Pittenger
+ */
+
+#include "AP_BattMonitor_TattuCAN.h"
+
+#if AP_BATT_MONITOR_TATTU_ENABLED
+#include <AP_HAL/AP_HAL.h>
+
+extern const AP_HAL::HAL& hal;
+
+/// Constructor
+AP_BattMonitor_TattuCAN::AP_BattMonitor_TattuCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params &params) :
+    CANSensor("Tattu"),
+    AP_BattMonitor_Backend(mon, mon_state, params)
+{
+    // starts with not healthy
+    _state.healthy = false;
+    register_driver(AP_CANManager::Driver_Type_Tattu);
+}
+
+void AP_BattMonitor_TattuCAN::handle_frame(AP_HAL::CANFrame &frame)
+{
+    if (frame.dlc == 0) {
+        // sanity check for if there's no payload
+        return;
+    }
+
+    const uint8_t payload_len = (frame.dlc - 1); // last payload byte is tail which is not _message data
+    const uint8_t tail_byte = frame.data[frame.dlc-1];
+    const bool start = (tail_byte & TAIL_BYTE_BITS_START_OF_TRANSFER) != 0;
+    const bool end = (tail_byte & TAIL_BYTE_BITS_END_OF_TRANSFER) != 0;
+    //const bool toggle = (tail_byte & TAIL_BYTE_BITS_TOGGLE) != 0;
+    //const bool id = (tail_byte & TAIL_BYTE_TRANSFER_ID_MASK);
+
+
+    if ((start && end) || (_message_offset == 0 && !start)) {
+        // invalid or we're expecting a start and did't get one (re-sync)
+        _message_resync++;
+        return;
+
+    } else if (start) {
+        // start of a frame
+        _message_offset = 0;
+        _message_toggle_expected = 0;
+
+    } else if ((_message_offset + frame.dlc - 1) > sizeof(_message)) {
+        // buffer overflow, reset
+        // TODO: this only supports 12S batteries. If a 14S is connected, 4 more bytes come in the middle of the packet
+        _message_offset = 0;
+        _message_resync++;
+        return;
+    }
+
+    memcpy(&data[_message_offset], frame.data, payload_len);
+    _message_offset += payload_len;
+    _message_toggle_expected = !_message_toggle_expected;
+
+    if (!end) {
+        return;
+    }
+
+    // message complete!
+    memcpy((uint8_t*)&_message, data, _message_offset);
+
+    WITH_SEMAPHORE(_sem_battmon);
+
+    _interim_state.last_time_micros = AP_HAL::micros();
+    _interim_state.voltage = _message.voltage_mV * 0.001f;
+    _interim_state.current_amps = _message.current_mA * -0.01f;
+
+    _interim_state.temperature_time = AP_HAL::millis();
+    _interim_state.temperature = _message.temperature_C;
+
+    _interim_state.consumed_mah = MIN(_message.standard_capacity_mAh, _message.standard_capacity_mAh - _message.remaining_capacity_mAh) * 0.001f;
+
+    _interim_state.healthy = true; // _message.health_status
+
+    for (uint8_t i=0; i<TATTUCAN_CELL_COUNT_12S; i++) {
+        _interim_state.cell_voltages.cells[i] = _message.cell_voltage_mV[i];
+    }
+
+    _have_received_a_msg = true;
+    _message_offset = 0;
+    _message_count_good++;
+}
+
+// read - read the voltage and current
+void AP_BattMonitor_TattuCAN::read()
+{
+    if (!_have_received_a_msg) {
+        return;
+    }
+
+    // timeout after 5 seconds
+    if (_interim_state.healthy && (AP_HAL::micros() - _interim_state.last_time_micros) > 5000000) {
+        _interim_state.healthy = false;
+    }
+
+    // Copy over relevant states over to main state
+    WITH_SEMAPHORE(_sem_battmon);
+    _state.temperature = _interim_state.temperature;
+    _state.temperature_time = _interim_state.temperature_time;
+    _state.voltage = _interim_state.voltage;
+    _state.current_amps = _interim_state.current_amps;
+    _state.consumed_mah = _interim_state.consumed_mah;
+    //_state.consumed_wh = _interim_state.consumed_wh;
+    _state.last_time_micros = _interim_state.last_time_micros;
+    _state.healthy = _interim_state.healthy;
+    //_state.time_remaining = _interim_state.time_remaining;
+    //_state.has_time_remaining = _interim_state.has_time_remaining;
+    //_state.is_powering_off = _interim_state.is_powering_off;
+    memcpy(_state.cell_voltages.cells, _interim_state.cell_voltages.cells, sizeof(_state.cell_voltages));
+}
+
+/// capacity_remaining_pct - returns true if the percentage is valid and writes to percentage argument
+bool AP_BattMonitor_TattuCAN::capacity_remaining_pct(uint8_t &percentage) const
+{
+    if (!_have_received_a_msg) {
+        return false;
+    }
+
+    percentage = _message.remaining_percent;
+    return true;
+}
+
+/// get_cycle_count - return true if cycle count can be provided and fills in cycles argument
+bool AP_BattMonitor_TattuCAN::get_cycle_count(uint16_t &cycles) const
+{
+    if (!_have_received_a_msg) {
+        return false;
+    }
+
+    cycles = _message.cycle_life;
+    return true;
+}
+
+// return mavlink fault bitmask (see MAV_BATTERY_FAULT enum)
+uint32_t AP_BattMonitor_TattuCAN::get_mavlink_fault_bitmask() const
+{
+    // TODO: populate errors
+    return 0;
+}
+
+#endif

libraries/AP_BattMonitor/AP_BattMonitor_TattuCAN.h

@@ -0,0 +1,101 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Author: Tom Pittenger
+ */
+
+#pragma once
+
+#include "AP_BattMonitor.h"
+
+#ifndef AP_BATT_MONITOR_TATTU_ENABLED
+#define AP_BATT_MONITOR_TATTU_ENABLED !HAL_MINIMIZE_FEATURES && (BOARD_FLASH_SIZE > 1024) && (HAL_MAX_CAN_PROTOCOL_DRIVERS > 1)
+#endif
+
+#if AP_BATT_MONITOR_TATTU_ENABLED
+
+#include "AP_BattMonitor_Backend.h"
+#include <AP_CANManager/AP_CANSensor.h>
+
+class AP_BattMonitor_TattuCAN : public AP_BattMonitor_Backend, public CANSensor {
+public:
+
+    /// Constructor
+    AP_BattMonitor_TattuCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params &params);
+
+    /// Read the battery voltage and current.  Should be called at 10hz
+    void read() override;
+
+    /// capacity_remaining_pct - returns true if the percentage is valid and writes to percentage argument
+    bool capacity_remaining_pct(uint8_t &percentage) const override;
+
+    bool has_temperature() const override { return _have_received_a_msg; }
+
+    bool has_current() const override { return _have_received_a_msg; }
+
+    bool has_consumed_energy() const override { return _have_received_a_msg && (_message.remaining_capacity_mAh < _message.standard_capacity_mAh); }
+
+    bool has_time_remaining() const override { return false; }
+
+    bool has_cell_voltages() const override { return _have_received_a_msg; }
+
+    bool get_cycle_count(uint16_t &cycles) const override;
+
+    // return mavlink fault bitmask (see MAV_BATTERY_FAULT enum)
+    uint32_t get_mavlink_fault_bitmask() const override;
+
+private:
+    void handle_frame(AP_HAL::CANFrame &frame) override;
+
+    static constexpr uint8_t TAIL_BYTE_BITS_START_OF_TRANSFER = (1<<7);
+    static constexpr uint8_t TAIL_BYTE_BITS_END_OF_TRANSFER = (1<<6);
+    static constexpr uint8_t TAIL_BYTE_BITS_TOGGLE = (1<<5);
+    static constexpr uint8_t TAIL_BYTE_TRANSFER_ID_MASK = 0x1F;
+
+    static constexpr uint8_t TATTUCAN_CELL_COUNT_12S = 12;
+
+    HAL_Semaphore _sem_battmon;
+
+    struct PACKED {
+        uint16_t    crc;
+        int16_t     manufacturer;
+        int16_t     sku;
+        uint16_t    voltage_mV;
+        int16_t     current_mA;
+        int16_t     temperature_C;
+        uint16_t    remaining_percent;
+        uint16_t    cycle_life;
+        int16_t     health_status;  // percent
+        uint16_t    cell_voltage_mV[TATTUCAN_CELL_COUNT_12S];
+        uint16_t    standard_capacity_mAh;
+        uint16_t    remaining_capacity_mAh;
+        uint32_t    error_info;
+    } _message;
+
+    uint8_t data[sizeof(_message)];
+
+    uint32_t _message_offset;
+    bool _message_toggle_expected;
+
+    uint32_t _frame_count_good;
+    uint32_t _frame_count_bad;
+    uint32_t _message_count_good;
+    uint32_t _message_count_bad;
+    uint32_t _message_resync;
+
+    bool    _have_received_a_msg;
+    AP_BattMonitor::BattMonitor_State _interim_state;
+};
+
+#endif // AP_BATT_MONITOR_TATTU_ENABLED

libraries/AP_CANManager/AP_CANManager.h

@@ -64,6 +64,7 @@ class AP_CANManager
         // 9 was Driver_Type_MPPT_PacketDigital
         Driver_Type_Scripting = 10,
         Driver_Type_Benewake = 11,
+        Driver_Type_Tattu = 12,
     };
 
     void init(void);