AP_Periph: add AP_BattMonitor support

Tools/AP_Periph/AP_Periph.cpp

@@ -121,6 +121,10 @@ void AP_Periph_FW::init()
     baro.init();
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_BATTERY
+    battery.lib.init();
+#endif
+
 #ifdef HAL_PERIPH_NEOPIXEL_COUNT
     hal.rcout->init();
     hal.rcout->set_serial_led_num_LEDs(HAL_PERIPH_NEOPIXEL_CHAN, AP_HAL::RCOutput::MODE_NEOPIXEL);
@@ -244,6 +248,15 @@ void AP_Periph_FW::update()
         hal.rcout->set_serial_led_num_LEDs(HAL_PERIPH_NEOPIXEL_CHAN, HAL_PERIPH_NEOPIXEL_COUNT, AP_HAL::RCOutput::MODE_NEOPIXEL);
 #endif
     }
+
+#ifdef HAL_PERIPH_ENABLE_BATTERY
+    if (now - battery.last_read_ms >= 100) {
+        // update battery at 10Hz
+        battery.last_read_ms = now;
+        battery.lib.read();
+    }
+#endif
+
     can_update();
     hal.scheduler->delay(1);
 #if defined(HAL_PERIPH_NEOPIXEL_COUNT) && HAL_PERIPH_NEOPIXEL_COUNT == 8

Tools/AP_Periph/AP_Periph.h

@@ -5,6 +5,7 @@
 #include <AP_GPS/AP_GPS.h>
 #include <AP_Compass/AP_Compass.h>
 #include <AP_Baro/AP_Baro.h>
+#include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_Airspeed/AP_Airspeed.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
 #include <AP_MSP/AP_MSP.h>
@@ -41,6 +42,7 @@ public:
     void can_baro_update();
     void can_airspeed_update();
     void can_rangefinder_update();
+    void can_battery_update();
 
     void load_parameters();
 
@@ -58,6 +60,17 @@ public:
     AP_Baro baro;
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_BATTERY
+    struct AP_Periph_Battery {
+        void handle_battery_failsafe(const char* type_str, const int8_t action) { }
+        AP_BattMonitor lib{0, FUNCTOR_BIND_MEMBER(&AP_Periph_FW::AP_Periph_Battery::handle_battery_failsafe, void, const char*, const int8_t), nullptr};
+
+        uint32_t last_read_ms;
+        uint32_t last_can_send_ms;
+    } battery;
+#endif
+
+
 #ifdef HAL_PERIPH_ENABLE_MSP
     struct {
         AP_MSP msp;

Tools/AP_Periph/Parameters.cpp

@@ -50,6 +50,12 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GOBJECT(gps, "GPS_", AP_GPS),
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_BATTERY
+    // @Group: BATT
+    // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
+    GOBJECT(battery, "BATT", AP_BattMonitor),
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_MAG
     // @Group: COMPASS_
     // @Path: ../../libraries/AP_Compass/AP_Compass.cpp

Tools/AP_Periph/Parameters.h

@@ -28,6 +28,7 @@ public:
         k_param_hardpoint_rate,
         k_param_baro_enable,
         k_param_esc_number,
+        k_param_battery,
     };
 
     AP_Int16 format_version;

Tools/AP_Periph/can.cpp

@@ -42,6 +42,7 @@
 #include <ardupilot/indication/Button.h>
 #include <ardupilot/equipment/trafficmonitor/TrafficReport.h>
 #include <uavcan/equipment/gnss/RTCMStream.h>
+#include <uavcan/equipment/power/BatteryInfo.h>
 #include <uavcan/protocol/debug/LogMessage.h>
 #include <stdio.h>
 #include <drivers/stm32/canard_stm32.h>
@@ -292,6 +293,9 @@ static void handle_param_executeopcode(CanardInstance* ins, CanardRxTransfer* tr
 #ifdef HAL_PERIPH_ENABLE_GPS
         AP_Param::setup_object_defaults(&periph.gps, periph.gps.var_info);
 #endif
+#ifdef HAL_PERIPH_ENABLE_BATTERY
+        AP_Param::setup_object_defaults(&periph.battery, periph.battery.lib.var_info);
+#endif
 #ifdef HAL_PERIPH_ENABLE_MAG
         AP_Param::setup_object_defaults(&periph.compass, periph.compass.var_info);
 #endif
@@ -1153,6 +1157,7 @@ void AP_Periph_FW::can_update()
     }
     can_mag_update();
     can_gps_update();
+    can_battery_update();
     can_baro_update();
     can_airspeed_update();
     can_rangefinder_update();
@@ -1227,6 +1232,59 @@ void AP_Periph_FW::can_mag_update(void)
 #endif // HAL_PERIPH_ENABLE_MAG
 }
 
+/*
+  update CAN battery monitor
+ */
+void AP_Periph_FW::can_battery_update(void)
+{
+#ifdef HAL_PERIPH_ENABLE_BATTERY
+    const uint32_t now_ms = AP_HAL::native_millis();
+    if (now_ms - battery.last_can_send_ms < 100) {
+        return;
+    }
+    battery.last_can_send_ms = now_ms;
+
+    const uint8_t battery_instances = battery.lib.num_instances();
+    for (uint8_t i=0; i<battery_instances; i++) {
+        if (!battery.lib.healthy(i)) {
+            continue;
+        }
+
+        uavcan_equipment_power_BatteryInfo pkt {};
+
+        // if a battery serial number is assigned, use that as the ID. Else, use the index.
+        const int32_t serial_number = battery.lib.get_serial_number(i);
+        pkt.battery_id = (serial_number >= 0) ? serial_number : i+1;
+
+        pkt.voltage = battery.lib.voltage(i);
+
+        float current;
+        if (battery.lib.current_amps(current, i)) {
+            pkt.current = current;
+        }
+        float temperature;
+        if (battery.lib.get_temperature(temperature, i)) {
+            pkt.temperature = temperature;
+        }
+
+        fix_float16(pkt.voltage);
+        fix_float16(pkt.current);
+        fix_float16(pkt.temperature);
+
+        uint8_t buffer[UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE] {};
+        const uint16_t total_size = uavcan_equipment_power_BatteryInfo_encode(&pkt, buffer);
+
+        canardBroadcast(&canard,
+                        UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE,
+                        UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID,
+                        &transfer_id,
+                        CANARD_TRANSFER_PRIORITY_LOW,
+                        &buffer[0],
+                        total_size);
+    }
+#endif
+}
+
 /*
   update CAN GPS
  */

Tools/AP_Periph/wscript

@@ -21,6 +21,7 @@ def build(bld):
         'AP_HAL_Empty',
         'AP_Math',
         'AP_BoardConfig',
+        'AP_BattMonitor',
         'AP_Param',
         'StorageManager',
         'AP_FlashStorage',