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

Conflicts:
	.gitmodules

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -63,6 +63,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vision_position_estimate.h>
 #include <drivers/drv_hrt.h>
 
 #include <lib/mathlib/mathlib.h>
@@ -134,7 +135,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 					      SCHED_PRIORITY_MAX - 5,
 					      7200,
 					      attitude_estimator_ekf_thread_main,
-					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					      (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
@@ -207,7 +208,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			     };		/**< init: identity matrix */
 
 	float debugOutput[4] = { 0.0f };
-
 	int overloadcounter = 19;
 
 	/* Initialize filter */
@@ -220,6 +220,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	memset(&raw, 0, sizeof(raw));
 	struct vehicle_gps_position_s gps;
 	memset(&gps, 0, sizeof(gps));
+	gps.eph = 100000;
+	gps.epv = 100000;
 	struct vehicle_global_position_s global_pos;
 	memset(&global_pos, 0, sizeof(global_pos));
 	struct vehicle_attitude_s att;
@@ -258,9 +260,12 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
-	/* subscribe to control mode*/
+	/* subscribe to control mode */
 	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 
+	/* subscribe to vision estimate */
+	int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
+
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
@@ -268,14 +273,11 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	thread_running = true;
 
-	/* advertise debug value */
-	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
-	// orb_advert_t pub_dbg = -1;
-
 	/* keep track of sensor updates */
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
-	struct attitude_estimator_ekf_params ekf_params = { 0 };
+	struct attitude_estimator_ekf_params ekf_params;
+	memset(&ekf_params, 0, sizeof(ekf_params));
 
 	struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };
 
@@ -293,6 +295,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	math::Matrix<3, 3> R_decl;
 	R_decl.identity();
 
+	struct vision_position_estimate vision {};
+
 	/* register the perf counter */
 	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
 
@@ -313,8 +317,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
 
 			if (!control_mode.flag_system_hil_enabled) {
-				fprintf(stderr,
-					"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
+				warnx("WARNING: Not getting sensors - sensor app running?");
 			}
 
 		} else {
@@ -449,9 +452,30 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
-					z_k[6] = raw.magnetometer_ga[0];
-					z_k[7] = raw.magnetometer_ga[1];
-					z_k[8] = raw.magnetometer_ga[2];
+					bool vision_updated = false;
+					orb_check(vision_sub, &vision_updated);
+
+					if (vision_updated) {
+						orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision);
+					}
+
+					if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) {
+
+						math::Quaternion q(vision.q);
+						math::Matrix<3, 3> Rvis = q.to_dcm();
+
+						math::Vector<3> v(1.0f, 0.0f, 0.4f);
+
+						math::Vector<3> vn = Rvis * v;
+
+						z_k[6] = vn(0);
+						z_k[7] = vn(1);
+						z_k[8] = vn(2);
+					} else {
+						z_k[6] = raw.magnetometer_ga[0];
+						z_k[7] = raw.magnetometer_ga[1];
+						z_k[8] = raw.magnetometer_ga[2];
+					}
 
 					uint64_t now = hrt_absolute_time();
 					unsigned int time_elapsed = now - last_run;

src/modules/attitude_estimator_ekf/module.mk

@@ -42,3 +42,7 @@ SRCS		 = attitude_estimator_ekf_main.cpp \
 		   codegen/AttitudeEKF.c
 
 MODULE_STACKSIZE = 1200
+
+EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3600
+
+EXTRACXXFLAGS = -Wframe-larger-than=2400

src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

@@ -153,7 +153,7 @@ int attitude_estimator_so3_main(int argc, char *argv[])
 					      SCHED_PRIORITY_MAX - 5,
 					      14000,
 					      attitude_estimator_so3_thread_main,
-					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					      (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 

src/modules/attitude_estimator_so3/module.mk

@@ -8,3 +8,5 @@ SRCS		 = attitude_estimator_so3_main.cpp \
 		   attitude_estimator_so3_params.c
 
 MODULE_STACKSIZE = 1200
+
+EXTRACXXFLAGS = -Wno-float-equal

src/modules/bottle_drop/bottle_drop.cpp

@@ -524,6 +524,9 @@ BottleDrop::task_main()
 			}
 
 			switch (_drop_state) {
+				case DROP_STATE_INIT:
+					// do nothing
+					break;
 
 				case DROP_STATE_TARGET_VALID:
 				{
@@ -690,6 +693,10 @@ BottleDrop::task_main()
 						orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
 					}
 					break;
+
+				case DROP_STATE_BAY_CLOSED:
+					// do nothing
+					break;
 			}
 
 			counter++;

src/modules/commander/commander.cpp

@@ -268,7 +268,7 @@ int commander_main(int argc, char *argv[])
 					     SCHED_PRIORITY_MAX - 40,
 					     3200,
 					     commander_thread_main,
-					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					     (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 
 		while (!thread_running) {
 			usleep(200);

src/modules/commander/module.mk

@@ -51,3 +51,6 @@ SRCS		 	= commander.cpp \
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACXXFLAGS = -Wframe-larger-than=2000
+

src/modules/ekf_att_pos_estimator/module.mk

@@ -42,4 +42,5 @@ SRCS		= ekf_att_pos_estimator_main.cpp \
 		  estimator_23states.cpp \
 		  estimator_utilities.cpp
 
-EXTRACXXFLAGS	= -Weffc++
+EXTRACXXFLAGS	= -Weffc++ -Wframe-larger-than=6100
+

src/modules/fixedwing_backside/fixedwing_backside_main.cpp

@@ -115,7 +115,7 @@ int fixedwing_backside_main(int argc, char *argv[])
 					 SCHED_PRIORITY_MAX - 10,
 					 5120,
 					 control_demo_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					 (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 

src/modules/fw_pos_control_l1/module.mk

@@ -47,3 +47,5 @@ SRCS		= fw_pos_control_l1_main.cpp \
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACXXFLAGS = -Wno-float-equal

src/modules/mavlink/mavlink_ftp.cpp

@@ -801,7 +801,7 @@ MavlinkFTP::_return_request(Request *req)
 
 /// @brief Copy file (with limited space)
 int
-MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length)
+MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length)
 {
 	char buff[512];
 	int src_fd = -1, dst_fd = -1;

src/modules/mavlink/mavlink_ftp.h

@@ -142,7 +142,7 @@ private:
 	static void	_worker_trampoline(void *arg);
 	void		_process_request(Request *req);
 	void		_reply(Request *req);
-	int		_copy_file(const char *src_path, const char *dst_path, ssize_t length);
+	int		_copy_file(const char *src_path, const char *dst_path, size_t length);
 
 	ErrorCode	_workList(PayloadHeader *payload);
 	ErrorCode	_workOpen(PayloadHeader *payload, int oflag);

src/modules/mavlink/mavlink_main.cpp

@@ -948,7 +948,6 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 				/* delete stream */
 				LL_DELETE(_streams, stream);
 				delete stream;
-				warnx("deleted stream %s", stream->get_name());
 			}
 
 			return OK;
@@ -1412,9 +1411,13 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("ATTITUDE", 50.0f);
 		configure_stream("GLOBAL_POSITION_INT", 50.0f);
+		configure_stream("LOCAL_POSITION_NED", 30.0f);
 		configure_stream("CAMERA_CAPTURE", 2.0f);
 		configure_stream("ATTITUDE_TARGET", 10.0f);
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
+		configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
+		configure_stream("DISTANCE_SENSOR", 10.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream("VFR_HUD", 10.0f);
 		break;
 
@@ -1638,7 +1641,7 @@ Mavlink::start(int argc, char *argv[])
 		       SCHED_PRIORITY_DEFAULT,
 		       2800,
 		       (main_t)&Mavlink::start_helper,
-		       (const char **)argv);
+		       (char * const *)argv);
 
 	// Ensure that this shell command
 	// does not return before the instance

src/modules/mavlink/mavlink_messages.cpp

@@ -1486,6 +1486,7 @@ protected:
 		if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
 			mavlink_position_target_global_int_t msg{};
 
+			msg.time_boot_ms = hrt_absolute_time()/1000;
 			msg.coordinate_frame = MAV_FRAME_GLOBAL;
 			msg.lat_int = pos_sp_triplet.current.lat * 1e7;
 			msg.lon_int = pos_sp_triplet.current.lon * 1e7;
@@ -1764,6 +1765,9 @@ protected:
 				case RC_INPUT_SOURCE_PX4IO_ST24:
 					msg.rssi |= (3 << 4);
 					break;
+				case RC_INPUT_SOURCE_UNKNOWN:
+					// do nothing
+					break;
 			}
 
 			if (rc.rc_lost) {

src/modules/mavlink/module.mk

@@ -53,4 +53,6 @@ MAXOPTIMIZATION	 = -Os
 
 MODULE_STACKSIZE = 1024
 
-EXTRACXXFLAGS	= -Weffc++
+EXTRACXXFLAGS	= -Weffc++ -Wno-attributes -Wno-packed
+
+EXTRACFLAGS	= -Wno-packed

src/modules/navigator/module.mk

@@ -62,3 +62,5 @@ INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION = -Os
+
+EXTRACXXFLAGS	= -Wno-sign-compare

src/modules/position_estimator_inav/module.mk

@@ -41,3 +41,6 @@ SRCS		 	= position_estimator_inav_main.c \
 			inertial_filter.c
 
 MODULE_STACKSIZE = 1200
+
+EXTRACFLAGS = -Wframe-larger-than=3500
+

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -152,7 +152,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 		position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
 					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
 					       position_estimator_inav_thread_main,
-					       (argv) ? (const char **) &argv[2] : (const char **) NULL);
+					       (argv) ? (char * const *) &argv[2] : (char * const *) NULL);
 		exit(0);
 	}
 
@@ -234,8 +234,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 	float eph_flow = 1.0f;
 
-	float eph_vision = 0.5f;
-	float epv_vision = 0.5f;
+	float eph_vision = 0.2f;
+	float epv_vision = 0.2f;
 
 	float x_est_prev[2], y_est_prev[2], z_est_prev[2];
 	memset(x_est_prev, 0, sizeof(x_est_prev));
@@ -641,6 +641,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				if (updated) {
 					orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);
 
+					static float last_vision_x = 0.0f;
+					static float last_vision_y = 0.0f;
+					static float last_vision_z = 0.0f;
+
 					/* reset position estimate on first vision update */
 					if (!vision_valid) {
 						x_est[0] = vision.x;
@@ -655,6 +659,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						}
 
 						vision_valid = true;
+
+						last_vision_x = vision.x;
+						last_vision_y = vision.y;
+						last_vision_z = vision.z;
+
 						warnx("VISION estimate valid");
 						mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid");
 					}
@@ -664,10 +673,30 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					corr_vision[1][0] = vision.y - y_est[0];
 					corr_vision[2][0] = vision.z - z_est[0];
 
-					/* calculate correction for velocity */
-					corr_vision[0][1] = vision.vx - x_est[1];
-					corr_vision[1][1] = vision.vy - y_est[1];
-					corr_vision[2][1] = vision.vz - z_est[1];
+					static hrt_abstime last_vision_time = 0;
+
+					float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f;
+					last_vision_time = vision.timestamp_boot;
+
+					if (vision_dt > 0.000001f && vision_dt < 0.2f) {
+						vision.vx = (vision.x - last_vision_x) / vision_dt;
+						vision.vy = (vision.y - last_vision_y) / vision_dt;
+						vision.vz = (vision.z - last_vision_z) / vision_dt;
+
+						last_vision_x = vision.x;
+						last_vision_y = vision.y;
+						last_vision_z = vision.z;
+
+						/* calculate correction for velocity */
+						corr_vision[0][1] = vision.vx - x_est[1];
+						corr_vision[1][1] = vision.vy - y_est[1];
+						corr_vision[2][1] = vision.vz - z_est[1];
+					} else {
+						/* assume zero motion */
+						corr_vision[0][1] = 0.0f - x_est[1];
+						corr_vision[1][1] = 0.0f - y_est[1];
+						corr_vision[2][1] = 0.0f - z_est[1];
+					}
 
 				}
 			}

src/modules/position_estimator_inav/position_estimator_inav_params.c

@@ -116,7 +116,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
  * @max 10.0
  * @group Position Estimator INAV
  */
-PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f);
 
 /**
  * XY axis weight for vision velocity

src/modules/sdlog2/module.mk

@@ -45,3 +45,6 @@ SRCS = sdlog2.c \
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACFLAGS = -Wframe-larger-than=1200
+

src/modules/sdlog2/sdlog2.c

@@ -304,7 +304,7 @@ int sdlog2_main(int argc, char *argv[])
 						 SCHED_PRIORITY_DEFAULT - 30,
 						 3000,
 						 sdlog2_thread_main,
-						 (const char **)argv);
+						 (char * const *)argv);
 		exit(0);
 	}
 
@@ -1089,6 +1089,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	if (_extended_logging) {
 		subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
+	} else {
+		subs.sat_info_sub = 0;
 	}
 
 	/* close non-needed fd's */

src/modules/sdlog2/sdlog2_messages.h

@@ -468,7 +468,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(OUT0, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	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(FLOW, "QBffffffLLHhB",	"IntT,ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"),
 	LOG_FORMAT(GPOS, "LLfffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHffiffH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),

src/modules/segway/segway_main.cpp

@@ -110,7 +110,7 @@ int segway_main(int argc, char *argv[])
 					 SCHED_PRIORITY_MAX - 10,
 					 5120,
 					 segway_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					 (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 

src/modules/sensors/module.mk

@@ -44,3 +44,5 @@ SRCS		= sensors.cpp \
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACXXFLAGS	= -Wno-type-limits

src/modules/systemlib/module.mk

@@ -58,3 +58,5 @@ SRCS		 = err.c \
 		   mcu_version.c
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACFLAGS	= -Wno-sign-compare

src/modules/systemlib/system_params.c

@@ -82,3 +82,17 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1);
 * @group System
 */
 PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
+
+/**
+* Companion computer interface
+*
+* Configures the baud rate of the companion computer interface.
+* Set to zero to disable, set to 921600 to enable.
+* CURRENTLY ONLY SUPPORTS 921600 BAUD! Use extras.txt for
+* other baud rates.
+*
+* @min 0
+* @max 921600
+* @group System
+*/
+PARAM_DEFINE_INT32(SYS_COMPANION, 0);

src/modules/systemlib/systemlib.c

@@ -87,7 +87,7 @@ static void kill_task(FAR struct tcb_s *tcb, FAR void *arg)
 	kill(tcb->pid, SIGUSR1);
 }
 
-int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
+int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[])
 {
 	int pid;
 

src/modules/systemlib/systemlib.h

@@ -64,7 +64,7 @@ __EXPORT int task_spawn_cmd(const char *name,
 			int scheduler,
 			int stack_size,
 			main_t entry,
-			const char *argv[]);
+			char * const argv[]);
 
 enum MULT_PORTS {
 	MULT_0_US2_RXTX = 0,

src/modules/uORB/uORB.h

@@ -67,6 +67,33 @@ typedef const struct orb_metadata *orb_id_t;
  */
 #define ORB_ID(_name)		&__orb_##_name
 
+/**
+ * Generates a pointer to the uORB metadata structure for
+ * a given topic.
+ *
+ * The topic must have been declared previously in scope
+ * with ORB_DECLARE().
+ *
+ * @param _name		The name of the topic.
+ * @param _count	The class ID of the topic
+ */
+#define ORB_ID_DOUBLE(_name, _count)	((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : &__orb_##_name##1)
+
+/**
+ * Generates a pointer to the uORB metadata structure for
+ * a given topic.
+ *
+ * The topic must have been declared previously in scope
+ * with ORB_DECLARE().
+ *
+ * @param _name		The name of the topic.
+ * @param _count	The class ID of the topic
+ */
+#define ORB_ID_TRIPLE(_name, _count)					\
+ 	((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : \
+			((_count == CLASS_DEVICE_SECONDARY) ? &__orb_##_name##1 : \
+				(((_count == CLASS_DEVICE_TERTIARY) ? &__orb_##_name##2 : 0))))
+
 /**
  * Declare (prototype) the uORB metadata for a topic.
  *

src/modules/vtol_att_control/module.mk

@@ -39,3 +39,6 @@ MODULE_COMMAND	= vtol_att_control
 
 SRCS		= vtol_att_control_main.cpp \
 			  vtol_att_control_params.c
+
+EXTRACXXFLAGS = -Wno-write-strings
+