Merged status changes

src/drivers/bma180/bma180.cpp

@@ -690,6 +690,7 @@ BMA180::measure()
 	 * measurement flow without using the external interrupt.
 	 */
 	report.timestamp = hrt_absolute_time();
+        report.error_count = 0;
 	/*
 	 * y of board is x of sensor and x of board is -y of sensor
 	 * perform only the axis assignment here.

src/drivers/drv_accel.h

@@ -52,6 +52,7 @@
  */
 struct accel_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float x;		/**< acceleration in the NED X board axis in m/s^2 */
 	float y;		/**< acceleration in the NED Y board axis in m/s^2 */
 	float z;		/**< acceleration in the NED Z board axis in m/s^2 */

src/drivers/drv_baro.h

@@ -55,6 +55,7 @@ struct baro_report {
 	float altitude;
 	float temperature;
 	uint64_t timestamp;
+	uint64_t error_count;
 };
 
 /*

src/drivers/drv_gyro.h

@@ -52,6 +52,7 @@
  */
 struct gyro_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float x;		/**< angular velocity in the NED X board axis in rad/s */
 	float y;		/**< angular velocity in the NED Y board axis in rad/s */
 	float z;		/**< angular velocity in the NED Z board axis in rad/s */

src/drivers/drv_mag.h

@@ -54,6 +54,7 @@
  */
 struct mag_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float x;
 	float y;
 	float z;

src/drivers/drv_range_finder.h

@@ -52,6 +52,7 @@
  */
 struct range_finder_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float distance; 			/** in meters */
 	uint8_t valid;				/** 1 == within sensor range, 0 = outside sensor range */
 };

src/drivers/ets_airspeed/ets_airspeed.cpp

@@ -153,35 +153,36 @@ ETSAirspeed::collect()
 	ret = transfer(nullptr, 0, &val[0], 2);
 
 	if (ret < 0) {
-		log("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
 		return ret;
 	}
 
 	uint16_t diff_pres_pa = val[1] << 8 | val[0];
         if (diff_pres_pa == 0) {
-            // a zero value means the pressure sensor cannot give us a
+		// a zero value means the pressure sensor cannot give us a
-            // value. We need to return, and not report a value or the
+		// value. We need to return, and not report a value or the
-            // caller could end up using this value as part of an
+		// caller could end up using this value as part of an
-            // average
+		// average
-            log("zero value from sensor"); 
+		perf_count(_comms_errors);
-            return -1;
+		log("zero value from sensor"); 
+		return -1;
         }
 
 	if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
 		diff_pres_pa = 0;
-
 	} else {
 		diff_pres_pa -= _diff_pres_offset;
 	}
 
 	// Track maximum differential pressure measured (so we can work out top speed).
 	if (diff_pres_pa > _max_differential_pressure_pa) {
-	    _max_differential_pressure_pa = diff_pres_pa;
+		_max_differential_pressure_pa = diff_pres_pa;
 	}
 
 	// XXX we may want to smooth out the readings to remove noise.
 	differential_pressure_s report;
 	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
 	report.differential_pressure_pa = diff_pres_pa;
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -209,7 +210,7 @@ ETSAirspeed::cycle()
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
+			perf_count(_comms_errors);
 			/* restart the measurement state machine */
 			start();
 			return;

src/drivers/hmc5883/hmc5883.cpp

@@ -791,6 +791,7 @@ HMC5883::collect()
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	new_report.timestamp = hrt_absolute_time();
+        new_report.error_count = perf_event_count(_comms_errors);
 
 	/*
 	 * @note  We could read the status register here, which could tell us that

src/drivers/l3gd20/l3gd20.cpp

@@ -771,6 +771,7 @@ L3GD20::measure()
 	 *		  74 from all measurements centers them around zero.
 	 */
 	report.timestamp = hrt_absolute_time();
+        report.error_count = 0; // not recorded
 	
 	switch (_orientation) {
 

src/drivers/lsm303d/lsm303d.cpp

@@ -1248,6 +1248,7 @@ LSM303D::measure()
 
 
 	accel_report.timestamp = hrt_absolute_time();
+        accel_report.error_count = 0; // not reported
 
 	accel_report.x_raw = raw_accel_report.x;
 	accel_report.y_raw = raw_accel_report.y;

src/drivers/mb12xx/mb12xx.cpp

@@ -490,6 +490,7 @@ MB12XX::collect()
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
 	report.distance = si_units;
 	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
 	

src/drivers/meas_airspeed/meas_airspeed.cpp

@@ -138,7 +138,6 @@ MEASAirspeed::measure()
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		log("i2c::transfer returned %d", ret);
 		return ret;
 	}
 
@@ -161,7 +160,6 @@ MEASAirspeed::collect()
 	ret = transfer(nullptr, 0, &val[0], 4);
 
 	if (ret < 0) {
-		log("error reading from sensor: %d", ret);
                 perf_count(_comms_errors);
                 perf_end(_sample_perf);
 		return ret;
@@ -207,6 +205,7 @@ MEASAirspeed::collect()
 	}
 
 	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
 	report.temperature = temperature;
 	report.differential_pressure_pa = diff_press_pa;
 	report.voltage = 0;
@@ -235,7 +234,6 @@ MEASAirspeed::cycle()
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
 			/* restart the measurement state machine */
 			start();
 			return;

src/drivers/mpu6000/mpu6000.cpp

@@ -1186,7 +1186,7 @@ MPU6000::measure()
 	 * Adjust and scale results to m/s^2.
 	 */
 	grb.timestamp = arb.timestamp = hrt_absolute_time();
-
+        grb.error_count = arb.error_count = 0; // not reported
 
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.

src/drivers/ms5611/ms5611.cpp

@@ -573,6 +573,7 @@ MS5611::collect()
 	struct baro_report report;
 	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
 
 	/* read the most recent measurement - read offset/size are hardcoded in the interface */
 	ret = _interface->read(0, (void *)&raw, 0);

src/drivers/px4io/px4io.cpp

@@ -80,6 +80,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/servorail_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <debug.h>
 
@@ -266,6 +267,7 @@ private:
 	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
 	orb_advert_t		_to_outputs;		///< mixed servo outputs topic
 	orb_advert_t		_to_battery;		///< battery status / voltage
+	orb_advert_t		_to_servorail;		///< servorail status
 	orb_advert_t		_to_safety;		///< status of safety
 
 	actuator_outputs_s	_outputs;		///<mixed outputs
@@ -424,8 +426,27 @@ private:
 	 */
 	void			dsm_bind_ioctl(int dsmMode);
 
-};
+	/**
+	 * Handle a battery update from IO.
+	 *
+	 * Publish IO battery information if necessary.
+	 *
+	 * @param vbatt		vbatt register
+	 * @param ibatt		ibatt register
+	 */
+	void			io_handle_battery(uint16_t vbatt, uint16_t ibatt);
 
+	/**
+	 * Handle a servorail update from IO.
+	 *
+	 * Publish servo rail information if necessary.
+	 *
+	 * @param vservo	vservo register
+	 * @param vrssi 	vrssi register
+	 */
+	void			io_handle_vservo(uint16_t vbatt, uint16_t ibatt);
+        
+};
 
 namespace
 {
@@ -461,6 +482,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_to_actuators_effective(0),
 	_to_outputs(0),
 	_to_battery(0),
+	_to_servorail(0),
 	_to_safety(0),
 	_primary_pwm_device(false),
 	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
@@ -1205,10 +1227,67 @@ PX4IO::io_handle_alarms(uint16_t alarms)
 	return 0;
 }
 
+void
+PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
+{
+	/* only publish if battery has a valid minimum voltage */
+	if (vbatt <= 3300) {
+		return;
+	}
+
+	battery_status_s	battery_status;
+	battery_status.timestamp = hrt_absolute_time();
+
+	/* voltage is scaled to mV */
+	battery_status.voltage_v = vbatt / 1000.0f;
+
+	/*
+	  ibatt contains the raw ADC count, as 12 bit ADC
+	  value, with full range being 3.3v
+	*/
+	battery_status.current_a = ibatt * (3.3f/4096.0f) * _battery_amp_per_volt;
+	battery_status.current_a += _battery_amp_bias;
+
+	/*
+	  integrate battery over time to get total mAh used
+	*/
+	if (_battery_last_timestamp != 0) {
+		_battery_mamphour_total += battery_status.current_a * 
+			(battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
+	}
+	battery_status.discharged_mah = _battery_mamphour_total;
+	_battery_last_timestamp = battery_status.timestamp;
+	
+	/* lazily publish the battery voltage */
+	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);
+	}
+}
+
+void
+PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
+{
+	servorail_status_s servorail_status;
+	servorail_status.timestamp = hrt_absolute_time();
+
+	/* voltage is scaled to mV */
+	servorail_status.voltage_v = vservo * 0.001f;
+	servorail_status.rssi_v    = vrssi * 0.001f;
+	
+	/* lazily publish the servorail voltages */
+	if (_to_servorail > 0) {
+		orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status);
+	} else {
+		_to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status);
+	}
+}
+
 int
 PX4IO::io_get_status()
 {
-	uint16_t	regs[4];
+	uint16_t	regs[6];
 	int		ret;
 
 	/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
@@ -1218,40 +1297,14 @@ PX4IO::io_get_status()
 
 	io_handle_status(regs[0]);
 	io_handle_alarms(regs[1]);
-	
-	/* only publish if battery has a valid minimum voltage */
-	if (regs[2] > 3300) {
-		battery_status_s	battery_status;
 
-		battery_status.timestamp = hrt_absolute_time();
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	io_handle_battery(regs[2], regs[3]);
+#endif
 
-		/* voltage is scaled to mV */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
-		battery_status.voltage_v = regs[2] / 1000.0f;
+	io_handle_vservo(regs[4], regs[5]);
-
+#endif
-		/*
-		  regs[3] contains the raw ADC count, as 12 bit ADC
-		  value, with full range being 3.3v
-		 */
-		battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt;
-		battery_status.current_a += _battery_amp_bias;
-
-		/*
-		  integrate battery over time to get total mAh used
-		 */
-		if (_battery_last_timestamp != 0) {
-			_battery_mamphour_total += battery_status.current_a * 
-				(battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
-		}
-		battery_status.discharged_mah = _battery_mamphour_total;
-		_battery_last_timestamp = battery_status.timestamp;
-
-		/* lazily publish the battery voltage */
-		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);
-		}
-	}
 
 	return ret;
 }

src/modules/px4iofirmware/registers.c

@@ -674,27 +674,25 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 #ifdef ADC_VSERVO
 		/* PX4IO_P_STATUS_VSERVO */
 		{
-			/*
-			 * Coefficients here derived by measurement of the 5-16V
-			 * range on one unit:
-			 *
-			 * XXX pending measurements
-			 *
-			 * slope = xxx
-			 * intercept = xxx
-			 *
-			 * Intercept corrected for best results @ 5.0V.
-			 */
 			unsigned counts = adc_measure(ADC_VSERVO);
 			if (counts != 0xffff) {
-				unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+				// use 3:1 scaling on 3.3V ADC input
-				unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000;
+				unsigned mV = counts * 9900 / 4096;
-
+				r_page_status[PX4IO_P_STATUS_VSERVO] = mV;
-				r_page_status[PX4IO_P_STATUS_VSERVO] = corrected;
+			}
+		}
+#endif
+#ifdef ADC_RSSI
+		/* PX4IO_P_STATUS_VRSSI */
+		{
+			unsigned counts = adc_measure(ADC_RSSI);
+			if (counts != 0xffff) {
+				// use 1:1 scaling on 3.3V ADC input
+				unsigned mV = counts * 3300 / 4096;
+				r_page_status[PX4IO_P_STATUS_VRSSI] = mV;
 			}
 		}
 #endif
-		/* XXX PX4IO_P_STATUS_VRSSI */
 		/* XXX PX4IO_P_STATUS_PRSSI */
 
 		SELECT_PAGE(r_page_status);

src/modules/systemlib/perf_counter.c

@@ -321,6 +321,32 @@ perf_print_counter(perf_counter_t handle)
 	}
 }
 
+uint64_t
+perf_event_count(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return 0;
+
+	switch (handle->type) {
+	case PC_COUNT:
+		return ((struct perf_ctr_count *)handle)->event_count;
+
+	case PC_ELAPSED: {
+		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+		return pce->event_count;
+	}
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		return pci->event_count;
+	}
+
+	default:
+		break;
+	}
+	return 0;
+}
+
 void
 perf_print_all(void)
 {

src/modules/systemlib/perf_counter.h

@@ -135,6 +135,14 @@ __EXPORT extern void		perf_print_all(void);
  */
 __EXPORT extern void		perf_reset_all(void);
 
+/**
+ * Return current event_count
+ *
+ * @param handle		The counter returned from perf_alloc.
+ * @return			event_count
+ */
+__EXPORT extern uint64_t	perf_event_count(perf_counter_t handle);
+
 __END_DECLS
 
 #endif

src/modules/uORB/objects_common.cpp

@@ -87,6 +87,9 @@ ORB_DEFINE(safety, struct safety_s);
 #include "topics/battery_status.h"
 ORB_DEFINE(battery_status, struct battery_status_s);
 
+#include "topics/servorail_status.h"
+ORB_DEFINE(servorail_status, struct servorail_status_s);
+
 #include "topics/vehicle_global_position.h"
 ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
 

src/modules/uORB/topics/differential_pressure.h

@@ -53,6 +53,7 @@
  */
 struct differential_pressure_s {
 	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
+	uint64_t	error_count;
 	uint16_t	differential_pressure_pa;	/**< Differential pressure reading */
 	uint16_t	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
 	float		voltage;			/**< Voltage from analog airspeed sensors (voltage divider already compensated) */

src/modules/uORB/topics/servorail_status.h

@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   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 servorail_status.h
+ *
+ * Definition of the servorail status uORB topic.
+ */
+
+#ifndef SERVORAIL_STATUS_H_
+#define SERVORAIL_STATUS_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Servorail voltages and status
+ */
+struct servorail_status_s {
+	uint64_t	timestamp;		/**< microseconds since system boot */
+	float   	voltage_v;		/**< Servo rail voltage in volts */
+	float   	rssi_v;			/**< RSSI pin voltage in volts */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(servorail_status);
+
+#endif