diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index a17914031c1ca..22c9ce99adb31 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/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 diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 330264b1670c6..30658e2ae73df 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -40,6 +40,7 @@ #include #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); diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index b289eda28a1d0..61a98abbdfe2c 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -636,6 +636,12 @@ 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 @@ -643,7 +649,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { #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), diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 138ed3bfb5793..95114a6dae530 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/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; diff --git a/Tools/AP_Periph/battery_tag.cpp b/Tools/AP_Periph/battery_tag.cpp new file mode 100644 index 0000000000000..4e92ba6c78c63 --- /dev/null +++ b/Tools/AP_Periph/battery_tag.cpp @@ -0,0 +1,155 @@ +/* + 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 . + */ +/* + battery info support. The battery infomation node is attached to a + specific battery and recorded the number of cycles, and total armed + time. + + The node generates BatteryTag messages which are consumed by a + BatteryTag lua script on the flight controller + */ +#include "AP_Periph.h" + +#if AP_PERIPH_BATTERY_TAG_ENABLED + +#include + +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 diff --git a/Tools/AP_Periph/battery_tag.h b/Tools/AP_Periph/battery_tag.h new file mode 100644 index 0000000000000..c2258a04d3036 --- /dev/null +++ b/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 + diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 27f346da85c3f..4f10c9463b7df 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -524,6 +524,21 @@ void AP_Periph_FW::handle_arming_status(CanardInstance* canard_instance, CanardR } +#if AP_PERIPH_RTC_GLOBALTIME_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_GLOBALTIME_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_GLOBALTIME_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_GLOBALTIME_ENABLED + case DRONECAN_PROTOCOL_GLOBALTIME_ID: + *out_data_type_signature = DRONECAN_PROTOCOL_GLOBALTIME_SIGNATURE; + return true; #endif default: break; diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 172dec4ff6bc7..25b414db0938a 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/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, @@ -987,6 +988,7 @@ def configure_env(self, cfg, env): AP_PERIPH_TOSHIBA_LED_WITHOUT_NOTIFY_ENABLED = 0, AP_PERIPH_BUZZER_ENABLED = 0, AP_PERIPH_BUZZER_WITHOUT_NOTIFY_ENABLED = 0, + AP_PERIPH_RTC_GLOBALTIME_ENABLED = 0, ) try: @@ -1051,6 +1053,25 @@ 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, + AP_PERIPH_RTC_GLOBALTIME_ENABLED = 1, + ) + class esp32(Board): abstract = True toolchain = 'xtensa-esp32-elf' diff --git a/Tools/bootloaders/MatekL431-BatteryTag_bl.bin b/Tools/bootloaders/MatekL431-BatteryTag_bl.bin new file mode 100755 index 0000000000000..8f1e8b9ed3800 Binary files /dev/null and b/Tools/bootloaders/MatekL431-BatteryTag_bl.bin differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-BatteryTag/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-BatteryTag/hwdef-bl.dat new file mode 100644 index 0000000000000..d394ea6a77af4 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-BatteryTag/hwdef-bl.dat @@ -0,0 +1 @@ +include ../MatekL431/hwdef-bl.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-BatteryTag/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-BatteryTag/hwdef.dat new file mode 100644 index 0000000000000..3f074229a25b3 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-BatteryTag/hwdef.dat @@ -0,0 +1,9 @@ +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 +define AP_PERIPH_RTC_GLOBALTIME_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h index d239bce8dbb0f..3138ca47068f1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h +++ b/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 @@ -311,6 +314,9 @@ #ifndef AP_PERIPH_RTC_ENABLED #define AP_PERIPH_RTC_ENABLED 0 #endif +#ifndef AP_PERIPH_RTC_GLOBALTIME_ENABLED +#define AP_PERIPH_RTC_GLOBALTIME_ENABLED 0 +#endif #ifndef AP_PERIPH_RCIN_ENABLED #define AP_PERIPH_RCIN_ENABLED 0 #endif diff --git a/libraries/AP_Scripting/applets/BatteryTag.lua b/libraries/AP_Scripting/applets/BatteryTag.lua new file mode 100644 index 0000000000000..93ecb47c81162 --- /dev/null +++ b/libraries/AP_Scripting/applets/BatteryTag.lua @@ -0,0 +1,129 @@ +--[[ + update and log battery tag information from BatteryTag periph nodes +--]] + +local GLOBALTIME_ID = 344 +local GLOBALTIME_SIGNATURE = uint64_t(0xA5517744, 0x8A490F33) + +local BATTERYTAG_ID = 20500 +local BATTERYTAG_SIGNATURE = uint64_t(0x4A5A9B42, 0x099F73E1) + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +PARAM_TABLE_KEY = 49 +PARAM_TABLE_PREFIX = "BTAG_" + +-- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end + +-- Setup Parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 6), 'BatteryTag: could not add param table') + +--[[ + // @Param: BTAG_ENABLE + // @DisplayName: enable battery info support + // @Description: enable battery info support + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local BTAG_ENABLE = bind_add_param('ENABLE', 1, 1) + +--[[ + // @Param: BTAG_MAX_CYCLES + // @DisplayName: max battery cycles + // @Description: max battery cycles for arming + // @Range: 0 10000 + // @User: Standard +--]] +local BTAG_MAX_CYCLES = bind_add_param('MAX_CYCLES', 2, 400) + +if BTAG_ENABLE:get() == 0 then + return +end + +-- a handle for receiving BatteryTag messages +local batterytag_handle = DroneCAN_Handle(0, BATTERYTAG_SIGNATURE, BATTERYTAG_ID) +batterytag_handle:subscribe() + +-- a handle for sending GlobalTime messages +local globaltime_handle = DroneCAN_Handle(0, GLOBALTIME_SIGNATURE, GLOBALTIME_ID) + +-- ID for an arming check +local auth_id = arming:get_aux_auth_id() + +local highest_cycles = 0 + +--[[ + check for BatteryTag messages +--]] +local function check_batterytag() + local payload, nodeid = batterytag_handle:check_message() + if not payload then + return + end + local serial_num, num_cycles, arm_hours, capacity, first_use, last_arm = string.unpack("IIfIII", payload) + if not serial_num then + return + end + + highest_cycles = math.max(num_cycles, highest_cycles) + + -- log battery information + logger:write("BTAG", + 'Node,Ser,NCycle,ArmHr,Cap,FirstUse,LastArm', + 'BIIfIII', + '#------', + '-------', + nodeid, + serial_num, num_cycles, arm_hours, + capacity, first_use, last_arm) + + if auth_id then + if highest_cycles > BTAG_MAX_CYCLES:get() then + arming:set_aux_auth_failed(auth_id, string.format("Battery cycles too high: %d", highest_cycles)) + else + arming:set_aux_auth_passed(auth_id) + end + end +end + +local last_globaltime_send = millis() + +--[[ + see if we should send GlobalTime message +--]] +local function check_globaltime() + local now = millis() + if now - last_globaltime_send < 1000 then + return + end + if gps:num_sensors() < 1 or gps:status(0) < 3 then + return + end + last_globaltime_send = now + + -- create 56 bit UTC microsecond timestamp + local utc_usec = gps:time_epoch_usec(0) + if utc_usec == 0 then + return + end + local usec_hi,usec_lo = utc_usec:split() + local payload8 = string.pack("II", usec_lo:toint(), usec_hi:toint()) + local payload7 = string.sub(payload8, 1, 7) + globaltime_handle:broadcast(payload7) +end + +local function update() + if BTAG_ENABLE:get() ~= 0 then + check_batterytag() + check_globaltime() + end + return update, 200 +end + +gcs:send_text(MAV_SEVERITY.INFO, "BatteryTag loaded") + +return update, 1000 diff --git a/libraries/AP_Scripting/applets/BatteryTag.md b/libraries/AP_Scripting/applets/BatteryTag.md new file mode 100644 index 0000000000000..af53afea1d9fc --- /dev/null +++ b/libraries/AP_Scripting/applets/BatteryTag.md @@ -0,0 +1,19 @@ +# Battery Tag Support + +This script implements logging of DroneCAN BatteryTag messages. It is +used in combination with a BatteryTag AP_Periph node to log +information about number of cycles a battery has been through along +with the serial number and number of hours in an armed state. + +The data for each battery is logged in the BTAG log message + +# Parameters + +## BTAG_ENABLE + +Allow for enable/disable of the script + +## BTAG_MAX_CYCLES + +Maximum number of battery cycles to allow arming + diff --git a/modules/DroneCAN/DSDL b/modules/DroneCAN/DSDL index 0856687a5acb4..1b2118cf35802 160000 --- a/modules/DroneCAN/DSDL +++ b/modules/DroneCAN/DSDL @@ -1 +1 @@ -Subproject commit 0856687a5acb42b55d29bbad8f4fc9e19b4989fc +Subproject commit 1b2118cf358027453830ef644838a3bedb9411ea