fix : print target gps fucntion by using dataman in ekf2

이충섭
Commit 7065b9da6e0edcb8cb90bfa4a063abcc3cd4405a 7065b9da 1 parent 67e0e540
Showing 3 changed files with 53 additions and 31 deletions
home/Firmware/src/modules/ekf2/EKF2.cpp
home/Firmware/src/modules/navigator/mission.cpp
home/Firmware/src/modules/navigator/mission.h
--- a/home/Firmware/src/modules/ekf2/EKF2.cpp
View file @7065b9d
+++ b/home/Firmware/src/modules/ekf2/EKF2.cpp
View file @7065b9d
@@ -32,7 +32,9 @@
  ****************************************************************************/
 #include "EKF2.hpp"
-
+#include <iostream>
+#include <px4_platform_common/defines.h>
+#include <dataman/dataman.h>
 using namespace time_literals;
 using math::constrain;
 using matrix::Eulerf;
@@ -44,7 +46,6 @@ static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
 #if !defined(CONSTRAINED_FLASH)
 static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
 #endif // !CONSTRAINED_FLASH
-
 EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, config),
@@ -169,6 +170,8 @@ EKF2::~EKF2()
 {
 	perf_free(_ecl_ekf_update_perf);
 	perf_free(_ecl_ekf_update_full_perf);
+	perf_free(_imu_missed_perf);
+	perf_free(_mag_missed_perf);
 }
 bool EKF2::multi_init(int imu, int mag)
@@ -193,17 +196,18 @@ bool EKF2::multi_init(int imu, int mag)
 	_estimator_visual_odometry_aligned_pub.advertised();
 	_yaw_est_pub.advertise();
-	_vehicle_imu_sub.ChangeInstance(imu);
+	bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
-	_magnetometer_sub.ChangeInstance(mag);
 	const int status_instance = _estimator_states_pub.get_instance();
-	if ((status_instance >= 0)
+	if ((status_instance >= 0) && changed_instance
 	    && (_attitude_pub.get_instance() == status_instance)
 	    && (_local_position_pub.get_instance() == status_instance)
 	    && (_global_position_pub.get_instance() == status_instance)) {
 		_instance = status_instance;
+
+		ScheduleNow();
 		return true;
 	}
@@ -219,6 +223,12 @@ int EKF2::print_status()
 		     _ekf.local_position_is_valid(), _ekf.global_position_is_valid());
 	perf_print_counter(_ecl_ekf_update_perf);
 	perf_print_counter(_ecl_ekf_update_full_perf);
+	perf_print_counter(_imu_missed_perf);
+
+	if (_device_id_mag != 0) {
+		perf_print_counter(_mag_missed_perf);
+	}
+
 	return 0;
 }
@@ -298,7 +308,8 @@ void EKF2::Run()
 		imu_updated = _vehicle_imu_sub.update(&imu);
 		if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) {
-			PX4_ERR("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation,
+			perf_count(_imu_missed_perf);
+			PX4_DEBUG("%d - vehicle_imu lost, generation %d -> %d", _instance, last_generation,
 				  _vehicle_imu_sub.get_last_generation());
 		}
@@ -1498,7 +1509,13 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 					      + vehicle_gps_position.vdop * vehicle_gps_position.vdop),
 			};
 			_ekf.setGpsData(gps_msg);
-
+			mission_s mission;
+			dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
+			for (size_t i = 0; i < mission.count; i++) {
+					struct mission_item_s mission_item {};
+					dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s));
+					std::cout<<"target\t"<<i<< "(lat/lon/alt)\t" << mission_item.lat << "\t"<<mission_item.lon<<"\t"<<mission_item.altitude<<std::endl;
+			}
 			_gps_time_usec = gps_msg.time_usec;
 			_gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
 		}
@@ -1513,7 +1530,8 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 	if (_magnetometer_sub.update(&magnetometer)) {
 		if (_magnetometer_sub.get_last_generation() != last_generation + 1) {
-			PX4_ERR("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation,
+			perf_count(_mag_missed_perf);
+			PX4_DEBUG("%d - vehicle_magnetometer lost, generation %d -> %d", _instance, last_generation,
 				  _magnetometer_sub.get_last_generation());
 		}
@@ -1738,7 +1756,8 @@ int EKF2::task_spawn(int argc, char *argv[])
 		while ((multi_instances_allocated < multi_instances)
 		       && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
-		       && (hrt_elapsed_time(&time_started) < 30_s)) {
+		       && ((hrt_elapsed_time(&time_started) < 30_s)
+			   || (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) {
 			vehicle_status_sub.update();
@@ -1763,16 +1782,21 @@ int EKF2::task_spawn(int argc, char *argv[])
 								if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) {
 									_objects[actual_instance].store(ekf2_inst);
-									ekf2_inst->ScheduleNow();
 									success = true;
 									multi_instances_allocated++;
 									ekf2_instance_created[imu][mag] = true;
+									if (actual_instance == 0) {
+										// force selector to run immediately if first instance started
+										_ekf2_selector.load()->ScheduleNow();
+									}
+
 									PX4_INFO("starting instance %d, IMU:%d (%d), MAG:%d (%d)", actual_instance,
 										 imu, vehicle_imu_sub.get().accel_device_id,
 										 mag, vehicle_mag_sub.get().device_id);
-									_ekf2_selector.load()->ScheduleNow();
+									// sleep briefly before starting more instances
+									px4_usleep(10000);
 								} else {
 									PX4_ERR("instance numbering problem instance: %d", actual_instance);
--- a/home/Firmware/src/modules/navigator/mission.cpp
View file @7065b9d
+++ b/home/Firmware/src/modules/navigator/mission.cpp
View file @7065b9d
@@ -47,7 +47,7 @@
 #include "mission.h"
 #include "navigator.h"
-#include <iostream>
+
 #include <string.h>
 #include <drivers/drv_hrt.h>
 #include <dataman/dataman.h>
@@ -381,6 +381,7 @@ Mission::set_execution_mode(const uint8_t mode)
 		_mission_execution_mode = mode;
 	}
 }
+
 bool
 Mission::find_mission_land_start()
 {
@@ -620,12 +621,11 @@ Mission::set_mission_items()
 	struct mission_item_s mission_item_after_next_position;
 	bool has_next_position_item = false;
 	bool has_after_next_position_item = false;
+
 	work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 	if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item,
 				  &mission_item_after_next_position, &has_after_next_position_item)) {
-		print_next_mission(&mission_item_next_position);
-
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_MISSION) {
 			mavlink_log_info(_navigator->get_mavlink_log_pub(),
@@ -633,6 +633,7 @@ Mission::set_mission_items()
 					 "Executing Mission");
 			user_feedback_done = true;
 		}
+
 		_mission_type = MISSION_TYPE_MISSION;
 	} else {
@@ -1436,14 +1437,17 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
 	if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
 		offset = -1;
 	}
+
 	if (read_mission_item(0, mission_item)) {
+
 		first_res = true;
+
 		/* trying to find next position mission item */
 		while (read_mission_item(offset, next_position_mission_item)) {
-			if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {\
+			if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
 				offset--;
-			}
+
-			else {
+			} else {
 				offset++;
 			}
@@ -1452,11 +1456,13 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
 				break;
 			}
 		}
+
 		if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE &&
 		    after_next_position_mission_item && has_after_next_position_item) {
 			/* trying to find next next position mission item */
 			while (read_mission_item(offset, after_next_position_mission_item)) {
 				offset++;
+
 				if (item_contains_position(*after_next_position_mission_item)) {
 					*has_after_next_position_item = true;
 					break;
@@ -1467,24 +1473,14 @@ Mission::prepare_mission_items(struct mission_item_s *mission_item,
 	return first_res;
 }
-bool
-Mission::print_next_mission(struct mission_item_s *mis)
-{
-	std::cout <<"----------------------target--------------------------"<<std::endl;
-	std::cout << "lat : "<<mis->lat <<std:: endl;
-	std::cout << "lon : "<<mis->lon <<std:: endl;
-	std::cout << "alt : "<<mis->altitude <<std:: endl;
-	//std::cout << "[whatever] "<<mis->timestamp <<std:: endl;
-	mission_index++;
-	return 0;
-}
 bool
 Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
 {
 	/* select mission */
 	const int current_index = _current_mission_index;
 	int index_to_read = current_index + offset;
+
 	int *mission_index_ptr = (offset == 0) ? &_current_mission_index : &index_to_read;
 	const dm_item_t dm_item = (dm_item_t)_mission.dataman_id;
@@ -1564,6 +1560,7 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
 			return true;
 		}
 	}
+
 	/* we have given up, we don't want to cycle forever */
 	mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP is cycling, giving up.");
 	return false;
@@ -1865,5 +1862,6 @@ void Mission::publish_navigator_mission_item()
 	navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition;
 	navigator_mission_item.timestamp = hrt_absolute_time();
+
 	_navigator_mission_item_pub.publish(navigator_mission_item);
 }
--- a/home/Firmware/src/modules/navigator/mission.h
View file @7065b9d
+++ b/home/Firmware/src/modules/navigator/mission.h
View file @7065b9d
@@ -49,7 +49,7 @@
 #include "navigator_mode.h"
 #include <float.h>
-#include <iostream>
+
 #include <dataman/dataman.h>
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
@@ -227,7 +227,7 @@ private:
 	 * Find and store the index of the landing sequence (DO_LAND_START)
 	 */
 	bool find_mission_land_start();
-	bool print_next_mission(struct mission_item_s *mis);
+
 	/**
 	 * Return the index of the closest mission item to the current global position.
 	 */
@@ -269,7 +269,7 @@ private:
 		MISSION_TYPE_NONE,
 		MISSION_TYPE_MISSION
 	} _mission_type{MISSION_TYPE_NONE};
-	int mission_index=-1;
+
 	bool _inited{false};
 	bool _home_inited{false};
 	bool _need_mission_reset{false};