Merge branch 'master' into rc_timeout

src/modules/commander/airspeed_calibration.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 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
@@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
 {
 	/* give directions */
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
-	mavlink_log_info(mavlink_fd, "don't move system");
+	mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
 
-	const int calibration_count = 2500;
+	const int calibration_count = 2000;
 
 	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	struct differential_pressure_s diff_pres;
@@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd)
 	if (fd > 0) {
 		if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
 			paramreset_successful = true;
+		} else {
+			mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
 		}
 		close(fd);
 	}
 
 	if (!paramreset_successful) {
-		warn("WARNING: failed to set scale / offsets for airspeed sensor");
-		mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
+		warn("FAILED to set scale / offsets for airspeed");
+		mavlink_log_critical(mavlink_fd, "dpress reset failed");
 		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 		return ERROR;
 	}
@@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (poll_ret) {
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
-			diff_pres_offset += diff_pres.differential_pressure_pa;
+			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
 			calibration_counter++;
 
 			if (calibration_counter % (calibration_count / 20) == 0)

src/modules/navigator/navigator_main.cpp

@@ -1503,7 +1503,7 @@ Navigator::check_mission_item_reached()
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
 
-			if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
+			if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */
 				_waypoint_yaw_reached = true;
 			}
 

src/modules/sdlog2/sdlog2.c

@@ -84,6 +84,8 @@
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/system_power.h>
+#include <uORB/topics/servorail_status.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -796,6 +798,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct telemetry_status_s telemetry;
 		struct range_finder_report range_finder;
 		struct estimator_status_report estimator_status;
+		struct system_power_s system_power;
+		struct servorail_status_s servorail_status;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -828,6 +832,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_DIST_s log_DIST;
 			struct log_TELE_s log_TELE;
 			struct log_ESTM_s log_ESTM;
+			struct log_PWR_s log_PWR;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -859,6 +864,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int telemetry_sub;
 		int range_finder_sub;
 		int estimator_status_sub;
+		int system_power_sub;
+		int servorail_status_sub;
 	} subs;
 
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -884,6 +891,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
 	subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
 	subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
+	subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
+	subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
 
 	thread_running = true;
 
@@ -1185,6 +1194,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.msg_type = LOG_AIRS_MSG;
 			log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
 			log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
+			log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
 			LOGBUFFER_WRITE_AND_COUNT(AIRS);
 		}
 
@@ -1227,6 +1237,24 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(BATT);
 		}
 
+		/* --- SYSTEM POWER RAILS --- */
+		if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
+			log_msg.msg_type = LOG_PWR_MSG;
+			log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
+			log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
+			log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid;
+			log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid;
+			log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC;
+			log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC;
+
+			/* 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 = buf.servorail_status.rssi_v;
+
+			LOGBUFFER_WRITE_AND_COUNT(PWR);
+		}
+
 		/* --- TELEMETRY --- */
 		if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
 			log_msg.msg_type = LOG_TELE_MSG;

src/modules/sdlog2/sdlog2_messages.h

@@ -175,6 +175,7 @@ struct log_OUT0_s {
 struct log_AIRS_s {
 	float indicated_airspeed;
 	float true_airspeed;
+	float air_temperature_celsius;
 };
 
 /* --- ARSP - ATTITUDE RATE SET POINT --- */
@@ -278,6 +279,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 --- */
@@ -300,23 +324,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;
-// };
-
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -333,7 +340,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
-	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
+	LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
@@ -343,16 +350,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(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_ */

src/modules/sensors/sensors.cpp

@@ -1013,12 +1013,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		raw.differential_pressure_timestamp = _diff_pres.timestamp;
 		raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
 
-		float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
 
 		_airspeed.timestamp = _diff_pres.timestamp;
 		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
 		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
-					      raw.baro_pres_mbar * 1e2f, air_temperature_celcius);
+					      raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
+		_airspeed.air_temperature_celsius = air_temperature_celsius;
 
 		/* announce the airspeed if needed, just publish else */
 		if (_airspeed_pub > 0) {

src/modules/uORB/objects_common.cpp

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

src/modules/uORB/topics/airspeed.h

@@ -52,9 +52,10 @@
  * Airspeed
  */
 struct airspeed_s {
-	uint64_t	timestamp;					/**< microseconds since system boot, needed to integrate */
+	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
 	float		indicated_airspeed_m_s;		/**< indicated airspeed in meters per second, -1 if unknown	 */
-	float		true_airspeed_m_s;			/**< true airspeed in meters per second, -1 if unknown */
+	float		true_airspeed_m_s;		/**< true airspeed in meters per second, -1 if unknown */
+	float		air_temperature_celsius;	/**< air temperature in degrees celsius, -1000 if unknown */
 };
 
 /**

src/modules/uORB/topics/differential_pressure.h

@@ -52,13 +52,14 @@
  * Differential pressure.
  */
 struct differential_pressure_s {
-	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
-	uint64_t	error_count;
+	uint64_t	timestamp;			/**< Microseconds since system boot, needed to integrate */
+	uint64_t	error_count;			/**< Number of errors detected by driver */
 	float	differential_pressure_pa;		/**< Differential pressure reading */
+	float	differential_pressure_raw_pa;		/**< Raw differential pressure reading (may be negative) */
 	float	differential_pressure_filtered_pa;	/**< Low pass filtered differential pressure reading */
 	float	max_differential_pressure_pa;		/**< Maximum differential pressure reading */
-	float		voltage;			/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
-	float		temperature;			/**< Temperature provided by sensor, -1000.0f if unknown */
+	float	voltage;				/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
+	float	temperature;				/**< Temperature provided by sensor, -1000.0f if unknown */
 
 };
 

src/modules/uORB/topics/system_power.h

@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   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 system_power.h
+ *
+ * Definition of the system_power voltage and power status uORB topic.
+ */
+
+#ifndef SYSTEM_POWER_H_
+#define SYSTEM_POWER_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * voltage and power supply status
+ */
+struct system_power_s {
+	uint64_t	timestamp;		/**< microseconds since system boot */
+	float   	voltage5V_v;		/**< peripheral 5V rail voltage */
+	uint8_t         usb_connected:1;        /**< USB is connected when 1 */
+	uint8_t         brick_valid:1;          /**< brick power is good when 1 */
+	uint8_t         servo_valid:1;          /**< servo power is good when 1 */
+	uint8_t		periph_5V_OC:1;         /**< peripheral overcurrent when 1 */
+	uint8_t		hipower_5V_OC:1;        /**< hi power peripheral overcurrent when 1 */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(system_power);
+
+#endif