added parameter which defines threshold for airspeed given to the filter

remove unnecessary replay fields

msg/ekf2_replay.msg

@@ -43,9 +43,6 @@ uint8 flow_quality			# Quality of accumulated optical flow data (0 - 255)
 # airspeed
 float32 indicated_airspeed_m_s		# indicated airspeed in meters per second, -1 if unknown
 float32 true_airspeed_m_s		# true filtered airspeed in meters per second, -1 if unknown
-float32 true_airspeed_unfiltered_m_s	# true airspeed in meters per second, -1 if unknown
-float32 air_temperature_celsius		# air temperature in degrees celsius, -1000 if unknown
-float32 confidence			# confidence value from 0 to 1 for this sensor
 
 # external vision measurements
 float32[3] pos_ev			# position in earth (NED) frame (metres)

src/modules/ekf2/ekf2_main.cpp

@@ -80,6 +80,7 @@
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_status.h>
 
 #include <ecl/EKF/ekf.h>
 
@@ -143,6 +144,7 @@ private:
 	int 	_ev_pos_sub = -1;
 	int	_actuator_armed_sub = -1;
 	int	_vehicle_land_detected_sub = -1;
+	int _status_sub = -1;
 
 	bool	_prev_landed = true;	// landed status from the previous frame
 
@@ -255,7 +257,10 @@ private:
 	control::BlockParamFloat _ev_pos_x;	// X position of VI sensor focal point in body frame (m)
 	control::BlockParamFloat _ev_pos_y;	// Y position of VI sensor focal point in body frame (m)
 	control::BlockParamFloat _ev_pos_z;	// Z position of VI sensor focal point in body frame (m)
-
+	// control of airspeed and sideslip fusion
+ 	control::BlockParamFloat _arspFusionThreshold; 	// a value of zero will disabled airspeed fusion. Any another positive value will determine
+ 													// the minimum airspeed which will still be fused
+ 	
 	// output predictor filter time constants
 	control::BlockParamFloat _tau_vel;	// time constant used by the output velocity complementary filter (s)
 	control::BlockParamFloat _tau_pos;	// time constant used by the output position complementary filter (s)
@@ -355,6 +360,7 @@ Ekf2::Ekf2():
 	_ev_pos_x(this, "EKF2_EV_POS_X", false, &_params->ev_pos_body(0)),
 	_ev_pos_y(this, "EKF2_EV_POS_Y", false, &_params->ev_pos_body(1)),
 	_ev_pos_z(this, "EKF2_EV_POS_Z", false, &_params->ev_pos_body(2)),
+	_arspFusionThreshold(this, "EKF2_ARSP_THR", false),
 	_tau_vel(this, "EKF2_TAU_VEL", false, &_params->vel_Tau),
 	_tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau),
 	_gyr_bias_init(this, "EKF2_GBIAS_INIT", false, &_params->switch_on_gyro_bias),
@@ -386,6 +392,7 @@ void Ekf2::task_main()
 	_range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
 	_ev_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate));
 	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
+	_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	px4_pollfd_struct_t fds[2] = {};
 	fds[0].fd = _sensors_sub;
@@ -406,6 +413,7 @@ void Ekf2::task_main()
 	distance_sensor_s range_finder = {};
 	vehicle_land_detected_s vehicle_land_detected = {};
 	vision_position_estimate_s ev = {};
+	vehicle_status_s _vehicle_status = {};
 
 	while (!_task_should_exit) {
 		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
@@ -440,9 +448,17 @@ void Ekf2::task_main()
 		bool range_finder_updated = false;
 		bool vehicle_land_detected_updated = false;
 		bool vision_position_updated = false;
+		bool vehicle_status_updated = false;
 
 		orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
 		// update all other topics if they have new data
+
+ 		orb_check(_status_sub, &vehicle_status_updated);
+
+		if (vehicle_status_updated) {
+ 			orb_copy(ORB_ID(vehicle_status), _status_sub, &_vehicle_status);
+ 		}
+
 		orb_check(_gps_sub, &gps_updated);
 
 		if (gps_updated) {
@@ -517,12 +533,12 @@ void Ekf2::task_main()
 			_ekf.setGpsData(gps.timestamp_position, &gps_msg);
 		}
 
-		// read airspeed data if available
-		float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
-
-		if (airspeed_updated && airspeed.true_airspeed_m_s > 7.0f) {
+		// only set airspeed data if condition for airspeed fusion are met
+ 		bool fuse_airspeed = airspeed_updated && !_vehicle_status.is_rotary_wing && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s;
+ 		if (fuse_airspeed) {
+ 			float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
 			_ekf.setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s, &eas2tas);
-		}
+  		}
 
 		if (optical_flow_updated) {
 			flow_message flow;
@@ -907,13 +923,10 @@ void Ekf2::task_main()
 				replay.rng_timestamp = 0;
 			}
 
-			if (airspeed_updated) {
+			if (fuse_airspeed) {
 				replay.asp_timestamp = airspeed.timestamp;
 				replay.indicated_airspeed_m_s = airspeed.indicated_airspeed_m_s;
 				replay.true_airspeed_m_s = airspeed.true_airspeed_m_s;
-				replay.true_airspeed_unfiltered_m_s = airspeed.true_airspeed_unfiltered_m_s;
-				replay.air_temperature_celsius = airspeed.air_temperature_celsius;
-				replay.confidence = airspeed.confidence;
 
 			} else {
 				replay.asp_timestamp = 0;

src/modules/ekf2/ekf2_params.c

@@ -782,6 +782,17 @@ PARAM_DEFINE_FLOAT(EKF2_EV_POS_Y, 0.0f);
  */
 PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f);
 
+ /**
+ * Airspeed fusion threshold. A value of zero will deactivate airspeed fusion. Any other positive
+ * value will determine the minimum airspeed which will still be fused.
+ *
+ * @group EKF2
+ * @min 0.0
+ * @unit m/s
+ * @decimal 1
+ */
+ PARAM_DEFINE_FLOAT(EKF2_ARSP_THR, 0.0f);
+
 /**
 
  * Time constant of the velocity output prediction and smoothing filter

src/modules/ekf2_replay/ekf2_replay_main.cpp

@@ -137,6 +137,7 @@ private:
 	orb_advert_t _range_pub;
 	orb_advert_t _airspeed_pub;
 	orb_advert_t _ev_pub;
+	orb_advert_t _vehicle_status_pub;
 
 	int _att_sub;
 	int _estimator_status_sub;
@@ -154,6 +155,7 @@ private:
 	struct distance_sensor_s _range;
 	struct airspeed_s _airspeed;
 	struct vision_position_estimate_s _ev;
+	struct vehicle_status_s _vehicle_status;
 
 	unsigned _message_counter; // counter which will increase with every message read from the log
 	unsigned _part1_counter_ref;		// this is the value of _message_counter when the part1 of the replay message is read (imu data)
@@ -211,6 +213,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
 	_range_pub(nullptr),
 	_airspeed_pub(nullptr),
 	_ev_pub(nullptr),
+	_vehicle_status_pub(nullptr),
 	_att_sub(-1),
 	_estimator_status_sub(-1),
 	_innov_sub(-1),
@@ -222,6 +225,8 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
 	_land_detected{},
 	_flow{},
 	_range{},
+	_airspeed{},
+ 	_vehicle_status{},
 	_message_counter(0),
 	_part1_counter_ref(0),
 	_read_part2(false),
@@ -364,6 +369,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 	struct log_RPL6_s replay_part6 = {};
 	struct log_RPL5_s replay_part5 = {};
 	struct log_LAND_s vehicle_landed = {};
+	struct log_STAT_s vehicle_status = {};
 
 	if (type == LOG_RPL1_MSG) {
 
@@ -431,9 +437,6 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		_airspeed.timestamp = replay_part6.time_airs_usec;
 		_airspeed.indicated_airspeed_m_s = replay_part6.indicated_airspeed_m_s;
 		_airspeed.true_airspeed_m_s = replay_part6.true_airspeed_m_s;
-		_airspeed.true_airspeed_unfiltered_m_s = replay_part6.true_airspeed_unfiltered_m_s;
-		_airspeed.air_temperature_celsius = replay_part6.air_temperature_celsius;
-		_airspeed.confidence = replay_part6.confidence;
 		_read_part6 = true;
 
 	} else if (type == LOG_RPL5_MSG) {
@@ -464,6 +467,19 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 			orb_publish(ORB_ID(vehicle_land_detected), _landed_pub, &_land_detected);
 		}
 	}
+
+	else if (type == LOG_STAT_MSG) {
+ 		uint8_t *dest_ptr = (uint8_t *)&vehicle_status.main_state;
+ 		parseMessage(data, dest_ptr, type);
+ 		_vehicle_status.is_rotary_wing = vehicle_status.is_rot_wing;
+ 
+ 		if (_vehicle_status_pub == nullptr) {
+ 			_vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &_vehicle_status);
+ 
+ 		} else if (_vehicle_status_pub != nullptr) {
+ 			orb_publish(ORB_ID(vehicle_status), _vehicle_status_pub, &_vehicle_status);
+ 		}
+ 	}
 }
 
 void Ekf2Replay::writeMessage(int &fd, void *data, size_t size)

src/modules/sdlog2/sdlog2.c

@@ -1517,6 +1517,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_STAT.nav_state = buf_status.nav_state;
 			log_msg.body.log_STAT.arming_state = buf_status.arming_state;
 			log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe;
+			log_msg.body.log_STAT.is_rot_wing = (uint8_t)buf_status.is_rotary_wing;
 			LOGBUFFER_WRITE_AND_COUNT(STAT);
 		}
 
@@ -1600,10 +1601,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 					log_msg.msg_type = LOG_RPL6_MSG;
 					log_msg.body.log_RPL6.time_airs_usec = buf.replay.asp_timestamp;
 					log_msg.body.log_RPL6.indicated_airspeed_m_s = buf.replay.indicated_airspeed_m_s;
-					log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s;
-					log_msg.body.log_RPL6.true_airspeed_unfiltered_m_s = buf.replay.true_airspeed_unfiltered_m_s;
-					log_msg.body.log_RPL6.air_temperature_celsius = buf.replay.air_temperature_celsius;
-					log_msg.body.log_RPL6.confidence = buf.replay.confidence;
+					log_msg.body.log_RPL6.true_airspeed_m_s = buf.replay.true_airspeed_m_s;;
 					LOGBUFFER_WRITE_AND_COUNT(RPL6);
 				}
 

src/modules/sdlog2/sdlog2_messages.h

@@ -182,6 +182,7 @@ struct log_STAT_s {
 	uint8_t nav_state;
 	uint8_t arming_state;
 	uint8_t failsafe;
+	uint8_t is_rot_wing;
 };
 
 /* --- RC - RC INPUT CHANNELS --- */
@@ -600,9 +601,6 @@ struct log_RPL6_s {
 	uint64_t time_airs_usec;
 	float indicated_airspeed_m_s;
 	float true_airspeed_m_s;
-	float true_airspeed_unfiltered_m_s;
-	float air_temperature_celsius;
-	float confidence;
 };
 
 /* --- EKF2 REPLAY Part 5 --- */
@@ -668,7 +666,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT_S(DGPS, GPS,	 "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
 	LOG_FORMAT_S(ATTC, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT_S(ATC1, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBBB",		"MainState,NavState,ArmS,Failsafe"),
+	LOG_FORMAT(STAT, "BBBBB",		"MainState,NavState,ArmS,Failsafe,IsRotWing"),
 	LOG_FORMAT(VTOL, "fBBB",		"Arsp,RwMode,TransMode,Failsafe"),
 	LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
 	LOG_FORMAT(RC, "ffffffffffffBBBL",		"C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"),
@@ -712,7 +710,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"),
 	LOG_FORMAT(RPL4, "Qf", "Trng,rng"),
 	LOG_FORMAT(RPL5, "Qfffffffff", "Tev,x,y,z,q0,q1,q2,q3,posErr,angErr"),
-	LOG_FORMAT(RPL6, "Qfffff", "Tasp,inAsp,trAsp,ufAsp,tpAsp,confAsp"),
+	LOG_FORMAT(RPL6, "Qff", "Tasp,inAsp,trAsp"),
 	LOG_FORMAT(LAND, "B", "Landed"),
 	LOG_FORMAT(LOAD, "f", "CPU"),
 	/* system-level messages, ID >= 0x80 */