Fixed APO bug in current waypoint update.

HAJMOUSA · Oct 27, 2011 · 9192143 · 9192143
1 parent fc12113
commit 9192143
Show file tree

Hide file tree

Showing 4 changed files with 6 additions and 5 deletions.
diff --git a/.gitignore b/.gitignore
@@ -16,4 +16,5 @@ build
 /Tools/ArdupilotMegaPlanner/bin/APM Planner.app
 /Tools/ArdupilotMegaPlanner/obj
 /Tools/ArdupilotMegaPlanner/resedit/bin
-/Tools/ArdupilotMegaPlanner/resedit/obj
+/Tools/ArdupilotMegaPlanner/resedit/obj
+tags
diff --git a/ArduRover/CarStampede.h b/ArduRover/CarStampede.h
@@ -11,6 +11,7 @@
 
 // vehicle options
 static const apo::vehicle_t vehicle = apo::VEHICLE_CAR;
+//static const apo::halMode_t halMode = apo::MODE_HIL_CNTL;
 static const apo::halMode_t halMode = apo::MODE_LIVE;
 static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
 static const uint8_t heartBeatTimeout = 3;

diff --git a/libraries/APO/AP_Autopilot.cpp b/libraries/APO/AP_Autopilot.cpp
@@ -80,18 +80,17 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
         } else if (hal->getMode() == MODE_HIL_CNTL) { // hil
             hal->hil->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
             hal->hil->receive();
-            Serial.println("HIL Receive Called");
             if (_navigator->getTimeStamp() != 0) {
                 // give hil a chance to send some packets
                 for (int i = 0; i < 5; i++) {
                     hal->debug->println_P(PSTR("reading initial hil packets"));
-                    hal->gcs->sendText(SEVERITY_LOW,
-                                       PSTR("reading initial hil packets"));
+                    hal->gcs->sendText(SEVERITY_LOW, PSTR("reading initial hil packets"));
                     delay(1000);
                 }
                 break;
             }
             hal->debug->println_P(PSTR("waiting for hil packet"));
+			hal->gcs->sendText(SEVERITY_LOW, PSTR("waiting for hil packets"));
         }
         delay(500);
     }

diff --git a/libraries/APO/AP_Guide.cpp b/libraries/APO/AP_Guide.cpp
@@ -128,7 +128,7 @@ void MavlinkGuide::nextCommand() {
         _nextCommandCalls = 0;
     }
 
-    _cmdIndex = getNextIndex();
+    setCurrentIndex(getNextIndex());
     //Serial.print("cmd       : "); Serial.println(int(_cmdIndex));
     //Serial.print("cmd prev  : "); Serial.println(int(getPreviousIndex()));
     //Serial.print("cmd num    : "); Serial.println(int(getNumberOfCommands()));