From 7977ace77e7631b2c7f03478949d98f98804d2ad Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 25 Aug 2020 18:47:49 +0200 Subject: [PATCH 1/7] BatterySimulator: remove SimulatorBattery It loads the battery parameters but then overwrites them with hardcoded values and it breaks the ModuleParams parent/child hierarchy. Both is undesired. --- ROMFS/px4fmu_common/init.d-posix/rcS | 2 +- .../battery_simulator/BatterySimulator.cpp | 5 ++-- .../battery_simulator/BatterySimulator.hpp | 27 +++---------------- 3 files changed, 8 insertions(+), 26 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 6ef02a169b21..8ee3752b22fc 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -119,7 +119,7 @@ if [ $AUTOCNF = yes ] then param set SYS_AUTOSTART $REQUESTED_AUTOSTART - param set BAT_N_CELLS 3 + param set BAT_N_CELLS 4 param set CAL_ACC0_ID 1311244 param set CAL_GYRO0_ID 1311244 diff --git a/src/modules/simulator/battery_simulator/BatterySimulator.cpp b/src/modules/simulator/battery_simulator/BatterySimulator.cpp index 4c6487561073..4faa9567913e 100644 --- a/src/modules/simulator/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulator/battery_simulator/BatterySimulator.cpp @@ -35,7 +35,8 @@ BatterySimulator::BatterySimulator() : ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + _battery(1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US) { } @@ -46,7 +47,7 @@ BatterySimulator::~BatterySimulator() bool BatterySimulator::init() { - ScheduleOnInterval(SimulatorBattery::SIMLATOR_BATTERY_SAMPLE_INTERVAL_US); + ScheduleOnInterval(BATTERY_SIMLATOR_SAMPLE_INTERVAL_US); return true; } diff --git a/src/modules/simulator/battery_simulator/BatterySimulator.hpp b/src/modules/simulator/battery_simulator/BatterySimulator.hpp index 04cdecbed299..2ea3590c2e5d 100644 --- a/src/modules/simulator/battery_simulator/BatterySimulator.hpp +++ b/src/modules/simulator/battery_simulator/BatterySimulator.hpp @@ -67,34 +67,15 @@ class BatterySimulator : public ModuleBase, public ModuleParam private: void Run() override; + static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ = 100; // Hz + static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_INTERVAL_US = 1_s / BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ; + uORB::Publication _battery_pub{ORB_ID(battery_status)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - class SimulatorBattery : public Battery - { - public: - static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ = 100; // Hz - static constexpr uint32_t SIMLATOR_BATTERY_SAMPLE_INTERVAL_US = 1_s / SIMLATOR_BATTERY_SAMPLE_FREQUENCY_HZ; - - SimulatorBattery() : Battery(1, nullptr, SIMLATOR_BATTERY_SAMPLE_INTERVAL_US) {} - - virtual void updateParams() override - { - Battery::updateParams(); - _params.v_empty = 3.5f; - _params.v_charged = 4.05f; - _params.n_cells = 4; - _params.capacity = 10.0f; - _params.v_load_drop = 0.0f; - _params.r_internal = 0.0f; - _params.low_thr = 0.15f; - _params.crit_thr = 0.07f; - _params.emergen_thr = 0.05f; - _params.source = 0; - } - } _battery; + Battery _battery; uint64_t _last_integration_us{0}; float _battery_percentage{1.f}; From f9a32caced919b91a3401e5946e601544218abcf Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 25 Aug 2020 19:38:55 +0200 Subject: [PATCH 2/7] battery: fix parameter migration and clarify --- src/lib/battery/battery.cpp | 5 +- src/lib/battery/battery.h | 53 +++++++-------------- src/lib/battery/battery_params_deprecated.c | 8 ++-- 3 files changed, 24 insertions(+), 42 deletions(-) diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 8386286880f1..75b1c6f5b6e3 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -107,7 +107,7 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) Battery::~Battery() { - orb_unadvertise(_orb_advert); + orb_unadvertise(_battery_status_pub); } void @@ -186,7 +186,8 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre void Battery::publish() { - orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance); + int dummy; // We're not interested in the instance ID we get + orb_publish_auto(ORB_ID(battery_status), &_battery_status_pub, &_battery_status, &dummy); } void diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index b8488c8516cd..22b494355b59 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -43,12 +43,10 @@ #pragma once #include -#include #include #include #include #include -#include #include #include #include @@ -157,61 +155,45 @@ class Battery : public ModuleParams const int _index; - bool _first_parameter_update{false}; - virtual void updateParams() override; + bool _first_parameter_update{true}; + void updateParams() override; /** - * This function helps with migrating to new parameters. It performs several tasks: - * - Update both the old and new parameter values using `param_get(...)` - * - Check if either parameter changed just now - * - If so, display a warning if the deprecated parameter was used - * - Copy the new value over to the other parameter - * - If this is the first time the parameters are fetched, check if they are equal - * - If not, display a warning and copy the value of the deprecated parameter over to the new one + * This function helps migrating and syncing from/to deprecated parameters. BAT_* BAT1_* * @tparam T Type of the parameter (int or float) - * @param old_param Handle to the old deprecated parameter (for example, param_find("BAT_N_CELLS") - * @param new_param Handle to the new replacement parameter (for example, param_find("BAT1_N_CELLS") + * @param old_param Handle to the old deprecated parameter (for example, param_find("BAT_N_CELLS")) + * @param new_param Handle to the new replacement parameter (for example, param_find("BAT1_N_CELLS")) * @param old_val Pointer to the value of the old deprecated parameter * @param new_val Pointer to the value of the new replacement parameter - * @param firstcall If true, then this function will not check to see if the values have changed - * (Since the old values are uninitialized) - * @return True iff either of these parameters changed just now and the migration was done. + * @param firstcall If true, this function prefers migrating old to new */ template - bool migrateParam(param_t old_param, param_t new_param, T *old_val, T *new_val, bool firstcall) + void migrateParam(param_t old_param, param_t new_param, T *old_val, T *new_val, bool firstcall) { - T previous_old_val = *old_val; T previous_new_val = *new_val; + // Update both the old and new parameter values param_get(old_param, old_val); param_get(new_param, new_val); - if (!firstcall) { - if ((float) fabs((float) *old_val - (float) previous_old_val) > FLT_EPSILON - && (float) fabs((float) *old_val - (float) *new_val) > FLT_EPSILON) { + // Check if the parameter values are different + if (!isFloatEqual(*old_val, *new_val)) { + // If so, copy the new value over to the unchanged parameter + // Note: If they differ from the beginning we migrate old to new + if (firstcall || !isFloatEqual(*old_val, previous_old_val)) { param_set_no_notification(new_param, old_val); param_get(new_param, new_val); - return true; - } else if ((float) fabs((float) *new_val - (float) previous_new_val) > FLT_EPSILON - && (float) fabs((float) *old_val - (float) *new_val) > FLT_EPSILON) { + } else if (!isFloatEqual(*new_val, previous_new_val)) { param_set_no_notification(old_param, new_val); param_get(old_param, old_val); - return true; - } - - } else { - if ((float) fabs((float) *old_val - (float) *new_val) > FLT_EPSILON) { - param_set_no_notification(new_param, old_val); - param_get(new_param, new_val); - return true; } } - - return false; } + bool isFloatEqual(float a, float b) { return fabsf(a - b) > FLT_EPSILON; } + private: void sumDischarged(hrt_abstime timestamp, float current_a); void estimateRemaining(const float voltage_v, const float current_a, const float throttle); @@ -230,6 +212,5 @@ class Battery : public ModuleParams uint8_t _warning; hrt_abstime _last_timestamp; - orb_advert_t _orb_advert{nullptr}; - int _orb_instance; + orb_advert_t _battery_status_pub{nullptr}; }; diff --git a/src/lib/battery/battery_params_deprecated.c b/src/lib/battery/battery_params_deprecated.c index d180552534b4..a1d4f715c23c 100644 --- a/src/lib/battery/battery_params_deprecated.c +++ b/src/lib/battery/battery_params_deprecated.c @@ -32,11 +32,11 @@ ****************************************************************************/ /** - * @file battery_params_1.c + * @file battery_params_deprecated.c * @author Timothy Scott * - * Defines parameters for Battery 1. For backwards compatibility, the - * parameter names do not have a "1" in them. + * Defines the deprcated single battery configuration which are temporarily kept for backwards compatibility with QGC. + * The new parameter set has a number after "BAT" e.g. BAT1_V_EMPTY. */ /** @@ -160,4 +160,4 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * @value 1 External * @group Battery Calibration */ -PARAM_DEFINE_INT32(BAT_SOURCE, 0); \ No newline at end of file +PARAM_DEFINE_INT32(BAT_SOURCE, 0); From 6c986492e8243b3c300dbe1bfde3d5396c35b367 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 25 Aug 2020 19:41:07 +0200 Subject: [PATCH 3/7] ina226/voxlpm: make sure parameter sub is reset The subscription to parameter updates has to get copied otherwise the change detection will not get reset for next time. --- src/drivers/power_monitor/ina226/ina226.cpp | 6 ++++-- src/drivers/power_monitor/ina226/ina226.h | 2 +- src/drivers/power_monitor/voxlpm/voxlpm.cpp | 6 ++++-- src/drivers/power_monitor/voxlpm/voxlpm.hpp | 2 +- 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index c14cca4c71ed..ee74c612df8f 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -210,9 +210,11 @@ INA226::collect() { perf_begin(_sample_perf); - parameter_update_s param_update; + if (_parameter_update_sub.updated()) { + // Read from topic to clear updated flag + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); - if (_parameters_sub.copy(¶m_update)) { updateParams(); } diff --git a/src/drivers/power_monitor/ina226/ina226.h b/src/drivers/power_monitor/ina226/ina226.h index 8bc73ff2ce4e..edd24a5cdec8 100644 --- a/src/drivers/power_monitor/ina226/ina226.h +++ b/src/drivers/power_monitor/ina226/ina226.h @@ -207,7 +207,7 @@ class INA226 : public device::I2C, public ModuleParams, public I2CSPIDriver _pm_pub_topic{ORB_ID(power_monitor)}; - uORB::Subscription _parameter_sub{ORB_ID(parameter_update)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; power_monitor_s _pm_status{}; From 5e648e0ee9fc8209a722d51836e9893cf5dd5f98 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 25 Aug 2020 19:44:35 +0200 Subject: [PATCH 4/7] syslink_main: remove empty lines and struct keyword --- .../crazyflie/syslink/syslink_main.cpp | 22 ++----------------- .../bitcraze/crazyflie/syslink/syslink_main.h | 7 ------ 2 files changed, 2 insertions(+), 27 deletions(-) diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp index e504bb28a37a..b6e7b06aaf54 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.cpp +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.cpp @@ -219,8 +219,6 @@ Syslink::update_params(bool force_set) this->_params_update[2] = t; this->_params_ack[2] = 0; } - - } // 1M 8N1 serial connection to NRF51 @@ -363,7 +361,7 @@ Syslink::task_main() } if (fds[1].revents & POLLIN) { - struct parameter_update_s update; + parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); update_params(false); } @@ -372,7 +370,6 @@ Syslink::task_main() } close(_fd); - } void @@ -497,7 +494,6 @@ Syslink::handle_message(syslink_message_t *msg) } else if (_params_ack[2] == 0 && t - _params_update[2] > 10000) { set_address(_addr); } - } void @@ -515,7 +511,6 @@ Syslink::handle_radio(syslink_message_t *sys) } else if (sys->type == SYSLINK_RADIO_ADDRESS) { _params_ack[2] = t; } - } void @@ -602,7 +597,6 @@ Syslink::handle_bootloader(syslink_message_t *sys) c->data[22] = 0x10; // Protocol version send_message(sys); } - } void @@ -797,7 +791,6 @@ void status() } printf("\n\n"); - } close(deckfd); @@ -827,20 +820,13 @@ void attached(int pid) exit(found ? 1 : 0); } - - void test() { // TODO: Ensure battery messages are recent // TODO: Read and write from memory to ensure it is working } - - - -} - - +} // namespace syslink int syslink_main(int argc, char *argv[]) { @@ -849,7 +835,6 @@ int syslink_main(int argc, char *argv[]) exit(1); } - const char *verb = argv[1]; if (!strcmp(verb, "start")) { @@ -873,9 +858,6 @@ int syslink_main(int argc, char *argv[]) syslink::test(); } - - - syslink::usage(); exit(1); diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.h b/boards/bitcraze/crazyflie/syslink/syslink_main.h index fc28a1b40db4..8babf288f247 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.h +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.h @@ -52,7 +52,6 @@ using namespace time_literals; #define MIN(X, Y) (((X) < (Y)) ? (X) : (Y)) - typedef enum { BAT_DISCHARGING = 0, BAT_CHARGING = 1, @@ -82,7 +81,6 @@ class Syslink int txrate; private: - friend class SyslinkBridge; friend class SyslinkMemory; @@ -153,7 +151,6 @@ class Syslink static int task_main_trampoline(int argc, char *argv[]); void task_main(); - }; @@ -174,11 +171,9 @@ class SyslinkBridge : public cdev::CDev void pipe_message(crtp_message_t *msg); protected: - virtual pollevent_t poll_state(struct file *filp); private: - Syslink *_link; // Stores data that was received from syslink but not yet read by another driver @@ -186,7 +181,6 @@ class SyslinkBridge : public cdev::CDev crtp_message_t _msg_to_send; int _msg_to_send_size_remaining; - }; @@ -219,5 +213,4 @@ class SyslinkMemory : public cdev::CDev int write(int i, uint16_t addr, const char *buf, int length); void sendAndWait(); - }; From 1628cbb40b832b00abd96f4d9633338f67987bd2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 25 Aug 2020 19:45:10 +0200 Subject: [PATCH 5/7] commander_params: remove some double spaces --- src/modules/commander/commander_params.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index dcc2ee44487c..72354ceb2e33 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -283,7 +283,7 @@ PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f); * @unit s * @decimal 2 */ -PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f); +PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f); /** @@ -641,8 +641,8 @@ PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 1); * * @min 0 * @max 3 - * @bit 0 Enable override in auto modes - * @bit 1 Enable override in offboard mode + * @bit 0 Enable override in auto modes + * @bit 1 Enable override in offboard mode * @group Commander */ PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1); @@ -664,7 +664,7 @@ PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0); * Navigation accuracy checks can be disabled using the CBRK_VELPOSERR parameter, but doing so will remove protection for all flight modes. * * @value 0 Altitude/Manual. Assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL. - * @value 1 Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. + * @value 1 Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. * * @group Commander */ From 4562bb3cc25370e1dd7088b26fbf6116ffeb60cb Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 26 Aug 2020 10:33:10 +0200 Subject: [PATCH 6/7] analog_battery: fix missing stdio include This was not a problem before because battery.h included the adc driver and implicitly snprintf was defined through there. --- src/modules/battery_status/analog_battery.cpp | 34 +++++++++++++++++++ src/modules/battery_status/analog_battery.h | 2 +- 2 files changed, 35 insertions(+), 1 deletion(-) diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index a2e2835875ed..e40825477922 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -1,3 +1,37 @@ +/**************************************************************************** + * + * Copyright (c) 2019-2020 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. + * + ****************************************************************************/ + +#include #include #include "analog_battery.h" diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h index 42b4e2c3fa39..ce42b103b5d8 100644 --- a/src/modules/battery_status/analog_battery.h +++ b/src/modules/battery_status/analog_battery.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2020 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 From 6cb56442738cc72c212147ee696d125750d87874 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 26 Aug 2020 11:39:54 +0200 Subject: [PATCH 7/7] battery: switch to PublicationMulti for battery_status --- src/lib/battery/battery.cpp | 9 ++------- src/lib/battery/battery.h | 8 ++++---- 2 files changed, 6 insertions(+), 11 deletions(-) diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 75b1c6f5b6e3..1c9509a54f4c 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -105,11 +105,6 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us) updateParams(); } -Battery::~Battery() -{ - orb_unadvertise(_battery_status_pub); -} - void Battery::reset() { @@ -179,6 +174,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre const bool should_publish = (source == _params.source); if (should_publish) { + _battery_status_pub.publish(_battery_status); publish(); } } @@ -186,8 +182,7 @@ Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float curre void Battery::publish() { - int dummy; // We're not interested in the instance ID we get - orb_publish_auto(ORB_ID(battery_status), &_battery_status_pub, &_battery_status, &dummy); + _battery_status_pub.publish(_battery_status); } void diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 22b494355b59..a0cc1fb70360 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -43,6 +43,7 @@ #pragma once #include +#include #include #include #include @@ -63,8 +64,7 @@ class Battery : public ModuleParams { public: Battery(int index, ModuleParams *parent, const int sample_interval_us); - - ~Battery(); + ~Battery() = default; /** * Reset all battery stats and report invalid/nothing. @@ -200,6 +200,8 @@ class Battery : public ModuleParams void determineWarning(bool connected); void computeScale(); + uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; + bool _battery_initialized = false; AlphaFilter _voltage_filter_v; AlphaFilter _current_filter_a; @@ -211,6 +213,4 @@ class Battery : public ModuleParams float _scale = 1.f; uint8_t _warning; hrt_abstime _last_timestamp; - - orb_advert_t _battery_status_pub{nullptr}; };