Daniel Agar
5 years ago
committed by
GitHub
53 changed files with 800 additions and 293 deletions
@ -0,0 +1,46 @@
@@ -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 |
||||
) |
||||
@ -0,0 +1,451 @@
@@ -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); |
||||
} |
||||
@ -0,0 +1,117 @@
@@ -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 ¶meter_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 ¶meter_handles, Parameters ¶meters) |
||||
{ |
||||
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, ¶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)); |
||||
|
||||
return ret; |
||||
} |
||||
|
||||
} /* namespace battery_status */ |
||||
@ -0,0 +1,84 @@
@@ -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 ¶meter_handles); |
||||
|
||||
|
||||
/**
|
||||
* Read out the parameters using the handles into the parameters struct. |
||||
* @return 0 on success, <0 on error |
||||
*/ |
||||
int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters); |
||||
|
||||
} /* namespace sensors */ |
||||
Loading…
Reference in new issue