helper.h
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #ifndef HELPER_H_
00033 #define HELPER_H_
00034 
00035 #include <asctec_hl_comm/mav_ctrl.h>
00036 #include <tf/transform_datatypes.h>
00037 
00038 namespace helper
00039 {
00040 
00042 const double ASCTEC_ACC_TO_SI = 9.81e-3;
00043 
00045 const double ASCTEC_OMEGA_TO_SI = 0.015 * M_PI / 180.0;
00046 
00048 const double ASCTEC_ATTITUDE_TO_SI = 0.01 * M_PI / 180.0;
00049 
00051 template<typename T>
00052   inline double asctecAccToSI(const T & val)
00053   {
00054     return static_cast<double> (val) * ASCTEC_ACC_TO_SI;
00055   }
00056 
00058 template<typename T>
00059   inline double asctecOmegaToSI(const T & val)
00060   {
00061     return static_cast<double> (val) * ASCTEC_OMEGA_TO_SI;
00062   }
00063 
00065 template<typename T>
00066   inline double asctecAttitudeToSI(const T & val)
00067   {
00068     return static_cast<double> (val) * ASCTEC_ATTITUDE_TO_SI;
00069   }
00070 
00072 inline uint16_t rateToPeriod(const double & rate)
00073 {
00074   if (rate > 0)
00075     return static_cast<uint16_t> (1000.0 / rate);
00076   else
00077     return 0;
00078 }
00079 
00081 template<typename T>
00082   inline T clamp(const T & min, const T & max, const T & val)
00083   {
00084     if (val >= max)
00085       return max;
00086     else if (val <= min)
00087       return min;
00088     else
00089       return val;
00090   }
00091 
00093 template<typename T>
00094   inline T clamp(const T & min, const T & max, const T & val, bool * clamped)
00095   {
00096     if (val >= max)
00097     {
00098       *clamped = true;
00099       return max;
00100     }
00101     else if (val <= min)
00102     {
00103       *clamped = true;
00104       return min;
00105     }
00106     else
00107     {
00108       *clamped = false;
00109       return val;
00110     }
00111   }
00112 
00114 
00117 inline uint32_t param2Fixpoint(const double & param)
00118 {
00119   return clamp<uint32_t> (0, 4294967296.0 - 1, param * 32768.0 + 2147483648.0 - 1);
00120   //                            2^32                    2^15         2^31
00121 }
00122 
00124 
00127 inline double param2Double(const uint32_t & param)
00128 {
00129   return ((double)param - 2147483648.0) / 32768.0;
00130 }
00131 
00133 
00136 inline double debug2Double(const int16_t & param)
00137 {
00138   return param * 0.001953125;
00139   //  return (double)param / 512.0;
00140 }
00141 
00143 inline int yaw2asctec(const double & yaw)
00144 {
00145   return ((yaw < 0 ? yaw + 2 * M_PI : yaw) * 180.0 / M_PI) * 1000.0;
00146 }
00147 
00148 #ifdef __APPLE__
00149 /* Possible method for setting __x87_inline_math__ */
00150 #if (defined(__i386__) || defined(i386) || defined(__amd64) || defined(__x86_64))
00151 #if (!defined(__x87_inline_math__) && defined(__FAST_MATH__))
00152 #define __x87_inline_math__
00153 #endif
00154 #endif
00155 
00156 #ifdef __x87_inline_math__
00157 /*
00158 ** Compute sine and cosine at same time (faster than separate calls).
00159 ** (*s) gets sin(x)
00160 ** (*c) gets cos(x)
00161 */
00162 #define sincos(x,s,c) sincos_x87_inline(x,s,c)
00163 void sincos_x87_inline(double x,double *s,double *c);
00164 extern __inline__  void sincos_x87_inline(double x,double *s,double *c)
00165     {
00166     __asm__ ("fsincos;" : "=t" (*c), "=u" (*s) : "0" (x) : "st(7)");
00167     }
00168 #else
00169 #define sincos(th,x,y) { (*(x))=sin(th); (*(y))=cos(th); }
00170 #endif
00171 #endif
00172 
00174 inline void angle2quaternion(const double &roll, const double &pitch, const double &yaw, double *w, double *x,
00175                              double *y, double *z)
00176 {
00177   double sR2, cR2, sP2, cP2, sY2, cY2;
00178   sincos(roll * 0.5, &sR2, &cR2);
00179   sincos(pitch * 0.5, &sP2, &cP2);
00180   sincos(yaw * 0.5, &sY2, &cY2);
00181 
00182   // TODO: change rotation order
00183   // this follows pre- 2012 firmware rotation order: Rz*Rx*Ry
00184 //  *w = cP2 * cR2 * cY2 - sP2 * sR2 * sY2;
00185 //  *x = cP2 * cY2 * sR2 - cR2 * sP2 * sY2;
00186 //  *y = cR2 * cY2 * sP2 + cP2 * sR2 * sY2;
00187 //  *z = cP2 * cR2 * sY2 + cY2 * sP2 * sR2;
00188 
00189   // Rz*Ry*Rx for 2012 firmware on the LL:
00190   *w = cP2 * cR2 * cY2 + sP2 * sR2 * sY2;
00191   *x = cP2 * cY2 * sR2 - cR2 * sP2 * sY2;
00192   *y = cR2 * cY2 * sP2 + cP2 * sR2 * sY2;
00193   *z = cP2 * cR2 * sY2 - cY2 * sP2 * sR2;
00194 }
00195 
00196 template<typename T>
00197   inline void setDiagonalCovariance(T & covariance_matrix, const double & covariance)
00198   {
00199     covariance_matrix[0] = covariance;
00200     covariance_matrix[4] = covariance;
00201     covariance_matrix[8] = covariance;
00202   }
00203 
00204 } // end of namespace helper
00205 
00206 #endif /* HELPER_H_ */


asctec_hl_interface
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Thu Jun 6 2019 20:57:17