take into account laneded and armed flag for ekf2 replay

src/modules/ekf2/ekf2_main.cpp

@@ -133,7 +133,6 @@ private:
 	int		_gps_sub = -1;
 	int		_airspeed_sub = -1;
 	int		_params_sub = -1;
-	int		_control_mode_sub = -1;
 	int 	_vehicle_status_sub = -1;
 
 	bool            _prev_motors_armed = false; // motors armed status from the previous frame
@@ -272,7 +271,6 @@ void Ekf2::task_main()
 	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
-	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	px4_pollfd_struct_t fds[2] = {};
@@ -321,7 +319,6 @@ void Ekf2::task_main()
 
 		bool gps_updated = false;
 		bool airspeed_updated = false;
-		bool control_mode_updated = false;
 		bool vehicle_status_updated = false;
 
 		orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
@@ -338,16 +335,6 @@ void Ekf2::task_main()
 			orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
 		}
 
-		// Use the control model data to determine if the motors are armed as a surrogate for an on-ground vs in-air status
-		// TODO implement a global vehicle on-ground/in-air check
-		// XXX: Look at the land_detection module!
-		orb_check(_control_mode_sub, &control_mode_updated);
-
-		if (control_mode_updated) {
-			orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &vehicle_control_mode);
-			_ekf->set_arm_status(vehicle_control_mode.flag_armed);
-		}
-
 		// in replay mode we are getting the actual timestamp from the sensor topic
 		hrt_abstime now = 0;
 		if (_replay_mode) {
@@ -402,6 +389,7 @@ void Ekf2::task_main()
 			struct vehicle_status_s status = {};
 			orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &status);
 			_ekf->set_in_air_status(!status.condition_landed);
+			_ekf->set_arm_status(status.arming_state & vehicle_status_s::ARMING_STATE_ARMED);
 		}
 
 		// run the EKF update

src/modules/ekf2_replay/ekf2_replay_main.cpp

@@ -61,6 +61,7 @@
 #include <uORB/topics/ekf2_innovations.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/control_state.h>
+#include <uORB/topics/vehicle_status.h>
 
 #include <sdlog2/sdlog2_messages.h>
 
@@ -128,6 +129,7 @@ private:
 
 	orb_advert_t _sensors_pub;
 	orb_advert_t _gps_pub;
+	orb_advert_t _status_pub;
 
 	int _att_sub;
 	int _estimator_status_sub;
@@ -140,6 +142,7 @@ private:
 	struct log_format_s _formats[100];
 	struct sensor_combined_s _sensors;
 	struct vehicle_gps_position_s _gps;
+	struct vehicle_status_s _status;
 
 	bool _read_part1;
 	bool _read_part2;
@@ -166,6 +169,7 @@ private:
 Ekf2Replay::Ekf2Replay(char *logfile) :
 	_sensors_pub(nullptr),
 	_gps_pub(nullptr),
+	_status_pub(nullptr),
 	_att_sub(-1),
 	_estimator_status_sub(-1),
 	_innov_sub(-1),
@@ -174,6 +178,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
 	_formats{},
 	_sensors{},
 	_gps{},
+	_status{},
 	_read_part1(false),
 	_read_part2(false),
 	_write_fd(-1)
@@ -185,6 +190,9 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
 	strcat(path_to_log, logfile);
 	_file_name = path_to_log;
 
+	// we always start landed
+	_status.condition_landed = true;
+
 }
 
 Ekf2Replay::~Ekf2Replay()
@@ -238,6 +246,11 @@ void Ekf2Replay::parseMessage(uint8_t *source, uint8_t *destination, uint8_t typ
 			write_index += sizeof(uint8_t);
 			break;
 
+		case 'B':
+			memcpy(&destination[write_index], &source[write_index], sizeof(uint8_t));
+			write_index += sizeof(uint8_t);
+			break;
+
 		default:
 			PX4_WARN("found unsupported data type in replay message, exiting!");
 			_task_should_exit = true;
@@ -252,6 +265,7 @@ void Ekf2Replay::setSensorData(uint8_t *data, uint8_t type)
 {
 	struct log_RPL1_s replay_part1 = {};
 	struct log_RPL2_s replay_part2 = {};
+	struct log_STAT_s vehicle_status = {};
 
 	if (type == LOG_RPL1_MSG) {
 
@@ -274,7 +288,6 @@ void Ekf2Replay::setSensorData(uint8_t *data, uint8_t type)
 		_sensors.baro_alt_meter[0] = replay_part1.baro_alt_meter;
 		_read_part1 = true;
 
-
 	} else if (type == LOG_RPL2_MSG) {
 		uint8_t *dest_ptr = (uint8_t *)&replay_part2.time_pos_usec;
 		parseMessage(data, dest_ptr, type);
@@ -291,6 +304,19 @@ void Ekf2Replay::setSensorData(uint8_t *data, uint8_t type)
 		_gps.vel_d_m_s = replay_part2.vel_d_m_s;
 		_gps.vel_ned_valid = replay_part2.vel_ned_valid;
 		_read_part2 = true;
+
+	} else if (type == LOG_STAT_MSG) {
+		uint8_t *dest_ptr = (uint8_t *)&vehicle_status.main_state;
+		parseMessage(data, dest_ptr, type);
+		_status.arming_state = vehicle_status.arming_state;
+		_status.condition_landed = (bool)vehicle_status.landed;
+
+		if (_status_pub == nullptr) {
+			_status_pub = orb_advertise(ORB_ID(vehicle_status), &_status);
+
+		} else if (_status_pub != nullptr) {
+			orb_publish(ORB_ID(vehicle_status), _status_pub, &_status);
+		}
 	}
 }