Merge pull request #7 from SweeWarman/master

Updates after CERTAIN 2 tests

.gitignore

@@ -2,7 +2,7 @@
 *.o
 .*
 *.dSYM
-icarous
+icarous.jar
 *.parm
 *.tlog.raw
 *.tlog
@@ -13,4 +13,10 @@ Java/params/
 C++/DAAGeofencingExample
 *.bin
 *.so
-*.tbl
+*.tbl
+*.bin
+elf2cfetbl
+!CFS/build/cpu1/Makefile
+!CFS/build/cpu1/exe/
+CFS/build/cpu1/*
+CFS/build/mission_inc/*

C++/include/ICAROUS/COM.h

@@ -73,6 +73,7 @@
         void CommandLongHandler();
         void CommandIntHandler();
         void KinematicBandsHandler();
+	void HandleUnprocessedMsgs();
  };
 
  #endif

C++/include/ICAROUS/Interface.h

@@ -63,7 +63,8 @@ class Interface_t{
   uint8_t recvbuffer[BUFFER_LENGTH];
   uint8_t sendbuffer[BUFFER_LENGTH];
   MAVLinkMessages_t *RcvdMessages;
-  pthread_mutex_t  lock;
+  pthread_mutex_t  lockrx;
+  pthread_mutex_t  locktx;
   mavlink_status_t lastStatus;
 
 
@@ -80,6 +81,7 @@ class Interface_t{
   uint8_t* GetRecvBuffer();
   virtual int ReadData(){return 0;};
   virtual void WriteData(uint8_t buffer[], uint16_t len){return;};
+  void PipeThrough(Interface_t *intf,int32_t len);
 };
 
 class SerialInterface_t: public Interface_t{

C++/run.sh

@@ -6,7 +6,7 @@ SITL_INPUT_PORT=14551
 COM_HOST=127.0.0.1
 COM_INPUT_PORT=14552
 COM_OUTPUT_PORT=14553
-PX4_PORT=/dev/ttyO1
+PX4_PORT=/dev/ttyACM0
 PX4_BAUD=57600
 #GS_MASTER=192.42.142.110:$COM_OUTPUT_PORT
 GS_MASTER=127.0.0.1:$COM_OUTPUT_PORT
@@ -29,7 +29,7 @@ elif [ "$1" == 'PX4' ];then
        ./icarous --verbose \
 	--px4 $PX4_PORT --px4baud $PX4_BAUD \
 	--gshost $COM_HOST --gsin $COM_INPUT_PORT --gsout $COM_OUTPUT_PORT \
-	--mode --config $2 $3 $MODE
+	--mode $MODE --config $2 $3
 
 elif [ "$1" == 'PX4R' ];then
     echo "Launching ICAROUS with Pixhawk and Radio"

C++/src/ICAROUS/COM.cpp

@@ -75,6 +75,9 @@ COM_t::COM_t(Interface_t* px4int, Interface_t* gsint,AircraftData_t *fdata):log(
          // Handle parameter set
          ParamSetHandler();
 
+	 // Handle parameter values
+	 ParamValueHandler();
+
          // Handle set mode
          SetModeHandler();
 
@@ -84,6 +87,9 @@ COM_t::COM_t(Interface_t* px4int, Interface_t* gsint,AircraftData_t *fdata):log(
          // Handle kinematic bands
          KinematicBandsHandler();
 
+	 // Handle any unprocessed messaged
+	 //HandleUnprocessedMsgs();
+
      }
 
 
@@ -142,11 +148,12 @@ COM_t::COM_t(Interface_t* px4int, Interface_t* gsint,AircraftData_t *fdata):log(
 
  void COM_t::ParamRequestListHandler(){
      mavlink_param_request_list_t msg;
-     bool have_msg = RcvdMessages->GetParamRequestList(msg);
+     bool have_msg = RcvdMessages->GetParamRequestList(msg);     
      if(have_msg){
+         std::cout<<"received param request handler"<<std::endl;
          mavlink_message_t msg2send;
          mavlink_msg_param_request_list_encode(255,0,&msg2send,&msg);
-         px4Intf->SendMAVLinkMsg(msg2send);
+         px4Intf->SendMAVLinkMsg(msg2send);	 
      }
  }
 
@@ -253,8 +260,7 @@ COM_t::COM_t(Interface_t* px4int, Interface_t* gsint,AircraftData_t *fdata):log(
      bool have_msg = RcvdMessages->GetParamValue(msg);
      if(have_msg){
          mavlink_message_t msg2send;
-         mavlink_msg_param_value_encode(255,0,&msg2send,&msg);
-
+         mavlink_msg_param_value_encode(255,0,&msg2send,&msg);	
      }
  }
 
@@ -276,9 +282,9 @@ COM_t::COM_t(Interface_t* px4int, Interface_t* gsint,AircraftData_t *fdata):log(
          FlightData->SetStartMissionFlag((uint8_t)msg.param1);
          cout<<"Flying to: "<<msg.param1<<endl;
      }
-     else if(have_msg && msg.command == MAV_CMD_DO_FENCE_ENABLE){
-    	 log.addWarning("Receiving geofence");
+     else if(have_msg && msg.command == MAV_CMD_DO_FENCE_ENABLE){   
     	 FlightData->GetGeofence(gsIntf,msg);
+	 log.addWarning("Received geofence");
      }
      else if(have_msg && msg.command == MAV_CMD_SPATIAL_USER_1){
     	 FlightData->AddTraffic((int)msg.param1,msg.param5,msg.param6,msg.param7,
@@ -314,3 +320,31 @@ COM_t::COM_t(Interface_t* px4int, Interface_t* gsint,AircraftData_t *fdata):log(
 	     gsIntf->SendMAVLinkMsg(msg2send);
 	 }
  }
+
+void COM_t::HandleUnprocessedMsgs(){
+
+  while(!gsIntf->msgQueue.empty()){
+    mavlink_message_t msg = gsIntf->msgQueue.front();
+    gsIntf->msgQueue.pop();
+
+    switch(msg.msgid){
+    case MAVLINK_MSG_ID_MISSION_COUNT: break;
+    case MAVLINK_MSG_ID_MISSION_ITEM: break;
+    case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:break;
+    case MAVLINK_MSG_ID_MISSION_REQUEST:break;
+    case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:break;
+    case MAVLINK_MSG_ID_PARAM_REQUEST_READ:break;
+    case MAVLINK_MSG_ID_PARAM_VALUE:break;
+    case MAVLINK_MSG_ID_PARAM_SET:break;
+    case MAVLINK_MSG_ID_COMMAND_LONG:break;
+    case MAVLINK_MSG_ID_COMMAND_INT:break;
+    case MAVLINK_MSG_ID_SET_MODE:break;
+    case MAVLINK_MSG_ID_FENCE_POINT:break;
+    default:
+      px4Intf->SendMAVLinkMsg(msg);
+      //printf("message id %d\n",msg.msgid);
+      break;
+
+  }
+  }
+}

C++/src/ICAROUS/DAQ.cpp

@@ -42,11 +42,13 @@ DAQ_t::DAQ_t(Interface_t* px4int, Interface_t* gsint):log("DAQ"){
  void DAQ_t::GetPixhawkData(){
      while(true){
          // Get data from the pixhawk
-         px4Intf->GetMAVLinkMsg();
+         int n = px4Intf->GetMAVLinkMsg();
 
-         // Send the raw data in the queue to the send interface
+	 //px4Intf->PipeThrough(gsIntf,n);
+
+         // Send the mavlink messages in the queue to the ground station interface
          while(!px4Intf->msgQueue.empty()){
-            gsIntf->SendMAVLinkMsg(px4Intf->msgQueue.front());
+	    gsIntf->SendMAVLinkMsg(px4Intf->msgQueue.front());
             px4Intf->msgQueue.pop();
          }
      }

C++/src/ICAROUS/GeoFence.cpp

@@ -138,7 +138,7 @@ void Geofence_t::CheckViolation(AircraftState acState,double elapsedTime,Plan fp
 												LLA.altitude(),"ft");
 
 
-	    geoCDIIPolygon.detection(fp,geoPolyPath,0,fp.getLastTime());
+	    geoCDIIPolygon.detection(fp,geoPolyPath,elapsedTime,fp.getLastTime());
 	    bool val = CollisionDetection(currentPosLLA,currentVel.vect2(),0,lookahead);
 	    if(val){
 	    	conflict = true;

C++/src/ICAROUS/Icarous.cpp

@@ -38,11 +38,11 @@
 #include "Icarous.h"
 #include "Constants.h"
 
-std::string Icarous_t::VERSION = "1.1";
+std::string Icarous_t::VERSION = "1.2.1";
 
 std::string Icarous_t::release() {
 	return "ICAROUS++ V-"+VERSION+
-			"-FormalATM-"+Constants::version+" (March-18-2017)";
+			"-FormalATM-"+Constants::version+" (July-24-2017)";
 }
 
 Icarous_t::Icarous_t(int argc,char* argv[],Mission_t* task){
@@ -76,17 +76,16 @@ void Icarous_t::GetOptions(int argc,char* argv[]){
 		  {"radio",    required_argument, 0, 'j'},
 		  {"radiobaud",required_argument, 0, 'k'},
 		  {"mode",     required_argument, 0, 'l'},
-		  {"debug",          no_argument, 0, 'm'},
-		  {"config",   required_argument, 0, 'n'},
+		  {"config",   required_argument, 0, 'm'},
+		  {"debug",          no_argument, 0, 'n'},		  
 		  {0,                          0, 0,   0}
 	  };
 
 	  int option_index = 0;
 
-	  c = getopt_long (argc, argv, "ab:c:d:e:f:g:h:i:j:k:l:",
+	  c = getopt_long (argc, argv, "ab:c:d:e:f:g:h:i:j:k:l:m:n",
 					   long_options, &option_index);
-
-
+
 	  if (c == -1)
 		break;
 
@@ -99,67 +98,67 @@ void Icarous_t::GetOptions(int argc,char* argv[]){
 		case 'b':
 		  //printf ("option -c with value `%s'\n", optarg);
 		  strcpy(px4port,optarg);
-		  //printf("Connecting to pixhawk at %s\n",px4port);
+		  printf("Connecting to pixhawk at %s\n",px4port);
 		  break;
 
 		case 'c':
 		  px4baud = atoi(optarg);
-		  //printf("pixhawk port baud rate %d\n",px4baud);
+		  printf("pixhawk port baud rate %d\n",px4baud);
 		  break;
 
 		case 'd':
 		  strcpy(sitlhost,optarg);
-		  //printf("Connecting to SITL host: %s\n",sitlhost);
+		  printf("Connecting to SITL host: %s\n",sitlhost);
 		  break;
 
 		case 'e':
 		  sitlin = atoi(optarg);
-		  //printf("SITL host input at port: %d\n",sitlin);
+		  printf("SITL host input at port: %d\n",sitlin);
 		  break;
 
 		case 'f':
 		  sitlout = atoi(optarg);
-		  //printf("SITL host output at port: %d\n",sitlout);
+		  printf("SITL host output at port: %d\n",sitlout);
 		  break;
 
 		case 'g':
 		  strcpy(gshost,optarg);
-		  //printf("Ground station host: %s\n",gshost);
+		  printf("Ground station host: %s\n",gshost);
 		  break;
 
 		case 'h':
 		  gsin = atoi(optarg);
-		  //printf("Ground station host input at port:%d\n",gsin);
+		  printf("Ground station host input at port:%d\n",gsin);
 		  break;
 
 		case 'i':
 		  gsout = atoi(optarg);
-		  //printf("Ground station host output at port:%d\n",gsout);
+		  printf("Ground station host output at port:%d\n",gsout);
 		  break;
 
 		case 'j':
 		  strcpy(gsradio,optarg);
-		  //printf("Connecting to radio on port %s\n",gsradio);
+		  printf("Connecting to radio on port %s\n",gsradio);
 		  break;
 
 		case 'k':
 		  radiobaud = atoi(optarg);
-		  //printf("Radio baud rate %d\n",radiobaud);
+		  printf("Radio baud rate %d\n",radiobaud);
 		  break;
 
 		case 'l':
 		  strcpy(mode,optarg);
-		  //printf("Launching ICAROUS in %s mode\n",mode);
+		  printf("Launching ICAROUS in %s mode\n",mode);
 		  break;
 
-		case 'm':
-		  //printf("debug mode enabled\n");
+		case 'n':
+		  printf("debug mode enabled\n");
 		  debug = true;
 		  break;
 
-		case 'n':
+		case 'm':
 		  strcpy(config,optarg);
-		  //printf("config file %s\n",config);
+		  printf("config file %s\n",config);
 		  break;
 
 		case '?':
@@ -172,7 +171,7 @@ void Icarous_t::GetOptions(int argc,char* argv[]){
 }
 
 void Icarous_t::Run(){
-
+  
 	// Read parameters from file and get the parameter data container
 	ifstream ConfigFile;
 	SeparatedInput sepInputReader(&ConfigFile);
@@ -189,10 +188,11 @@ void Icarous_t::Run(){
 
 	SerialInterface_t apPort,gsPort;
 	SocketInterface_t SITL,comSock;
-
+	
 	if(px4baud > 0){
 		apPort = SerialInterface_t(px4port,px4baud,0,&RcvdMessages);
 		AP     = &apPort;
+		AP->EnableDataStream(1);
 	}
 	else{
 		SITL  = SocketInterface_t(sitlhost,sitlin,sitlout,&RcvdMessages);
@@ -201,7 +201,7 @@ void Icarous_t::Run(){
 
 	if(radiobaud > 0){
 		gsPort = SerialInterface_t(gsradio,radiobaud,0,&RcvdMessages);
-		COM    = &gsPort;
+		COM    = &gsPort;		
 	}
 	else{
 		comSock = SocketInterface_t(gshost,gsin,gsout,&RcvdMessages);