U-blox driver rework, step 2

Moved satellite info from vehicle_gps_position_s into a new uORB topic
satellite_info.
Renamed satellites_visible to satellites_used to reflect true content.
sdlog2 will log info for GPS satellites only for now.

src/drivers/blinkm/blinkm.cpp

@@ -559,13 +559,7 @@ BlinkM::led()
 		}
 
 		/* get number of used satellites in navigation */
-		num_of_used_sats = 0;
-
-		for(unsigned satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
-			if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
-				num_of_used_sats++;
-			}
-		}
+		num_of_used_sats = vehicle_gps_position_raw.satellites_used;
 
 		if (new_data_vehicle_status || no_data_vehicle_status < 3) {
 			if (num_of_cells == 0) {

src/drivers/gps/gps.cpp

@@ -449,7 +449,7 @@ GPS::print_info()
 
 	if (_report.timestamp_position != 0) {
 		warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
-				_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
+				_report.satellites_used, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
 		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
 		warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m);
 		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());

src/drivers/gps/mtk.cpp

@@ -263,7 +263,7 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 	_gps_position->epv_m = _gps_position->eph_m; // unknown in mtk custom mode, so we cheat with eph
 	_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
 	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
-	_gps_position->satellites_visible = packet.satellites;
+	_gps_position->satellites_used = packet.satellites;
 
 	/* convert time and date information to unix timestamp */
 	struct tm timeinfo; //TODO: test this conversion

src/drivers/gps/ubx.cpp

@@ -55,6 +55,7 @@
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
 #include <drivers/drv_hrt.h>
 
 #include "ubx.h"
@@ -451,7 +452,7 @@ UBX::handle_message()
 					_gps_position->s_variance_m_s = packet->sAcc;
 					_gps_position->p_variance_m = packet->pAcc;
 					_gps_position->timestamp_variance = hrt_absolute_time();
-					_gps_position->satellites_visible = packet->numSV;
+					_gps_position->satellites_used = packet->numSV;
 
 					ret = 1;
 					break;

src/drivers/hott/messages.cpp

@@ -226,7 +226,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 	msg.sensor_id = GPS_SENSOR_ID;
 	msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
 
-	msg.gps_num_sat = gps.satellites_visible;
+	msg.gps_num_sat = gps.satellites_used;
 
 	/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
 	msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);

src/modules/mavlink/mavlink_messages.cpp

@@ -548,7 +548,7 @@ protected:
 						     cm_uint16_from_m_float(gps->epv_m),
 						     gps->vel_m_s * 100.0f,
 						     _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
-						     gps->satellites_visible);
+						     gps->satellites_used);
 		}
 	}
 };

src/modules/mavlink/mavlink_receiver.cpp

@@ -682,9 +682,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	hil_gps.vel_ned_valid = true;
 	hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
 
-	hil_gps.timestamp_satellites = timestamp;
 	hil_gps.fix_type = gps.fix_type;
-	hil_gps.satellites_visible = gps.satellites_visible;
+	hil_gps.satellites_used = gps.satellites_visible;  //TODO: rename mavlink_hil_gps_t sats visible to used?
 
 	if (_gps_pub < 0) {
 		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);

src/modules/sdlog2/sdlog2.c

@@ -74,6 +74,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -139,6 +140,8 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1);
 	fds[fdsc_count].events = POLLIN; \
 	fdsc_count++;
 
+#define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
+
 static bool main_thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;			/**< Deamon status flag */
 static int deamon_task;						/**< Handle of deamon task / thread */
@@ -941,6 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct estimator_status_report estimator_status;
 		struct system_power_s system_power;
 		struct servorail_status_s servorail_status;
+		struct satellite_info_s sat_info;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -1000,6 +1004,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int global_pos_sub;
 		int triplet_sub;
 		int gps_pos_sub;
+		int sat_info_sub;
 		int vicon_pos_sub;
 		int flow_sub;
 		int rc_sub;
@@ -1017,6 +1022,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
 	subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
 	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
@@ -1053,6 +1059,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 	hrt_abstime barometer_timestamp = 0;
 	hrt_abstime differential_pressure_timestamp = 0;
 
+	/* initialize calculated mean SNR */
+	float snr_mean = 0.0f;
+
 	/* enable logging on start if needed */
 	if (log_on_start) {
 		/* check GPS topic to get GPS time */
@@ -1115,14 +1124,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- GPS POSITION - UNIT #1 --- */
 		if (gps_pos_updated) {
 
-			float snr_mean = 0.0f;
-
-			for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) {
-				snr_mean += buf_gps_pos.satellite_snr[i];
-			}
-
-			snr_mean /= buf_gps_pos.satellites_visible;
-
 			log_msg.msg_type = LOG_GPS_MSG;
 			log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
 			log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
@@ -1135,44 +1136,55 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s;
 			log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s;
 			log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad;
-			log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible;
+			log_msg.body.log_GPS.sats = buf_gps_pos.satellites_used;
 			log_msg.body.log_GPS.snr_mean = snr_mean;
 			log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms;
 			log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator;
 			LOGBUFFER_WRITE_AND_COUNT(GPS);
+		}
+
+		/* --- SATELLITE INFO - UNIT #1 --- */
+		if (_extended_logging) {
+
+			if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) {
 
-			if (_extended_logging) {
 				/* log the SNR of each satellite for a detailed view of signal quality */
-				unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]);
+				unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0]));
 				unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]);
 
 				log_msg.msg_type = LOG_GS0A_MSG;
 				memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A));
-				/* fill set A */
-				for (unsigned i = 0; i < gps_msg_max_snr; i++) {
+				snr_mean = 0.0f;
 
-					int satindex = buf_gps_pos.satellite_prn[i] - 1;
+				/* fill set A and calculate mean SNR */
+				for (unsigned i = 0; i < sat_info_count; i++) {
+
+					snr_mean += buf.sat_info.snr[i];
+
+					int satindex = buf.sat_info.svid[i] - 1;
 
 					/* handles index exceeding and wraps to to arithmetic errors */
 					if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
 						/* map satellites by their ID so that logs from two receivers can be compared */
-						log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
+						log_msg.body.log_GS0A.satellite_snr[satindex] = buf.sat_info.snr[i];
 					}
 				}
 				LOGBUFFER_WRITE_AND_COUNT(GS0A);
+				snr_mean /= sat_info_count;
 
 				log_msg.msg_type = LOG_GS0B_MSG;
 				memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B));
+
 				/* fill set B */
-				for (unsigned i = 0; i < gps_msg_max_snr; i++) {
+				for (unsigned i = 0; i < sat_info_count; i++) {
 
 					/* get second bank of satellites, thus deduct bank size from index */
-					int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr;
+					int satindex = buf.sat_info.svid[i] - 1 - log_max_snr;
 
 					/* handles index exceeding and wraps to to arithmetic errors */
 					if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
 						/* map satellites by their ID so that logs from two receivers can be compared */
-						log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
+						log_msg.body.log_GS0B.satellite_snr[satindex] = buf.sat_info.snr[i];
 					}
 				}
 				LOGBUFFER_WRITE_AND_COUNT(GS0B);

src/modules/uORB/Subscription.cpp

@@ -40,6 +40,7 @@
 #include "topics/parameter_update.h"
 #include "topics/actuator_controls.h"
 #include "topics/vehicle_gps_position.h"
+#include "topics/satellite_info.h"
 #include "topics/sensor_combined.h"
 #include "topics/vehicle_attitude.h"
 #include "topics/vehicle_global_position.h"
@@ -88,6 +89,7 @@ T Subscription<T>::getData() {
 template class __EXPORT Subscription<parameter_update_s>;
 template class __EXPORT Subscription<actuator_controls_s>;
 template class __EXPORT Subscription<vehicle_gps_position_s>;
+template class __EXPORT Subscription<satellite_info_s>;
 template class __EXPORT Subscription<sensor_combined_s>;
 template class __EXPORT Subscription<vehicle_attitude_s>;
 template class __EXPORT Subscription<vehicle_global_position_s>;

src/modules/uORB/objects_common.cpp

@@ -75,6 +75,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s);
 #include "topics/vehicle_gps_position.h"
 ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
 
+#include "topics/satellite_info.h"
+ORB_DEFINE(satellite_info, struct satellite_info_s);
+
 #include "topics/home_position.h"
 ORB_DEFINE(home_position, struct home_position_s);
 

src/modules/uORB/topics/satellite_info.h

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 satellite_info.h
+ * Definition of the GNSS satellite info uORB topic.
+ */
+
+#ifndef TOPIC_SAT_INFO_H_
+#define TOPIC_SAT_INFO_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * GNSS Satellite Info.
+ */
+struct satellite_info_s {
+	uint64_t timestamp;	/**< Timestamp of satellite information */
+	uint8_t count;		/**< Number of satellites in satellite_...[] arrays */
+	uint8_t svid[20]; 	/**< Space vehicle ID [1..255], see scheme below  */
+	uint8_t used[20];	/**< 0: Satellite not used, 1: used for navigation */
+	uint8_t elevation[20];	/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
+	uint8_t azimuth[20];	/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
+	uint8_t snr[20];	/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
+};
+
+/**
+ * NAV_SVINFO space vehicle ID (svid) scheme according to u-blox protocol specs
+ * u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_(UBX-13003221).pdf
+ *
+ * GPS		1-32
+ * SBAS		120-158
+ * Galileo	211-246
+ * BeiDou	159-163, 33-64
+ * QZSS		193-197
+ * GLONASS	65-96, 255
+ *
+ */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(satellite_info);
+
+#endif

src/modules/uORB/topics/vehicle_gps_position.h

@@ -82,14 +82,7 @@ struct vehicle_gps_position_s {
 	uint64_t timestamp_time;			/**< Timestamp for time information */
 	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module   */
 
-	uint64_t timestamp_satellites;			/**< Timestamp for sattelite information */
-	uint8_t satellites_visible;			/**< Number of satellites visible. If unknown, set to 255 */
-	uint8_t satellite_prn[20]; 			/**< Global satellite ID */
-	uint8_t satellite_used[20];			/**< 0: Satellite not used, 1: used for localization */
-	uint8_t satellite_elevation[20]; 		/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
-	uint8_t satellite_azimuth[20];			/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
-	uint8_t satellite_snr[20];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
-	bool satellite_info_available;			/**< 0 for no info, 1 for info available */
+	uint8_t satellites_used;			/**< Number of satellites used */
 };
 
 /**