yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

publish battery_status directly

Browse Source 
This is temporary until there is a solution that works for adc and posix power modules
This commit is contained in:
ChristophTobler
2018-01-04 09:41:26 +01:00
committed by Lorenz Meier
parent 856a523d29
commit 7c019b7da3
1 changed files with 36 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
src/platforms/posix/drivers/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp
View File

@@ -42,9 +42,15 @@
#include <string>
#include <px4_config.h>
#include <systemlib/err.h>
#include <systemlib/battery.h>
#include <drivers/drv_hrt.h>
#include <ltc2946/LTC2946.hpp>
#include <uORB/uORB.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
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;
}