JAUS++: jaus++/2.0/src/jaus/mobility/drivers/setglobalwaypoint.cpp Source File

Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 
00021 
00022 
00023 
00024 
00025 
00026 
00027 
00028 
00029 
00030 
00031 
00032 
00033 
00034 
00035 
00036 
00037 
00038 
00039 
00040 #include "jaus/mobility/drivers/setglobalwaypoint.h"
00041 #include "jaus/mobility/drivers/globalwaypoint.h"
00042 #include "jaus/core/scaledinteger.h"
00043 #include <iomanip>
00044 
00045 
00046 using namespace JAUS;
00047 
00048 
00057 SetGlobalWaypoint::SetGlobalWaypoint(const Address& dest, const Address& src) : Message(SET_GLOBAL_WAYPOINT, dest, src)
00058 {
00059     mPresenceVector = 0;
00060     mLatitude = 0;
00061     mLongitude = 0;
00062     mAltitude = 0;
00063     mRoll = 0;
00064     mPitch = 0;
00065     mYaw = 0;
00066     mWaypointTolerance = 0;
00067     mPathTolerance = 0;
00068 }
00069 
00070 
00076 SetGlobalWaypoint::SetGlobalWaypoint(const SetGlobalWaypoint& message) : Message(SET_GLOBAL_WAYPOINT)
00077 {
00078     *this = message;
00079 }
00080 
00081 
00087 SetGlobalWaypoint::~SetGlobalWaypoint()
00088 {
00089 }
00090 
00091 
00103 int SetGlobalWaypoint::WriteMessageBody(Packet& packet) const
00104 {
00105     int expected = BYTE_SIZE;
00106     int written = 0;
00107 
00108     written += packet.Write(mPresenceVector);
00109 
00110     expected += UINT_SIZE;
00111     written += ScaledInteger::Write(packet, mLatitude, Limits::MaxLatitude, Limits::MinLatitude, ScaledInteger::UInt);
00112 
00113     expected += UINT_SIZE;
00114     written += ScaledInteger::Write(packet, mLongitude, Limits::MaxLongitude, Limits::MinLongitude, ScaledInteger::UInt);
00115 
00116     if((mPresenceVector & PresenceVector::Altitude) > 0)
00117     {
00118         expected += UINT_SIZE;
00119         written += ScaledInteger::Write(packet, mAltitude, Limits::MaxElevation, Limits::MinElevation, ScaledInteger::UInt);
00120     }
00121     if((mPresenceVector & PresenceVector::Roll) > 0)
00122     {
00123         expected += USHORT_SIZE;
00124         written += ScaledInteger::Write(packet, mRoll, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00125     }
00126     if((mPresenceVector & PresenceVector::Pitch) > 0)
00127     {
00128         expected += USHORT_SIZE;
00129         written += ScaledInteger::Write(packet, mPitch, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00130     }
00131     if((mPresenceVector & PresenceVector::Yaw) > 0)
00132     {
00133         expected += USHORT_SIZE;
00134         written += ScaledInteger::Write(packet, mYaw, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00135     }
00136     if((mPresenceVector & PresenceVector::WaypointTolerance) > 0)
00137     {
00138         expected += USHORT_SIZE;
00139         written += ScaledInteger::Write(packet, mWaypointTolerance, Limits::MaxWaypointTolerance, Limits::MinWaypointTolerance, ScaledInteger::UShort);
00140     }
00141     if((mPresenceVector & PresenceVector::PathTolerance) > 0)
00142     {
00143         expected += UINT_SIZE;
00144         written += ScaledInteger::Write(packet, mPathTolerance, Limits::MaxPathTolerance, Limits::MinPathTolerance, ScaledInteger::UInt);
00145     }
00146 
00147     return expected == written ? written : -1;
00148 }
00149 
00150 
00162 int SetGlobalWaypoint::ReadMessageBody(const Packet& packet)
00163 {
00164     int expected = BYTE_SIZE;
00165     int read = 0;
00166 
00167     read += packet.Read(mPresenceVector);
00168 
00169     expected += UINT_SIZE;
00170     read += ScaledInteger::Read(packet, mLatitude, Limits::MaxLatitude, Limits::MinLatitude, ScaledInteger::UInt);
00171 
00172     expected += UINT_SIZE;
00173     read += ScaledInteger::Read(packet, mLongitude, Limits::MaxLongitude, Limits::MinLongitude, ScaledInteger::UInt);
00174 
00175     if((mPresenceVector & PresenceVector::Altitude) > 0)
00176     {
00177         expected += UINT_SIZE;
00178         read += ScaledInteger::Read(packet, mAltitude, Limits::MaxElevation, Limits::MinElevation, ScaledInteger::UInt);
00179     }
00180     if((mPresenceVector & PresenceVector::Roll) > 0)
00181     {
00182         expected += USHORT_SIZE;
00183         read += ScaledInteger::Read(packet, mRoll, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00184     }
00185     if((mPresenceVector & PresenceVector::Pitch) > 0)
00186     {
00187         expected += USHORT_SIZE;
00188         read += ScaledInteger::Read(packet, mPitch, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00189     }
00190     if((mPresenceVector & PresenceVector::Yaw) > 0)
00191     {
00192         expected += USHORT_SIZE;
00193         read += ScaledInteger::Read(packet, mYaw, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00194     }
00195     if((mPresenceVector & PresenceVector::WaypointTolerance) > 0)
00196     {
00197         expected += USHORT_SIZE;
00198         read += ScaledInteger::Read(packet, mWaypointTolerance, Limits::MaxWaypointTolerance, Limits::MinWaypointTolerance, ScaledInteger::UShort);
00199     }
00200     if((mPresenceVector & PresenceVector::PathTolerance) > 0)
00201     {
00202         expected += UINT_SIZE;
00203         read += ScaledInteger::Read(packet, mPathTolerance, Limits::MaxPathTolerance, Limits::MinPathTolerance, ScaledInteger::UInt);
00204     }
00205 
00206     return expected == read ? read : -1;
00207 }
00208 
00209 
00215 void SetGlobalWaypoint::ClearMessageBody()
00216 {
00217     mPresenceVector = 0;
00218     mLatitude = 0;
00219     mLongitude = 0;
00220     mAltitude = 0;
00221     mRoll = 0;
00222     mPitch = 0;
00223     mYaw = 0;
00224     mWaypointTolerance = 0;
00225     mPathTolerance = 0;
00226 }
00227 
00228 
00234 void SetGlobalWaypoint::PrintMessageBody() const
00235 {
00236     std::cout << "WGS: [" << std::fixed << std::setprecision(5) << mLatitude << ", " 
00237                           << std::fixed << std::setprecision(5) << mLongitude << ", " 
00238                           << std::fixed << std::setprecision(2) << mAltitude << "]\n";
00239     if( (mPresenceVector & (PresenceVector::Yaw | PresenceVector::Pitch | PresenceVector::Roll)) > 0)
00240     {
00241         std::cout << "Orientation: [" << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mRoll) << ", " 
00242                                       << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mPitch) << ", " 
00243                                       << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mYaw) << "]\n";
00244     }
00245     if( (mPresenceVector & (PresenceVector::PathTolerance | PresenceVector::WaypointTolerance)) > 0)
00246     {
00247         std::cout << "Tolerance (Waypoint, Path): [" << mWaypointTolerance << ", " << mPathTolerance << "]" << std::endl;
00248     }
00249 }
00250 
00251 
00259 int SetGlobalWaypoint::RunTestCase() const
00260 {
00261     int result = 0;
00262 
00263     Packet packet;
00264 
00265     SetGlobalWaypoint msg1, msg2;
00266 
00267     msg1.SetPitch(1.25);
00268     msg1.SetWaypointTolerance(50);
00269 
00270     if((msg1.WriteMessageBody(packet) != -1) &&
00271         (msg2.ReadMessageBody(packet) != -1))
00272     {
00273         if(msg2.AreFieldsPresent(PresenceVector::Pitch | PresenceVector::WaypointTolerance) )
00274          {
00275                 result = 1;
00276          }
00277     }
00278 
00279     return result;
00280 }
00281 
00282 
00288 SetGlobalWaypoint& SetGlobalWaypoint::operator=(const SetGlobalWaypoint& message)
00289 {
00290     if(this != &message)
00291     {
00292         CopyHeaderData(&message);
00293         mPresenceVector = message.mPresenceVector;
00294         mLatitude = message.mLatitude;
00295         mLongitude = message.mLongitude;
00296         mAltitude = message.mAltitude;
00297         mRoll = message.mRoll;
00298         mPitch = message.mPitch;
00299         mYaw = message.mYaw;
00300         mWaypointTolerance = message.mWaypointTolerance;
00301         mPathTolerance = message.mPathTolerance;
00302         
00303     }
00304     return *this;
00305 }
00306 
00307 
00313 SetGlobalWaypoint& SetGlobalWaypoint::operator=(const GlobalWaypoint& waypoint)
00314 {
00315     mPresenceVector = (Byte)waypoint.GetPresenceVector();
00316     mLatitude = waypoint.GetLatitude();
00317     mLongitude = waypoint.GetLongitude();
00318     mAltitude = waypoint.GetAltitude();
00319     mRoll = waypoint.GetRoll();
00320     mPitch = waypoint.GetPitch();
00321     mYaw = waypoint.GetYaw();
00322     mWaypointTolerance = waypoint.GetWaypointTolerance();
00323     mPathTolerance = waypoint.GetPathTolerance();
00324     return *this;
00325 }
00326 
00327 
00328 /*  End of File */