feat : caculate inclination among all of waypoints and between starting point and current position

이충섭
Commit 1fd483af2cc893c2b4ef0f9cdd022318b9bae16b 1fd483af 1 parent 7065b9da
Showing 2 changed files with 81 additions and 10 deletions
home/Firmware/src/modules/ekf2/EKF2.cpp
home/Firmware/src/modules/ekf2/EKF2.hpp
--- a/home/Firmware/src/modules/ekf2/EKF2.cpp
View file @1fd483a
+++ b/home/Firmware/src/modules/ekf2/EKF2.cpp
View file @1fd483a
@@ -40,7 +40,7 @@ using math::constrain;
 using matrix::Eulerf;
 using matrix::Quatf;
 using matrix::Vector3f;
-
+int static hold=0;
 pthread_mutex_t ekf2_module_mutex = PTHREAD_MUTEX_INITIALIZER;
 static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
 #if !defined(CONSTRAINED_FLASH)
@@ -1509,19 +1509,83 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 					      + vehicle_gps_position.vdop * vehicle_gps_position.vdop),
 			};
 			_ekf.setGpsData(gps_msg);
-			mission_s mission;
+			if(hold != int(gps_msg.time_usec/1000000))
-			dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
+			{
-			for (size_t i = 0; i < mission.count; i++) {
+				dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
-					struct mission_item_s mission_item {};
+				hold=int(gps_msg.time_usec/1000000);
-					dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s));
+				if(hold%2==0)
-					std::cout<<"target\t"<<i<< "(lat/lon/alt)\t" << mission_item.lat << "\t"<<mission_item.lon<<"\t"<<mission_item.altitude<<std::endl;
+				{
+					calculate_inclination_target();
+					print_target_gps();
+					//print_current_gps(gps_msg);
+					calculate_inclination_current(gps_msg);
+				}
 			}
 			_gps_time_usec = gps_msg.time_usec;
 			_gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
 		}
 	}
 }
+void EKF2::print_current_gps(gps_message gps_msg)
+{
+	std::cout<<"curre\t#"<<mission.current_seq<<"(lat/lon/alt)\t" << gps_msg.lat << "\t"<<gps_msg.lon<<"\t"<<gps_msg.alt<<std::endl;
+}
+void EKF2::calculate_inclination_target()
+{
+	double x,y;
+	inclination=new double[int(mission.count)];
+	for (size_t i = 0; i < mission.count-1; i++) {
+		struct mission_item_s mission_item {};
+		struct mission_item_s next_mission_item {};
+		dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s));
+		dm_read((dm_item_t)mission.dataman_id, i+1, &next_mission_item, sizeof(mission_item_s));
+		//std:: cout << "next : "<<next_mission_item.lat<<std::endl;
+		//std:: cout << "curr : "<<mission_item.lat<<std::endl;
+		x=next_mission_item.lat-mission_item.lat;
+		y=next_mission_item.lon-mission_item.lon;
+		std::cout.setf(std::ios::fixed);
+		std::cout.precision(7);
+		if(x!=0.0){
+			inclination[i]=y/x;
+		}
+		else
+		{
+			inclination[i]=0.0;
+		}
+		/*if(int(i)==int(mission.current_seq)-1){
+		std::cout <<"[inclina]#"<<i<<"\t"<<inclination[i]<<std::endl;
+		}*/
+		}
+}
+void EKF2::calculate_inclination_current(gps_message gps_msg)
+{
+	double x,y;
+	double check_inclination=0;
+	x=target_lat-gps_msg.lat;
+	y=target_lon-gps_msg.lon;
+	if(x!=0.0){
+		check_inclination=y/x;
+	}
+	std::cout.setf(std::ios::fixed);
+	std::cout.precision(7);
+	std::cout << "[Mission] #"<<mission.current_seq<<"\t"<<check_inclination<<std::endl;
+}
+void EKF2::print_target_gps()
+{
+	std::cout.unsetf(std::ios::fixed);
+
+	for (size_t i = 0; i < mission.count; i++) {
+		struct mission_item_s mission_item {};
+		if(int(mission.current_seq)==int(i)){
+			dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s));
+			target_lat=int32_t(mission_item.lat * long(pow(10,7)));
+			target_lon=int32_t(mission_item.lon * long(pow(10,7)));
+			target_alt=int32_t(mission_item.altitude * long(pow(10,4)));
+			//std::cout<<"start\t#"<<i<< "(lat/lon/alt)\t" << target_lat<< "\t"<<target_lon<<"\t"<<target_alt<<std::endl;
+		}
+	}
+}
 void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	const unsigned last_generation = _magnetometer_sub.get_last_generation();
--- a/home/Firmware/src/modules/ekf2/EKF2.hpp
View file @1fd483a
+++ b/home/Firmware/src/modules/ekf2/EKF2.hpp
View file @1fd483a
@@ -43,7 +43,7 @@
 #include "EKF2Selector.hpp"
 #include <float.h>
-
+#include <dataman/dataman.h>
 #include <containers/LockGuard.hpp>
 #include <drivers/drv_hrt.h>
 #include <lib/ecl/EKF/ekf.h>
@@ -155,14 +155,19 @@ private:
 	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateMagCalibration(const hrt_abstime &timestamp);
-
+	void print_target_gps();
+	void print_current_gps(gps_message gps_msg);
+	void calculate_inclination_target();
+	void calculate_inclination_current(gps_message gps_msg);
+	double* inclination=nullptr;
+	int32_t target_lat,target_lon,target_alt;
+	mission_s mission;
 	/*
 	 * Calculate filtered WGS84 height from estimated AMSL height
 	 */
 	float filter_altitude_ellipsoid(float amsl_hgt);
 	static constexpr float sq(float x) { return x * x; };
-
 	const bool _replay_mode{false};			///< true when we use replay data from a log
 	const bool _multi_mode;
 	int _instance{0};
@@ -176,6 +181,8 @@ private:
 	perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")};
 	perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")};
+	perf_counter_t _imu_missed_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
+	perf_counter_t _mag_missed_perf{perf_alloc(PC_COUNT, MODULE_NAME": mag message missed")};
 	// Used to check, save and use learned magnetometer biases
 	hrt_abstime _mag_cal_last_us{0};	///< last time the EKF was operating a mode that estimates magnetomer biases (uSec)