JAUS++: jaus++/2.0/src/jaus/mobility/drivers/setlocalvector.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/setlocalvector.h"
00041 #include "jaus/core/scaledinteger.h"
00042 #include <cxutils/math/cxmath.h>
00043 
00044 const double JAUS::SetLocalVector::Limits::MinSpeed = 0.0;
00045 const double JAUS::SetLocalVector::Limits::MaxSpeed = 327.67;
00046 const double JAUS::SetLocalVector::Limits::MinElevation = -10000;
00047 const double JAUS::SetLocalVector::Limits::MaxElevation = 35000;
00048 const double JAUS::SetLocalVector::Limits::MinAngle = -CxUtils::PI;
00049 const double JAUS::SetLocalVector::Limits::MaxAngle = CxUtils::PI;
00050 
00051 using namespace JAUS;
00052 
00053 
00062 SetLocalVector::SetLocalVector(const Address& dest, const Address& src) : Message(SET_LOCAL_VECTOR, dest, src)
00063 {
00064     mPresenceVector = 0;
00065     mSpeed = 0;
00066     mZ = 0;
00067     mHeading = 0;
00068     mRoll = 0;
00069     mPitch = 0;
00070 }
00071 
00072 
00078 SetLocalVector::SetLocalVector(const SetLocalVector& message) : Message(SET_LOCAL_VECTOR)
00079 {
00080     *this = message;
00081 }
00082 
00083 
00089 SetLocalVector::~SetLocalVector()
00090 {
00091 }
00092 
00093 
00103 bool SetLocalVector::SetSpeed(const double value)
00104 {
00105     if(value >= Limits::MinSpeed && value <= Limits::MaxSpeed)
00106     {
00107         mSpeed = value;
00108         mPresenceVector |= PresenceVector::Speed;
00109         return true;
00110     }
00111     return false;
00112 }
00113 
00114 
00125 bool SetLocalVector::SetZ(const double value)
00126 {
00127     if(value >= Limits::MinElevation && value <= Limits::MaxElevation)
00128     {
00129         mZ = value;
00130         mPresenceVector |= PresenceVector::Z;
00131         return true;
00132     }
00133     return false;
00134 }
00135 
00136 
00146 bool SetLocalVector::SetHeading(const double radians)
00147 {
00148     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00149     {
00150         mHeading = radians;
00151         mPresenceVector |= PresenceVector::Heading;
00152         return true;
00153     }
00154     return false;
00155 }
00156 
00157 
00167 bool SetLocalVector::SetRoll(const double radians)
00168 {
00169     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00170     {
00171         mRoll = radians;
00172         mPresenceVector |= PresenceVector::Roll;
00173         return true;
00174     }
00175     return false;
00176 }
00177 
00178 
00188 bool SetLocalVector::SetPitch(const double radians)
00189 {
00190     if(radians >= Limits::MinAngle && radians <= Limits::MaxAngle)
00191     {
00192         mPitch = radians;
00193         mPresenceVector |= PresenceVector::Pitch;
00194         return true;
00195     }
00196     return false;
00197 }
00198 
00199 
00211 int SetLocalVector::WriteMessageBody(Packet& packet) const
00212 {
00213     int expected = BYTE_SIZE;
00214     int written = 0;
00215 
00216     written += packet.Write(mPresenceVector);
00217 
00218     if((mPresenceVector & PresenceVector::Speed) > 0)
00219     {
00220         expected += USHORT_SIZE;
00221         written += ScaledInteger::Write(packet, mSpeed, Limits::MaxSpeed, Limits::MinSpeed, ScaledInteger::UShort);
00222     }
00223     if((mPresenceVector & PresenceVector::Z) > 0)
00224     {
00225         expected += UINT_SIZE;
00226         written += ScaledInteger::Write(packet, mZ, Limits::MaxElevation, Limits::MinElevation, ScaledInteger::UInt);
00227     }
00228     if((mPresenceVector & PresenceVector::Heading) > 0)
00229     {
00230         expected += USHORT_SIZE;
00231         written += ScaledInteger::Write(packet, mHeading, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00232     }
00233     if((mPresenceVector & PresenceVector::Roll) > 0)
00234     {
00235         expected += USHORT_SIZE;
00236         written += ScaledInteger::Write(packet, mRoll, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00237     }
00238     if((mPresenceVector & PresenceVector::Pitch) > 0)
00239     {
00240         expected += USHORT_SIZE;
00241         written += ScaledInteger::Write(packet, mPitch, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00242     }
00243 
00244     return expected == written ? written : -1;
00245 }
00246 
00247 
00259 int SetLocalVector::ReadMessageBody(const Packet& packet)
00260 {
00261     int expected = BYTE_SIZE;
00262     int read = 0;
00263 
00264     read += packet.Read(mPresenceVector);
00265 
00266     if((mPresenceVector & PresenceVector::Speed) > 0)
00267     {
00268         expected += USHORT_SIZE;
00269         read += ScaledInteger::Read(packet, mSpeed, Limits::MaxSpeed, Limits::MinSpeed, ScaledInteger::UShort);
00270     }
00271     if((mPresenceVector & PresenceVector::Z) > 0)
00272     {
00273         expected += UINT_SIZE;
00274         read += ScaledInteger::Read(packet, mZ, Limits::MaxElevation, Limits::MinElevation, ScaledInteger::UInt);
00275     }
00276     if((mPresenceVector & PresenceVector::Heading) > 0)
00277     {
00278         expected += USHORT_SIZE;
00279         read += ScaledInteger::Read(packet, mHeading, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00280     }
00281     if((mPresenceVector & PresenceVector::Roll) > 0)
00282     {
00283         expected += USHORT_SIZE;
00284         read += ScaledInteger::Read(packet, mRoll, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00285     }
00286     if((mPresenceVector & PresenceVector::Pitch) > 0)
00287     {
00288         expected += USHORT_SIZE;
00289         read += ScaledInteger::Read(packet, mPitch, Limits::MaxAngle, Limits::MinAngle, ScaledInteger::UShort);
00290     }
00291 
00292     return expected == read ? read : -1;
00293 }
00294 
00295 
00301 void SetLocalVector::ClearMessageBody()
00302 {
00303     mPresenceVector = 0;
00304     mSpeed = 0;
00305     mZ = 0;
00306     mHeading = 0;
00307     mRoll = 0;
00308     mPitch = 0;
00309 }
00310 
00311 
00319 int SetLocalVector::RunTestCase() const
00320 {
00321     int result = 0;
00322 
00323     Packet packet;
00324 
00325     SetLocalVector msg1, msg2;
00326 
00327     msg1.SetSpeed(60);
00328     msg1.SetHeading(-1.333);
00329 
00330     if((msg1.WriteMessageBody(packet) != -1) &&
00331         (msg2.ReadMessageBody(packet) != -1))
00332     {
00333         if(msg2.AreFieldsPresent(PresenceVector::Heading | PresenceVector::Speed) )
00334          {
00335                 result = 1;
00336          }
00337     }
00338 
00339     return result;
00340 }
00341 
00342 
00348 SetLocalVector& SetLocalVector::operator=(const SetLocalVector& message)
00349 {
00350     if(this != &message)
00351     {
00352         CopyHeaderData(&message);
00353         mPresenceVector = message.mPresenceVector;
00354         mSpeed = message.mSpeed;
00355         mZ = message.mZ;
00356         mHeading = message.mHeading;
00357         mRoll = message.mRoll;
00358         mPitch = message.mPitch;
00359         
00360     }
00361     return *this;
00362 }
00363 
00364 
00365 /*  End of File */