JAUS++: jaus++/2.0/src/jaus/mobility/drivers/setaccelerationlimit.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/setaccelerationlimit.h"
00041 #include "jaus/core/scaledinteger.h"
00042 #include <cxutils/math/cxmath.h>
00043 
00044 const double JAUS::SetAccelerationLimit::Limits::MinLinearAcceleration = -1310.68;
00045 const double JAUS::SetAccelerationLimit::Limits::MaxLinearAcceleration = 1310.68;
00046 const double JAUS::SetAccelerationLimit::Limits::MinRotationalAcceleration = -13106.8;
00047 const double JAUS::SetAccelerationLimit::Limits::MaxRotationalAcceleration = 13106.8;
00048 
00049 using namespace JAUS;
00050 
00051 
00060 SetAccelerationLimit::SetAccelerationLimit(const Address& dest, const Address& src) : Message(SET_ACCELERATION_LIMIT, dest, src)
00061 {
00062     mPresenceVector = 0;
00063     mCommandType = SetMaximumAllowedValues;
00064     mAccelerationX = 0;
00065     mAccelerationY = 0;
00066     mAccelerationZ = 0;
00067     mRollAcceleration = 0;
00068     mPitchAcceleration = 0;
00069     mYawAcceleration = 0;
00070 }
00071 
00072 
00078 SetAccelerationLimit::SetAccelerationLimit(const SetAccelerationLimit& message) : Message(SET_ACCELERATION_LIMIT)
00079 {
00080     *this = message;
00081 }
00082 
00083 
00089 SetAccelerationLimit::~SetAccelerationLimit()
00090 {
00091 }
00092 
00093 
00104 bool SetAccelerationLimit::SetAccelerationX(const double value)
00105 {
00106     if(value >= Limits::MinLinearAcceleration && value <= Limits::MaxLinearAcceleration)
00107     {
00108         mAccelerationX = value;
00109         mPresenceVector |= PresenceVector::AccelerationX;
00110         return true;
00111     }
00112     return false;
00113 }
00114 
00115 
00126 bool SetAccelerationLimit::SetAccelerationY(const double value)
00127 {
00128     if(value >= Limits::MinLinearAcceleration && value <= Limits::MaxLinearAcceleration)
00129     {
00130         mAccelerationY = value;
00131         mPresenceVector |= PresenceVector::AccelerationY;
00132         return true;
00133     }
00134     return false;
00135 }
00136 
00137 
00148 bool SetAccelerationLimit::SetAccelerationZ(const double value)
00149 {
00150     if(value >= Limits::MinLinearAcceleration && value <= Limits::MaxLinearAcceleration)
00151     {
00152         mAccelerationZ = value;
00153         mPresenceVector |= PresenceVector::AccelerationZ;
00154         return true;
00155     }
00156     return false;
00157 }
00158 
00159 
00170 bool SetAccelerationLimit::SetRollAcceleration(const double value)
00171 {
00172     if(value >= Limits::MinRotationalAcceleration && value <= Limits::MaxRotationalAcceleration)
00173     {
00174         mRollAcceleration = value;
00175         mPresenceVector |= PresenceVector::RollAcceleration;
00176         return true;
00177     }
00178     return false;
00179 }
00180 
00181 
00193 bool SetAccelerationLimit::SetPitchAcceleration(const double value)
00194 {
00195     if(value >= Limits::MinRotationalAcceleration && value <= Limits::MaxRotationalAcceleration)
00196     {
00197         mPitchAcceleration = value;
00198         mPresenceVector |= PresenceVector::PitchAcceleration;
00199         return true;
00200     }
00201     return false;
00202 }
00203 
00204 
00215 bool SetAccelerationLimit::SetYawAcceleration(const double value)
00216 {
00217     if(value >= Limits::MinRotationalAcceleration && value <= Limits::MaxRotationalAcceleration)
00218     {
00219         mYawAcceleration = value;
00220         mPresenceVector |= PresenceVector::YawAcceleration;
00221         return true;
00222     }
00223     return false;
00224 }
00225 
00226 
00238 int SetAccelerationLimit::WriteMessageBody(Packet& packet) const
00239 {
00240     int expected = BYTE_SIZE;
00241     int written = 0;
00242 
00243     written += packet.Write(mPresenceVector);
00244 
00245     expected += BYTE_SIZE;
00246     written += packet.Write((Byte)mCommandType);
00247 
00248     if((mPresenceVector & PresenceVector::AccelerationX) > 0)
00249     {
00250         expected += UINT_SIZE;
00251         written += ScaledInteger::Write(packet, mAccelerationX, Limits::MaxLinearAcceleration, Limits::MinLinearAcceleration, ScaledInteger::UInt);
00252     }
00253     if((mPresenceVector & PresenceVector::AccelerationY) > 0)
00254     {
00255         expected += UINT_SIZE;
00256         written += ScaledInteger::Write(packet, mAccelerationY, Limits::MaxLinearAcceleration, Limits::MinLinearAcceleration, ScaledInteger::UInt);
00257     }
00258     if((mPresenceVector & PresenceVector::AccelerationZ) > 0)
00259     {
00260         expected += UINT_SIZE;
00261         written += ScaledInteger::Write(packet, mAccelerationZ, Limits::MaxLinearAcceleration, Limits::MinLinearAcceleration, ScaledInteger::UInt);
00262     }
00263     if((mPresenceVector & PresenceVector::RollAcceleration) > 0)
00264     {
00265         expected += UINT_SIZE;
00266         written += ScaledInteger::Write(packet, mRollAcceleration, Limits::MaxRotationalAcceleration, Limits::MinRotationalAcceleration, ScaledInteger::UInt);
00267     }
00268     if((mPresenceVector & PresenceVector::PitchAcceleration) > 0)
00269     {
00270         expected += UINT_SIZE;
00271         written += ScaledInteger::Write(packet, mPitchAcceleration, Limits::MaxRotationalAcceleration, Limits::MinRotationalAcceleration, ScaledInteger::UInt);
00272     }
00273     if((mPresenceVector & PresenceVector::YawAcceleration) > 0)
00274     {
00275         expected += UINT_SIZE;
00276         written += ScaledInteger::Write(packet, mYawAcceleration, Limits::MaxRotationalAcceleration, Limits::MinRotationalAcceleration, ScaledInteger::UInt);
00277     }
00278 
00279     return expected == written ? written : -1;
00280 }
00281 
00282 
00294 int SetAccelerationLimit::ReadMessageBody(const Packet& packet)
00295 {
00296     int expected = BYTE_SIZE;
00297     int read = 0;
00298 
00299     read += packet.Read(mPresenceVector);
00300 
00301     expected += BYTE_SIZE;
00302     read += packet.Read((Byte &)mCommandType);
00303 
00304     if((mPresenceVector & PresenceVector::AccelerationX) > 0)
00305     {
00306         expected += UINT_SIZE;
00307         read += ScaledInteger::Read(packet, mAccelerationX, Limits::MaxLinearAcceleration, Limits::MinLinearAcceleration, ScaledInteger::UInt);
00308     }
00309     if((mPresenceVector & PresenceVector::AccelerationY) > 0)
00310     {
00311         expected += UINT_SIZE;
00312         read += ScaledInteger::Read(packet, mAccelerationY, Limits::MaxLinearAcceleration, Limits::MinLinearAcceleration, ScaledInteger::UInt);
00313     }
00314     if((mPresenceVector & PresenceVector::AccelerationZ) > 0)
00315     {
00316         expected += UINT_SIZE;
00317         read += ScaledInteger::Read(packet, mAccelerationZ, Limits::MaxLinearAcceleration, Limits::MinLinearAcceleration, ScaledInteger::UInt);
00318     }
00319     if((mPresenceVector & PresenceVector::RollAcceleration) > 0)
00320     {
00321         expected += UINT_SIZE;
00322         read += ScaledInteger::Read(packet, mRollAcceleration, Limits::MaxRotationalAcceleration, Limits::MinRotationalAcceleration, ScaledInteger::UInt);
00323     }
00324     if((mPresenceVector & PresenceVector::PitchAcceleration) > 0)
00325     {
00326         expected += UINT_SIZE;
00327         read += ScaledInteger::Read(packet, mPitchAcceleration, Limits::MaxRotationalAcceleration, Limits::MinRotationalAcceleration, ScaledInteger::UInt);
00328     }
00329     if((mPresenceVector & PresenceVector::YawAcceleration) > 0)
00330     {
00331         expected += UINT_SIZE;
00332         read += ScaledInteger::Read(packet, mYawAcceleration, Limits::MaxRotationalAcceleration, Limits::MinRotationalAcceleration, ScaledInteger::UInt);
00333     }
00334 
00335     return expected == read ? read : -1;
00336 }
00337 
00338 
00344 void SetAccelerationLimit::ClearMessageBody()
00345 {
00346     mPresenceVector = 0;
00347     mCommandType = SetMaximumAllowedValues;
00348     mAccelerationX = 0;
00349     mAccelerationY = 0;
00350     mAccelerationZ = 0;
00351     mRollAcceleration = 0;
00352     mPitchAcceleration = 0;
00353     mYawAcceleration = 0;
00354 }
00355 
00356 
00364 int SetAccelerationLimit::RunTestCase() const
00365 {
00366     int result = 0;
00367 
00368     Packet packet;
00369 
00370     SetAccelerationLimit msg1, msg2;
00371 
00372     msg1.SetCommandType(SetMinimumAllowedValues);
00373     msg1.SetRollAcceleration(60);
00374     msg1.SetPitchAcceleration(30);
00375 
00376     if((msg1.WriteMessageBody(packet) != -1) &&
00377         (msg2.ReadMessageBody(packet) != -1))
00378     {
00379         if(msg2.AreFieldsPresent(PresenceVector::RollAcceleration | PresenceVector::PitchAcceleration)  &&
00380              msg1.GetCommandType() == msg2.GetCommandType())
00381          {
00382                 result = 1;
00383          }
00384     }
00385 
00386     return result;
00387 }
00388 
00389 
00395 SetAccelerationLimit& SetAccelerationLimit::operator=(const SetAccelerationLimit& message)
00396 {
00397     if(this != &message)
00398     {
00399         CopyHeaderData(&message);
00400         mPresenceVector = message.mPresenceVector;
00401         mCommandType = message.mCommandType;
00402         mAccelerationX = message.mAccelerationX;
00403         mAccelerationY = message.mAccelerationY;
00404         mAccelerationZ = message.mAccelerationZ;
00405         mRollAcceleration = message.mRollAcceleration;
00406         mPitchAcceleration = message.mPitchAcceleration;
00407         mYawAcceleration = message.mYawAcceleration;
00408         
00409     }
00410     return *this;
00411 }
00412 
00413 
00414 /*  End of File */