Merge pull request #50 from UWARG/Killout

Minor changes to path following/kill modes

Autopilot/AttitudeManager/Anaconda.c

@@ -180,7 +180,7 @@ void checkLimits(int* channels){
 void startArm()
 {
     vehicleArmed = 1;
-    armVehicle(2000);
+    armVehicle(500);
 }
 
 void stopArm()

Autopilot/AttitudeManager/AttitudeManager.c

@@ -228,7 +228,7 @@ char checkDMA(){
     //Transfer data from PATHMANAGER CHIP
     lastNumSatellites = gps_Satellites; //get the last number of satellites
     DMADataAvailable = 0;
-    if (generatePMDataDMAChecksum1() == pmData.checkbyteDMA1 && generatePMDataDMAChecksum2() == pmData.checkbyteDMA2) {
+    if (generatePMDataDMAChecksum1() == pmData.checkbyteDMA1 && generatePMDataDMAChecksum2() == pmData.checkbyteDMA2 && pmData.longitude < -98.0 && pmData.latitude > 49.0) {
         gps_Time = pmData.time;
         input_AP_Altitude = pmData.sp_Altitude;
         gps_Satellites = pmData.satellites;
@@ -1057,15 +1057,15 @@ void checkHeartbeat(){
 }
 
 void checkGPS(){
-    if (gps_PositionFix == 0){
-        setProgramStatus(KILL_MODE_WARNING);
-        if (getTime() - gpsTimer > GPS_TIMEOUT){
-            killPlane(TRUE);
-        }
-    }
-    else{
-        gpsTimer = getTime();
-    }
+//    if (gps_PositionFix == 0){
+//        setProgramStatus(KILL_MODE_WARNING);
+//        if (getTime() - gpsTimer > GPS_TIMEOUT){
+//            killPlane(TRUE);
+//        }
+//    }
+//    else{
+//        gpsTimer = getTime();
+//    }
 }
 
 

Autopilot/AttitudeManager/StateMachine.c

@@ -69,10 +69,6 @@ void StateMachine(char entryLocation){
         lowLevelControl();
 
     }
-    else if(UPLINK_CHECK_FREQUENCY <= uplinkTimer){
-        uplinkTimer = 0;
-        readDatalink();
-    }
     else if(isDMADataAvailable() && checkDMA()){
         //Input from Controller
         inputCapture();
@@ -82,6 +78,12 @@ void StateMachine(char entryLocation){
     }
     else{
     }
+
+    if(UPLINK_CHECK_FREQUENCY <= uplinkTimer){
+        uplinkTimer = 0;
+        readDatalink();
+    }
+
     if(P0_SEND_FREQUENCY <= downlinkP0Timer){
 //        debug("P0");
         //Compile and send data

Autopilot/AttitudeManager/main.h

@@ -27,7 +27,7 @@
 #define ATTITUDE_MANAGER !PATH_MANAGER
 
 //Define this for competition, includes higher restrictions for safety
-#define COMP_MODE 0
+#define COMP_MODE 1
 
 /* CHANGE THIS HEADER FILE WHEN MODIFYING THIS FOR A NEW PLANE OR VEHICLE */
 #define ANACONDA_VEHICLE 1

Autopilot/AttitudeManager/net.h

@@ -9,7 +9,7 @@
 extern "C" {
 #endif
 
-#define P0_SEND_FREQUENCY 250 //Time in miliseconds
+#define P0_SEND_FREQUENCY 300 //Time in miliseconds
 #define P1_SEND_FREQUENCY 1000 //Time in milliseconds
 #define P2_SEND_FREQUENCY 20000 //Time in milliseconds
 #define UPLINK_CHECK_FREQUENCY 100 //Time in milliseconds
@@ -41,7 +41,7 @@ extern "C" {
 
 #define GPS_TIMEOUT 30000 //In Milliseconds
 
-#define HEARTBEAT_KILL_TIMEOUT 120000 //In Milliseconds
+#define HEARTBEAT_KILL_TIMEOUT 20000 //In Milliseconds
 
 #define UHF_KILL_TIMEOUT 10000
 

Autopilot/Path Manager/PathManager.c

@@ -46,8 +46,8 @@ char pathCount = 0;
 
 int lastKnownHeadingHome = 10;
 char returnHome = 0;
-char doProbeDrop;
-char followPath;
+char doProbeDrop = 0;
+char followPath = 0;
 
 void pathManagerInit(void) {
 
@@ -153,7 +153,7 @@ void pathManagerRuntime(void) {
 
     if (returnHome || (pathCount - currentIndex < 1 && pathCount >= 0)){
         pmData.sp_Heading = lastKnownHeadingHome;
-    } else if (pathCount - currentIndex >= 1 && pmData.positionFix > 0) {
+    } else if (followPath && pathCount - currentIndex >= 1 && pmData.positionFix > 0) {
         currentIndex = followWaypoints(path[currentIndex], (float*)position, heading, (int*)&pmData.sp_Heading);
     }
     if (pmData.positionFix >= 1){
@@ -446,10 +446,8 @@ float followStraightPath(float* waypointDirection, float* targetWaypoint, float*
     }
 
     float pathError = -sin(courseAngle) * (position[0] - targetWaypoint[0]) + cos(courseAngle) * (position[1] - targetWaypoint[1]);
-    char str[20];
-    sprintf(str,"PathError: %f",pathError);
-    debug(str);
-    return 90 - rad2deg(courseAngle - MAX_PATH_APPROACH_ANGLE * 2/PI * atan(k_gain[PATH] * pathError)); //Heading in degrees (magnetic)
+    float calcHeading = 90 - rad2deg(courseAngle - MAX_PATH_APPROACH_ANGLE * 2/PI * atan(k_gain[PATH] * pathError)); //Heading in degrees (magnetic)
+    return calcHeading;
 
 }
 
@@ -630,7 +628,7 @@ unsigned int insertPathNode(PathData* node, unsigned int previousID, unsigned in
 }
 
 void copyGPSData(){
-    if (newGPSDataAvailable){
+    if (newGPSDataAvailable && gpsData.longitude < -98.0 && gpsData.latitude > 49.0){ //Hack fix for competition
         newGPSDataAvailable = 0;
         pmData.time = gpsData.time;
         pmData.longitude = gpsData.longitude;
@@ -657,7 +655,7 @@ void checkAMData(){
     if (amData.checkbyteDMA == generateAMDataDMACheckbyte()){
         if (lastAMDataChecksum != amData.checksum && amData.checksum == generateAMDataChecksum(&amData)){
             lastAMDataChecksum = amData.checksum;
-            debug("Command");
+//            debug("Command");
            // All commands/actions that need to be run go here
            switch (amData.command){
                 case PM_DEBUG_TEST:
@@ -671,6 +669,11 @@ void checkAMData(){
                     node->radius = amData.waypoint.radius;
                     node->type = amData.waypoint.type;
                     appendPathNode(node);
+//                    debug("new");
+//                    char str[20];
+//                    sprintf(str, "Lat: %f, Lon: %f, count: %d", node->latitude, node->longitude, pathCount);
+//                    debug(str);
+
                     break;
                 case PM_CLEAR_WAYPOINTS:
                     clearPathNodes();
@@ -704,7 +707,6 @@ void checkAMData(){
 //                    node->radius = 1; //Arbitrary value
 //                    node->type = DEFAULT_WAYPOINT;
                     if (path[getIndexFromID(amData.waypoint.id)] && path[getIndexFromID(amData.waypoint.id)]->previous){
-                        debug("TEST");
 //                        insertPathNode(node,path[getIndexFromID(amData.waypoint.id)]->previous->id,amData.waypoint.id);
                         currentIndex = getIndexFromID(amData.waypoint.id);
                     }