feat : detect abnormal gps

@@ -1251,6 +1251,8 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			_ekf.setGpsData(gps_msg);
 			_ekf.setGpsData(gps_msg);
 			if(hold != int(gps_msg.time_usec/1000000))
 			if(hold != int(gps_msg.time_usec/1000000))
 			{
 			{
+				if(!check_warning())
+				{
 				dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
 				dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
 				hold=int(gps_msg.time_usec/1000000);
 				hold=int(gps_msg.time_usec/1000000);
 				if(hold%2==0)
 				if(hold%2==0)
@@ -1258,7 +1260,9 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 					calculate_inclination_target();
 					calculate_inclination_target();
 					print_target_gps();
 					print_target_gps();
 					//print_current_gps(gps_msg);
 					//print_current_gps(gps_msg);
-					check_inclination(calculate_inclination_current(gps_msg));
+					double check=calculate_inclination_current(gps_msg);
+					check_inclination(check);
+				}
 				}
 				}
 			}
 			}
 			_gps_time_usec = gps_msg.time_usec;
 			_gps_time_usec = gps_msg.time_usec;
@@ -1269,15 +1273,26 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 void EKF2::check_inclination(double check)
 void EKF2::check_inclination(double check)
 {
 {
 	double origin=inclination[int(mission.current_seq)-1];
 	double origin=inclination[int(mission.current_seq)-1];
-	std:: cout << "origin : "<<origin<<"\tcheck : "<<check<<std::endl;
+	std:: cout << "(origin : "<<origin<<" | check : "<<check<<")";
 	if(origin-1<=check && check <= origin+1)
 	if(origin-1<=check && check <= origin+1)
 	{
 	{
-		std::cout<<"*----Good moving----*"<<std::endl;
+		std::cout<<"\t--- Good moving"<<std::endl;
+		warning_count=0;
 	}
 	}
 	else
 	else
 	{
 	{
-		std::cout<<"*----Bad moving----*"<<std::endl;
+		std::cout<<"\t--- Bad moving"<<std::endl;
+		warning_count++;
+	}
+}
+bool EKF2::check_warning()
+{
+	if(warning_count>=15)
+	{
+		std::cout<<"* [abnormal moving detected] *"<<std::endl;
+		return true;
 	}
 	}
+	return false;
 }
 }
 void EKF2::print_current_gps(gps_message gps_msg)
 void EKF2::print_current_gps(gps_message gps_msg)
 {
 {
@@ -1292,8 +1307,6 @@ void EKF2::calculate_inclination_target()
 		struct mission_item_s next_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, &mission_item, sizeof(mission_item_s));
 		dm_read((dm_item_t)mission.dataman_id, i+1, &next_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;
 		x=next_mission_item.lat-mission_item.lat;
 		y=next_mission_item.lon-mission_item.lon;
 		y=next_mission_item.lon-mission_item.lon;
 		std::cout.setf(std::ios::fixed);
 		std::cout.setf(std::ios::fixed);
@@ -1305,9 +1318,6 @@ void EKF2::calculate_inclination_target()
 		{
 		{
 			inclination[i]=0.0;
 			inclination[i]=0.0;
 		}
 		}
-		/*if(int(i)==int(mission.current_seq)-1){
-		std::cout <<"[inclina]#"<<i<<"\t"<<inclination[i]<<std::endl;
-		}*/
 		}
 		}
 }
 }
 double EKF2::calculate_inclination_current(gps_message gps_msg)
 double EKF2::calculate_inclination_current(gps_message gps_msg)
@@ -1321,10 +1331,9 @@ double EKF2::calculate_inclination_current(gps_message gps_msg)
 	}
 	}
 	std::cout.setf(std::ios::fixed);
 	std::cout.setf(std::ios::fixed);
 	std::cout.precision(7);
 	std::cout.precision(7);
-	//std::cout << "[Mission] #"<<mission.current_seq<<"\t"<<check_inclination<<std::endl;
 	return check_inclination;
 	return check_inclination;
 }
 }
-void EKF2::print_target_gps()
+void EKF2::set_target_gps()
 {
 {
 	std::cout.unsetf(std::ios::fixed);
 	std::cout.unsetf(std::ios::fixed);
 	for (size_t i = 0; i < mission.count; i++) {
 	for (size_t i = 0; i < mission.count; i++) {
@@ -1334,7 +1343,6 @@ void EKF2::print_target_gps()
 			target_lat=int32_t(mission_item.lat * long(pow(10,7)));
 			target_lat=int32_t(mission_item.lat * long(pow(10,7)));
 			target_lon=int32_t(mission_item.lon * 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)));
 			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;
 		}
 		}
 	}
 	}
 }
 }

@@ -135,12 +135,14 @@ private:
 	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps);
 	void UpdateMagCalibration(const hrt_abstime &timestamp);
 	void UpdateMagCalibration(const hrt_abstime &timestamp);
-	void print_target_gps();
+	void set_target_gps();
 	void print_current_gps(gps_message gps_msg);
 	void print_current_gps(gps_message gps_msg);
 	void calculate_inclination_target();
 	void calculate_inclination_target();
 	double calculate_inclination_current(gps_message gps_msg);
 	double calculate_inclination_current(gps_message gps_msg);
 	void check_inclination(double check);
 	void check_inclination(double check);
+	bool check_warning();
 	double* inclination=nullptr;
 	double* inclination=nullptr;
+	int warning_count=0;
 	int32_t target_lat,target_lon,target_alt;
 	int32_t target_lat,target_lon,target_alt;
 	mission_s mission;
 	mission_s mission;
 	/*
 	/*