JAUS++: jaus++/2.0/src/jaus/mobility/sensors/reportvelocitystate.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/sensors/reportvelocitystate.h"
00041 #include "jaus/core/scaledinteger.h"
00042 #include <cxutils/math/cxmath.h>
00043 #include <iostream>
00044 #include <iomanip>
00045 
00046 const double JAUS::ReportVelocityState::Limits::MinVelocity = -327.68;
00047 const double JAUS::ReportVelocityState::Limits::MaxVelocity = 327.67;
00048 const double JAUS::ReportVelocityState::Limits::MinVelocityRMS = 0.0;
00049 const double JAUS::ReportVelocityState::Limits::MaxVelocityRMS = 100.0;
00050 const double JAUS::ReportVelocityState::Limits::MinRotationalRate = -32.768;
00051 const double JAUS::ReportVelocityState::Limits::MaxRotationalRate = 32.767;
00052 const double JAUS::ReportVelocityState::Limits::MinRateRMS = 0.0;
00053 const double JAUS::ReportVelocityState::Limits::MaxRateRMS = CxUtils::PI;
00054 
00055 using namespace JAUS;
00056 
00057 
00066 ReportVelocityState::ReportVelocityState(const Address& dest, const Address& src) : Message(REPORT_VELOCITY_STATE, dest, src)
00067 {
00068     mPresenceVector = 0;
00069     mVelocityX = 0;
00070     mVelocityY = 0;
00071     mVelocityZ = 0;
00072     mVelocityRMS = 0;
00073     mRollRate = 0;
00074     mPitchRate = 0;
00075     mYawRate = 0;
00076     mRateRMS = 0;
00077     mTimeStamp.SetCurrentTime();
00078 }
00079 
00080 
00086 ReportVelocityState::ReportVelocityState(const ReportVelocityState& message) : Message(REPORT_VELOCITY_STATE)
00087 {
00088     *this = message;
00089 }
00090 
00091 
00097 ReportVelocityState::~ReportVelocityState()
00098 {
00099 }
00100 
00101 
00111 bool ReportVelocityState::SetVelocityX(const double value)
00112 {
00113     if(value >= Limits::MinVelocity && value <= Limits::MaxVelocity)
00114     {
00115         mVelocityX = value;
00116         mPresenceVector |= PresenceVector::VelocityX;
00117         return true;
00118     }
00119     return false;
00120 }
00121 
00122 
00132 bool ReportVelocityState::SetVelocityY(const double value)
00133 {
00134     if(value >= Limits::MinVelocity && value <= Limits::MaxVelocity)
00135     {
00136         mVelocityY = value;
00137         mPresenceVector |= PresenceVector::VelocityY;
00138         return true;
00139     }
00140     return false;
00141 }
00142 
00143 
00154 bool ReportVelocityState::SetVelocityZ(const double value)
00155 {
00156     if(value >= Limits::MinVelocity && value <= Limits::MaxVelocity)
00157     {
00158         mVelocityZ = value;
00159         mPresenceVector |= PresenceVector::VelocityZ;
00160         return true;
00161     }
00162     return false;
00163 }
00164 
00165 
00176 bool ReportVelocityState::SetVelocityRMS(const double value)
00177 {
00178     if(value >= Limits::MinVelocityRMS && value <= Limits::MaxVelocityRMS)
00179     {
00180         mVelocityRMS = value;
00181         mPresenceVector |= PresenceVector::VelocityRMS;
00182         return true;
00183     }
00184     return false;
00185 }
00186 
00187 
00197 bool ReportVelocityState::SetRollRate(const double value)
00198 {
00199     if(value >= Limits::MinRotationalRate && value <= Limits::MaxRotationalRate)
00200     {
00201         mRollRate = value;
00202         mPresenceVector |= PresenceVector::RollRate;
00203         return true;
00204     }
00205     return false;
00206 }
00207 
00208 
00218 bool ReportVelocityState::SetPitchRate(const double value)
00219 {
00220     if(value >= Limits::MinRotationalRate && value <= Limits::MaxRotationalRate)
00221     {
00222         mPitchRate = value;
00223         mPresenceVector |= PresenceVector::PitchRate;
00224         return true;
00225     }
00226     return false;
00227 }
00228 
00229 
00239 bool ReportVelocityState::SetYawRate(const double value)
00240 {
00241     if(value >= Limits::MinRotationalRate && value <= Limits::MaxRotationalRate)
00242     {
00243         mYawRate = value;
00244         mPresenceVector |= PresenceVector::YawRate;
00245         return true;
00246     }
00247     return false;
00248 }
00249 
00250 
00261 bool ReportVelocityState::SetRateRMS(const double value)
00262 {
00263     if(value >= Limits::MinRateRMS && value <= Limits::MaxRateRMS)
00264     {
00265         mRateRMS = value;
00266         mPresenceVector |= PresenceVector::RateRMS;
00267         return true;
00268     }
00269     return false;
00270 }
00271 
00272 
00283 bool ReportVelocityState::SetTimeStamp(const JAUS::Time& time)
00284 {
00285     mTimeStamp = time;
00286     mPresenceVector |= PresenceVector::TimeStamp;
00287     return true;
00288 }
00289 
00290 
00302 int ReportVelocityState::WriteMessageBody(Packet& packet) const
00303 {
00304     int expected = USHORT_SIZE;
00305     int written = 0;
00306 
00307     written += packet.Write(mPresenceVector);
00308 
00309     if((mPresenceVector & PresenceVector::VelocityX) > 0)
00310     {
00311         expected += UINT_SIZE;
00312         written += ScaledInteger::Write(packet, mVelocityX, Limits::MaxVelocity, Limits::MinVelocity, ScaledInteger::UInt);
00313     }
00314     if((mPresenceVector & PresenceVector::VelocityY) > 0)
00315     {
00316         expected += UINT_SIZE;
00317         written += ScaledInteger::Write(packet, mVelocityY, Limits::MaxVelocity, Limits::MinVelocity, ScaledInteger::UInt);
00318     }
00319     if((mPresenceVector & PresenceVector::VelocityZ) > 0)
00320     {
00321         expected += UINT_SIZE;
00322         written += ScaledInteger::Write(packet, mVelocityZ, Limits::MaxVelocity, Limits::MinVelocity, ScaledInteger::UInt);
00323     }
00324     if((mPresenceVector & PresenceVector::VelocityRMS) > 0)
00325     {
00326         expected += UINT_SIZE;
00327         written += ScaledInteger::Write(packet, mVelocityRMS, Limits::MaxVelocityRMS, Limits::MinVelocityRMS, ScaledInteger::UInt);
00328     }
00329     if((mPresenceVector & PresenceVector::RollRate) > 0)
00330     {
00331         expected += USHORT_SIZE;
00332         written += ScaledInteger::Write(packet, mRollRate, Limits::MaxRotationalRate, Limits::MinRotationalRate, ScaledInteger::UShort);
00333     }
00334     if((mPresenceVector & PresenceVector::PitchRate) > 0)
00335     {
00336         expected += USHORT_SIZE;
00337         written += ScaledInteger::Write(packet, mPitchRate, Limits::MaxRotationalRate, Limits::MinRotationalRate, ScaledInteger::UShort);
00338     }
00339     if((mPresenceVector & PresenceVector::YawRate) > 0)
00340     {
00341         expected += USHORT_SIZE;
00342         written += ScaledInteger::Write(packet, mYawRate, Limits::MaxRotationalRate, Limits::MinRotationalRate, ScaledInteger::UShort);
00343     }
00344     if((mPresenceVector & PresenceVector::RateRMS) > 0)
00345     {
00346         expected += USHORT_SIZE;
00347         written += ScaledInteger::Write(packet, mRateRMS, Limits::MaxRateRMS, Limits::MinRateRMS, ScaledInteger::UShort);
00348     }
00349     if((mPresenceVector & PresenceVector::TimeStamp) > 0)
00350     {
00351         expected += UINT_SIZE;
00352         written += packet.Write(mTimeStamp.ToUInt());
00353     }
00354 
00355     return expected == written ? written : -1;
00356 }
00357 
00358 
00370 int ReportVelocityState::ReadMessageBody(const Packet& packet)
00371 {
00372     int expected = USHORT_SIZE;
00373     int read = 0;
00374 
00375     read += packet.Read(mPresenceVector);
00376 
00377     if((mPresenceVector & PresenceVector::VelocityX) > 0)
00378     {
00379         expected += UINT_SIZE;
00380         read += ScaledInteger::Read(packet, mVelocityX, Limits::MaxVelocity, Limits::MinVelocity, ScaledInteger::UInt);
00381     }
00382     if((mPresenceVector & PresenceVector::VelocityY) > 0)
00383     {
00384         expected += UINT_SIZE;
00385         read += ScaledInteger::Read(packet, mVelocityY, Limits::MaxVelocity, Limits::MinVelocity, ScaledInteger::UInt);
00386     }
00387     if((mPresenceVector & PresenceVector::VelocityZ) > 0)
00388     {
00389         expected += UINT_SIZE;
00390         read += ScaledInteger::Read(packet, mVelocityZ, Limits::MaxVelocity, Limits::MinVelocity, ScaledInteger::UInt);
00391     }
00392     if((mPresenceVector & PresenceVector::VelocityRMS) > 0)
00393     {
00394         expected += UINT_SIZE;
00395         read += ScaledInteger::Read(packet, mVelocityRMS, Limits::MaxVelocityRMS, Limits::MinVelocityRMS, ScaledInteger::UInt);
00396     }
00397     if((mPresenceVector & PresenceVector::RollRate) > 0)
00398     {
00399         expected += USHORT_SIZE;
00400         read += ScaledInteger::Read(packet, mRollRate, Limits::MaxRotationalRate, Limits::MinRotationalRate, ScaledInteger::UShort);
00401     }
00402     if((mPresenceVector & PresenceVector::PitchRate) > 0)
00403     {
00404         expected += USHORT_SIZE;
00405         read += ScaledInteger::Read(packet, mPitchRate, Limits::MaxRotationalRate, Limits::MinRotationalRate, ScaledInteger::UShort);
00406     }
00407     if((mPresenceVector & PresenceVector::YawRate) > 0)
00408     {
00409         expected += USHORT_SIZE;
00410         read += ScaledInteger::Read(packet, mYawRate, Limits::MaxRotationalRate, Limits::MinRotationalRate, ScaledInteger::UShort);
00411     }
00412     if((mPresenceVector & PresenceVector::RateRMS) > 0)
00413     {
00414         expected += USHORT_SIZE;
00415         read += ScaledInteger::Read(packet, mRateRMS, Limits::MaxRateRMS, Limits::MinRateRMS, ScaledInteger::UShort);
00416     }
00417 
00418     if((mPresenceVector & PresenceVector::TimeStamp) > 0)
00419     {
00420         UInt time = 0;
00421         expected += UINT_SIZE;
00422         read += packet.Read(time);
00423         mTimeStamp.SetTime(time);
00424     }
00425 
00426     return expected == read ? read : -1;
00427 }
00428 
00429 
00435 void ReportVelocityState::ClearMessageBody()
00436 {
00437     mPresenceVector = 0;
00438     mVelocityX = 0;
00439     mVelocityY = 0;
00440     mVelocityZ = 0;
00441     mVelocityRMS = 0;
00442     mRollRate = 0;
00443     mPitchRate = 0;
00444     mYawRate = 0;
00445     mRateRMS = 0;
00446     mTimeStamp.SetCurrentTime();
00447 }
00448 
00449 
00457 int ReportVelocityState::RunTestCase() const
00458 {
00459     int result = 0;
00460 
00461     Packet packet;
00462 
00463     ReportVelocityState msg1, msg2;
00464 
00465     msg1.SetVelocityZ(123);
00466     msg1.SetRateRMS(.155);
00467 
00468     if((msg1.WriteMessageBody(packet) != -1) &&
00469         (msg2.ReadMessageBody(packet) != -1))
00470     {
00471         if(msg2.AreFieldsPresent(PresenceVector::VelocityZ | PresenceVector::RateRMS) )
00472          {
00473                 result = 1;
00474          }
00475     }
00476 
00477     return result;
00478 }
00479 
00480 
00486 void ReportVelocityState::PrintMessageBody() const
00487 {
00488     std::cout << "Velocity: [" << std::fixed << std::setprecision(2) << mVelocityX << ", " 
00489                                << std::fixed << std::setprecision(2) << mVelocityY << ", " 
00490                                << std::fixed << std::setprecision(2) << mVelocityZ << "]\n";
00491     std::cout << "Rotation: [" << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mRollRate) << ", " 
00492                                << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mPitchRate) << ", " 
00493                                << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mYawRate) << "]\n";
00494     std::cout << "Timestamp: " << mTimeStamp.ToString() << std::endl;
00495 }
00496 
00497 
00503 ReportVelocityState& ReportVelocityState::operator=(const ReportVelocityState& message)
00504 {
00505     if(this != &message)
00506     {
00507         CopyHeaderData(&message);
00508         mPresenceVector = message.mPresenceVector;
00509         mVelocityX = message.mVelocityX;
00510         mVelocityY = message.mVelocityY;
00511         mVelocityZ = message.mVelocityZ;
00512         mVelocityRMS = message.mVelocityRMS;
00513         mRollRate = message.mRollRate;
00514         mPitchRate = message.mPitchRate;
00515         mYawRate = message.mYawRate;
00516         mRateRMS = message.mRateRMS;
00517         mTimeStamp = message.mTimeStamp;
00518         
00519     }
00520     return *this;
00521 }
00522 
00523 
00524 /*  End of File */