JAUS++: jaus++/2.0/src/jaus/mobility/drivers/setvelocitycommand.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/setvelocitycommand.h"
00041 #include "jaus/core/scaledinteger.h"
00042 #include <cxutils/math/cxmath.h>
00043 
00044 const double JAUS::SetVelocityCommand::Limits::MinVelocity = -327.68;
00045 const double JAUS::SetVelocityCommand::Limits::MaxVelocity = 327.67;
00046 const double JAUS::SetVelocityCommand::Limits::MinRotationalRate = -32.768;
00047 const double JAUS::SetVelocityCommand::Limits::MaxRotationalRate = 32.767;
00048 
00049 using namespace JAUS;
00050 
00051 
00060 SetVelocityCommand::SetVelocityCommand(const Address& dest, const Address& src) : Message(SET_VELOCITY_COMMAND, dest, src)
00061 {
00062     mPresenceVector = 0;
00063     mCommandType = SetCurrentCommand;
00064     mVelocityX = 0;
00065     mVelocityY = 0;
00066     mVelocityZ = 0;
00067     mRollRate = 0;
00068     mPitchRate = 0;
00069     mYawRate = 0;
00070 }
00071 
00072 
00078 SetVelocityCommand::SetVelocityCommand(const SetVelocityCommand& message) : Message(SET_VELOCITY_COMMAND)
00079 {
00080     *this = message;
00081 }
00082 
00083 
00089 SetVelocityCommand::~SetVelocityCommand()
00090 {
00091 }
00092 
00093 
00103 bool SetVelocityCommand::SetVelocityX(const double value)
00104 {
00105     if(value >= Limits::MinVelocity && value <= Limits::MaxVelocity)
00106     {
00107         mVelocityX = value;
00108         mPresenceVector |= PresenceVector::VelocityX;
00109         return true;
00110     }
00111     return false;
00112 }
00113 
00114 
00124 bool SetVelocityCommand::SetVelocityY(const double value)
00125 {
00126     if(value >= Limits::MinVelocity && value <= Limits::MaxVelocity)
00127     {
00128         mVelocityY = value;
00129         mPresenceVector |= PresenceVector::VelocityY;
00130         return true;
00131     }
00132     return false;
00133 }
00134 
00135 
00145 bool SetVelocityCommand::SetVelocityZ(const double value)
00146 {
00147     if(value >= Limits::MinVelocity && value <= Limits::MaxVelocity)
00148     {
00149         mVelocityZ = value;
00150         mPresenceVector |= PresenceVector::VelocityZ;
00151         return true;
00152     }
00153     return false;
00154 }
00155 
00156 
00166 bool SetVelocityCommand::SetRollRate(const double value)
00167 {
00168     if(value >= Limits::MinRotationalRate && value <= Limits::MaxRotationalRate)
00169     {
00170         mRollRate = value;
00171         mPresenceVector |= PresenceVector::RollRate;
00172         return true;
00173     }
00174     return false;
00175 }
00176 
00177 
00187 bool SetVelocityCommand::SetPitchRate(const double value)
00188 {
00189     if(value >= Limits::MinRotationalRate && value <= Limits::MaxRotationalRate)
00190     {
00191         mPitchRate = value;
00192         mPresenceVector |= PresenceVector::PitchRate;
00193         return true;
00194     }
00195     return false;
00196 }
00197 
00198 
00208 bool SetVelocityCommand::SetYawRate(const double value)
00209 {
00210     if(value >= Limits::MinRotationalRate && value <= Limits::MaxRotationalRate)
00211     {
00212         mYawRate = value;
00213         mPresenceVector |= PresenceVector::YawRate;
00214         return true;
00215     }
00216     return false;
00217 }
00218 
00219 
00231 int SetVelocityCommand::WriteMessageBody(Packet& packet) const
00232 {
00233     int expected = BYTE_SIZE;
00234     int written = 0;
00235 
00236     written += packet.Write(mPresenceVector);
00237 
00238     expected += BYTE_SIZE;
00239     written += packet.Write((Byte)mCommandType);
00240 
00241     if((mPresenceVector & PresenceVector::VelocityX) > 0)
00242     {
00243         expected += UINT_SIZE;
00244         written += ScaledInteger::Write(packet, mVelocityX, Limits::MaxVelocity, Limits::MinVelocity, ScaledInteger::UInt);
00245     }
00246     if((mPresenceVector & PresenceVector::VelocityY) > 0)
00247     {
00248         expected += UINT_SIZE;
00249         written += ScaledInteger::Write(packet, mVelocityY, Limits::MaxVelocity, Limits::MinVelocity, ScaledInteger::UInt);
00250     }
00251     if((mPresenceVector & PresenceVector::VelocityZ) > 0)
00252     {
00253         expected += UINT_SIZE;
00254         written += ScaledInteger::Write(packet, mVelocityZ, Limits::MaxVelocity, Limits::MinVelocity, ScaledInteger::UInt);
00255     }
00256     if((mPresenceVector & PresenceVector::RollRate) > 0)
00257     {
00258         expected += USHORT_SIZE;
00259         written += ScaledInteger::Write(packet, mRollRate, Limits::MaxRotationalRate, Limits::MinRotationalRate, ScaledInteger::UShort);
00260     }
00261     if((mPresenceVector & PresenceVector::PitchRate) > 0)
00262     {
00263         expected += USHORT_SIZE;
00264         written += ScaledInteger::Write(packet, mPitchRate, Limits::MaxRotationalRate, Limits::MinRotationalRate, ScaledInteger::UShort);
00265     }
00266     if((mPresenceVector & PresenceVector::YawRate) > 0)
00267     {
00268         expected += USHORT_SIZE;
00269         written += ScaledInteger::Write(packet, mYawRate, Limits::MaxRotationalRate, Limits::MinRotationalRate, ScaledInteger::UShort);
00270     }
00271 
00272     return expected == written ? written : -1;
00273 }
00274 
00275 
00287 int SetVelocityCommand::ReadMessageBody(const Packet& packet)
00288 {
00289     int expected = BYTE_SIZE;
00290     int read = 0;
00291 
00292     read += packet.Read(mPresenceVector);
00293 
00294     expected += BYTE_SIZE;
00295     read += packet.Read((Byte &)mCommandType);
00296 
00297     if((mPresenceVector & PresenceVector::VelocityX) > 0)
00298     {
00299         expected += UINT_SIZE;
00300         read += ScaledInteger::Read(packet, mVelocityX, Limits::MaxVelocity, Limits::MinVelocity, ScaledInteger::UInt);
00301     }
00302     if((mPresenceVector & PresenceVector::VelocityY) > 0)
00303     {
00304         expected += UINT_SIZE;
00305         read += ScaledInteger::Read(packet, mVelocityY, Limits::MaxVelocity, Limits::MinVelocity, ScaledInteger::UInt);
00306     }
00307     if((mPresenceVector & PresenceVector::VelocityZ) > 0)
00308     {
00309         expected += UINT_SIZE;
00310         read += ScaledInteger::Read(packet, mVelocityZ, Limits::MaxVelocity, Limits::MinVelocity, ScaledInteger::UInt);
00311     }
00312     if((mPresenceVector & PresenceVector::RollRate) > 0)
00313     {
00314         expected += USHORT_SIZE;
00315         read += ScaledInteger::Read(packet, mRollRate, Limits::MaxRotationalRate, Limits::MinRotationalRate, ScaledInteger::UShort);
00316     }
00317     if((mPresenceVector & PresenceVector::PitchRate) > 0)
00318     {
00319         expected += USHORT_SIZE;
00320         read += ScaledInteger::Read(packet, mPitchRate, Limits::MaxRotationalRate, Limits::MinRotationalRate, ScaledInteger::UShort);
00321     }
00322     if((mPresenceVector & PresenceVector::YawRate) > 0)
00323     {
00324         expected += USHORT_SIZE;
00325         read += ScaledInteger::Read(packet, mYawRate, Limits::MaxRotationalRate, Limits::MinRotationalRate, ScaledInteger::UShort);
00326     }
00327 
00328     return expected == read ? read : -1;
00329 }
00330 
00331 
00337 void SetVelocityCommand::ClearMessageBody()
00338 {
00339     mPresenceVector = 0;
00340     mCommandType = SetCurrentCommand;
00341     mVelocityX = 0;
00342     mVelocityY = 0;
00343     mVelocityZ = 0;
00344     mRollRate = 0;
00345     mPitchRate = 0;
00346     mYawRate = 0;
00347 }
00348 
00349 
00357 int SetVelocityCommand::RunTestCase() const
00358 {
00359     int result = 0;
00360 
00361     Packet packet;
00362 
00363     SetVelocityCommand msg1, msg2;
00364 
00365     msg1.SetCommandType(SetMaximumAllowedValues);
00366     msg1.SetVelocityY(56);
00367     msg1.SetYawRate(-15.25);
00368 
00369     if((msg1.WriteMessageBody(packet) != -1) &&
00370         (msg2.ReadMessageBody(packet) != -1))
00371     {
00372         if(msg2.AreFieldsPresent(PresenceVector::VelocityY | PresenceVector::YawRate) &&
00373             msg1.GetCommandType() == msg2.GetCommandType())
00374          {
00375                 result = 1;
00376          }
00377     }
00378 
00379     return result;
00380 }
00381 
00382 
00388 SetVelocityCommand& SetVelocityCommand::operator=(const SetVelocityCommand& message)
00389 {
00390     if(this != &message)
00391     {
00392         CopyHeaderData(&message);
00393         mPresenceVector = message.mPresenceVector;
00394         mCommandType = message.mCommandType;
00395         mVelocityX = message.mVelocityX;
00396         mVelocityY = message.mVelocityY;
00397         mVelocityZ = message.mVelocityZ;
00398         mRollRate = message.mRollRate;
00399         mPitchRate = message.mPitchRate;
00400         mYawRate = message.mYawRate;
00401         
00402     }
00403     return *this;
00404 }
00405 
00406 
00407 /*  End of File */