Merge branch 'airspeed_scaling' of github.com:PX4/Firmware into airspeed_scaling

6 files changed, 59 insertions, 54 deletions

diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index cb5fd53a5..147d7123a 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -147,6 +147,7 @@ enum {
 	TONE_BATTERY_WARNING_SLOW_TUNE,
 	TONE_BATTERY_WARNING_FAST_TUNE,
 	TONE_GPS_WARNING_TUNE,
+	TONE_ARMING_FAILURE_TUNE,
 	TONE_NUMBER_OF_TUNES
 };
 
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 4c85c0cda..cb97354ec 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -169,6 +169,8 @@ private:
 
 	int			_bus;			/**< the bus the device is connected to */
 
+	struct mag_report	_last_report;           /**< used for info() */
+
 	/**
 	 * Test whether the device supported by the driver is present at a
 	 * specific address.
@@ -870,6 +872,8 @@ HMC5883::collect()
 		}
 	}
 
+	_last_report = new_report;
+
 	/* post a report to the ring */
 	if (_reports->force(&new_report)) {
 		perf_count(_buffer_overflows);
@@ -1042,31 +1046,28 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 
 	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
 
-	/* set back to normal mode */
-	/* Set to 1.1 Gauss */
-	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
-		warnx("failed to set 1.1 Ga range");
-		goto out;
-	}
-
-	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
-		warnx("failed to disable sensor calibration mode");
-		goto out;
-	}
-
 	/* set scaling in device */
 	mscale_previous.x_scale = scaling[0];
 	mscale_previous.y_scale = scaling[1];
 	mscale_previous.z_scale = scaling[2];
 
+	ret = OK;
+
+out:
+
 	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
 		warn("WARNING: failed to set new scale / offsets for mag");
-		goto out;
 	}
 
-	ret = OK;
+	/* set back to normal mode */
+	/* Set to 1.1 Gauss */
+	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
+		warnx("failed to set 1.1 Ga range");
+	}
 
-out:
+	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
+		warnx("failed to disable sensor calibration mode");
+	}
 
 	if (ret == OK) {
 		if (!check_scale()) {
@@ -1221,6 +1222,7 @@ HMC5883::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("output  (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z);
 	printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
 	printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", 
 	       (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index 07163035b..a0cf98340 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -124,6 +124,8 @@ private:
 
 	orb_advert_t			_range_finder_topic;
 
+	unsigned			_consecutive_fail_count;
+
 	perf_counter_t			_sample_perf;
 	perf_counter_t			_comms_errors;
 	perf_counter_t			_buffer_overflows;
@@ -186,6 +188,7 @@ SF0X::SF0X(const char *port) :
 	_linebuf_index(0),
 	_last_read(0),
 	_range_finder_topic(-1),
+	_consecutive_fail_count(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows"))
@@ -720,12 +723,17 @@ SF0X::cycle()
 		if (OK != collect_ret) {
 
 			/* we know the sensor needs about four seconds to initialize */
-			if (hrt_absolute_time() > 5 * 1000 * 1000LL) {
-				log("collection error");
+			if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
+				log("collection error #%u", _consecutive_fail_count);
 			}
+			_consecutive_fail_count++;
+
 			/* restart the measurement state machine */
 			start();
 			return;
+		} else {
+			/* apparently success */
+			_consecutive_fail_count = 0;
 		}
 
 		/* next phase is measurement */
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index f36f2091e..8ed0de58c 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -334,6 +334,7 @@ ToneAlarm::ToneAlarm() :
 	_default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8";					//battery warning slow
 	_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8";	//battery warning fast
 	_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#";					//gps warning slow
+	_default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP";
 
 	_tune_names[TONE_STARTUP_TUNE] = "startup";			// startup tune
 	_tune_names[TONE_ERROR_TUNE] = "error";				// ERROR tone
@@ -344,6 +345,7 @@ ToneAlarm::ToneAlarm() :
 	_tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat";	// battery warning slow
 	_tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat";	// battery warning fast
 	_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning";	            // gps warning
+	_tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure";            //fail to arm
 }
 
 ToneAlarm::~ToneAlarm()
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 9f2fca446..d2cf6d662 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1248,7 +1248,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			/* copy servo rail status topic here too */
 			orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status);
 			log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v;
-			log_msg.body.log_PWR.servo_rssi_5v = buf.servorail_status.rssi_v;
+			log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v;
 
 			LOGBUFFER_WRITE_AND_COUNT(PWR);
 		}
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index d629259c0..354bb08e8 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -277,6 +277,29 @@ struct log_TELE_s {
 	uint8_t txbuf;
 };
 
+/* --- ESTM - ESTIMATOR STATUS --- */
+#define LOG_ESTM_MSG 23
+struct log_ESTM_s {
+	float s[10];
+	uint8_t n_states;
+	uint8_t states_nan;
+	uint8_t covariance_nan;
+	uint8_t kalman_gain_nan;
+};
+
+/* --- PWR - ONBOARD POWER SYSTEM --- */
+#define LOG_PWR_MSG 24
+struct log_PWR_s {
+	float peripherals_5v;
+	float servo_rail_5v;
+	float servo_rssi;
+	uint8_t usb_ok;
+	uint8_t brick_ok;
+	uint8_t servo_ok;
+	uint8_t low_power_rail_overcurrent;
+	uint8_t high_power_rail_overcurrent;
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -299,36 +322,6 @@ struct log_PARM_s {
 	float value;
 };
 
-/* --- ESTM - ESTIMATOR STATUS --- */
-#define LOG_ESTM_MSG 132
-struct log_ESTM_s {
-	float s[10];
-	uint8_t n_states;
-	uint8_t states_nan;
-	uint8_t covariance_nan;
-	uint8_t kalman_gain_nan;
-};
-// struct log_ESTM_s {
-// 	float s[32];
-// 	uint8_t n_states;
-// 	uint8_t states_nan;
-// 	uint8_t covariance_nan;
-// 	uint8_t kalman_gain_nan;
-// };
-
-/* --- PWR - ONBOARD POWER SYSTEM --- */
-#define LOG_PWR_MSG 133
-struct log_PWR_s {
-	float peripherals_5v;
-	float servo_rail_5v;
-	float servo_rssi_5v;
-	uint8_t usb_ok;
-	uint8_t brick_ok;
-	uint8_t servo_ok;
-	uint8_t low_power_rail_overcurrent;
-	uint8_t high_power_rail_overcurrent;
-};
-
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -355,17 +348,16 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
 	LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
 	LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
+	LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
+	LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
 
 	/* system-level messages, ID >= 0x80 */
-	// FMT: don't write format of format message, it's useless
+	/* FMT: don't write format of format message, it's useless */
 	LOG_FORMAT(TIME, "Q", "StartTime"),
 	LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
-	LOG_FORMAT(PARM, "Nf", "Name,Value"),
-	LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
-	LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
-	//LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"),
+	LOG_FORMAT(PARM, "Nf", "Name,Value")
 };
 
-static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
+static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]);
 
 #endif /* SDLOG2_MESSAGES_H_ */