Fix Battery Parameter Migration

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

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);
 

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,19 +171,16 @@ 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
 	ringbuffer::RingBuffer _readbuffer;
 
 	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();
-
 };

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(&parameter_update);
 
-	if (_parameters_sub.copy(&param_update)) {
 		updateParams();
 	}
 

src/drivers/power_monitor/ina226/ina226.h

@@ -207,7 +207,7 @@ class INA226 : public device::I2C, public ModuleParams, public I2CSPIDriver<INA2
 
 	Battery 		  _battery;
 	uORB::Subscription  _actuators_sub{ORB_ID(actuator_controls_0)};
-	uORB::Subscription  _parameters_sub{ORB_ID(parameter_update)};
+	uORB::Subscription  _parameter_update_sub{ORB_ID(parameter_update)};
 
 	int read(uint8_t address, int16_t &data);
 	int write(uint8_t address, uint16_t data);

src/drivers/power_monitor/voxlpm/voxlpm.cpp

@@ -316,9 +316,11 @@ VOXLPM::measure()
 		}
 	}
 
-	parameter_update_s update;
+	if (_parameter_update_sub.updated()) {
+		// Read from topic to clear updated flag
+		parameter_update_s parameter_update;
+		_parameter_update_sub.copy(&parameter_update);
 
-	if (_parameter_sub.update(&update)) {
 		updateParams();
 	}
 

src/drivers/power_monitor/voxlpm/voxlpm.hpp

@@ -253,7 +253,7 @@ class VOXLPM : public device::I2C, public ModuleParams, public I2CSPIDriver<VOXL
 	perf_counter_t		_comms_errors;
 
 	uORB::PublicationMulti<power_monitor_s>		_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{};
 

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(_orb_advert);
-}
-
 void
 Battery::reset()
 {
@@ -179,14 +174,15 @@ 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();
 	}
 }
 
 void
 Battery::publish()
 {
-	orb_publish_auto(ORB_ID(battery_status), &_orb_advert, &_battery_status, &_orb_instance);
+	_battery_status_pub.publish(_battery_status);
 }
 
 void

src/lib/battery/battery.h

@@ -43,12 +43,11 @@
 #pragma once
 
 #include <uORB/uORB.h>
-#include <uORB/Subscription.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/battery_status.h>
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
 #include <parameters/param.h>
-#include <drivers/drv_adc.h>
 #include <board_config.h>
 #include <px4_platform_common/board_common.h>
 #include <math.h>
@@ -65,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.
@@ -157,67 +155,53 @@ 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<typename T>
-	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);
 	void determineWarning(bool connected);
 	void computeScale();
 
+	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
+
 	bool _battery_initialized = false;
 	AlphaFilter<float> _voltage_filter_v;
 	AlphaFilter<float> _current_filter_a;
@@ -229,7 +213,4 @@ class Battery : public ModuleParams
 	float _scale = 1.f;
 	uint8_t _warning;
 	hrt_abstime _last_timestamp;
-
-	orb_advert_t _orb_advert{nullptr};
-	int _orb_instance;
 };

src/lib/battery/battery_params_deprecated.c

@@ -32,11 +32,11 @@
  ****************************************************************************/
 
 /**
- * @file battery_params_1.c
+ * @file battery_params_deprecated.c
  * @author Timothy Scott <[email protected]>
  *
- * 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);
+PARAM_DEFINE_INT32(BAT_SOURCE, 0);