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

sensors: split out analog battery handling to new standalone battery_status module

Browse Source
This commit is contained in:
Daniel Agar
2019-10-21 13:40:23 -04:00
committed by GitHub
parent f956bafa4e
commit 6a0f5249f8
53 changed files with 801 additions and 294 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
ROMFS/px4fmu_common/init.d/rcS
View File

@@ -340,6 +340,7 @@ else
fi
unset BOARD_RC_SENSORS
battery_status start
sh /etc/init.d/rc.sensors
commander start

1
boards/aerotenna/ocpoc/ubuntu.cmake
View File

@@ -47,6 +47,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
#simulator

1
boards/airmind/mindpx-v2/default.cmake
View File

@@ -65,6 +65,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/auav/x21/default.cmake
View File

@@ -70,6 +70,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/av/x-v1/default.cmake
View File

@@ -71,6 +71,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/beaglebone/blue/cross.cmake
View File

@@ -46,6 +46,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/beaglebone/blue/native.cmake
View File

@@ -44,6 +44,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

6
boards/bitcraze/crazyflie/src/board_config.h
View File

@@ -153,12 +153,6 @@
*/
#define ADC_CHANNELS 0
// ADC defines to be used in sensors.cpp to read from a particular channel
// Crazyflie 2 performs battery sensing via the NRF module
#define ADC_BATTERY_VOLTAGE_CHANNEL ((uint8_t)(-1))
#define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1))
#define ADC_AIRSPEED_VOLTAGE_CHANNEL ((uint8_t)(-1))
/* Tone alarm output : These are only applicable when the buzzer deck is attached */
#define TONE_ALARM_TIMER 5 /* timer 5 */
#define TONE_ALARM_CHANNEL 3 /* channel 3 */

2
boards/bitcraze/crazyflie/syslink/CMakeLists.txt
View File

@@ -42,4 +42,6 @@ px4_add_module(
syslink_memory.cpp
syslink_params.c
syslink.c
DEPENDS
battery
)

1
boards/emlid/navio2/cross.cmake
View File

@@ -50,6 +50,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
#simulator

1
boards/emlid/navio2/native.cmake
View File

@@ -48,6 +48,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
#simulator

1
boards/holybro/kakutef7/default.cmake
View File

@@ -44,6 +44,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
SYSTEMCMDS

1
boards/intel/aerofc-v1/default.cmake
View File

@@ -50,6 +50,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
#sih
vmount

1
boards/intel/aerofc-v1/rtps.cmake
View File

@@ -54,6 +54,7 @@ px4_add_board(
mc_pos_control
micrortps_bridge
navigator
battery_status
sensors
sih
vmount

1
boards/mro/ctrl-zero-f7/default.cmake
View File

@@ -73,6 +73,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/nxp/fmuk66-v3/default.cmake
View File

@@ -69,6 +69,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/omnibus/f4sd/default.cmake
View File

@@ -61,6 +61,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
#vmount

1
boards/px4/fmu-v2/default.cmake
View File

@@ -80,6 +80,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
vmount
vtol_att_control

1
boards/px4/fmu-v2/fixedwing.cmake
View File

@@ -54,6 +54,7 @@ px4_add_board(
logger
mavlink
navigator
battery_status
sensors
vmount
airspeed_selector

1
boards/px4/fmu-v2/lpe.cmake
View File

@@ -76,6 +76,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
vmount
#vtol_att_control

1
boards/px4/fmu-v2/multicopter.cmake
View File

@@ -54,6 +54,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
vmount

1
boards/px4/fmu-v2/rover.cmake
View File

@@ -46,6 +46,7 @@ px4_add_board(
logger
mavlink
navigator
battery_status
sensors
vmount

1
boards/px4/fmu-v2/test.cmake
View File

@@ -76,6 +76,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
vmount
vtol_att_control

1
boards/px4/fmu-v3/default.cmake
View File

@@ -79,6 +79,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/px4/fmu-v3/rtps.cmake
View File

@@ -78,6 +78,7 @@ px4_add_board(
mc_pos_control
micrortps_bridge
navigator
battery_status
sensors
sih
vmount

1
boards/px4/fmu-v3/stackcheck.cmake
View File

@@ -77,6 +77,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

5
boards/px4/fmu-v4/default.cmake
View File

@@ -47,7 +47,9 @@ px4_add_board(
uavcan
MODULES
airspeed_selector
attitude_estimator_q
battery_status
camera_feedback
commander
dataman
@@ -55,7 +57,6 @@ px4_add_board(
events
fw_att_control
fw_pos_control_l1
rover_pos_control
land_detector
landing_target_estimator
load_mon
@@ -65,11 +66,11 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
rover_pos_control
sensors
sih
vmount
vtol_att_control
airspeed_selector
SYSTEMCMDS
bl_update

1
boards/px4/fmu-v4/rtps.cmake
View File

@@ -67,6 +67,7 @@ px4_add_board(
mc_pos_control
micrortps_bridge
navigator
battery_status
sensors
sih
vmount

1
boards/px4/fmu-v4/stackcheck.cmake
View File

@@ -64,6 +64,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/px4/fmu-v4pro/default.cmake
View File

@@ -76,6 +76,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/px4/fmu-v4pro/rtps.cmake
View File

@@ -76,6 +76,7 @@ px4_add_board(
mc_pos_control
micrortps_bridge
navigator
battery_status
sensors
sih
vmount

1
boards/px4/fmu-v5/critmonitor.cmake
View File

@@ -77,6 +77,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/px4/fmu-v5/default.cmake
View File

@@ -78,6 +78,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/px4/fmu-v5/fixedwing.cmake
View File

@@ -55,6 +55,7 @@ px4_add_board(
logger
mavlink
navigator
battery_status
sensors
vmount
airspeed_selector

1
boards/px4/fmu-v5/irqmonitor.cmake
View File

@@ -77,6 +77,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/px4/fmu-v5/multicopter.cmake
View File

@@ -62,6 +62,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/px4/fmu-v5/rover.cmake
View File

@@ -57,6 +57,7 @@ px4_add_board(
logger
mavlink
navigator
battery_status
sensors
vmount

1
boards/px4/fmu-v5/rtps.cmake
View File

@@ -76,6 +76,7 @@ px4_add_board(
mc_pos_control
micrortps_bridge
navigator
battery_status
sensors
sih
vmount

1
boards/px4/fmu-v5/stackcheck.cmake
View File

@@ -76,6 +76,7 @@ px4_add_board(
mc_pos_control
#micrortps_bridge
navigator
battery_status
sensors
sih
vmount

1
boards/px4/raspberrypi/cross.cmake
View File

@@ -43,6 +43,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
boards/px4/raspberrypi/native.cmake
View File

@@ -41,6 +41,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
#simulator

1
boards/uvify/core/default.cmake
View File

@@ -70,6 +70,7 @@ px4_add_board(
mc_att_control
mc_pos_control
navigator
battery_status
sensors
sih
vmount

1
src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/CMakeLists.txt
View File

@@ -42,4 +42,5 @@ px4_add_module(
git_driverframework
df_driver_framework
df_bebop_bus
battery
)

1
src/drivers/power_monitor/voxlpm/CMakeLists.txt
View File

@@ -39,5 +39,6 @@ px4_add_module(
voxlpm.cpp
voxlpm_main.cpp
DEPENDS
battery
px4_work_queue
)

46
src/modules/battery_status/CMakeLists.txt Normal file
View File

@@ -0,0 +1,46 @@
############################################################################
#
# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE modules__battery_status
MAIN battery_status
SRCS
battery_status.cpp
parameters.cpp
DEPENDS
battery
conversion
drivers__device
mathlib
)

451
src/modules/battery_status/battery_status.cpp Normal file
View File

@@ -0,0 +1,451 @@
/****************************************************************************
*
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sensors.cpp
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomas@px4.io>
* @author Anton Babushkin <anton@px4.io>
* @author Beat Küng <beat-kueng@gmx.net>
*/
#include <px4_config.h>
#include <px4_module.h>
#include <px4_module_params.h>
#include <px4_getopt.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <px4_time.h>
#include <px4_log.h>
#include <lib/mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_adc.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <lib/battery/battery.h>
#include <lib/conversion/rotation.h>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <DevMgr.hpp>
#include "parameters.h"
using namespace DriverFramework;
using namespace battery_status;
using namespace time_literals;
/**
* Analog layout:
* FMU:
* IN2 - battery voltage
* IN3 - battery current
* IN4 - 5V sense
* IN10 - spare (we could actually trim these from the set)
* IN11 - spare on FMUv2 & v3, RC RSSI on FMUv4
* IN12 - spare (we could actually trim these from the set)
* IN13 - aux1 on FMUv2, unavaible on v3 & v4
* IN14 - aux2 on FMUv2, unavaible on v3 & v4
* IN15 - pressure sensor on FMUv2, unavaible on v3 & v4
*
* IO:
* IN4 - servo supply rail
* IN5 - analog RSSI on FMUv2 & v3
*
* The channel definitions (e.g., ADC_BATTERY_VOLTAGE_CHANNEL, ADC_BATTERY_CURRENT_CHANNEL, and ADC_AIRSPEED_VOLTAGE_CHANNEL) are defined in board_config.h
*/
/**
* Battery status app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int battery_status_main(int argc, char *argv[]);
#ifndef BOARD_NUMBER_BRICKS
#error "battery_status module requires power bricks"
#endif
#if BOARD_NUMBER_BRICKS == 0
#error "battery_status module requires power bricks"
#endif
class BatteryStatus : public ModuleBase<BatteryStatus>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
BatteryStatus();
~BatteryStatus() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
void Run() override;
bool init();
/** @see ModuleBase::print_status() */
int print_status() override;
private:
DevHandle _h_adc; /**< ADC driver handle */
bool _armed{false}; /**< arming status of the vehicle */
uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
orb_advert_t _battery_pub[BOARD_NUMBER_BRICKS] {}; /**< battery status */
Battery _battery[BOARD_NUMBER_BRICKS]; /**< Helper lib to publish battery_status topic. */
#if BOARD_NUMBER_BRICKS > 1
int _battery_pub_intance0ndx {0}; /**< track the index of instance 0 */
#endif /* BOARD_NUMBER_BRICKS > 1 */
perf_counter_t _loop_perf; /**< loop performance counter */
hrt_abstime _last_config_update{0};
Parameters _parameters{}; /**< local copies of interesting parameters */
ParameterHandles _parameter_handles{}; /**< handles for interesting parameters */
/**
* Update our local parameter cache.
*/
int parameters_update();
/**
* Do adc-related initialisation.
*/
int adc_init();
/**
* Check for changes in parameters.
*/
void parameter_update_poll(bool forced = false);
/**
* Poll the ADC and update readings to suit.
*
* @param raw Combined sensor data structure into which
* data should be returned.
*/
void adc_poll();
};
BatteryStatus::BatteryStatus() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME))
{
initialize_parameter_handles(_parameter_handles);
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
_battery[b].setParent(this);
}
}
BatteryStatus::~BatteryStatus()
{
ScheduleClear();
}
int
BatteryStatus::parameters_update()
{
/* read the parameter values into _parameters */
int ret = update_parameters(_parameter_handles, _parameters);
if (ret) {
return ret;
}
return ret;
}
int
BatteryStatus::adc_init()
{
DevMgr::getHandle(ADC0_DEVICE_PATH, _h_adc);
if (!_h_adc.isValid()) {
PX4_ERR("no ADC found: %s (%d)", ADC0_DEVICE_PATH, _h_adc.getError());
return PX4_ERROR;
}
return OK;
}
void
BatteryStatus::parameter_update_poll(bool forced)
{
// check for parameter updates
if (_parameter_update_sub.updated() || forced) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
parameters_update();
updateParams();
}
}
void
BatteryStatus::adc_poll()
{
/* make space for a maximum of twelve channels (to ensure reading all channels at once) */
px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS];
/* read all channels available */
int ret = _h_adc.read(&buf_adc, sizeof(buf_adc));
//todo:abosorb into new class Power
/* For legacy support we publish the battery_status for the Battery that is
* associated with the Brick that is the selected source for VDD_5V_IN
* Selection is done in HW ala a LTC4417 or similar, or may be hard coded
* Like in the FMUv4
*/
/* The ADC channels that are associated with each brick, in power controller
* priority order highest to lowest, as defined by the board config.
*/
int bat_voltage_v_chan[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST;
int bat_voltage_i_chan[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST;
if (_parameters.battery_adc_channel >= 0) { // overwrite default
bat_voltage_v_chan[0] = _parameters.battery_adc_channel;
}
/* The valid signals (HW dependent) are associated with each brick */
bool valid_chan[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST;
/* Per Brick readings with default unread channels at 0 */
float bat_current_a[BOARD_NUMBER_BRICKS] = {0.0f};
float bat_voltage_v[BOARD_NUMBER_BRICKS] = {0.0f};
/* Based on the valid_chan, used to indicate the selected the lowest index
* (highest priority) supply that is the source for the VDD_5V_IN
* When < 0 none selected
*/
int selected_source = -1;
if (ret >= (int)sizeof(buf_adc[0])) {
/* Read add channels we got */
for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) {
{
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
/* Once we have subscriptions, Do this once for the lowest (highest priority
* supply on power controller) that is valid.
*/
if (_battery_pub[b] != nullptr && selected_source < 0 && valid_chan[b]) {
/* Indicate the lowest brick (highest priority supply on power controller)
* that is valid as the one that is the selected source for the
* VDD_5V_IN
*/
selected_source = b;
# if BOARD_NUMBER_BRICKS > 1
/* Move the selected_source to instance 0 */
if (_battery_pub_intance0ndx != selected_source) {
orb_advert_t tmp_h = _battery_pub[_battery_pub_intance0ndx];
_battery_pub[_battery_pub_intance0ndx] = _battery_pub[selected_source];
_battery_pub[selected_source] = tmp_h;
_battery_pub_intance0ndx = selected_source;
}
# endif /* BOARD_NUMBER_BRICKS > 1 */
}
// todo:per brick scaling
/* look for specific channels and process the raw voltage to measurement data */
if (bat_voltage_v_chan[b] == buf_adc[i].am_channel) {
/* Voltage in volts */
bat_voltage_v[b] = (buf_adc[i].am_data * _parameters.battery_voltage_scaling) * _parameters.battery_v_div;
} else if (bat_voltage_i_chan[b] == buf_adc[i].am_channel) {
bat_current_a[b] = ((buf_adc[i].am_data * _parameters.battery_current_scaling)
- _parameters.battery_current_offset) * _parameters.battery_a_per_v;
}
}
}
}
if (_parameters.battery_source == 0) {
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
/* Consider the brick connected if there is a voltage */
bool connected = bat_voltage_v[b] > BOARD_ADC_OPEN_CIRCUIT_V;
/* In the case where the BOARD_ADC_OPEN_CIRCUIT_V is
* greater than the BOARD_VALID_UV let the HW qualify that it
* is connected.
*/
if (BOARD_ADC_OPEN_CIRCUIT_V > BOARD_VALID_UV) {
connected &= valid_chan[b];
}
actuator_controls_s ctrl{};
_actuator_ctrl_0_sub.copy(&ctrl);
battery_status_s battery_status{};
_battery[b].updateBatteryStatus(hrt_absolute_time(), bat_voltage_v[b], bat_current_a[b],
connected, selected_source == b, b,
ctrl.control[actuator_controls_s::INDEX_THROTTLE],
_armed, &battery_status);
int instance;
orb_publish_auto(ORB_ID(battery_status), &_battery_pub[b], &battery_status, &instance, ORB_PRIO_DEFAULT);
}
}
}
}
void
BatteryStatus::Run()
{
if (should_exit()) {
exit_and_cleanup();
return;
}
if (!_h_adc.isValid()) {
adc_init();
}
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
if (_vcontrol_mode_sub.updated()) {
vehicle_control_mode_s vcontrol_mode{};
_vcontrol_mode_sub.copy(&vcontrol_mode);
_armed = vcontrol_mode.flag_armed;
}
/* check parameters for updates */
parameter_update_poll();
/* check battery voltage */
adc_poll();
perf_end(_loop_perf);
}
int
BatteryStatus::task_spawn(int argc, char *argv[])
{
BatteryStatus *instance = new BatteryStatus();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
bool
BatteryStatus::init()
{
ScheduleOnInterval(10_ms); // 100 Hz
return true;
}
int BatteryStatus::print_status()
{
return 0;
}
int BatteryStatus::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int BatteryStatus::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
The provided functionality includes:
- Read the output from the ADC driver (via ioctl interface) and publish `battery_status`.
### Implementation
It runs in its own thread and polls on the currently selected gyro topic.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("battery_status", "system");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
int battery_status_main(int argc, char *argv[])
{
return BatteryStatus::main(argc, argv);
}

117
src/modules/battery_status/parameters.cpp Normal file
View File

@@ -0,0 +1,117 @@
/****************************************************************************
*
* Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file parameters.cpp
*
* @author Beat Kueng <beat-kueng@gmx.net>
*/
#include "parameters.h"
#include <px4_log.h>
namespace battery_status
{
void initialize_parameter_handles(ParameterHandles &parameter_handles)
{
parameter_handles.battery_voltage_scaling = param_find("BAT_CNT_V_VOLT");
parameter_handles.battery_current_scaling = param_find("BAT_CNT_V_CURR");
parameter_handles.battery_current_offset = param_find("BAT_V_OFFS_CURR");
parameter_handles.battery_v_div = param_find("BAT_V_DIV");
parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V");
parameter_handles.battery_source = param_find("BAT_SOURCE");
parameter_handles.battery_adc_channel = param_find("BAT_ADC_CHANNEL");
}
int update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters)
{
int ret = PX4_OK;
const char *paramerr = "FAIL PARM LOAD";
/* scaling of ADC ticks to battery voltage */
if (param_get(parameter_handles.battery_voltage_scaling, &(parameters.battery_voltage_scaling)) != OK) {
PX4_WARN("%s", paramerr);
} else if (parameters.battery_voltage_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_voltage_scaling = (3.3f / 4096);
param_set_no_notification(parameter_handles.battery_voltage_scaling, &parameters.battery_voltage_scaling);
}
/* scaling of ADC ticks to battery current */
if (param_get(parameter_handles.battery_current_scaling, &(parameters.battery_current_scaling)) != OK) {
PX4_WARN("%s", paramerr);
} else if (parameters.battery_current_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_current_scaling = (3.3f / 4096);
param_set_no_notification(parameter_handles.battery_current_scaling, &parameters.battery_current_scaling);
}
if (param_get(parameter_handles.battery_current_offset, &(parameters.battery_current_offset)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.battery_v_div, &(parameters.battery_v_div)) != OK) {
PX4_WARN("%s", paramerr);
parameters.battery_v_div = 0.0f;
} else if (parameters.battery_v_div <= 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_v_div = BOARD_BATTERY1_V_DIV;
param_set_no_notification(parameter_handles.battery_v_div, &parameters.battery_v_div);
}
if (param_get(parameter_handles.battery_a_per_v, &(parameters.battery_a_per_v)) != OK) {
PX4_WARN("%s", paramerr);
parameters.battery_a_per_v = 0.0f;
} else if (parameters.battery_a_per_v <= 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_a_per_v = BOARD_BATTERY1_A_PER_V;
param_set_no_notification(parameter_handles.battery_a_per_v, &parameters.battery_a_per_v);
}
param_get(parameter_handles.battery_source, &(parameters.battery_source));
param_get(parameter_handles.battery_adc_channel, &(parameters.battery_adc_channel));
return ret;
}
} /* namespace battery_status */

84
src/modules/battery_status/parameters.h Normal file
View File

@@ -0,0 +1,84 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
/**
* @file parameters.h
*
* defines the list of parameters that are used within the sensors module
*
* @author Beat Kueng <beat-kueng@gmx.net>
*/
#include <px4_config.h>
#include <drivers/drv_rc_input.h>
#include <parameters/param.h>
#include <mathlib/mathlib.h>
namespace battery_status
{
struct Parameters {
float battery_voltage_scaling;
float battery_current_scaling;
float battery_current_offset;
float battery_v_div;
float battery_a_per_v;
int32_t battery_source;
int32_t battery_adc_channel;
};
struct ParameterHandles {
param_t battery_voltage_scaling;
param_t battery_current_scaling;
param_t battery_current_offset;
param_t battery_v_div;
param_t battery_a_per_v;
param_t battery_source;
param_t battery_adc_channel;
};
/**
* initialize ParameterHandles struct
*/
void initialize_parameter_handles(ParameterHandles &parameter_handles);
/**
* Read out the parameters using the handles into the parameters struct.
* @return 0 on success, <0 on error
*/
int update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters);
} /* namespace sensors */

0
src/modules/sensors/sensor_params_battery.c → src/modules/battery_status/sensor_params_battery.c
View File

1
src/modules/sensors/CMakeLists.txt
View File

@@ -47,7 +47,6 @@ px4_add_module(
DEPENDS
airspeed
battery
conversion
drivers__device
git_ecl

59
src/modules/sensors/parameters.cpp
View File

@@ -140,14 +140,6 @@ void initialize_parameter_handles(ParameterHandles &parameter_handles)
parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC");
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
parameter_handles.battery_voltage_scaling = param_find("BAT_CNT_V_VOLT");
parameter_handles.battery_current_scaling = param_find("BAT_CNT_V_CURR");
parameter_handles.battery_current_offset = param_find("BAT_V_OFFS_CURR");
parameter_handles.battery_v_div = param_find("BAT_V_DIV");
parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V");
parameter_handles.battery_source = param_find("BAT_SOURCE");
parameter_handles.battery_adc_channel = param_find("BAT_ADC_CHANNEL");
/* rotations */
parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
@@ -194,7 +186,6 @@ void initialize_parameter_handles(ParameterHandles &parameter_handles)
int update_parameters(const ParameterHandles &parameter_handles, Parameters &parameters)
{
bool rc_valid = true;
float tmpScaleFactor = 0.0f;
float tmpRevFactor = 0.0f;
@@ -379,56 +370,6 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par
param_get(parameter_handles.diff_pres_analog_scale, &(parameters.diff_pres_analog_scale));
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
/* scaling of ADC ticks to battery voltage */
if (param_get(parameter_handles.battery_voltage_scaling, &(parameters.battery_voltage_scaling)) != OK) {
PX4_WARN("%s", paramerr);
} else if (parameters.battery_voltage_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_voltage_scaling = (3.3f / 4096);
param_set_no_notification(parameter_handles.battery_voltage_scaling, &parameters.battery_voltage_scaling);
}
/* scaling of ADC ticks to battery current */
if (param_get(parameter_handles.battery_current_scaling, &(parameters.battery_current_scaling)) != OK) {
PX4_WARN("%s", paramerr);
} else if (parameters.battery_current_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_current_scaling = (3.3f / 4096);
param_set_no_notification(parameter_handles.battery_current_scaling, &parameters.battery_current_scaling);
}
if (param_get(parameter_handles.battery_current_offset, &(parameters.battery_current_offset)) != OK) {
PX4_WARN("%s", paramerr);
}
if (param_get(parameter_handles.battery_v_div, &(parameters.battery_v_div)) != OK) {
PX4_WARN("%s", paramerr);
parameters.battery_v_div = 0.0f;
} else if (parameters.battery_v_div <= 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_v_div = BOARD_BATTERY1_V_DIV;
param_set_no_notification(parameter_handles.battery_v_div, &parameters.battery_v_div);
}
if (param_get(parameter_handles.battery_a_per_v, &(parameters.battery_a_per_v)) != OK) {
PX4_WARN("%s", paramerr);
parameters.battery_a_per_v = 0.0f;
} else if (parameters.battery_a_per_v <= 0.0f) {
/* apply scaling according to defaults if set to default */
parameters.battery_a_per_v = BOARD_BATTERY1_A_PER_V;
param_set_no_notification(parameter_handles.battery_a_per_v, &parameters.battery_a_per_v);
}
param_get(parameter_handles.battery_source, &(parameters.battery_source));
param_get(parameter_handles.battery_adc_channel, &(parameters.battery_adc_channel));
param_get(parameter_handles.board_rotation, &(parameters.board_rotation));
param_get(parameter_handles.board_offset[0], &(parameters.board_offset[0]));

16
src/modules/sensors/parameters.h
View File

@@ -138,14 +138,6 @@ struct Parameters {
float rc_flt_smp_rate;
float rc_flt_cutoff;
float battery_voltage_scaling;
float battery_current_scaling;
float battery_current_offset;
float battery_v_div;
float battery_a_per_v;
int32_t battery_source;
int32_t battery_adc_channel;
float baro_qnh;
int32_t air_cmodel;
@@ -220,14 +212,6 @@ struct ParameterHandles {
param_t rc_flt_smp_rate;
param_t rc_flt_cutoff;
param_t battery_voltage_scaling;
param_t battery_current_scaling;
param_t battery_current_offset;
param_t battery_v_div;
param_t battery_a_per_v;
param_t battery_source;
param_t battery_adc_channel;
param_t board_rotation;
param_t board_offset[3];

207
src/modules/sensors/sensors.cpp
View File

@@ -71,7 +71,6 @@
#include <parameters/param.h>
#include <systemlib/err.h>
#include <perf/perf_counter.h>
#include <battery/battery.h>
#include <conversion/rotation.h>
@@ -79,7 +78,6 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/sensor_preflight.h>
@@ -99,27 +97,6 @@ using namespace DriverFramework;
using namespace sensors;
using namespace time_literals;
/**
* Analog layout:
* FMU:
* IN2 - battery voltage
* IN3 - battery current
* IN4 - 5V sense
* IN10 - spare (we could actually trim these from the set)
* IN11 - spare on FMUv2 & v3, RC RSSI on FMUv4
* IN12 - spare (we could actually trim these from the set)
* IN13 - aux1 on FMUv2, unavaible on v3 & v4
* IN14 - aux2 on FMUv2, unavaible on v3 & v4
* IN15 - pressure sensor on FMUv2, unavaible on v3 & v4
*
* IO:
* IN4 - servo supply rail
* IN5 - analog RSSI on FMUv2 & v3
*
* The channel definitions (e.g., ADC_BATTERY_VOLTAGE_CHANNEL, ADC_BATTERY_CURRENT_CHANNEL, and ADC_AIRSPEED_VOLTAGE_CHANNEL) are defined in board_config.h
*/
/**
* HACK - true temperature is much less than indicated temperature in baro,
* subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
@@ -159,10 +136,6 @@ public:
int print_status() override;
private:
DevHandle _h_adc; /**< ADC driver handle */
hrt_abstime _last_adc{0}; /**< last time we took input from the ADC */
const bool _hil_enabled; /**< if true, HIL is active */
bool _armed{false}; /**< arming status of the vehicle */
@@ -177,21 +150,15 @@ private:
uORB::Publication<vehicle_air_data_s> _airdata_pub{ORB_ID(vehicle_air_data)}; /**< combined sensor data topic */
uORB::Publication<vehicle_magnetometer_s> _magnetometer_pub{ORB_ID(vehicle_magnetometer)}; /**< combined sensor data topic */
#if BOARD_NUMBER_BRICKS > 0
orb_advert_t _battery_pub[BOARD_NUMBER_BRICKS] {}; /**< battery status */
Battery _battery[BOARD_NUMBER_BRICKS]; /**< Helper lib to publish battery_status topic. */
#endif /* BOARD_NUMBER_BRICKS > 0 */
#if BOARD_NUMBER_BRICKS > 1
int _battery_pub_intance0ndx {0}; /**< track the index of instance 0 */
#endif /* BOARD_NUMBER_BRICKS > 1 */
perf_counter_t _loop_perf; /**< loop performance counter */
DataValidator _airspeed_validator; /**< data validator to monitor airspeed */
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
DevHandle _h_adc; /**< ADC driver handle */
hrt_abstime _last_adc{0}; /**< last time we took input from the ADC */
differential_pressure_s _diff_pres {};
uORB::PublicationMulti<differential_pressure_s> _diff_pres_pub{ORB_ID(differential_pressure)}; /**< differential_pressure */
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
@@ -212,11 +179,6 @@ private:
*/
int parameters_update();
/**
* Do adc-related initialisation.
*/
int adc_init();
/**
* Poll the differential pressure sensor for updated data.
*
@@ -230,6 +192,11 @@ private:
*/
void parameter_update_poll(bool forced = false);
/**
* Do adc-related initialisation.
*/
int adc_init();
/**
* Poll the ADC and update readings to suit.
*
@@ -237,6 +204,7 @@ private:
* data should be returned.
*/
void adc_poll();
};
Sensors::Sensors(bool hil_enabled) :
@@ -251,14 +219,6 @@ Sensors::Sensors(bool hil_enabled) :
_airspeed_validator.set_timeout(300000);
_airspeed_validator.set_equal_value_threshold(100);
#if BOARD_NUMBER_BRICKS > 0
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
_battery[b].setParent(this);
}
#endif /* BOARD_NUMBER_BRICKS > 0 */
_vehicle_acceleration.Start();
_vehicle_angular_velocity.Start();
}
@@ -289,10 +249,14 @@ Sensors::parameters_update()
return ret;
}
int
Sensors::adc_init()
{
if (!_hil_enabled) {
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
DevMgr::getHandle(ADC0_DEVICE_PATH, _h_adc);
if (!_h_adc.isValid()) {
@@ -300,6 +264,10 @@ Sensors::adc_init()
return PX4_ERROR;
}
#endif // ADC_AIRSPEED_VOLTAGE_CHANNEL
}
return OK;
}
@@ -396,11 +364,15 @@ Sensors::parameter_update_poll(bool forced)
void
Sensors::adc_poll()
{
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
/* only read if not in HIL mode */
if (_hil_enabled) {
return;
}
if (_parameters.diff_pres_analog_scale > 0.0f) {
hrt_abstime t = hrt_absolute_time();
/* rate limit to 100 Hz */
@@ -410,49 +382,10 @@ Sensors::adc_poll()
/* read all channels available */
int ret = _h_adc.read(&buf_adc, sizeof(buf_adc));
#if BOARD_NUMBER_BRICKS > 0
//todo:abosorb into new class Power
/* For legacy support we publish the battery_status for the Battery that is
* associated with the Brick that is the selected source for VDD_5V_IN
* Selection is done in HW ala a LTC4417 or similar, or may be hard coded
* Like in the FMUv4
*/
#if !defined(BOARD_NUMBER_DIGITAL_BRICKS)
/* The ADC channels that are associated with each brick, in power controller
* priority order highest to lowest, as defined by the board config.
*/
int bat_voltage_v_chan[BOARD_NUMBER_BRICKS] = BOARD_BATT_V_LIST;
int bat_voltage_i_chan[BOARD_NUMBER_BRICKS] = BOARD_BATT_I_LIST;
if (_parameters.battery_adc_channel >= 0) { // overwrite default
bat_voltage_v_chan[0] = _parameters.battery_adc_channel;
}
#endif
/* The valid signals (HW dependent) are associated with each brick */
bool valid_chan[BOARD_NUMBER_BRICKS] = BOARD_BRICK_VALID_LIST;
/* Per Brick readings with default unread channels at 0 */
float bat_current_a[BOARD_NUMBER_BRICKS] = {0.0f};
float bat_voltage_v[BOARD_NUMBER_BRICKS] = {0.0f};
/* Based on the valid_chan, used to indicate the selected the lowest index
* (highest priority) supply that is the source for the VDD_5V_IN
* When < 0 none selected
*/
int selected_source = -1;
#endif /* BOARD_NUMBER_BRICKS > 0 */
if (ret >= (int)sizeof(buf_adc[0])) {
/* Read add channels we got */
for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) {
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
/* calculate airspeed, raw is the difference from */
@@ -463,8 +396,7 @@ Sensors::adc_poll()
* a valid voltage from a connected sensor. Also assume a non-
* zero offset from the sensor if its connected.
*/
if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) {
if (voltage > 0.4f) {
const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
_diff_pres.timestamp = t;
@@ -475,105 +407,21 @@ Sensors::adc_poll()
_diff_pres_pub.publish(_diff_pres);
}
} else
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
{
#if BOARD_NUMBER_BRICKS > 0
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
/* Once we have subscriptions, Do this once for the lowest (highest priority
* supply on power controller) that is valid.
*/
if (_battery_pub[b] != nullptr && selected_source < 0 && valid_chan[b]) {
/* Indicate the lowest brick (highest priority supply on power controller)
* that is valid as the one that is the selected source for the
* VDD_5V_IN
*/
selected_source = b;
# if BOARD_NUMBER_BRICKS > 1
/* Move the selected_source to instance 0 */
if (_battery_pub_intance0ndx != selected_source) {
orb_advert_t tmp_h = _battery_pub[_battery_pub_intance0ndx];
_battery_pub[_battery_pub_intance0ndx] = _battery_pub[selected_source];
_battery_pub[selected_source] = tmp_h;
_battery_pub_intance0ndx = selected_source;
}
# endif /* BOARD_NUMBER_BRICKS > 1 */
}
# if !defined(BOARD_NUMBER_DIGITAL_BRICKS)
// todo:per brick scaling
/* look for specific channels and process the raw voltage to measurement data */
if (bat_voltage_v_chan[b] == buf_adc[i].am_channel) {
/* Voltage in volts */
bat_voltage_v[b] = (buf_adc[i].am_data * _parameters.battery_voltage_scaling) * _parameters.battery_v_div;
} else if (bat_voltage_i_chan[b] == buf_adc[i].am_channel) {
bat_current_a[b] = ((buf_adc[i].am_data * _parameters.battery_current_scaling)
- _parameters.battery_current_offset) * _parameters.battery_a_per_v;
}
# endif /* !defined(BOARD_NUMBER_DIGITAL_BRICKS) */
}
#endif /* BOARD_NUMBER_BRICKS > 0 */
}
}
#if BOARD_NUMBER_BRICKS > 0
if (_parameters.battery_source == 0) {
for (int b = 0; b < BOARD_NUMBER_BRICKS; b++) {
/* Consider the brick connected if there is a voltage */
bool connected = bat_voltage_v[b] > BOARD_ADC_OPEN_CIRCUIT_V;
/* In the case where the BOARD_ADC_OPEN_CIRCUIT_V is
* greater than the BOARD_VALID_UV let the HW qualify that it
* is connected.
*/
if (BOARD_ADC_OPEN_CIRCUIT_V > BOARD_VALID_UV) {
connected &= valid_chan[b];
}
actuator_controls_s ctrl{};
_actuator_ctrl_0_sub.copy(&ctrl);
battery_status_s battery_status{};
_battery[b].updateBatteryStatus(t, bat_voltage_v[b], bat_current_a[b],
connected, selected_source == b, b,
ctrl.control[actuator_controls_s::INDEX_THROTTLE],
_armed, &battery_status);
int instance;
orb_publish_auto(ORB_ID(battery_status), &_battery_pub[b], &battery_status, &instance, ORB_PRIO_DEFAULT);
}
}
#endif /* BOARD_NUMBER_BRICKS > 0 */
_last_adc = t;
}
}
}
#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */
}
void
Sensors::run()
{
if (!_hil_enabled) {
#if !defined(__PX4_QURT) && BOARD_NUMBER_BRICKS > 0
adc_init();
#endif
}
sensor_combined_s raw = {};
sensor_preflight_s preflt = {};
@@ -639,7 +487,7 @@ Sensors::run()
_voted_sensors_update.sensorsPoll(raw, airdata, magnetometer);
/* check battery voltage */
/* check analog airspeed */
adc_poll();
diff_pres_poll(airdata);
@@ -754,7 +602,6 @@ The provided functionality includes:
- Do RC channel mapping: read the raw input channels (`input_rc`), then apply the calibration, map the RC channels
to the configured channels & mode switches, low-pass filter, and then publish as `rc_channels` and
`manual_control_setpoint`.
- Read the output from the ADC driver (via ioctl interface) and publish `battery_status`.
- Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or
on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the
sensor drivers must already be running when `sensors` is started.