Merge pull request #11 from SweeWarman/master

Updated version number
nasa · Jul 28, 2017 · 8d38535 · 8d38535
2 parents 4acdd08 + 19b579c
commit 8d38535
Show file tree

Hide file tree

Showing 19 changed files with 51 additions and 16 deletions.
diff --git a/C++/RELEASE-NOTES.txt b/C++/RELEASE-NOTES.txt
@@ -1,9 +1,15 @@
 ICAROUS: Integrated Configurable Algorithms for Reliable Operations of Unmanned Systems
 -------------------------------------------------------------------------
-Release: 1.1, March 18, 2017
+Release: 1.2.2, July 28, 2017
 Authors: Swee Balachadran, Cesar Munoz
 Contact: Cesar A. Munoz ([email protected])
 
+Release ICAROUS 1.2.2 (July 28, 2017)
+-------------------------------------
+- Bug fix in DAA functionality.
+- Minor changes to logging DAA events
+- ICAROUS sets original mission speed after resolutions are completed.
+
 Release ICAROUS 1.1 (March 18, 2017)
 ----------------
 - Includes DAIDALUS 1.0.1.

diff --git a/C++/include/ICAROUS/AircraftData.h b/C++/include/ICAROUS/AircraftData.h
@@ -70,6 +70,7 @@ class AircraftData_t{
 		bool reset;
 		double speed;
 		double acTime;
+		double missionSpeed;
 
         uint16_t nextMissionWP;
         uint16_t nextResolutionWP;
@@ -97,6 +98,7 @@ class AircraftData_t{
         void GetTraffic(int id,Position &pos,Velocity &vel);
         void ClearMissionList();
         void Reset();
+        double getFlightPlanSpeed(Plan *fp,int nextWP);
         void AddTrackingObject(int id,double x,double y,double z,double vx,double vy,double vz);
         void AddMissionObject(int id,double x,double y,double z,double vx,double vy,double vz);
 };

diff --git a/C++/params/ISAAC/IsaacDAA.txt → C++/params/ISAAC/IsaacDAA_Tau00.txt b/C++/params/ISAAC/IsaacDAA.txt → C++/params/ISAAC/IsaacDAA_Tau00.txt
diff --git a/C++/params/ISAAC/IsaacDAA_Tau.txt → C++/params/ISAAC/IsaacDAA_Tau15.txt b/C++/params/ISAAC/IsaacDAA_Tau.txt → C++/params/ISAAC/IsaacDAA_Tau15.txt
diff --git a/C++/params/ISAAC/IsaacInput01.txt b/C++/params/ISAAC/IsaacInput01.txt
@@ -41,7 +41,7 @@ TRK_DISTH = 3
 TRK_DISTV = 0
 
 #DAA parameters
-DAA_CONFIG = params/ISAAC/IsaacDAA.txt
+DAA_CONFIG = params/ISAAC/IsaacDAA_Tau00.txt
 CHEAP_DAA = 1
 GOTO_NEXTWP = 1
 CONFLICT_HOLD = 3

diff --git a/C++/params/ISAAC/IsaacInput02.txt b/C++/params/ISAAC/IsaacInput02.txt
@@ -41,7 +41,7 @@ TRK_DISTH = 3
 TRK_DISTV = 0
 
 #DAA parameters
-DAA_CONFIG = params/ISAAC/IsaacDAA_Tau.txt
+DAA_CONFIG = params/ISAAC/IsaacDAA_Tau15.txt
 CHEAP_DAA = 1
 GOTO_NEXTWP = 1
 CONFLICT_HOLD = 3

diff --git a/C++/src/ICAROUS/AircraftData.cpp b/C++/src/ICAROUS/AircraftData.cpp
@@ -54,6 +54,7 @@
      maneuverVe = 0;
      maneuverVn = 0;
      maneuverVu = 0;
+     missionSpeed = 1;
      reset = false;
  }
 
@@ -104,7 +105,7 @@ void AircraftData_t::ConstructPlan(){
 			double wptime = 0;
 			if(ic > 0){
 
-				double vel = it->param4;
+				double vel = missionSpeed;
 
 				if(vel < 0.5){
 					vel = 1;
@@ -116,6 +117,8 @@ void AircraftData_t::ConstructPlan(){
 
 			NavPoint navPoint(WP,wptime);
 			MissionPlan.addNavPoint(navPoint);
+		}else if(it->command == MAV_CMD_DO_CHANGE_SPEED){
+			missionSpeed = it->param2;
 		}
 	}
 }
@@ -198,6 +201,11 @@ void AircraftData_t::GetGeofence(Interface_t *gsIntf,mavlink_command_long_t msgI
 	}
 }
 
+double AircraftData_t::getFlightPlanSpeed(Plan* fp,int nextWP){
+	double speed = fp->pathDistance(nextWP-1,true)/(fp->time(nextWP) - fp->time(nextWP-1));
+	return speed;
+}
+
 void AircraftData_t::AddTraffic(int id,double lat,double lon,double alt,double vx,double vy,double vz){
 
 	pthread_mutex_lock(&lock);

diff --git a/C++/src/ICAROUS/Icarous.cpp b/C++/src/ICAROUS/Icarous.cpp
@@ -38,11 +38,11 @@
 #include "Icarous.h"
 #include "Constants.h"
 
-std::string Icarous_t::VERSION = "1.2.1";
+std::string Icarous_t::VERSION = "1.2.2";
 
 std::string Icarous_t::release() {
 	return "ICAROUS++ V-"+VERSION+
-			"-FormalATM-"+Constants::version+" (July-24-2017)";
+			"-FormalATM-"+Constants::version+" (July-28-2017)";
 }
 
 Icarous_t::Icarous_t(int argc,char* argv[],Mission_t* task){

diff --git a/C++/src/ICAROUS/QuadFMS.cpp b/C++/src/ICAROUS/QuadFMS.cpp
@@ -234,6 +234,7 @@ uint8_t QuadFMS_t::Resolve(){
 			if(resumeMission){
 				resolutionState = IDLE_r;
 				SetMissionItem(FlightData->nextMissionWP);
+				SetSpeed(FlightData->getFlightPlanSpeed(&FlightData->MissionPlan,FlightData->nextMissionWP));
 				planType = QuadFMS_t::MISSION;
 				SetMode(AUTO);
 			}

diff --git a/Java/RELEASE-NOTES.txt b/Java/RELEASE-NOTES.txt
@@ -1,11 +1,21 @@
 ICAROUS: Integrated Configurable Algorithms for Reliable Operations of Unmanned Systems
 -------------------------------------------------------------------------
-Release: 1.1, March 18, 2017
+Release: 1.2.2, July 28, 2017
 Authors: Swee Balachadran, Cesar Munoz
 Contact: Cesar A. Munoz ([email protected])
 
+Release ICAROUS 1.2.2 (July 28, 2017)
+-------------------------------------
+- Minor changes to logging DAA events
+- ICAROUS sets original mission speed after resolutions are completed.
+
+Release ICAROUS 1.2.1 (July 24, 2017)
+-------------------------------------
+- Implementation of Safeguard interface for ICAROUS.
+- Setting MISSION speed is disabled within ICAROUS.
+
 Release ICAROUS 1.1 (March 18, 2017)
-----------------
+------------------------------------
 - Includes DAIDALUS 1.0.1.
 - ICAROUS has been extensively tested using the Ardupilot flight stack. Several minor bugs have been addressed.
 - C++ interface has been debugged and added to the code.

diff --git a/Java/params/AmaseConfig.txt b/Java/params/AmaseConfig.txt
@@ -41,6 +41,7 @@ TRK_DISTH = 3
 TRK_DISTV = 0
 
 #DAA parameters
+DAA_CONFIG = params/DaidalusQuadConfig.txt
 CHEAP_DAA = 0
 GOTO_NEXTWP = 1
 CONFLICT_HOLD = 3

diff --git a/Java/params/ISAAC/IsaacDAA.txt → Java/params/ISAAC/IsaacDAA_Tau00.txt b/Java/params/ISAAC/IsaacDAA.txt → Java/params/ISAAC/IsaacDAA_Tau00.txt
diff --git a/Java/params/ISAAC/IsaacDAA_Tau.txt → Java/params/ISAAC/IsaacDAA_Tau15.txt b/Java/params/ISAAC/IsaacDAA_Tau.txt → Java/params/ISAAC/IsaacDAA_Tau15.txt
diff --git a/Java/params/ISAAC/IsaacInput01.txt b/Java/params/ISAAC/IsaacInput01.txt
@@ -41,7 +41,7 @@ TRK_DISTH = 3
 TRK_DISTV = 0
 
 #DAA parameters
-DAA_CONFIG = params/ISAAC/IsaacDAA.txt
+DAA_CONFIG = params/ISAAC/IsaacDAA_Tau00.txt
 CHEAP_DAA = 1
 GOTO_NEXTWP = 1
 CONFLICT_HOLD = 3

diff --git a/Java/params/ISAAC/IsaacInput02.txt b/Java/params/ISAAC/IsaacInput02.txt
@@ -41,7 +41,7 @@ TRK_DISTH = 3
 TRK_DISTV = 0
 
 #DAA parameters
-DAA_CONFIG = params/ISAAC/IsaacDAA_Tau.txt
+DAA_CONFIG = params/ISAAC/IsaacDAA_Tau15.txt
 CHEAP_DAA = 1
 GOTO_NEXTWP = 1
 CONFLICT_HOLD = 3

diff --git a/Java/src/gov/nasa/larcfm/ICAROUS/AircraftData.java b/Java/src/gov/nasa/larcfm/ICAROUS/AircraftData.java
@@ -112,6 +112,8 @@ public enum FP_READ_COM{
 	public double planTime;
 	public ParameterData pData;
 	public boolean reset;
+
+	public double missionSpeed;
 
 	double crossTrackDeviation;
 	double crossTrackOffset;
@@ -136,6 +138,7 @@ public AircraftData(ParameterData pdata){
 		MissionPlan         = new Plan();
 		ResolutionPlan      = new Plan();
 		speed               = 1;
+		missionSpeed        = 1;
 	}
 
 	// Function to get flight plan
@@ -249,7 +252,8 @@ public void ConstructFlightPlan(){
 				double wptime= 0;
 				Position nextWP = Position.makeLatLonAlt(msgMissionItem.x,"degree",msgMissionItem.y,"degree",msgMissionItem.z,"m");				
 				if(count > 0 ){
-					double vel = msgMissionItem.param4;
+					//double vel = msgMissionItem.param4;
+					double vel = missionSpeed;
 					if(vel < 0.5){
 						vel = 1;
 					}
@@ -265,6 +269,8 @@ public void ConstructFlightPlan(){
 				}		     
 				MissionPlan.addNavPoint(new NavPoint(nextWP,wptime));
 				count++;
+			}else if(msgMissionItem.command == MAV_CMD.MAV_CMD_DO_CHANGE_SPEED){
+				missionSpeed = msgMissionItem.param2;					
 			}
 		}
 		numMissionWP           = MissionPlan.size();
@@ -273,8 +279,7 @@ public void ConstructFlightPlan(){
 
 	public float GetFlightPlanSpeed(Plan fp,int nextWP){
 
-		Plan CurrentFlightPlan = fp;
-		System.out.println("nextWP:"+nextWP);
+		Plan CurrentFlightPlan = fp;		
 		float speed = (float)(CurrentFlightPlan.pathDistance(nextWP-1,true)/
 				(CurrentFlightPlan.time(nextWP) -
 						CurrentFlightPlan.time(nextWP-1)));

diff --git a/Java/src/gov/nasa/larcfm/ICAROUS/Icarous.java b/Java/src/gov/nasa/larcfm/ICAROUS/Icarous.java
@@ -45,7 +45,7 @@
 
 public class Icarous{
 
-	public static final String VERSION = "1.2.1";
+	public static final String VERSION = "1.2.2";
 
         private boolean verbose;
 	private String sitlhost;
@@ -205,7 +205,7 @@ else if(args[i].startsWith("-")) {
 
 	public static String release() {
 		return "ICAROUSj V-"+VERSION+
-				"-FormalATM-"+Constants.version+" (July-24-2017)"; 
+				"-FormalATM-"+Constants.version+" (July-28-2017)"; 
 	}
 
 	public void run(){

diff --git a/Java/src/gov/nasa/larcfm/ICAROUS/QuadFMS.java b/Java/src/gov/nasa/larcfm/ICAROUS/QuadFMS.java
@@ -279,6 +279,8 @@ else if(Detector.flightPlanDeviationConflict){
 					resolveState = resolve_state_t.IDLE;
 					planType = plan_type_t.MISSION;
 					SetMissionItem(FlightData.nextMissionWP);
+					System.out.println("Setting mission speed:"+FlightData.GetFlightPlanSpeed(FlightData.MissionPlan, FlightData.nextMissionWP));
+					SetSpeed(FlightData.GetFlightPlanSpeed(FlightData.MissionPlan, FlightData.nextMissionWP));
 					SetMode(ARDUPILOT_MODES.AUTO);					
 				}
 				else{

diff --git a/README.md b/README.md
@@ -16,7 +16,7 @@ safety criteria are violated or about to be violated.
 
 ### Current Release
 
-ICAROUS V-1.2.1 - July 24, 2017
+ICAROUS V-1.2.2 - July 28, 2017
 
 ### License