Line data    Source code

       1             : #pragma once
       2             : 
       3             : /*
       4             :    This program is free software: you can redistribute it and/or modify
       5             :    it under the terms of the GNU General Public License as published by
       6             :    the Free Software Foundation, either version 3 of the License, or
       7             :    (at your option) any later version.
       8             : 
       9             :    This program is distributed in the hope that it will be useful,
      10             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
      11             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      12             :    GNU General Public License for more details.
      13             : 
      14             :    You should have received a copy of the GNU General Public License
      15             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      16             :  */
      17             : 
      18             : /*
      19             :  *  AHRS View class - for creating a 2nd view of the vehicle attitude
      20             :  *
      21             :  */
      22             : 
      23             : #include "AP_AHRS.h"
      24             : 
      25             : // fwd declarations to avoid include errors
      26             : class AC_AttitudeControl;
      27             : class AC_PosControl;
      28             : 
      29             : class AP_AHRS_View
      30             : {
      31             : public:
      32             :     // Constructor
      33             :     AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation, float pitch_trim_deg=0);
      34             : 
      35             :     // update state
      36             :     void update();
      37             : 
      38             :     // empty virtual destructor
      39          14 :     virtual ~AP_AHRS_View() {}
      40             : 
      41             :     // return a smoothed and corrected gyro vector
      42     1634812 :     const Vector3f &get_gyro(void) const {
      43     1634812 :         return gyro;
      44             :     }
      45             : 
      46             :     // return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
      47             :     Vector3f get_gyro_latest(void) const;
      48             : 
      49             :     // return a DCM rotation matrix representing our current attitude in this view
      50           0 :     const Matrix3f &get_rotation_body_to_ned(void) const {
      51           0 :         return rot_body_to_ned;
      52             :     }
      53             : 
      54             :     // return a Quaternion representing our current attitude in this view
      55    36328918 :     void get_quat_body_to_ned(Quaternion &quat) const {
      56    36328918 :         quat.from_rotation_matrix(rot_body_to_ned);
      57    36328918 :     }
      58             : 
      59             :     // apply pitch trim
      60             :     void set_pitch_trim(float trim_deg);
      61             : 
      62             :     // roll/pitch/yaw euler angles, all in radians
      63      921436 :     float get_roll_rad() const { return roll; }
      64      684152 :     float get_pitch_rad() const { return pitch; }
      65     1137220 :     float get_yaw_rad() const { return yaw; }
      66             : 
      67             :     // helper trig value accessors
      68    35495375 :     float cos_roll() const {
      69    35495375 :         return trig.cos_roll;
      70             :     }
      71    32839029 :     float cos_pitch() const {
      72    32839029 :         return trig.cos_pitch;
      73             :     }
      74    40555138 :     float cos_yaw() const {
      75    40555138 :         return trig.cos_yaw;
      76             :     }
      77             :     float sin_roll() const {
      78             :         return trig.sin_roll;
      79             :     }
      80             :     float sin_pitch() const {
      81             :         return trig.sin_pitch;
      82             :     }
      83    40555138 :     float sin_yaw() const {
      84    40555138 :         return trig.sin_yaw;
      85             :     }
      86             : 
      87             : 
      88             :     /*
      89             :       wrappers around ahrs functions which pass-thru directly. See
      90             :       AP_AHRS.h for description of each function
      91             :      */
      92             :     bool get_location(Location &loc) const WARN_IF_UNUSED {
      93             :         return ahrs.get_location(loc);
      94             :     }
      95             : 
      96             :     bool get_wind(Vector3f &wind) {
      97             :         return ahrs.get_wind(wind);
      98             :     }
      99             : 
     100             :     bool airspeed_EAS(float &airspeed_ret) const WARN_IF_UNUSED {
     101             :         return ahrs.airspeed_EAS(airspeed_ret);
     102             :     }
     103             : 
     104             :     bool airspeed_TAS(float &airspeed_ret) const WARN_IF_UNUSED {
     105             :         return ahrs.airspeed_TAS(airspeed_ret);
     106             :     }
     107             : 
     108             :     float get_EAS2TAS(void) const {
     109             :         return ahrs.get_EAS2TAS();
     110             :     }
     111             : 
     112             :     Vector2f groundspeed_vector(void) {
     113             :         return ahrs.groundspeed_vector();
     114             :     }
     115             : 
     116      550704 :     bool get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED {
     117      550704 :         return ahrs.get_velocity_NED(vec);
     118             :     }
     119             : 
     120             :     bool get_relative_position_NED_home(Vector3f &vec) const WARN_IF_UNUSED {
     121             :         return ahrs.get_relative_position_NED_home(vec);
     122             :     }
     123             : 
     124             :     bool get_relative_position_NED_origin_float(Vector3f &vec) const WARN_IF_UNUSED {
     125             :         return ahrs.get_relative_position_NED_origin_float(vec);
     126             :     }
     127             : 
     128             :     bool get_relative_position_NE_home(Vector2f &vecNE) const WARN_IF_UNUSED {
     129             :         return ahrs.get_relative_position_NE_home(vecNE);
     130             :     }
     131             : 
     132             :     bool get_relative_position_NE_origin_float(Vector2f &vecNE) const WARN_IF_UNUSED {
     133             :         return ahrs.get_relative_position_NE_origin_float(vecNE);
     134             :     }
     135             : 
     136             :     void get_relative_position_D_home(float &posD) const {
     137             :         ahrs.get_relative_position_D_home(posD);
     138             :     }
     139             : 
     140             :     bool get_relative_position_D_origin_float(float &posD) const WARN_IF_UNUSED {
     141             :         return ahrs.get_relative_position_D_origin_float(posD);
     142             :     }
     143             : 
     144             :     float groundspeed(void) {
     145             :         return ahrs.groundspeed();
     146             :     }
     147             : 
     148    43559357 :     const Vector3f &get_accel_ef(void) const {
     149    43559357 :         return ahrs.get_accel_ef();
     150             :     }
     151             : 
     152    22972942 :     uint16_t get_position_NE_reset_count(void) WARN_IF_UNUSED {
     153    22972942 :         return ahrs.get_position_NE_reset_count();
     154             :     }
     155             : 
     156    31751271 :     uint16_t get_position_D_reset_count(void) WARN_IF_UNUSED {
     157    31751271 :         return ahrs.get_position_D_reset_count();
     158             :     }
     159             : 
     160             :     // rotate a 2D vector from earth frame to body frame
     161             :     // in result, x is forward, y is right
     162             :     Vector2f earth_to_body2D(const Vector2f &ef_vector) const;
     163             : 
     164             :     // rotate a 2D vector from body frame to earth frame
     165             :     // in input, x is forward, y is right
     166             :     Vector2f body_to_earth2D(const Vector2f &bf) const;
     167             : 
     168             :     // return the average size of the roll/pitch error estimate
     169             :     // since last call
     170             :     float get_error_rp(void) const {
     171             :         return ahrs.get_error_rp();
     172             :     }
     173             : 
     174             :     // return the average size of the yaw error estimate
     175             :     // since last call
     176             :     float get_error_yaw(void) const {
     177             :         return ahrs.get_error_yaw();
     178             :     }
     179             : 
     180             :     // Logging Functions
     181             :     void Write_AttitudeView(const Vector3f &targets) const;    
     182             : 
     183             :     float roll;
     184             :     float pitch;
     185             :     float yaw;
     186             :     int32_t roll_sensor;
     187             :     int32_t pitch_sensor;
     188             :     int32_t yaw_sensor;
     189             : 
     190             : 
     191             :     // get current rotation
     192             :     // note that this may not be the rotation were actually using, see _pitch_trim_deg
     193           0 :     enum Rotation get_rotation(void) const {
     194           0 :         return rotation;
     195             :     }
     196             : 
     197             :     // get pitch trim (deg)
     198           0 :     float get_pitch_trim() const { return _pitch_trim_deg; }
     199             : 
     200             :     // Rotate vector from AHRS reference frame to AHRS view refences frame
     201             :     void rotate(Vector3f &vec) const;
     202             : 
     203             : private:
     204             :     const enum Rotation rotation;
     205             :     AP_AHRS &ahrs;
     206             : 
     207             :     // body frame rotation for this View
     208             :     Matrix3f rot_view;
     209             :     // transpose of rot_view
     210             :     Matrix3f rot_view_T;
     211             :     Matrix3f rot_body_to_ned;
     212             :     Vector3f gyro;
     213             : 
     214             :     struct {
     215             :         float cos_roll;
     216             :         float cos_pitch;
     217             :         float cos_yaw;
     218             :         float sin_roll;
     219             :         float sin_pitch;
     220             :         float sin_yaw;
     221             :     } trig;
     222             : 
     223             :     float y_angle;
     224             :     float _pitch_trim_deg;
     225             : };