mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 17:43:35 +08:00
sensors: split out analog battery handling to new standalone battery_status module
This commit is contained in:
Daniel Agar
@@ -47,7 +47,6 @@ px4_add_module(
|
||||
|
||||
DEPENDS
|
||||
airspeed
|
||||
battery
|
||||
conversion
|
||||
drivers__device
|
||||
git_ecl
|
||||
|
||||
@@ -140,14 +140,6 @@ void initialize_parameter_handles(ParameterHandles ¶meter_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 ¶meter_handles)
|
||||
|
||||
int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters)
|
||||
{
|
||||
|
||||
bool rc_valid = true;
|
||||
float tmpScaleFactor = 0.0f;
|
||||
float tmpRevFactor = 0.0f;
|
||||
@@ -379,56 +370,6 @@ int update_parameters(const ParameterHandles ¶meter_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, ¶meters.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, ¶meters.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, ¶meters.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, ¶meters.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]));
|
||||
|
||||
@@ -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];
|
||||
|
||||
@@ -1,117 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2017 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* Scaling from ADC counts to volt on the ADC input (battery voltage)
|
||||
*
|
||||
* This is not the battery voltage, but the intermediate ADC voltage.
|
||||
* A value of -1 signifies that the board defaults are used, which is
|
||||
* highly recommended.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @decimal 8
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_CNT_V_VOLT, -1.0f);
|
||||
|
||||
/**
|
||||
* Scaling from ADC counts to volt on the ADC input (battery current)
|
||||
*
|
||||
* This is not the battery current, but the intermediate ADC voltage.
|
||||
* A value of -1 signifies that the board defaults are used, which is
|
||||
* highly recommended.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @decimal 8
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_CNT_V_CURR, -1.0);
|
||||
|
||||
/**
|
||||
* Offset in volt as seen by the ADC input of the current sensor.
|
||||
*
|
||||
* This offset will be subtracted before calculating the battery
|
||||
* current based on the voltage.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @decimal 8
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_V_OFFS_CURR, 0.0);
|
||||
|
||||
/**
|
||||
* Battery voltage divider (V divider)
|
||||
*
|
||||
* This is the divider from battery voltage to 3.3V ADC voltage.
|
||||
* If using e.g. Mauch power modules the value from the datasheet
|
||||
* can be applied straight here. A value of -1 means to use
|
||||
* the board default.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @decimal 8
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_V_DIV, -1.0);
|
||||
|
||||
/**
|
||||
* Battery current per volt (A/V)
|
||||
*
|
||||
* The voltage seen by the 3.3V ADC multiplied by this factor
|
||||
* will determine the battery current. A value of -1 means to use
|
||||
* the board default.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
* @decimal 8
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(BAT_A_PER_V, -1.0);
|
||||
|
||||
/**
|
||||
* Battery monitoring source.
|
||||
*
|
||||
* This parameter controls the source of battery data. The value 'Power Module'
|
||||
* means that measurements are expected to come from a power module. If the value is set to
|
||||
* 'External' then the system expects to receive mavlink battery status messages.
|
||||
*
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @value 0 Power Module
|
||||
* @value 1 External
|
||||
* @group Battery Calibration
|
||||
*/
|
||||
PARAM_DEFINE_INT32(BAT_SOURCE, 0);
|
||||
|
||||
/**
|
||||
* Battery ADC Channel
|
||||
*
|
||||
* This parameter specifies the ADC channel used to monitor voltage of main power battery.
|
||||
* A value of -1 means to use the board default.
|
||||
*
|
||||
* @group Battery Calibration
|
||||
*/
|
||||
PARAM_DEFINE_INT32(BAT_ADC_CHANNEL, -1);
|
||||
+57
-210
@@ -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,15 +249,23 @@ Sensors::parameters_update()
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
int
|
||||
Sensors::adc_init()
|
||||
{
|
||||
DevMgr::getHandle(ADC0_DEVICE_PATH, _h_adc);
|
||||
if (!_hil_enabled) {
|
||||
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
|
||||
if (!_h_adc.isValid()) {
|
||||
PX4_ERR("no ADC found: %s (%d)", ADC0_DEVICE_PATH, _h_adc.getError());
|
||||
return PX4_ERROR;
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
#endif // ADC_AIRSPEED_VOLTAGE_CHANNEL
|
||||
}
|
||||
|
||||
return OK;
|
||||
@@ -396,184 +364,64 @@ 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;
|
||||
}
|
||||
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
if (_parameters.diff_pres_analog_scale > 0.0f) {
|
||||
|
||||
/* rate limit to 100 Hz */
|
||||
if (t - _last_adc >= 10000) {
|
||||
/* 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));
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
#if BOARD_NUMBER_BRICKS > 0
|
||||
//todo:abosorb into new class Power
|
||||
/* rate limit to 100 Hz */
|
||||
if (t - _last_adc >= 10000) {
|
||||
/* 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));
|
||||
|
||||
/* 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 (ret >= (int)sizeof(buf_adc[0])) {
|
||||
|
||||
if (_parameters.battery_adc_channel >= 0) { // overwrite default
|
||||
bat_voltage_v_chan[0] = _parameters.battery_adc_channel;
|
||||
}
|
||||
/* Read add channels we got */
|
||||
for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) {
|
||||
if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
|
||||
|
||||
#endif
|
||||
/* calculate airspeed, raw is the difference from */
|
||||
const float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor)
|
||||
|
||||
/* 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 */
|
||||
const float voltage = (float)(buf_adc[i].am_data) * 3.3f / 4096.0f * 2.0f; // V_ref/4096 * (voltage divider factor)
|
||||
|
||||
/**
|
||||
* The voltage divider pulls the signal down, only act on
|
||||
* 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)) {
|
||||
|
||||
const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
|
||||
|
||||
_diff_pres.timestamp = t;
|
||||
_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
|
||||
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) +
|
||||
(diff_pres_pa_raw * 0.1f);
|
||||
_diff_pres.temperature = -1000.0f;
|
||||
|
||||
_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.
|
||||
/**
|
||||
* The voltage divider pulls the signal down, only act on
|
||||
* a valid voltage from a connected sensor. Also assume a non-
|
||||
* zero offset from the sensor if its connected.
|
||||
*/
|
||||
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 (voltage > 0.4f) {
|
||||
const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
|
||||
|
||||
# if BOARD_NUMBER_BRICKS > 1
|
||||
_diff_pres.timestamp = t;
|
||||
_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
|
||||
_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) +
|
||||
(diff_pres_pa_raw * 0.1f);
|
||||
_diff_pres.temperature = -1000.0f;
|
||||
|
||||
/* 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 */
|
||||
_diff_pres_pub.publish(_diff_pres);
|
||||
}
|
||||
|
||||
# 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 */
|
||||
}
|
||||
|
||||
_last_adc = t;
|
||||
}
|
||||
|
||||
#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
|
||||
}
|
||||
adc_init();
|
||||
|
||||
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.
|
||||
|
||||
Reference in New Issue
Block a user