Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : /*
      17             :  *  AHRS View class - for creating a 2nd view of the vehicle attitude
      18             :  *
      19             :  */
      20             : 
      21             : #include "AP_AHRS_View.h"
      22             : #include <stdio.h>
      23             : 
      24        1285 : AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_trim_deg) :
      25        1285 :     rotation(_rotation),
      26        1285 :     ahrs(_ahrs)
      27             : {
      28        1285 :     switch (rotation) {
      29        1281 :     case ROTATION_NONE:
      30        1281 :         y_angle = 0;
      31        1281 :         break;
      32           4 :     case ROTATION_PITCH_90:
      33           4 :         y_angle = 90;
      34           4 :         break;
      35           0 :     case ROTATION_PITCH_270:
      36           0 :         y_angle =  270;
      37           0 :         break;
      38           0 :     default:
      39           0 :         AP_HAL::panic("Unsupported AHRS view %u", (unsigned)rotation);
      40             :     }
      41             : 
      42        1285 :     _pitch_trim_deg = pitch_trim_deg;
      43             :     // Add pitch trim
      44        1285 :     rot_view.from_euler(0, radians(wrap_360(y_angle + pitch_trim_deg)), 0);
      45        1285 :     rot_view_T = rot_view;
      46        1285 :     rot_view_T.transpose();
      47             : 
      48             :     // setup initial state
      49        1285 :     update();
      50        1285 : }
      51             : 
      52             : // apply pitch trim
      53           0 : void AP_AHRS_View::set_pitch_trim(float trim_deg) {
      54           0 :     _pitch_trim_deg = trim_deg; 
      55           0 :     rot_view.from_euler(0, radians(wrap_360(y_angle + _pitch_trim_deg)), 0);
      56           0 :     rot_view_T = rot_view;
      57           0 :     rot_view_T.transpose();
      58           0 : };
      59             : 
      60             : // update state
      61    39098897 : void AP_AHRS_View::update()
      62             : {
      63    39098897 :     rot_body_to_ned = ahrs.get_rotation_body_to_ned();
      64    39098897 :     gyro = ahrs.get_gyro();
      65             : 
      66    39098897 :     if (!is_zero(y_angle + _pitch_trim_deg)) {
      67      156869 :         rot_body_to_ned = rot_body_to_ned * rot_view_T;
      68      156869 :         gyro = rot_view * gyro;
      69             :     }
      70             : 
      71    39098897 :     rot_body_to_ned.to_euler(&roll, &pitch, &yaw);
      72             : 
      73    39098897 :     roll_sensor  = degrees(roll) * 100;
      74    39098897 :     pitch_sensor = degrees(pitch) * 100;
      75    39098897 :     yaw_sensor   = degrees(yaw) * 100;
      76    39098897 :     if (yaw_sensor < 0) {
      77    24114878 :         yaw_sensor += 36000;
      78             :     }
      79             : 
      80    39098897 :     ahrs.calc_trig(rot_body_to_ned,
      81    39098897 :                    trig.cos_roll, trig.cos_pitch, trig.cos_yaw,
      82    39098897 :                    trig.sin_roll, trig.sin_pitch, trig.sin_yaw);
      83    39098897 : }
      84             : 
      85             : // return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
      86    38148955 : Vector3f AP_AHRS_View::get_gyro_latest(void) const {
      87    38148955 :     return rot_view * ahrs.get_gyro_latest();
      88             : }
      89             : 
      90             : // rotate a 2D vector from earth frame to body frame
      91           0 : Vector2f AP_AHRS_View::earth_to_body2D(const Vector2f &ef) const
      92             : {
      93           0 :     return Vector2f(ef.x * trig.cos_yaw + ef.y * trig.sin_yaw,
      94           0 :                     -ef.x * trig.sin_yaw + ef.y * trig.cos_yaw);
      95             : }
      96             : 
      97             : // rotate a 2D vector from body frame to earth frame
      98           0 : Vector2f AP_AHRS_View::body_to_earth2D(const Vector2f &bf) const
      99             : {
     100           0 :     return Vector2f(bf.x * trig.cos_yaw - bf.y * trig.sin_yaw,
     101           0 :                     bf.x * trig.sin_yaw + bf.y * trig.cos_yaw);
     102             : }
     103             : 
     104             : // Rotate vector from AHRS reference frame to AHRS view reference frame
     105      414050 : void AP_AHRS_View::rotate(Vector3f &vec) const
     106             : {
     107      414050 :     vec = rot_view * vec;
     108      414050 : }