diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 8386286880..75b1c6f5b6 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 b8488c8516..22b494355b 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 @@ protected: 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 @@ private: 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 d180552534..a1d4f715c2 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);