Working on yaw command.

libraries/APO/AP_Guide.cpp

@@ -189,6 +189,7 @@ void MavlinkGuide::handleCommand() {
             temp = -_crossTrackLim * deg2Rad;
         float bearing = _previousCommand.bearingTo(_command);
         _headingCommand = bearing - temp;
+        _yawCommand = _command.getYawCommand();
         //_hal->debug->printf_P(
         //	PSTR("nav: bCurrent2Dest: %f\tdXt: %f\tcmdHeading: %f\tnextWpDistance: %f\talongTrack: %f\n"),
         //	bearing * rad2Deg, dXt, _headingCommand * rad2Deg, distanceToNext, alongTrack);

libraries/APO/AP_Guide.h

@@ -85,9 +85,14 @@ public:
 
     float getHeadingError();
 
+    /// the commanded course over ground for the vehicle
     float getHeadingCommand() {
         return _headingCommand;
     }
+    /// the yaw attitude command of the vehicle
+    float getYawCommand(){
+        return _yawCommand;
+    }
     float getAirSpeedCommand() {
         return _airSpeedCommand;
     }
@@ -115,6 +120,7 @@ protected:
     AP_HardwareAbstractionLayer * _hal;
     AP_MavlinkCommand _command, _previousCommand;
     float _headingCommand;
+    float _yawCommand;
     float _airSpeedCommand;
     float _groundSpeedCommand;
     float _altitudeCommand;

libraries/APO/AP_MavlinkCommand.h

@@ -123,6 +123,11 @@ public:
     void setZ(float val) {
         _data.get().z = val;
     }
+
+    float getYawCommand() const {
+        return getParam4();
+    }
+
     float getLatDeg() const {
         switch (getFrame()) {
         case MAV_FRAME_GLOBAL: