Introduced battery_status uORB topic, changed sensors app to publish to it, extended px4io driver to publish to it. Both do only so if the battery voltage is reasonably high, at 3.3V

7 files changed, 141 insertions, 22 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 7277e9fa4..17087ab8a 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -70,6 +70,7 @@
 #include <poll.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/battery_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -1262,6 +1263,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct vehicle_command_s cmd;
 	memset(&cmd, 0, sizeof(cmd));
 
+	/* subscribe to battery topic */
+	int battery_sub = orb_subscribe(ORB_ID(battery_status));
+	struct battery_status_s battery;
+	memset(&battery, 0, sizeof(battery));
+
 	// uint8_t vehicle_state_previous = current_status.state_machine;
 	float voltage_previous = 0.0f;
 
@@ -1300,15 +1306,19 @@ int commander_thread_main(int argc, char *argv[])
 			handle_command(stat_pub, &current_status, &cmd);
 		}
 
-		battery_voltage = sensors.battery_voltage_v;
-		battery_voltage_valid = sensors.battery_voltage_valid;
-
-		/*
-		 * Only update battery voltage estimate if voltage is
-		 * valid and system has been running for two and a half seconds
-		 */
-		if (battery_voltage_valid && (hrt_absolute_time() - start_time > 2500000)) {
-			bat_remain = battery_remaining_estimate_voltage(battery_voltage);
+		orb_check(battery_sub, &new_data);
+		if (new_data) {
+			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+			battery_voltage = battery.voltage_v;
+			battery_voltage_valid = true;
+
+			/*
+			 * Only update battery voltage estimate if system has
+			 * been running for two and a half seconds.
+			 */
+			if (hrt_absolute_time() - start_time > 2500000) {
+				bat_remain = battery_remaining_estimate_voltage(battery_voltage);
+			}
 		}
 
 		/* Slow but important 8 Hz checks */
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 9f3dba047..90c077363 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -73,6 +73,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/battery_status.h>
 
 #include <px4io/protocol.h>
 #include "uploader.h"
@@ -109,6 +110,9 @@ private:
 	orb_advert_t 		_to_input_rc;	///< rc inputs from io
 	rc_input_values		_input_rc;	///< rc input values
 
+	orb_advert_t		_to_battery;	///< battery status / voltage
+	battery_status_s	_battery_status;///< battery status data
+
 	orb_advert_t		_t_outputs;	///< mixed outputs topic
 	actuator_outputs_s	_outputs;	///< mixed outputs
 
@@ -321,6 +325,10 @@ PX4IO::task_main()
 	memset(&_input_rc, 0, sizeof(_input_rc));
 	_to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
 
+	/* do not advertise the battery status until its clear that a battery is connected */
+	memset(&_input_rc, 0, sizeof(_input_rc));
+	_to_battery = -1;
+
 	/* poll descriptor */
 	pollfd fds[3];
 	fds[0].fd = _serial_fd;
@@ -479,6 +487,23 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 	/* remember the latched arming switch state */
 	_switch_armed = rep->armed;
 
+	/* publish battery information */
+
+	/* only publish if battery has a valid minimum voltage */
+	if (rep->battery_mv > 3300) {
+		_battery_status.timestamp = hrt_absolute_time();
+		_battery_status.voltage_v = rep->battery_mv / 1000.0f;
+		/* current and discharge are unknown */
+		_battery_status.current_a = -1.0f;
+		_battery_status.discharged_mah = -1.0f;
+		/* announce the battery voltage if needed, just publish else */
+		if (_to_battery > 0) {
+			orb_publish(ORB_ID(battery_status), _to_battery, &_battery_status);
+		} else {
+			_to_battery = orb_advertise(ORB_ID(battery_status), &_battery_status);
+		}
+	}
+
 	_send_needed = true;
 
 	/* if monitoring, dump the received info */
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index d38bf9122..9b4cd1622 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -554,7 +554,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
 			.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
 			.actuators = {buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
 					buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
-			.vbat = buf.raw.battery_voltage_v,
+			.vbat = 0.0f, /* XXX use battery_status uORB topic */
 			.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
 			.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
 			.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 07a9015fe..55786333f 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -73,6 +73,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/battery_status.h>
 
 #define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20   /* 40 ms downtime at 500 Hz update rate   */
 #define ACC_HEALTH_COUNTER_LIMIT_ERROR  20   /* 40 ms downtime at 500 Hz update rate   */
@@ -156,10 +157,12 @@ private:
 	orb_advert_t	_sensor_pub;			/**< combined sensor data topic */
 	orb_advert_t	_manual_control_pub;		/**< manual control signal topic */
 	orb_advert_t	_rc_pub;			/**< raw r/c control topic */
+	orb_advert_t	_battery_pub;			/**< battery status */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	struct rc_channels_s _rc;			/**< r/c channel data */
+	struct battery_status_s _battery_status;	/**< battery status */
 
 	struct {
 		float min[_rc_max_chan_count];
@@ -348,6 +351,7 @@ Sensors::Sensors() :
 	_sensor_pub(-1),
 	_manual_control_pub(-1),
 	_rc_pub(-1),
+	_battery_pub(-1),
 
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
@@ -844,14 +848,22 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 
 		if (ADC_BATTERY_VOLATGE_CHANNEL == buf_adc.am_channel1) {
 			/* Voltage in volts */
-			raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * _parameters.battery_voltage_scaling)));
+			float voltage = (buf_adc.am_data1 * _parameters.battery_voltage_scaling);
 
-			if ((raw.battery_voltage_v) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
-				raw.battery_voltage_valid = false;
-				raw.battery_voltage_v = 0.f;
+			if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
 
-			} else {
-				raw.battery_voltage_valid = true;
+				_battery_status.timestamp = hrt_absolute_time();
+				_battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));;
+				/* current and discharge are unknown */
+				_battery_status.current_a = -1.0f;
+				_battery_status.discharged_mah = -1.0f;
+
+				/* announce the battery voltage if needed, just publish else */
+				if (_battery_pub > 0) {
+					orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
+				} else {
+					_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
+				}
 			}
 
 			raw.battery_voltage_counter++;
@@ -879,7 +891,7 @@ Sensors::ppm_poll()
 	 */
 	if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
 
-		for (int i = 0; i < ppm_decoded_channels; i++) {
+		for (unsigned i = 0; i < ppm_decoded_channels; i++) {
 			raw.values[i] = ppm_buffer[i];
 		}
 
@@ -1039,12 +1051,13 @@ Sensors::task_main()
 	struct sensor_combined_s raw;
 	memset(&raw, 0, sizeof(raw));
 	raw.timestamp = hrt_absolute_time();
-	raw.battery_voltage_v = BAT_VOL_INITIAL;
 	raw.adc_voltage_v[0] = 0.9f;
 	raw.adc_voltage_v[1] = 0.0f;
 	raw.adc_voltage_v[2] = 0.0f;
-	raw.battery_voltage_counter = 0;
-	raw.battery_voltage_valid = false;
+	raw.adc_voltage_v[3] = 0.0f;
+
+	memset(&_battery_status, 0, sizeof(_battery_status));
+	_battery_status.voltage_v = BAT_VOL_INITIAL;
 
 	/* get a set of initial values */
 	accel_poll(raw);
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index dbee15050..2d249a47f 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -71,6 +71,9 @@ ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
 #include "topics/vehicle_status.h"
 ORB_DEFINE(vehicle_status, struct vehicle_status_s);
 
+#include "topics/battery_status.h"
+ORB_DEFINE(battery_status, struct battery_status_s);
+
 #include "topics/vehicle_global_position.h"
 ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
 
diff --git a/apps/uORB/topics/battery_status.h b/apps/uORB/topics/battery_status.h
new file mode 100644
index 000000000..c40d0d4e5
--- /dev/null
+++ b/apps/uORB/topics/battery_status.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 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 battery_status.h
+ *
+ * Definition of the battery status uORB topic.
+ */
+
+#ifndef BATTERY_STATUS_H_
+#define BATTERY_STATUS_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Battery voltages and status
+ */
+struct battery_status_s {
+	uint64_t	timestamp;		/**< microseconds since system boot, needed to integrate */
+	float   	voltage_v;		/**< Battery voltage in volts, filtered           	 */
+	float		current_a;		/**< Battery current in amperes, filtered, -1 if unknown */
+	float		discharged_mah;		/**< Discharged amount in mAh, filtered, -1 if unknown	 */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(battery_status);
+
+#endif
\ No newline at end of file
diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h
index 0324500ac..1d25af35a 100644
--- a/apps/uORB/topics/sensor_combined.h
+++ b/apps/uORB/topics/sensor_combined.h
@@ -99,8 +99,8 @@ struct sensor_combined_s {
 	float baro_pres_mbar;			/**< Barometric pressure, already temp. comp.     */
 	float baro_alt_meter;			/**< Altitude, already temp. comp.                */
 	float baro_temp_celcius;		/**< Temperature in degrees celsius               */
-	float battery_voltage_v;		/**< Battery voltage in volts, filtered           */
-	float adc_voltage_v[3];			/**< ADC voltages of ADC Chan 11/12/13 or -1      */
+	float adc_voltage_v[4];			/**< ADC voltages of ADC Chan 10/11/12/13 or -1      */
+	float mcu_temp_celcius;			/**< Internal temperature measurement of MCU */
 	uint32_t baro_counter;			/**< Number of raw baro measurements taken        */
 	uint32_t battery_voltage_counter;	/**< Number of voltage measurements taken         */
 	bool battery_voltage_valid;		/**< True if battery voltage can be measured      */