AP_Periph: added BatteryTag support

Tools/AP_Periph/AP_Periph.cpp

@@ -553,6 +553,9 @@ void AP_Periph_FW::update()
 #if AP_PERIPH_ADSB_ENABLED
     adsb_update();
 #endif
+#if AP_PERIPH_BATTERY_TAG_ENABLED
+    battery_tag.update();
+#endif
 }
 
 #ifdef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT

Tools/AP_Periph/AP_Periph.h

@@ -40,6 +40,7 @@
 #include <AP_RCProtocol/AP_RCProtocol_config.h>
 #include "rc_in.h"
 #include "batt_balance.h"
+#include "battery_tag.h"
 #include "networking.h"
 #include "serial_options.h"
 #if AP_SIM_ENABLED
@@ -103,6 +104,10 @@
 #define HAL_PERIPH_CAN_MIRROR 0
 #endif
 
+#ifndef AP_SIM_PARAM_ENABLED
+#define AP_SIM_PARAM_ENABLED AP_SIM_ENABLED
+#endif
+
 #if defined(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT) && !defined(HAL_DEBUG_BUILD) && !defined(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_NON_DEBUG)
 /* this checking for reboot can lose bytes on GPS modules and other
  * serial devices. It is really only relevant on a debug build if you
@@ -383,6 +388,10 @@ class AP_Periph_FW {
     BattBalance battery_balance;
 #endif
 
+#if AP_PERIPH_BATTERY_TAG_ENABLED
+    BatteryTag battery_tag;
+#endif
+
 #if AP_PERIPH_SERIAL_OPTIONS_ENABLED
     SerialOptions serial_options;
 #endif
@@ -552,6 +561,7 @@ class AP_Periph_FW {
     void handle_lightscommand(CanardInstance* canard_instance, CanardRxTransfer* transfer);
     void handle_notify_state(CanardInstance* canard_instance, CanardRxTransfer* transfer);
     void handle_hardpoint_command(CanardInstance* canard_instance, CanardRxTransfer* transfer);
+    void handle_globaltime(CanardInstance* canard_instance, CanardRxTransfer* transfer);
 
     void process1HzTasks(uint64_t timestamp_usec);
     void processTx(void);

Tools/AP_Periph/Parameters.cpp

@@ -636,14 +636,20 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GOBJECT(battery_balance, "BAL",  BattBalance),
 #endif
 
+#if AP_PERIPH_BATTERY_TAG_ENABLED
+    // @Group: BTAG
+    // @Path: battery_tag.cpp
+    GOBJECT(battery_tag, "BTAG",  BatteryTag),
+#endif
+
 #if AP_PERIPH_SERIAL_OPTIONS_ENABLED
     // @Group: UART
     // @Path: serial_options.cpp
     GOBJECT(serial_options, "UART",  SerialOptions),
 #endif
 
     // NOTE: sim parameters should go last
-#if AP_SIM_ENABLED
+#if AP_SIM_PARAM_ENABLED
     // @Group: SIM_
     // @Path: ../libraries/SITL/SITL.cpp
     GOBJECT(sitl, "SIM_", SITL::SIM),

Tools/AP_Periph/Parameters.h

@@ -101,6 +101,7 @@ class Parameters {
         k_param_imu,
         k_param_dac,
         k_param__gcs,
+        k_param_battery_tag,
     };
 
     AP_Int16 format_version;

Tools/AP_Periph/battery_tag.cpp

@@ -0,0 +1,153 @@
+/*
+   This program 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 program 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/>.
+ */
+/*
+  battery info support. This feature has a set of parameters which are
+  controlled by a lua script on the FMU. The battery infomation node
+  is attached to a specific battery and recorded the number of cycles,
+  and total armed time
+ */
+#include "AP_Periph.h"
+
+#if AP_PERIPH_BATTERY_TAG_ENABLED
+
+#include <dronecan_msgs.h>
+
+extern const AP_HAL::HAL &hal;
+
+const AP_Param::GroupInfo BatteryTag::var_info[] {
+    // @Param: _NUM_CYCLES
+    // @DisplayName: Number of cycles
+    // @Description: Number of cycles the battery has been through
+    AP_GROUPINFO("_NUM_CYCLES", 1, BatteryTag, num_cycles, 0),
+
+    // @Param: _ARM_HOURS
+    // @DisplayName: Number of armed hours
+    // @Description: Number of hours the battery has been armed
+    AP_GROUPINFO("_ARM_HOURS", 2, BatteryTag, armed_hours, 0),
+
+    // @Param: _CAPACITY
+    // @DisplayName: Battery capacity
+    // @Description: Battery capacity in mAh
+    AP_GROUPINFO("_CAPACITY", 3, BatteryTag, capacity_mAh, 0),
+
+    // @Param: _FIRST_USE
+    // @DisplayName: First use time
+    // @Description: First use time in minutes since 1/1/1970
+    AP_GROUPINFO("_FIRST_USE", 4, BatteryTag, first_use_min, 0),
+
+    // @Param: _LAST_USE
+    // @DisplayName: Last use time
+    // @Description: Last use time in minutes since 1/1/1970
+    AP_GROUPINFO("_LAST_USE", 5, BatteryTag, last_use_min, 0),
+
+    // @Param: _SERIAL
+    // @DisplayName: Serial number
+    // @Description: Serial number
+    AP_GROUPINFO("_SERIAL", 6, BatteryTag, serial_num, 0),
+
+    // @Param: _CYCLE_MIN
+    // @DisplayName: Cycle minimum time
+    // @Description: Cycle minimum time. Minimum time that vehicle is armed in minutes for counting a battery cycle
+    AP_GROUPINFO("_CYCLE_MIN", 7, BatteryTag, cycle_min, 1),
+
+    AP_GROUPEND
+};
+
+BatteryTag::BatteryTag(void)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+void BatteryTag::update(void)
+{
+    const uint32_t now = AP_HAL::millis();
+    if (now - last_update_ms < 1000U) {
+        return;
+    }
+    last_update_ms = now;
+
+    const bool armed = hal.util->get_soft_armed();
+    if (armed && arm_start_ms == 0) {
+        arm_start_ms = now;
+    }
+
+    if (!armed && was_armed) {
+        /*
+          update total armed time
+         */
+        const float armed_minutes = (now - arm_start_ms)*(0.001/60);
+        armed_hours.set_and_save(armed_hours + armed_minutes/60);
+
+        /*
+          update number of cycles if we were armed for more than
+          BTAG_CYCLE_MIN minutes
+         */
+        if (armed_minutes >= cycle_min) {
+            num_cycles.set_and_save(num_cycles + 1);
+        }
+    }
+
+    uint64_t utc_usec;
+    if (AP::rtc().get_utc_usec(utc_usec)) {
+        const uint32_t utc_minutes = utc_usec / 60e6;
+        if (armed && first_use_min == 0) {
+            /*
+              first time the battery has been armed
+             */
+            first_use_min.set_and_save(utc_minutes);
+        }
+        if (!armed && was_armed) {
+            // record when battery was last used
+            last_use_min.set_and_save(utc_minutes);
+        }
+    }
+
+    if (armed && serial_num == 0) {
+        // auto-create serial number if not set by vendor
+        char sysid[50];
+        if (hal.util->get_system_id(sysid)) {
+            // 31 bit for AP_Int32
+            const auto crc = crc32_small(0, (uint8_t*)sysid, strnlen(sysid, sizeof(sysid))) & 0x7FFFFFFFUL;
+            serial_num.set_and_save(crc);
+        }
+    }
+
+    was_armed = armed;
+
+    if (now - last_bcast_ms >= 10000U) {
+        last_bcast_ms = now;
+
+        // announce the battery tag information every 10s
+        ardupilot_equipment_power_BatteryTag pkt {};
+
+        pkt.serial_number = serial_num;
+        pkt.num_cycles = num_cycles;
+        pkt.armed_hours = armed_hours;
+        pkt.battery_capacity_mAh = capacity_mAh;
+        pkt.first_use_mins = first_use_min;
+        pkt.last_arm_time_mins = last_use_min;
+
+        uint8_t buffer[ARDUPILOT_EQUIPMENT_POWER_BATTERYTAG_MAX_SIZE];
+        uint16_t total_size = ardupilot_equipment_power_BatteryTag_encode(&pkt, buffer, !periph.canfdout());
+
+        periph.canard_broadcast(ARDUPILOT_EQUIPMENT_POWER_BATTERYTAG_SIGNATURE,
+                                ARDUPILOT_EQUIPMENT_POWER_BATTERYTAG_ID,
+                                CANARD_TRANSFER_PRIORITY_LOW,
+                                &buffer[0],
+                                total_size);
+    }
+}
+
+#endif  // AP_PERIPH_BATTERY_TAG_ENABLED

Tools/AP_Periph/battery_tag.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#if AP_PERIPH_BATTERY_TAG_ENABLED
+
+class BatteryTag {
+public:
+    friend class AP_Periph_FW;
+    BatteryTag(void);
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+    void update(void);
+
+private:
+    uint32_t last_update_ms;
+    uint32_t last_bcast_ms;
+
+    AP_Int32 num_cycles;
+    AP_Int32 serial_num;
+    AP_Int32 capacity_mAh;
+    AP_Int32 first_use_min;
+    AP_Int32 last_use_min;
+    AP_Float armed_hours;
+    AP_Float cycle_min;
+
+    uint32_t arm_start_ms;
+    bool was_armed;
+};
+
+#endif // AP_PERIPH_BATTERY_TAG_ENABLED
+

Tools/AP_Periph/can.cpp

@@ -524,6 +524,21 @@ void AP_Periph_FW::handle_arming_status(CanardInstance* canard_instance, CanardR
 }
 
 
+#if AP_PERIPH_RTC_ENABLED
+/*
+  handle GlobalTime
+ */
+void AP_Periph_FW::handle_globaltime(CanardInstance* canard_instance, CanardRxTransfer* transfer)
+{
+    dronecan_protocol_GlobalTime req;
+    if (dronecan_protocol_GlobalTime_decode(transfer, &req)) {
+        return;
+    }
+    AP::rtc().set_utc_usec(req.timestamp.usec, AP_RTC::source_type::SOURCE_GPS);
+}
+#endif // AP_PERIPH_RTC_ENABLED
+
+
 
 #if AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY || AP_PERIPH_NOTIFY_ENABLED
 void AP_Periph_FW::set_rgb_led(uint8_t red, uint8_t green, uint8_t blue)
@@ -885,6 +900,11 @@ void AP_Periph_FW::onTransferReceived(CanardInstance* canard_instance,
         handle_hardpoint_command(canard_instance, transfer);
         break;
 #endif
+#if AP_PERIPH_RTC_ENABLED
+    case DRONECAN_PROTOCOL_GLOBALTIME_ID:
+        handle_globaltime(canard_instance, transfer);
+        break;
+#endif
 
     }
 }
@@ -999,6 +1019,11 @@ bool AP_Periph_FW::shouldAcceptTransfer(const CanardInstance* canard_instance,
     case UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID:
         *out_data_type_signature = UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE;
         return true;
+#endif
+#if AP_PERIPH_RTC_ENABLED
+    case DRONECAN_PROTOCOL_GLOBALTIME_ID:
+        *out_data_type_signature = DRONECAN_PROTOCOL_GLOBALTIME_SIGNATURE;
+        return true;
 #endif
     default:
         break;

Tools/ardupilotwaf/boards.py

@@ -967,6 +967,7 @@ def configure_env(self, cfg, env):
             AP_PERIPH_IMU_ENABLED = 0,
             AP_PERIPH_MAG_ENABLED = 0,
             AP_PERIPH_BATTERY_BALANCE_ENABLED = 0,
+            AP_PERIPH_BATTERY_TAG_ENABLED = 0,
             AP_PERIPH_MSP_ENABLED = 0,
             AP_PERIPH_BARO_ENABLED = 0,
             AP_PERIPH_EFI_ENABLED = 0,
@@ -1051,6 +1052,24 @@ def configure_env(self, cfg, env):
             AP_PERIPH_BATTERY_ENABLED = 1,
         )
 
+class sitl_periph_battery_tag(sitl_periph):
+    def configure_env(self, cfg, env):
+        cfg.env.AP_PERIPH = 1
+        super(sitl_periph_battery_tag, self).configure_env(cfg, env)
+        env.DEFINES.update(
+            HAL_BUILD_AP_PERIPH = 1,
+            PERIPH_FW = 1,
+            CAN_APP_NODE_NAME = '"org.ardupilot.battery_tag"',
+            APJ_BOARD_ID = 101,
+
+            AP_SIM_PARAM_ENABLED = 0,
+            AP_KDECAN_ENABLED = 0,
+            AP_TEMPERATURE_SENSOR_ENABLED = 0,
+            AP_PERIPH_BATTERY_TAG_ENABLED = 1,
+            AP_RTC_ENABLED = 1,
+            AP_PERIPH_RTC_ENABLED = 1,
+        )
+
 class esp32(Board):
     abstract = True
     toolchain = 'xtensa-esp32-elf'

libraries/AP_HAL_ChibiOS/hwdef/MatekL431-BatteryTag/hwdef-bl.dat

@@ -0,0 +1 @@
+include ../MatekL431/hwdef-bl.inc

libraries/AP_HAL_ChibiOS/hwdef/MatekL431-BatteryTag/hwdef.dat

@@ -0,0 +1,8 @@
+include ../MatekL431/hwdef.inc
+
+define HAL_USE_ADC FALSE
+define STM32_ADC_USE_ADC1 FALSE
+define HAL_DISABLE_ADC_DRIVER TRUE
+
+define AP_PERIPH_BATTERY_TAG_ENABLED 1
+define AP_PERIPH_RTC_ENABLED 1

libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h

@@ -278,6 +278,9 @@
 #ifndef AP_PERIPH_BATTERY_BALANCE_ENABLED
 #define AP_PERIPH_BATTERY_BALANCE_ENABLED 0
 #endif
+#ifndef AP_PERIPH_BATTERY_TAG_ENABLED
+#define AP_PERIPH_BATTERY_TAG_ENABLED 0
+#endif
 #ifndef AP_PERIPH_PROXIMITY_ENABLED
 #define AP_PERIPH_PROXIMITY_ENABLED 0
 #endif