JAUS++: jaus++/2.0/src/jaus/mobility/drivers/reportglobalwaypoint.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/reportglobalwaypoint.h"
00041 #include "jaus/mobility/drivers/globalwaypoint.h"
00042 #include "jaus/core/scaledinteger.h"
00043 
00044 using namespace JAUS;
00045 
00046 
00055 ReportGlobalWaypoint::ReportGlobalWaypoint(const Address& dest, const Address& src) : Message(REPORT_GLOBAL_WAYPOINT, dest, src)
00056 {
00057     mPresenceVector = 0;
00058     mLatitude = 0;
00059     mLongitude = 0;
00060     mAltitude = 0;
00061     mRoll = 0;
00062     mPitch = 0;
00063     mYaw = 0;
00064     mWaypointTolerance = 0;
00065     mPathTolerance = 0;
00066 }
00067 
00068 
00074 ReportGlobalWaypoint::ReportGlobalWaypoint(const ReportGlobalWaypoint& message) : Message(REPORT_GLOBAL_WAYPOINT)
00075 {
00076     *this = message;
00077 }
00078 
00079 
00085 ReportGlobalWaypoint::~ReportGlobalWaypoint()
00086 {
00087 }
00088 
00089 
00101 int ReportGlobalWaypoint::WriteMessageBody(Packet& packet) const
00102 {
00103     int expected = BYTE_SIZE;
00104     int written = 0;
00105 
00106     written += packet.Write(mPresenceVector);
00107 
00108     expected += UINT_SIZE;
00109     written += ScaledInteger::Write(packet, mLatitude, Limits::MaxLatitude, Limits::MinLatitude, ScaledInteger::UInt);
00110 
00111     expected += UINT_SIZE;
00112     written += ScaledInteger::Write(packet, mLongitude, Limits::MaxLongitude, Limits::MinLongitude, ScaledInteger::UInt);
00113 
00114     if((mPresenceVector & PresenceVector::Altitude) > 0)
00115     {
00116         expected += UINT_SIZE;
00117         written += ScaledInteger::Write(packet, mAltitude, Limits::MaxElevation, Limits::MinElevation, ScaledInteger::UInt);
00118     }
00119     if((mPresenceVector & PresenceVector::Roll) > 0)
00120     {
00121         expected += USHORT_SIZE;
00122         written += ScaledInteger::Write(packet, mRoll, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00123     }
00124     if((mPresenceVector & PresenceVector::Pitch) > 0)
00125     {
00126         expected += USHORT_SIZE;
00127         written += ScaledInteger::Write(packet, mPitch, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00128     }
00129     if((mPresenceVector & PresenceVector::Yaw) > 0)
00130     {
00131         expected += USHORT_SIZE;
00132         written += ScaledInteger::Write(packet, mYaw, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00133     }
00134     if((mPresenceVector & PresenceVector::WaypointTolerance) > 0)
00135     {
00136         expected += USHORT_SIZE;
00137         written += ScaledInteger::Write(packet, mWaypointTolerance, Limits::MaxWaypointTolerance, Limits::MinWaypointTolerance, ScaledInteger::UShort);
00138     }
00139     if((mPresenceVector & PresenceVector::PathTolerance) > 0)
00140     {
00141         expected += UINT_SIZE;
00142         written += ScaledInteger::Write(packet, mPathTolerance, Limits::MaxPathTolerance, Limits::MinPathTolerance, ScaledInteger::UInt);
00143     }
00144 
00145     return expected == written ? written : -1;
00146 }
00147 
00148 
00160 int ReportGlobalWaypoint::ReadMessageBody(const Packet& packet)
00161 {
00162     int expected = BYTE_SIZE;
00163     int read = 0;
00164 
00165     read += packet.Read(mPresenceVector);
00166 
00167     expected += UINT_SIZE;
00168     read += ScaledInteger::Read(packet, mLatitude, Limits::MaxLatitude, Limits::MinLatitude, ScaledInteger::UInt);
00169 
00170     expected += UINT_SIZE;
00171     read += ScaledInteger::Read(packet, mLongitude, Limits::MaxLongitude, Limits::MinLongitude, ScaledInteger::UInt);
00172 
00173     if((mPresenceVector & PresenceVector::Altitude) > 0)
00174     {
00175         expected += UINT_SIZE;
00176         read += ScaledInteger::Read(packet, mAltitude, Limits::MaxElevation, Limits::MinElevation, ScaledInteger::UInt);
00177     }
00178     if((mPresenceVector & PresenceVector::Roll) > 0)
00179     {
00180         expected += USHORT_SIZE;
00181         read += ScaledInteger::Read(packet, mRoll, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00182     }
00183     if((mPresenceVector & PresenceVector::Pitch) > 0)
00184     {
00185         expected += USHORT_SIZE;
00186         read += ScaledInteger::Read(packet, mPitch, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00187     }
00188     if((mPresenceVector & PresenceVector::Yaw) > 0)
00189     {
00190         expected += USHORT_SIZE;
00191         read += ScaledInteger::Read(packet, mYaw, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00192     }
00193     if((mPresenceVector & PresenceVector::WaypointTolerance) > 0)
00194     {
00195         expected += USHORT_SIZE;
00196         read += ScaledInteger::Read(packet, mWaypointTolerance, Limits::MaxWaypointTolerance, Limits::MinWaypointTolerance, ScaledInteger::UShort);
00197     }
00198     if((mPresenceVector & PresenceVector::PathTolerance) > 0)
00199     {
00200         expected += UINT_SIZE;
00201         read += ScaledInteger::Read(packet, mPathTolerance, Limits::MaxPathTolerance, Limits::MinPathTolerance, ScaledInteger::UInt);
00202     }
00203 
00204     return expected == read ? read : -1;
00205 }
00206 
00207 
00213 void ReportGlobalWaypoint::ClearMessageBody()
00214 {
00215     mPresenceVector = 0;
00216     mLatitude = 0;
00217     mLongitude = 0;
00218     mAltitude = 0;
00219     mRoll = 0;
00220     mPitch = 0;
00221     mYaw = 0;
00222     mWaypointTolerance = 0;
00223     mPathTolerance = 0;
00224 }
00225 
00226 
00234 int ReportGlobalWaypoint::RunTestCase() const
00235 {
00236     int result = 0;
00237 
00238     Packet packet;
00239 
00240     ReportGlobalWaypoint msg1, msg2;
00241 
00242     msg1.SetRoll(-1.25);
00243     msg1.SetPathTolerance(50000);
00244 
00245     if((msg1.WriteMessageBody(packet) != -1) &&
00246         (msg2.ReadMessageBody(packet) != -1))
00247     {
00248         if(msg2.AreFieldsPresent(PresenceVector::Roll | PresenceVector::PathTolerance) )
00249          {
00250                 result = 1;
00251          }
00252     }
00253 
00254     return result;
00255 }
00256 
00257 
00263 ReportGlobalWaypoint& ReportGlobalWaypoint::operator=(const ReportGlobalWaypoint& message)
00264 {
00265     if(this != &message)
00266     {
00267         CopyHeaderData(&message);
00268         mPresenceVector = message.mPresenceVector;
00269         mLatitude = message.mLatitude;
00270         mLongitude = message.mLongitude;
00271         mAltitude = message.mAltitude;
00272         mRoll = message.mRoll;
00273         mPitch = message.mPitch;
00274         mYaw = message.mYaw;
00275         mWaypointTolerance = message.mWaypointTolerance;
00276         mPathTolerance = message.mPathTolerance;
00277         
00278     }
00279     return *this;
00280 }
00281 
00282 
00288 ReportGlobalWaypoint& ReportGlobalWaypoint::operator=(const GlobalWaypoint& waypoint)
00289 {
00290     mPresenceVector = (Byte)waypoint.GetPresenceVector();
00291     mLatitude = waypoint.GetLatitude();
00292     mLongitude = waypoint.GetLongitude();
00293     mAltitude = waypoint.GetAltitude();
00294     mRoll = waypoint.GetRoll();
00295     mPitch = waypoint.GetPitch();
00296     mYaw = waypoint.GetYaw();
00297     mWaypointTolerance = waypoint.GetWaypointTolerance();
00298     mPathTolerance = waypoint.GetPathTolerance();
00299     return *this;
00300 }
00301 
00302 
00303 /*  End of File */