diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 4c61b338e2..8dc71cffd3 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -340,6 +340,7 @@ else fi unset BOARD_RC_SENSORS + battery_status start sh /etc/init.d/rc.sensors commander start diff --git a/boards/aerotenna/ocpoc/ubuntu.cmake b/boards/aerotenna/ocpoc/ubuntu.cmake index 655b39e95e..f5a9b2bf2f 100644 --- a/boards/aerotenna/ocpoc/ubuntu.cmake +++ b/boards/aerotenna/ocpoc/ubuntu.cmake @@ -47,6 +47,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih #simulator diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 4c2276ef89..2707f24cc1 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -65,6 +65,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/auav/x21/default.cmake b/boards/auav/x21/default.cmake index 075920bee0..23f44146e3 100644 --- a/boards/auav/x21/default.cmake +++ b/boards/auav/x21/default.cmake @@ -70,6 +70,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 8b0e606f51..2ee43312d0 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -71,6 +71,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/beaglebone/blue/cross.cmake b/boards/beaglebone/blue/cross.cmake index b37d97499c..ae19a81487 100644 --- a/boards/beaglebone/blue/cross.cmake +++ b/boards/beaglebone/blue/cross.cmake @@ -46,6 +46,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/beaglebone/blue/native.cmake b/boards/beaglebone/blue/native.cmake index a057859925..978de48660 100644 --- a/boards/beaglebone/blue/native.cmake +++ b/boards/beaglebone/blue/native.cmake @@ -44,6 +44,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/bitcraze/crazyflie/src/board_config.h b/boards/bitcraze/crazyflie/src/board_config.h index 1d864cdaf1..91ef17dfd6 100644 --- a/boards/bitcraze/crazyflie/src/board_config.h +++ b/boards/bitcraze/crazyflie/src/board_config.h @@ -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 */ diff --git a/boards/bitcraze/crazyflie/syslink/CMakeLists.txt b/boards/bitcraze/crazyflie/syslink/CMakeLists.txt index f740aab0fd..ded0b30288 100644 --- a/boards/bitcraze/crazyflie/syslink/CMakeLists.txt +++ b/boards/bitcraze/crazyflie/syslink/CMakeLists.txt @@ -42,4 +42,6 @@ px4_add_module( syslink_memory.cpp syslink_params.c syslink.c + DEPENDS + battery ) diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index d517442a43..a72074b59e 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -50,6 +50,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih #simulator diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index 162abfc13f..d001994211 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -48,6 +48,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih #simulator diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake index ef769e8bd7..98cd405c4e 100644 --- a/boards/holybro/kakutef7/default.cmake +++ b/boards/holybro/kakutef7/default.cmake @@ -44,6 +44,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors SYSTEMCMDS diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index 31c86719a9..574d2981c3 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -50,6 +50,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors #sih vmount diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index 37015ee33d..f4aa2f4fe3 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -54,6 +54,7 @@ px4_add_board( mc_pos_control micrortps_bridge navigator + battery_status sensors sih vmount diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 4313391427..473b53ba66 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -73,6 +73,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index f6ddf851f7..b6901eb731 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -69,6 +69,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index d75071d668..002f95eab6 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -61,6 +61,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih #vmount diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 8e1712ea36..c6dfa2bb21 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -80,6 +80,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors vmount vtol_att_control diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake index f2b4fb8c52..c90b5b88e8 100644 --- a/boards/px4/fmu-v2/fixedwing.cmake +++ b/boards/px4/fmu-v2/fixedwing.cmake @@ -54,6 +54,7 @@ px4_add_board( logger mavlink navigator + battery_status sensors vmount airspeed_selector diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake index f2d2a8516a..48ead8e6ee 100644 --- a/boards/px4/fmu-v2/lpe.cmake +++ b/boards/px4/fmu-v2/lpe.cmake @@ -76,6 +76,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors vmount #vtol_att_control diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index f3c19283f5..1d0eb6cc5e 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -54,6 +54,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors vmount diff --git a/boards/px4/fmu-v2/rover.cmake b/boards/px4/fmu-v2/rover.cmake index 4b1c3ffd53..1fa9c5dcf9 100644 --- a/boards/px4/fmu-v2/rover.cmake +++ b/boards/px4/fmu-v2/rover.cmake @@ -46,6 +46,7 @@ px4_add_board( logger mavlink navigator + battery_status sensors vmount diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index f187551193..a3de52cf9b 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -76,6 +76,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors vmount vtol_att_control diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 92ae0f02f3..e1d3b55586 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -79,6 +79,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 107f0aebb5..41bf518432 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -78,6 +78,7 @@ px4_add_board( mc_pos_control micrortps_bridge navigator + battery_status sensors sih vmount diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index cfe1efeb3e..2da5d12796 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -77,6 +77,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index d189b479fd..cfdcc98e52 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -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 diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index 53711ca863..074a670b18 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -67,6 +67,7 @@ px4_add_board( mc_pos_control micrortps_bridge navigator + battery_status sensors sih vmount diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index e80640a310..4090796eeb 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -64,6 +64,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 1ca9081ee1..70c3ba5c54 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -76,6 +76,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index b5d3bce982..12240d8827 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -76,6 +76,7 @@ px4_add_board( mc_pos_control micrortps_bridge navigator + battery_status sensors sih vmount diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index 8a598a750a..2230bd1683 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -77,6 +77,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 6ee897c489..ba965890f6 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -78,6 +78,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index 9dae74abe4..57bf198919 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -55,6 +55,7 @@ px4_add_board( logger mavlink navigator + battery_status sensors vmount airspeed_selector diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index 05ef47c317..d4e36c90ab 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -77,6 +77,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index 4175135720..96c9d0b61f 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -62,6 +62,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 340fc1b4c7..8d5524cb59 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -57,6 +57,7 @@ px4_add_board( logger mavlink navigator + battery_status sensors vmount diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 5dce25381b..0c7c515dcf 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -76,6 +76,7 @@ px4_add_board( mc_pos_control micrortps_bridge navigator + battery_status sensors sih vmount diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 83ddb3d6f1..04613c0cba 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -76,6 +76,7 @@ px4_add_board( mc_pos_control #micrortps_bridge navigator + battery_status sensors sih vmount diff --git a/boards/px4/raspberrypi/cross.cmake b/boards/px4/raspberrypi/cross.cmake index 43443ef45b..339da358c2 100644 --- a/boards/px4/raspberrypi/cross.cmake +++ b/boards/px4/raspberrypi/cross.cmake @@ -43,6 +43,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/boards/px4/raspberrypi/native.cmake b/boards/px4/raspberrypi/native.cmake index 0b9d1fcf3c..4c0bff6d7e 100644 --- a/boards/px4/raspberrypi/native.cmake +++ b/boards/px4/raspberrypi/native.cmake @@ -41,6 +41,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih #simulator diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake index 56d5df30e8..b17507af90 100644 --- a/boards/uvify/core/default.cmake +++ b/boards/uvify/core/default.cmake @@ -70,6 +70,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + battery_status sensors sih vmount diff --git a/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/CMakeLists.txt index bd2fada0ee..4f5ab6a2a3 100644 --- a/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/CMakeLists.txt +++ b/src/drivers/driver_framework_wrapper/df_bebop_bus_wrapper/CMakeLists.txt @@ -42,4 +42,5 @@ px4_add_module( git_driverframework df_driver_framework df_bebop_bus + battery ) diff --git a/src/drivers/power_monitor/voxlpm/CMakeLists.txt b/src/drivers/power_monitor/voxlpm/CMakeLists.txt index 3d35bc2589..4f320da767 100644 --- a/src/drivers/power_monitor/voxlpm/CMakeLists.txt +++ b/src/drivers/power_monitor/voxlpm/CMakeLists.txt @@ -39,5 +39,6 @@ px4_add_module( voxlpm.cpp voxlpm_main.cpp DEPENDS + battery px4_work_queue ) diff --git a/src/modules/battery_status/CMakeLists.txt b/src/modules/battery_status/CMakeLists.txt new file mode 100644 index 0000000000..c01021aef6 --- /dev/null +++ b/src/modules/battery_status/CMakeLists.txt @@ -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 + ) diff --git a/src/modules/battery_status/battery_status.cpp b/src/modules/battery_status/battery_status.cpp new file mode 100644 index 0000000000..cbd8d52942 --- /dev/null +++ b/src/modules/battery_status/battery_status.cpp @@ -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 + * @author Julian Oes + * @author Thomas Gubler + * @author Anton Babushkin + * @author Beat Küng + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#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, 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); +} diff --git a/src/modules/battery_status/parameters.cpp b/src/modules/battery_status/parameters.cpp new file mode 100644 index 0000000000..ba1a611a67 --- /dev/null +++ b/src/modules/battery_status/parameters.cpp @@ -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 + */ + +#include "parameters.h" + +#include + +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 */ diff --git a/src/modules/battery_status/parameters.h b/src/modules/battery_status/parameters.h new file mode 100644 index 0000000000..3cd23d0008 --- /dev/null +++ b/src/modules/battery_status/parameters.h @@ -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 + */ +#include +#include + +#include +#include + +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 */ diff --git a/src/modules/sensors/sensor_params_battery.c b/src/modules/battery_status/sensor_params_battery.c similarity index 100% rename from src/modules/sensors/sensor_params_battery.c rename to src/modules/battery_status/sensor_params_battery.c diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index d977b7ed81..0a5b6620f1 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -47,7 +47,6 @@ px4_add_module( DEPENDS airspeed - battery conversion drivers__device git_ecl diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index 67a81f8b3a..9df49a7304 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -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])); diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h index 19004fce09..1cfc92f51c 100644 --- a/src/modules/sensors/parameters.h +++ b/src/modules/sensors/parameters.h @@ -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]; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 448fff2580..5efcf28592 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -71,7 +71,6 @@ #include #include #include -#include #include @@ -79,7 +78,6 @@ #include #include #include -#include #include #include #include @@ -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 _airdata_pub{ORB_ID(vehicle_air_data)}; /**< combined sensor data topic */ uORB::Publication _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 _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.