JAUS++: jaus++/2.0/src/jaus/mobility/sensors/reportaccelerationstate.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/reportaccelerationstate.h"
00041 #include "jaus/core/scaledinteger.h"
00042 #include <cxutils/math/cxmath.h>
00043 #include <iomanip>
00044 
00045 const double JAUS::ReportAccelerationState::Limits::MinLinearAcceleration = -1310.68;
00046 const double JAUS::ReportAccelerationState::Limits::MaxLinearAcceleration = 1310.68;
00047 const double JAUS::ReportAccelerationState::Limits::MinAccelerationRMS = 0.0;
00048 const double JAUS::ReportAccelerationState::Limits::MaxAccelerationRMS = 100.0;
00049 const double JAUS::ReportAccelerationState::Limits::MinRotationalAcceleration = -13106.8;
00050 const double JAUS::ReportAccelerationState::Limits::MaxRotationalAcceleration = 13106.8;
00051 const double JAUS::ReportAccelerationState::Limits::MinRotationalAccelerationRMS = 0.0;
00052 const double JAUS::ReportAccelerationState::Limits::MaxRotationalAccelerationRMS = CxUtils::PI;
00053 
00054 using namespace JAUS;
00055 
00056 
00065 ReportAccelerationState::ReportAccelerationState(const Address& dest, const Address& src) : Message(REPORT_ACCELERATION_STATE, dest, src)
00066 {
00067     mPresenceVector = 0;
00068     mAccelerationX = 0;
00069     mAccelerationY = 0;
00070     mAccelerationZ = 0;
00071     mAccelerationRMS = 0;
00072     mRollAcceleration = 0;
00073     mPitchAcceleration = 0;
00074     mYawAcceleration = 0;
00075     mRotationalAccelerationRMS = 0;
00076     mTimeStamp.SetCurrentTime();
00077 }
00078 
00079 
00085 ReportAccelerationState::ReportAccelerationState(const ReportAccelerationState& message) : Message(REPORT_ACCELERATION_STATE)
00086 {
00087     *this = message;
00088 }
00089 
00090 
00096 ReportAccelerationState::~ReportAccelerationState()
00097 {
00098 }
00099 
00100 
00111 bool ReportAccelerationState::SetAccelerationX(const double value)
00112 {
00113     if(value >= Limits::MinLinearAcceleration && value <= Limits::MaxLinearAcceleration)
00114     {
00115         mAccelerationX = value;
00116         mPresenceVector |= PresenceVector::AccelerationX;
00117         return true;
00118     }
00119     return false;
00120 }
00121 
00122 
00133 bool ReportAccelerationState::SetAccelerationY(const double value)
00134 {
00135     if(value >= Limits::MinLinearAcceleration && value <= Limits::MaxLinearAcceleration)
00136     {
00137         mAccelerationY = value;
00138         mPresenceVector |= PresenceVector::AccelerationY;
00139         return true;
00140     }
00141     return false;
00142 }
00143 
00144 
00156 bool ReportAccelerationState::SetAccelerationZ(const double value)
00157 {
00158     if(value >= Limits::MinLinearAcceleration && value <= Limits::MaxLinearAcceleration)
00159     {
00160         mAccelerationZ = value;
00161         mPresenceVector |= PresenceVector::AccelerationZ;
00162         return true;
00163     }
00164     return false;
00165 }
00166 
00167 
00179 bool ReportAccelerationState::SetAccelerationRMS(const double value)
00180 {
00181     if(value >= Limits::MinAccelerationRMS && value <= Limits::MaxAccelerationRMS)
00182     {
00183         mAccelerationRMS = value;
00184         mPresenceVector |= PresenceVector::AccelerationRMS;
00185         return true;
00186     }
00187     return false;
00188 }
00189 
00190 
00202 bool ReportAccelerationState::SetRollAcceleration(const double value)
00203 {
00204     if(value >= Limits::MinRotationalAcceleration && value <= Limits::MaxRotationalAcceleration)
00205     {
00206         mRollAcceleration = value;
00207         mPresenceVector |= PresenceVector::RollAcceleration;
00208         return true;
00209     }
00210     return false;
00211 }
00212 
00213 
00225 bool ReportAccelerationState::SetPitchAcceleration(const double value)
00226 {
00227     if(value >= Limits::MinRotationalAcceleration && value <= Limits::MaxRotationalAcceleration)
00228     {
00229         mPitchAcceleration = value;
00230         mPresenceVector |= PresenceVector::PitchAcceleration;
00231         return true;
00232     }
00233     return false;
00234 }
00235 
00236 
00248 bool ReportAccelerationState::SetYawAcceleration(const double value)
00249 {
00250     if(value >= Limits::MinRotationalAcceleration && value <= Limits::MaxRotationalAcceleration)
00251     {
00252         mYawAcceleration = value;
00253         mPresenceVector |= PresenceVector::YawAcceleration;
00254         return true;
00255     }
00256     return false;
00257 }
00258 
00259 
00271 bool ReportAccelerationState::SetRotationalAccelerationRMS(const double value)
00272 {
00273     if(value >= Limits::MinRotationalAccelerationRMS && value <= Limits::MaxRotationalAccelerationRMS)
00274     {
00275         mRotationalAccelerationRMS = value;
00276         mPresenceVector |= PresenceVector::RotationalAccelerationRMS;
00277         return true;
00278     }
00279     return false;
00280 }
00281 
00282 
00293 bool ReportAccelerationState::SetTimeStamp(const JAUS::Time& time)
00294 {
00295     mTimeStamp = time;
00296     mPresenceVector |= PresenceVector::TimeStamp;
00297     return true;
00298 }
00299 
00300 
00312 int ReportAccelerationState::WriteMessageBody(Packet& packet) const
00313 {
00314     int expected = USHORT_SIZE;
00315     int written = 0;
00316 
00317     written += packet.Write(mPresenceVector);
00318 
00319     if((mPresenceVector & PresenceVector::AccelerationX) > 0)
00320     {
00321         expected += UINT_SIZE;
00322         written += ScaledInteger::Write(packet, mAccelerationX, Limits::MaxLinearAcceleration, Limits::MinLinearAcceleration, ScaledInteger::UInt);
00323     }
00324     if((mPresenceVector & PresenceVector::AccelerationY) > 0)
00325     {
00326         expected += UINT_SIZE;
00327         written += ScaledInteger::Write(packet, mAccelerationY, Limits::MaxLinearAcceleration, Limits::MinLinearAcceleration, ScaledInteger::UInt);
00328     }
00329     if((mPresenceVector & PresenceVector::AccelerationZ) > 0)
00330     {
00331         expected += UINT_SIZE;
00332         written += ScaledInteger::Write(packet, mAccelerationZ, Limits::MaxLinearAcceleration, Limits::MinLinearAcceleration, ScaledInteger::UInt);
00333     }
00334     if((mPresenceVector & PresenceVector::AccelerationRMS) > 0)
00335     {
00336         expected += UINT_SIZE;
00337         written += ScaledInteger::Write(packet, mAccelerationRMS, Limits::MaxAccelerationRMS, Limits::MinAccelerationRMS, ScaledInteger::UInt);
00338     }
00339     if((mPresenceVector & PresenceVector::RollAcceleration) > 0)
00340     {
00341         expected += UINT_SIZE;
00342         written += ScaledInteger::Write(packet, mRollAcceleration, Limits::MaxRotationalAcceleration, Limits::MinRotationalAcceleration, ScaledInteger::UInt);
00343     }
00344     if((mPresenceVector & PresenceVector::PitchAcceleration) > 0)
00345     {
00346         expected += UINT_SIZE;
00347         written += ScaledInteger::Write(packet, mPitchAcceleration, Limits::MaxRotationalAcceleration, Limits::MinRotationalAcceleration, ScaledInteger::UInt);
00348     }
00349     if((mPresenceVector & PresenceVector::YawAcceleration) > 0)
00350     {
00351         expected += UINT_SIZE;
00352         written += ScaledInteger::Write(packet, mYawAcceleration, Limits::MaxRotationalAcceleration, Limits::MinRotationalAcceleration, ScaledInteger::UInt);
00353     }
00354     if((mPresenceVector & PresenceVector::RotationalAccelerationRMS) > 0)
00355     {
00356         expected += USHORT_SIZE;
00357         written += ScaledInteger::Write(packet, mRotationalAccelerationRMS, Limits::MaxRotationalAccelerationRMS, Limits::MinRotationalAccelerationRMS, ScaledInteger::UShort);
00358     }
00359     if((mPresenceVector & PresenceVector::TimeStamp) > 0)
00360     {
00361         expected += UINT_SIZE;
00362         written += packet.Write(mTimeStamp.ToUInt());
00363     }
00364 
00365     return expected == written ? written : -1;
00366 }
00367 
00368 
00380 int ReportAccelerationState::ReadMessageBody(const Packet& packet)
00381 {
00382     int expected = USHORT_SIZE;
00383     int read = 0;
00384 
00385     read += packet.Read(mPresenceVector);
00386 
00387     if((mPresenceVector & PresenceVector::AccelerationX) > 0)
00388     {
00389         expected += UINT_SIZE;
00390         read += ScaledInteger::Read(packet, mAccelerationX, Limits::MaxLinearAcceleration, Limits::MinLinearAcceleration, ScaledInteger::UInt);
00391     }
00392     if((mPresenceVector & PresenceVector::AccelerationY) > 0)
00393     {
00394         expected += UINT_SIZE;
00395         read += ScaledInteger::Read(packet, mAccelerationY, Limits::MaxLinearAcceleration, Limits::MinLinearAcceleration, ScaledInteger::UInt);
00396     }
00397     if((mPresenceVector & PresenceVector::AccelerationZ) > 0)
00398     {
00399         expected += UINT_SIZE;
00400         read += ScaledInteger::Read(packet, mAccelerationZ, Limits::MaxLinearAcceleration, Limits::MinLinearAcceleration, ScaledInteger::UInt);
00401     }
00402     if((mPresenceVector & PresenceVector::AccelerationRMS) > 0)
00403     {
00404         expected += UINT_SIZE;
00405         read += ScaledInteger::Read(packet, mAccelerationRMS, Limits::MaxAccelerationRMS, Limits::MinAccelerationRMS, ScaledInteger::UInt);
00406     }
00407     if((mPresenceVector & PresenceVector::RollAcceleration) > 0)
00408     {
00409         expected += UINT_SIZE;
00410         read += ScaledInteger::Read(packet, mRollAcceleration, Limits::MaxRotationalAcceleration, Limits::MinRotationalAcceleration, ScaledInteger::UInt);
00411     }
00412     if((mPresenceVector & PresenceVector::PitchAcceleration) > 0)
00413     {
00414         expected += UINT_SIZE;
00415         read += ScaledInteger::Read(packet, mPitchAcceleration, Limits::MaxRotationalAcceleration, Limits::MinRotationalAcceleration, ScaledInteger::UInt);
00416     }
00417     if((mPresenceVector & PresenceVector::YawAcceleration) > 0)
00418     {
00419         expected += UINT_SIZE;
00420         read += ScaledInteger::Read(packet, mYawAcceleration, Limits::MaxRotationalAcceleration, Limits::MinRotationalAcceleration, ScaledInteger::UInt);
00421     }
00422     if((mPresenceVector & PresenceVector::RotationalAccelerationRMS) > 0)
00423     {
00424         expected += USHORT_SIZE;
00425         read += ScaledInteger::Read(packet, mRotationalAccelerationRMS, Limits::MaxRotationalAccelerationRMS, Limits::MinRotationalAccelerationRMS, ScaledInteger::UShort);
00426     }
00427 
00428     if((mPresenceVector & PresenceVector::TimeStamp) > 0)
00429     {
00430         UInt time = 0;
00431         expected += UINT_SIZE;
00432         read += packet.Read(time);
00433         mTimeStamp.SetTime(time);
00434     }
00435 
00436     return expected == read ? read : -1;
00437 }
00438 
00439 
00445 void ReportAccelerationState::ClearMessageBody()
00446 {
00447     mPresenceVector = 0;
00448     mAccelerationX = 0;
00449     mAccelerationY = 0;
00450     mAccelerationZ = 0;
00451     mAccelerationRMS = 0;
00452     mRollAcceleration = 0;
00453     mPitchAcceleration = 0;
00454     mYawAcceleration = 0;
00455     mRotationalAccelerationRMS = 0;
00456     mTimeStamp.SetCurrentTime();
00457 }
00458 
00459 
00467 int ReportAccelerationState::RunTestCase() const
00468 {
00469     int result = 0;
00470 
00471     Packet packet;
00472 
00473     ReportAccelerationState msg1, msg2;
00474 
00475     msg1.SetAccelerationX(1023);
00476     msg1.SetAccelerationRMS(.155);
00477 
00478     if((msg1.WriteMessageBody(packet) != -1) &&
00479         (msg2.ReadMessageBody(packet) != -1))
00480     {
00481         if(msg2.AreFieldsPresent(PresenceVector::AccelerationX | PresenceVector::AccelerationRMS) )
00482          {
00483                 result = 1;
00484          }
00485     }
00486 
00487     return result;
00488 }
00489 
00490 
00496 void ReportAccelerationState::PrintMessageBody() const
00497 {
00498     std::cout << "Acceleration (XYZ): [" << std::fixed << std::setprecision(2) << mAccelerationX << ", " 
00499                                          << std::fixed << std::setprecision(2) << mAccelerationY << ", " 
00500                                          << std::fixed << std::setprecision(2) << mAccelerationZ << "]\n";
00501     std::cout << "Acceleration (RPY): [" << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mRollAcceleration) << ", " 
00502                                          << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mPitchAcceleration) << ", " 
00503                                          << std::fixed << std::setprecision(1) << CxUtils::CxToDegrees(mYawAcceleration) << "]\n";
00504     std::cout << "Timestamp: " << mTimeStamp.ToString() << std::endl;
00505 }
00506 
00507 
00513 ReportAccelerationState& ReportAccelerationState::operator=(const ReportAccelerationState& message)
00514 {
00515     if(this != &message)
00516     {
00517         CopyHeaderData(&message);
00518         mPresenceVector = message.mPresenceVector;
00519         mAccelerationX = message.mAccelerationX;
00520         mAccelerationY = message.mAccelerationY;
00521         mAccelerationZ = message.mAccelerationZ;
00522         mAccelerationRMS = message.mAccelerationRMS;
00523         mRollAcceleration = message.mRollAcceleration;
00524         mPitchAcceleration = message.mPitchAcceleration;
00525         mYawAcceleration = message.mYawAcceleration;
00526         mRotationalAccelerationRMS = message.mRotationalAccelerationRMS;
00527         mTimeStamp = message.mTimeStamp;
00528         
00529     }
00530     return *this;
00531 }
00532 
00533 
00534 /*  End of File */