Merge remote-tracking branch 'upstream/master' into hkmicropcb

ROMFS/px4fmu_common/init.d/10015_tbs_discovery

@@ -20,7 +20,7 @@ then
 	param set MC_PITCHRATE_D 0.0025
 	param set MC_YAW_P 2.8
 	param set MC_YAWRATE_P 0.28
-	param set MC_YAWRATE_I 0.0
+	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
 fi
 

ROMFS/px4fmu_common/init.d/4010_dji_f330

@@ -19,7 +19,7 @@ then
 	param set MC_PITCHRATE_D 0.003
 	param set MC_YAW_P 2.8
 	param set MC_YAWRATE_P 0.2
-	param set MC_YAWRATE_I 0.0
+	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
 fi
 

ROMFS/px4fmu_common/init.d/4011_dji_f450

@@ -20,7 +20,7 @@ then
 	param set MC_PITCHRATE_D 0.003
 	param set MC_YAW_P 2.8
 	param set MC_YAWRATE_P 0.2
-	param set MC_YAWRATE_I 0.0
+	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
 fi
 

ROMFS/px4fmu_common/init.d/rc.mc_defaults

@@ -14,7 +14,7 @@ then
 	param set MC_PITCHRATE_D 0.003
 	param set MC_YAW_P 2.8
 	param set MC_YAWRATE_P 0.2
-	param set MC_YAWRATE_I 0.0
+	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
 	param set MC_YAW_FF 0.5
 

ROMFS/px4fmu_common/init.d/rcS

@@ -132,6 +132,10 @@ then
 	#
 	if param compare SYS_AUTOCONFIG 1
 	then
+		# We can't be sure the defaults haven't changed, so
+		# if someone requests a re-configuration, we do it
+		# cleanly from scratch (except autostart / autoconfig)
+		param reset_nostart
 		set DO_AUTOCONFIG yes
 	else
 		set DO_AUTOCONFIG no

Tools/px_generate_xml.sh

@@ -0,0 +1,2 @@
+#!/bin/sh
+python px_process_params.py --xml

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

@@ -63,6 +63,7 @@
 #include <drivers/drv_gps.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
 
 #include <board_config.h>
 
@@ -79,10 +80,18 @@
 #endif
 static const int ERROR = -1;
 
+/* class for dynamic allocation of satellite info data */
+class GPS_Sat_Info
+{
+public:
+	struct satellite_info_s 	_data;
+};
+
+
 class GPS : public device::CDev
 {
 public:
-	GPS(const char *uart_path, bool fake_gps);
+	GPS(const char *uart_path, bool fake_gps, bool enable_sat_info);
 	virtual ~GPS();
 
 	virtual int			init();
@@ -100,14 +109,17 @@ private:
 	int				_serial_fd;					///< serial interface to GPS
 	unsigned			_baudrate;					///< current baudrate
 	char				_port[20];					///< device / serial port path
-	volatile int			_task;						//< worker task
+	volatile int			_task;						///< worker task
 	bool				_healthy;					///< flag to signal if the GPS is ok
-	bool 				_baudrate_changed;				///< flag to signal that the baudrate with the GPS has changed
+	bool				_baudrate_changed;				///< flag to signal that the baudrate with the GPS has changed
 	bool				_mode_changed;					///< flag that the GPS mode has changed
 	gps_driver_mode_t		_mode;						///< current mode
 	GPS_Helper			*_Helper;					///< instance of GPS parser
-	struct vehicle_gps_position_s 	_report;					///< uORB topic for gps position
-	orb_advert_t			_report_pub;					///< uORB pub for gps position
+	GPS_Sat_Info			*_Sat_Info;					///< instance of GPS sat info data object
+	struct vehicle_gps_position_s	_report_gps_pos;				///< uORB topic for gps position
+	orb_advert_t			_report_gps_pos_pub;				///< uORB pub for gps position
+	struct satellite_info_s		*_p_report_sat_info;				///< pointer to uORB topic for satellite info
+	orb_advert_t			_report_sat_info_pub;				///< uORB pub for satellite info
 	float				_rate;						///< position update rate
 	bool				_fake_gps;					///< fake gps output
 
@@ -154,14 +166,17 @@ GPS	*g_dev;
 }
 
 
-GPS::GPS(const char *uart_path, bool fake_gps) :
+GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) :
 	CDev("gps", GPS_DEVICE_PATH),
 	_task_should_exit(false),
 	_healthy(false),
 	_mode_changed(false),
 	_mode(GPS_DRIVER_MODE_UBX),
 	_Helper(nullptr),
-	_report_pub(-1),
+	_Sat_Info(nullptr),
+	_report_gps_pos_pub(-1),
+	_p_report_sat_info(nullptr),
+	_report_sat_info_pub(-1),
 	_rate(0.0f),
 	_fake_gps(fake_gps)
 {
@@ -172,7 +187,14 @@ GPS::GPS(const char *uart_path, bool fake_gps) :
 
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
-	memset(&_report, 0, sizeof(_report));
+	memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
+
+	/* create satellite info data object if requested */
+	if (enable_sat_info) {
+		_Sat_Info = new(GPS_Sat_Info);
+		_p_report_sat_info = &_Sat_Info->_data;
+		memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
+	}
 
 	_debug_enabled = true;
 }
@@ -207,7 +229,7 @@ GPS::init()
 
 	/* start the GPS driver worker task */
 	_task = task_spawn_cmd("gps", SCHED_DEFAULT,
-				SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr);
+				SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr);
 
 	if (_task < 0) {
 		warnx("task start failed: %d", errno);
@@ -271,33 +293,33 @@ GPS::task_main()
 
 		if (_fake_gps) {
 
-			_report.timestamp_position = hrt_absolute_time();
-			_report.lat = (int32_t)47.378301e7f;
-			_report.lon = (int32_t)8.538777e7f;
-			_report.alt = (int32_t)1200e3f;
-			_report.timestamp_variance = hrt_absolute_time();
-			_report.s_variance_m_s = 10.0f;
-			_report.p_variance_m = 10.0f;
-			_report.c_variance_rad = 0.1f;
-			_report.fix_type = 3;
-			_report.eph = 0.9f;
-			_report.epv = 1.8f;
-			_report.timestamp_velocity = hrt_absolute_time();
-			_report.vel_n_m_s = 0.0f;
-			_report.vel_e_m_s = 0.0f;
-			_report.vel_d_m_s = 0.0f;
-			_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
-			_report.cog_rad = 0.0f;
-			_report.vel_ned_valid = true;
+			_report_gps_pos.timestamp_position = hrt_absolute_time();
+			_report_gps_pos.lat = (int32_t)47.378301e7f;
+			_report_gps_pos.lon = (int32_t)8.538777e7f;
+			_report_gps_pos.alt = (int32_t)1200e3f;
+			_report_gps_pos.timestamp_variance = hrt_absolute_time();
+			_report_gps_pos.s_variance_m_s = 10.0f;
+			_report_gps_pos.p_variance_m = 10.0f;
+			_report_gps_pos.c_variance_rad = 0.1f;
+			_report_gps_pos.fix_type = 3;
+			_report_gps_pos.eph = 0.9f;
+			_report_gps_pos.epv = 1.8f;
+			_report_gps_pos.timestamp_velocity = hrt_absolute_time();
+			_report_gps_pos.vel_n_m_s = 0.0f;
+			_report_gps_pos.vel_e_m_s = 0.0f;
+			_report_gps_pos.vel_d_m_s = 0.0f;
+			_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
+			_report_gps_pos.cog_rad = 0.0f;
+			_report_gps_pos.vel_ned_valid = true;
 
 			//no time and satellite information simulated
 
 			if (!(_pub_blocked)) {
-				if (_report_pub > 0) {
-					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+				if (_report_gps_pos_pub > 0) {
+					orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
 
 				} else {
-					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+					_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
 				}
 			}
 
@@ -313,11 +335,11 @@ GPS::task_main()
 
 			switch (_mode) {
 			case GPS_DRIVER_MODE_UBX:
-				_Helper = new UBX(_serial_fd, &_report);
+				_Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info);
 				break;
 
 			case GPS_DRIVER_MODE_MTK:
-				_Helper = new MTK(_serial_fd, &_report);
+				_Helper = new MTK(_serial_fd, &_report_gps_pos);
 				break;
 
 			default:
@@ -332,20 +354,33 @@ GPS::task_main()
 				// GPS is obviously detected successfully, reset statistics
 				_Helper->reset_update_rates();
 
-				while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
+				int helper_ret;
+				while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
 	//				lock();
 					/* opportunistic publishing - else invalid data would end up on the bus */
 
 					if (!(_pub_blocked)) {
-						if (_report_pub > 0) {
-							orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+						if (helper_ret & 1) {
+							if (_report_gps_pos_pub > 0) {
+								orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
 
-						} else {
-							_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+							} else {
+								_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
+							}
+						}
+						if (_p_report_sat_info && (helper_ret & 2)) {
+							if (_report_sat_info_pub > 0) {
+								orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
+
+							} else {
+								_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
+							}
 						}
 					}
 
-					last_rate_count++;
+					if (helper_ret & 1) {	// consider only pos info updates for rate calculation */
+						last_rate_count++;
+					}
 
 					/* measure update rate every 5 seconds */
 					if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
@@ -446,12 +481,13 @@ GPS::print_info()
 	}
 
 	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
+	warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled");
 
-	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.0d);
-		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
-		warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv);
+	if (_report_gps_pos.timestamp_position != 0) {
+		warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
+				_report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
+		warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
+		warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv);
 		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
 		warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
 		warnx("rate publication:\t%6.2f Hz", (double)_rate);
@@ -469,7 +505,7 @@ namespace gps
 
 GPS	*g_dev;
 
-void	start(const char *uart_path, bool fake_gps);
+void	start(const char *uart_path, bool fake_gps, bool enable_sat_info);
 void	stop();
 void	test();
 void	reset();
@@ -479,7 +515,7 @@ void	info();
  * Start the driver.
  */
 void
-start(const char *uart_path, bool fake_gps)
+start(const char *uart_path, bool fake_gps, bool enable_sat_info)
 {
 	int fd;
 
@@ -487,7 +523,7 @@ start(const char *uart_path, bool fake_gps)
 		errx(1, "already started");
 
 	/* create the driver */
-	g_dev = new GPS(uart_path, fake_gps);
+	g_dev = new GPS(uart_path, fake_gps, enable_sat_info);
 
 	if (g_dev == nullptr)
 		goto fail;
@@ -580,6 +616,7 @@ gps_main(int argc, char *argv[])
 	/* set to default */
 	const char *device_name = GPS_DEFAULT_UART_PORT;
 	bool fake_gps = false;
+	bool enable_sat_info = false;
 
 	/*
 	 * Start/load the driver.
@@ -601,7 +638,13 @@ gps_main(int argc, char *argv[])
 				fake_gps = true;
 		}
 
-		gps::start(device_name, fake_gps);
+		/* Detect sat info option */
+		for (int i = 2; i < argc; i++) {
+			if (!strcmp(argv[i], "-s"))
+				enable_sat_info = true;
+		}
+
+		gps::start(device_name, fake_gps, enable_sat_info);
 	}
 
 	if (!strcmp(argv[1], "stop"))
@@ -626,5 +669,5 @@ gps_main(int argc, char *argv[])
 		gps::info();
 
 out:
-	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]");
+	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]");
 }

src/drivers/gps/gps_helper.h

@@ -62,8 +62,8 @@ protected:
 	uint8_t _rate_count_lat_lon;
 	uint8_t _rate_count_vel;
 
-	float _rate_lat_lon;
-	float _rate_vel;
+	float _rate_lat_lon = 0.0f;
+	float _rate_vel = 0.0f;
 
 	uint64_t _interval_rate_start;
 };

src/drivers/gps/mtk.cpp

@@ -263,7 +263,7 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 	_gps_position->epv = _gps_position->eph; // 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/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/lib/external_lgpl/tecs/tecs.cpp

@@ -310,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f);
 
 		if (STEdot_dem >= 0) {
-			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
+			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (_THRmaxf - nomThr);
 
 		} else {
 			ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;

src/modules/commander/state_machine_helper.cpp

@@ -182,7 +182,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 						if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) &&
 							(status->avionics_power_rail_voltage < 4.9f)) {
 
-							mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage);
+							mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
 							valid_transition = false;
 						}
 					}
@@ -638,7 +638,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 	}
 
 	if (!status->is_rotary_wing) {
-		int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+		fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
 
 		if (fd < 0) {
 			mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");

src/modules/mavlink/mavlink_messages.cpp

@@ -677,7 +677,7 @@ protected:
 						     cm_uint16_from_m_float(gps.epv),
 						     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

@@ -751,9 +751,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/position_estimator_inav/position_estimator_inav_main.c

@@ -915,6 +915,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				memcpy(x_est_prev, x_est, sizeof(x_est));
 				memcpy(y_est_prev, y_est, sizeof(y_est));
 			}
+		} else {
+			/* gradually reset xy velocity estimates */
+			inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
+			inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
 		}
 
 		/* detect land */
@@ -930,6 +934,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				landed = false;
 				landed_time = 0;
 			}
+			/* reset xy velocity estimates when landed */
+			x_est[1] = 0.0f;
+			y_est[1] = 0.0f;
 
 		} else {
 			if (alt_disp2 < land_disp2 && thrust < params.land_thr) {

src/modules/position_estimator_inav/position_estimator_inav_params.c

@@ -46,6 +46,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
 PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
 PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
@@ -65,6 +66,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
 	h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
 	h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
 	h->w_xy_flow = param_find("INAV_W_XY_FLOW");
+	h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
 	h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
 	h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
 	h->flow_k = param_find("INAV_FLOW_K");
@@ -87,6 +89,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 	param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
 	param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
 	param_get(h->w_xy_flow, &(p->w_xy_flow));
+	param_get(h->w_xy_res_v, &(p->w_xy_res_v));
 	param_get(h->w_gps_flow, &(p->w_gps_flow));
 	param_get(h->w_acc_bias, &(p->w_acc_bias));
 	param_get(h->flow_k, &(p->flow_k));

src/modules/position_estimator_inav/position_estimator_inav_params.h

@@ -47,6 +47,7 @@ struct position_estimator_inav_params {
 	float w_xy_gps_p;
 	float w_xy_gps_v;
 	float w_xy_flow;
+	float w_xy_res_v;
 	float w_gps_flow;
 	float w_acc_bias;
 	float flow_k;
@@ -66,6 +67,7 @@ struct position_estimator_inav_param_handles {
 	param_t w_xy_gps_p;
 	param_t w_xy_gps_v;
 	param_t w_xy_flow;
+	param_t w_xy_res_v;
 	param_t w_gps_flow;
 	param_t w_acc_bias;
 	param_t flow_k;

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>
@@ -141,6 +142,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 */
@@ -944,6 +947,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct tecs_status_s tecs_status;
 		struct system_power_s system_power;
 		struct servorail_status_s servorail_status;
+		struct satellite_info_s sat_info;
 		struct wind_estimate_s wind_estimate;
 	} buf;
 
@@ -1007,6 +1011,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;
@@ -1026,6 +1031,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));
@@ -1066,11 +1072,14 @@ 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 */
 		if (log_name_timestamp) {
-			if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
+			if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
 				gps_time = buf_gps_pos.time_gps_usec;
 			}
 		}
@@ -1128,14 +1137,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;
@@ -1148,44 +1149,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/systemlib/circuit_breaker.c

@@ -86,7 +86,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
 bool circuit_breaker_enabled(const char* breaker, int32_t magic)
 {
 	int32_t val;
-	(void)param_get(param_find(breaker), val);
+	(void)param_get(param_find(breaker), &val);
 
 	return (val == magic);
 }

src/modules/systemlib/circuit_breaker.h

@@ -40,6 +40,15 @@
 #ifndef CIRCUIT_BREAKER_H_
 #define CIRCUIT_BREAKER_H_
 
+/* SAFETY WARNING  --  SAFETY WARNING  --  SAFETY WARNING
+ *
+ * OBEY THE DOCUMENTATION FOR ALL CIRCUIT BREAKERS HERE,
+ * ENSURE TO READ CAREFULLY ALL SAFETY WARNINGS.
+ * http://pixhawk.org/dev/circuit_breakers
+ *
+ * CIRCUIT BREAKERS ARE NOT PART OF THE STANDARD OPERATION PROCEDURE
+ * AND MAY DISABLE CHECKS THAT ARE VITAL FOR SAFE FLIGHT.
+ */
 #define CBRK_SUPPLY_CHK_KEY	894281
 #define CBRK_RATE_CTRL_KEY	140253
 #define CBRK_IO_SAFETY_KEY	22027

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,89 @@
+/****************************************************************************
+ *
+ *   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.
+ */
+
+#define SAT_INFO_MAX_SATELLITES  20
+
+struct satellite_info_s {
+	uint64_t timestamp;				/**< Timestamp of satellite info */
+	uint8_t count;					/**< Number of satellites in satellite info */
+	uint8_t svid[SAT_INFO_MAX_SATELLITES]; 		/**< Space vehicle ID [1..255], see scheme below  */
+	uint8_t used[SAT_INFO_MAX_SATELLITES];		/**< 0: Satellite not used, 1: used for navigation */
+	uint8_t elevation[SAT_INFO_MAX_SATELLITES];	/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
+	uint8_t azimuth[SAT_INFO_MAX_SATELLITES];	/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
+	uint8_t snr[SAT_INFO_MAX_SATELLITES];		/**< 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 */
 };
 
 /**

src/systemcmds/param/param.c

@@ -64,6 +64,7 @@ static void	do_show_print(void *arg, param_t param);
 static void	do_set(const char* name, const char* val, bool fail_on_not_found);
 static void	do_compare(const char* name, const char* vals[], unsigned comparisons);
 static void	do_reset(void);
+static void	do_reset_nostart(void);
 
 int
 param_main(int argc, char *argv[])
@@ -142,6 +143,10 @@ param_main(int argc, char *argv[])
 		if (!strcmp(argv[1], "reset")) {
 			do_reset();
 		}
+
+		if (!strcmp(argv[1], "reset_nostart")) {
+			do_reset_nostart();
+		}
 	}
 	
 	errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'");
@@ -427,3 +432,26 @@ do_reset(void)
 		exit(0);
 	}
 }
+
+static void
+do_reset_nostart(void)
+{
+
+	int32_t autostart;
+	int32_t autoconfig;
+
+	(void)param_get(param_find("SYS_AUTOSTART"), &autostart);
+	(void)param_get(param_find("SYS_AUTOCONFIG"), &autoconfig);
+
+	param_reset_all();
+
+	(void)param_set(param_find("SYS_AUTOSTART"), &autostart);
+	(void)param_set(param_find("SYS_AUTOCONFIG"), &autoconfig);
+
+	if (param_save_default()) {
+		warnx("Param export failed.");
+		exit(1);
+	} else {
+		exit(0);
+	}
+}