Data structure for compass in three dimensions. More...

#include <JCompass.hh>

Inheritance diagram for JDETECTOR::JCompass:

Public Member Functions
	JCompass ()
	Default constructor. More...

	JCompass (const double yaw, const double pitch, const double roll)
	Constructor. More...

	JCompass (const JAHRS &data, const JAHRSCalibration &calibration)
	Constructor. More...

	JCompass (const JQuaternion3D &Q)
	Constructor. More...

const JCompass &	getCompass () const
	Get compass. More...

void	setCompass (const JCompass &compass)
	Set compass. More...

JRotation3D	getRotation () const
	Get rotation matrix. More...

JQuaternion3D	getQuaternion () const
	Get quaternion. More...

double	getYaw () const
	Get yaw compass. More...

double	getPitch () const
	Get pitch compass. More...

double	getRoll () const
	Get roll compass. More...

bool	equals (const JCompass &compass, const double precision=std::numeric_limits< double >::min()) const
	Check equality. More...

void	correct (const double declination, const double meridian)
	Correct compass for magnetic declination and meridian convergence angle. More...

Protected Attributes
double	yaw

double	pitch

double	roll

Friends
std::istream &	operator>> (std::istream &in, JCompass &compass)
	Read compasss from input. More...

std::ostream &	operator<< (std::ostream &out, const JCompass &compass)
	Write compasss to output. More...

JReader &	operator>> (JReader &in, JCompass &compass)
	Read compasss from input. More...

JWriter &	operator<< (JWriter &out, const JCompass &compass)
	Write compasss to output. More...

Detailed Description

Data structure for compass in three dimensions.

Definitions of yaw, pitch and roll.

Note that the z-axis of the KM3NeT reference system points up.
So, the yaw angle, measured by the compass is measured from North to East (since in the compass system z-axis points down).
Note also that the sign of the angle JCompass::pitch is maintained and the signs of the angles JCompass::yaw and JCompass::roll inverted when converting to/from a rotation matrix or quaternion.

This class implements the JMATH::JMath and JLANG::JEquals interfaces.

Definition at line 49 of file JCompass.hh.

Constructor & Destructor Documentation

JDETECTOR::JCompass::JCompass ( )

inline

Default constructor.

Definition at line 56 of file JCompass.hh.

                :
       yaw  (0.0),
       pitch(0.0),
       roll (0.0)
     {}

JDETECTOR::JCompass::JCompass	(	const double	yaw,
		const double	pitch,
		const double	roll
	)

inline

Constructor.

Parameters

yaw	yaw angle [rad]
pitch	pitch angle [rad]
roll	roll angle [rad]

Definition at line 70 of file JCompass.hh.

                                 :
       yaw  (yaw),
       pitch(pitch),
       roll (roll)
     {}

JDETECTOR::JCompass::JCompass	(	const JAHRS &	data,
		const JAHRSCalibration &	calibration
	)

inline

Constructor.

Parameters

data	AHRS data
calibration	AHRS calibration

Definition at line 85 of file JCompass.hh.

     {
       using namespace std;
       using namespace JPP;
 
       double A0 = data.AHRS_A0;
       double A1 = data.AHRS_A1;
       double A2 = data.AHRS_A2;
     
       double H0 = data.AHRS_H0;
       double H1 = data.AHRS_H1;
       double H2 = data.AHRS_H2;
     
       A0  -=  calibration.ACC_OFFSET_X;
       A1  -=  calibration.ACC_OFFSET_Y;
       A2  -=  calibration.ACC_OFFSET_Z;
 
       JMatrix3D(calibration.ACC_ROT_XX, calibration.ACC_ROT_XY, calibration.ACC_ROT_XZ,
                 calibration.ACC_ROT_YX, calibration.ACC_ROT_YY, calibration.ACC_ROT_YZ,
                 calibration.ACC_ROT_ZX, calibration.ACC_ROT_ZY, calibration.ACC_ROT_ZZ).transform(A0, A1, A2);
     
       H0  -=  0.5 * (calibration.MAG_XMIN + calibration.MAG_XMAX);
       H1  -=  0.5 * (calibration.MAG_YMIN + calibration.MAG_YMAX);
       H2  -=  0.5 * (calibration.MAG_ZMIN + calibration.MAG_ZMAX);
 
       JMatrix3D(calibration.MAG_ROT_XX, calibration.MAG_ROT_XY, calibration.MAG_ROT_XZ,
                 calibration.MAG_ROT_YX, calibration.MAG_ROT_YY, calibration.MAG_ROT_YZ,
                 calibration.MAG_ROT_ZX, calibration.MAG_ROT_ZY, calibration.MAG_ROT_ZZ).transform(H0, H1, H2);
 
       // invert axis for central-logic board being upside down in optical module
 
       A0 =  A0;
       A1 = -A1;
       A2 = -A2;
     
       H0 =  H0;
       H1 = -H1;
       H2 = -H2;
 
       this->roll  = atan2(-A1, -A2);
       this->pitch = atan2(A0, sqrt(A1*A1 + A2*A2));
       this->yaw   = atan2(H2 * sin(roll) - H1 * cos(roll), H0 * cos(pitch) + H1 * sin(pitch) * sin(roll) + H2 * sin(pitch) * cos(roll));
     }

JDETECTOR::JCompass::JCompass ( const JQuaternion3D & Q )

inline

Constructor.

Parameters

Q	quaternion

Definition at line 135 of file JCompass.hh.

                                      :
       yaw  (0.0),
       pitch(0.0),
       roll (0.0)
     {
       using namespace std;
 
       this->yaw   = -atan2(2.0*(Q.getA()*Q.getD() + Q.getB()*Q.getC()), 1.0 - 2.0*(Q.getC()*Q.getC() + Q.getD()*Q.getD())); 
 
       const double sp = 2.0*(Q.getA()*Q.getC() - Q.getD()*Q.getB());
 
       if      (sp >= +1.0)
         this->pitch = asin(+1.0);
       else if (sp <= -1.0)
         this->pitch = asin(-1.0);
       else
         this->pitch = asin(sp);
 
       this->roll  = -atan2(2.0*(Q.getA()*Q.getB() + Q.getC()*Q.getD()), 1.0 - 2.0*(Q.getB()*Q.getB() + Q.getC()*Q.getC()));
     }

Member Function Documentation

const JCompass& JDETECTOR::JCompass::getCompass ( ) const

inline

Get compass.

Returns: this compass

Definition at line 162 of file JCompass.hh.

     {
       return static_cast<const JCompass&>(*this);
     }

void JDETECTOR::JCompass::setCompass ( const JCompass & compass )

inline

Set compass.

Parameters

compass compass

Definition at line 173 of file JCompass.hh.

     {
       static_cast<JCompass&>(*this) = compass;
     }

JRotation3D JDETECTOR::JCompass::getRotation ( ) const

inline

Get rotation matrix.

Returns: rotation matrix

Definition at line 184 of file JCompass.hh.

     {
       using namespace JPP;
 
       const JRotation3D Rx = JRotation3X(-this->getRoll());
       const JRotation3D Ry = JRotation3Y(+this->getPitch());
       const JRotation3D Rz = JRotation3Z(-this->getYaw());
 
       return (Rz * Ry * Rx);
     }

JQuaternion3D JDETECTOR::JCompass::getQuaternion ( ) const

inline

Get quaternion.

Returns: quaternion

Definition at line 201 of file JCompass.hh.

     {
       using namespace JPP;
 
       const double cy = cos(-0.5 * yaw);
       const double sy = sin(-0.5 * yaw);
       const double cp = cos(+0.5 * pitch);
       const double sp = sin(+0.5 * pitch);
       const double cr = cos(-0.5 * roll);
       const double sr = sin(-0.5 * roll);
 
       return JQuaternion3D(cr * cp * cy + sr * sp * sy,
                            sr * cp * cy - cr * sp * sy,
                            cr * sp * cy + sr * cp * sy,
                            cr * cp * sy - sr * sp * cy);
     }

double JDETECTOR::JCompass::getYaw ( ) const

inline

Get yaw compass.

Returns: yaw compass

Definition at line 224 of file JCompass.hh.

     { 
       return yaw;
     }

double JDETECTOR::JCompass::getPitch ( ) const

inline

Get pitch compass.

Returns: pitch compass

Definition at line 235 of file JCompass.hh.

     { 
       return pitch;
     }

double JDETECTOR::JCompass::getRoll ( ) const

inline

Get roll compass.

Returns: roll compass

Definition at line 246 of file JCompass.hh.

     { 
       return roll;
     }

bool JDETECTOR::JCompass::equals	(	const JCompass &	compass,
		const double	precision = `std::numeric_limits<double>::min()`
	)		const

inline

Check equality.

Parameters

compass	compass
precision	numerical precision

Returns: true if compass's are equal; else false

Definition at line 259 of file JCompass.hh.

     {
       return (fabs(getYaw()   - compass.getYaw())   <= precision &&
               fabs(getPitch() - compass.getPitch()) <= precision &&
               fabs(getRoll()  - compass.getRoll())  <= precision);
     }

void JDETECTOR::JCompass::correct	(	const double	declination,
		const double	meridian
	)

inline

Correct compass for magnetic declination and meridian convergence angle.

Parameters

declination	magnetic declination [rad]
meridian	meridian convergence angle [rad]

Definition at line 274 of file JCompass.hh.

     {
       this->yaw += declination;
       this->yaw -= meridian;
     }

Friends And Related Function Documentation

std::istream& operator>>	(	std::istream &	in,
		JCompass &	compass
	)

friend

Read compasss from input.

Parameters

in	input stream
compass	compasss

Returns: input stream

Definition at line 288 of file JCompass.hh.

     {
       in >> compass.yaw >> compass.pitch  >> compass.roll;
 
       return in;
     }

std::ostream& operator<<	(	std::ostream &	out,
		const JCompass &	compass
	)

friend

Write compasss to output.

Parameters

out	output stream
compass	compass

Returns: output stream

Definition at line 303 of file JCompass.hh.

     {
       out << compass.getYaw() << ' ' << compass.getPitch()  << ' ' << compass.getRoll();
 
       return out;
     }

JReader& operator>>	(	JReader &	in,
		JCompass &	compass
	)

friend

Read compasss from input.

Parameters

in	reader
compass	compasss

Returns: reader

Definition at line 318 of file JCompass.hh.

     {
       in >> compass.yaw;
       in >> compass.pitch;
       in >> compass.roll;
 
       return in;
     }

JWriter& operator<<	(	JWriter &	out,
		const JCompass &	compass
	)

friend

Write compasss to output.

Parameters

out	writer
compass	compasss

Returns: writer

Definition at line 335 of file JCompass.hh.

     {
       out << compass.yaw;
       out << compass.pitch;
       out << compass.roll;
 
       return out;
     }

Member Data Documentation

double JDETECTOR::JCompass::yaw

protected

Definition at line 346 of file JCompass.hh.

double JDETECTOR::JCompass::pitch

protected

Definition at line 347 of file JCompass.hh.

double JDETECTOR::JCompass::roll

protected

Definition at line 348 of file JCompass.hh.

The documentation for this class was generated from the following file:

JCompass.hh

Public Member Functions

Protected Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Friends And Related Function Documentation

Member Data Documentation