mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-22 09:28:58 +00:00
sensors: split out analog battery handling to new standalone battery_status module
This commit is contained in:
Daniel Agar
GitHub
@@ -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