diff --git a/src/platforms/posix/drivers/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp b/src/platforms/posix/drivers/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp index 05fdaa000e..1bdd8f9678 100644 --- a/src/platforms/posix/drivers/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp +++ b/src/platforms/posix/drivers/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp @@ -42,9 +42,15 @@ #include #include #include +#include +#include #include +#include +#include +#include +#include extern "C" { __EXPORT int df_ltc2946_wrapper_main(int argc, char *argv[]); } @@ -64,15 +70,29 @@ public: private: int _publish(const struct ltc2946_sensor_data &data); + orb_advert_t _battery_pub{nullptr}; + battery_status_s _battery_status{}; + Battery _battery{}; + + int _actuator_ctrl_0_sub{-1}; + int _vcontrol_mode_sub{-1}; + }; DfLtc2946Wrapper::DfLtc2946Wrapper() : LTC2946(LTC2946_DEVICE_PATH) { + _battery.reset(&_battery_status); + + // subscriptions + _actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); } DfLtc2946Wrapper::~DfLtc2946Wrapper() { + orb_unsubscribe(_actuator_ctrl_0_sub); + orb_unsubscribe(_vcontrol_mode_sub); } int DfLtc2946Wrapper::start() @@ -113,7 +133,22 @@ int DfLtc2946Wrapper::stop() int DfLtc2946Wrapper::_publish(const struct ltc2946_sensor_data &data) { - // TODO + hrt_abstime t = hrt_absolute_time(); + bool connected = data.battery_voltage_V > BOARD_ADC_OPEN_CIRCUIT_V; + + actuator_controls_s ctrl; + orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl); + vehicle_control_mode_s vcontrol_mode; + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); + + _battery.updateBatteryStatus(t, data.battery_voltage_V, data.battery_current_A, + connected, true, 1, + ctrl.control[actuator_controls_s::INDEX_THROTTLE], + vcontrol_mode.flag_armed, &_battery_status); + + int instance; + orb_publish_auto(ORB_ID(battery_status), &_battery_pub, &_battery_status, &instance, ORB_PRIO_DEFAULT); + return 0; }