Line data    Source code

       1             : #include "AP_AHRS_External.h"
       2             : 
       3             : #if AP_AHRS_EXTERNAL_ENABLED
       4             : 
       5             : #include <AP_ExternalAHRS/AP_ExternalAHRS.h>
       6             : #include <AP_AHRS/AP_AHRS.h>
       7             : 
       8    41686716 : void AP_AHRS_External::update()
       9             : {
      10    41686716 :     AP::externalAHRS().update();
      11    41686716 : }
      12             : 
      13    41686716 : void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results)
      14             : {
      15    41686716 :     auto &extahrs = AP::externalAHRS();
      16             : 
      17    41686716 :     results.initialised = extahrs.initialised();
      18             : 
      19    41686716 :     results.healthy = extahrs.healthy();
      20             : 
      21             : #if AP_INERTIALSENSOR_ENABLED
      22    41686716 :     const AP_InertialSensor &_ins = AP::ins();
      23             :     // not using specific sensors:
      24    41686716 :     results.primary_gyro = _ins.get_first_usable_gyro();
      25    41686716 :     results.primary_accel = _ins.get_first_usable_accel();
      26             : #endif  // AP_INERTIALSENSOR_ENABLED
      27             : 
      28             :     // no limit on gains, large vel limit
      29    41686716 :     results.control_ground_speed_limit_ms = 400.0;
      30    41686716 :     results.control_gain_scaler_XY = 1;
      31    41686716 :     results.control_gain_scaler_Z = 1;
      32             : 
      33             :     // control height is never limited:
      34             :     // results.control_height_limit_valid = false;
      35             :     // results.control_height_limit_m = 0;
      36             : 
      37    41686716 :     if (!extahrs.get_quaternion(results.quaternion)) {
      38    41627223 :         results.attitude_valid = false;
      39    41627223 :         return;
      40             :     }
      41       59493 :     results.attitude_valid = true;
      42       59493 :     results.quaternion.rotation_matrix(results.dcm_matrix);
      43       59493 :     results.dcm_matrix.to_euler(&results.roll_rad, &results.pitch_rad, &results.yaw_rad);
      44             : 
      45       59493 :     results.gyro_drift.zero();
      46       59493 :     if (!extahrs.get_gyro(results.gyro_estimate)) {
      47             : #if AP_INERTIALSENSOR_ENABLED
      48       38868 :         results.gyro_estimate = _ins.get_gyro();
      49             : #endif  // AP_INERTIALSENSOR_ENABLED
      50             :     }
      51             : 
      52       59493 :     Vector3f accel;
      53       59493 :     if (!extahrs.get_accel(accel)) {
      54             : #if AP_INERTIALSENSOR_ENABLED
      55       38868 :         accel = _ins.get_accel();
      56             : #endif  // AP_INERTIALSENSOR_ENABLED
      57             :     }
      58             : 
      59             :     /*
      60             :      * acceleration estimates
      61             :      */
      62             :     // results.accel_bias = {} - External does not estimate accel bias
      63       59493 :     const Vector3f accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel;
      64       59493 :     results.accel_ef = accel_ef;
      65             : 
      66       59493 :     results.velocity_NED_valid = AP::externalAHRS().get_velocity_NED(results.velocity_NED);
      67             :     // a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
      68             :     // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for.
      69       59493 :     results.vert_pos_rate_D_valid = AP::externalAHRS().get_speed_down(results.vert_pos_rate_D);
      70             : 
      71             :     // ground velocity estimate in meters/second, in North/East order
      72       59493 :     results.velocity_NE = AP::externalAHRS().get_groundspeed_vector();
      73             : 
      74             :     /*
      75             :      * position estimates
      76             :      */
      77       59493 :     results.location_valid = AP::externalAHRS().get_location(results.location);
      78             : 
      79             :     // origin-relative functions
      80       59493 :     results.provides_common_origin = true;
      81             : 
      82             :     // origin-relative position:
      83       59493 :     Location orgn;
      84      118986 :     if (extahrs.get_origin(orgn) &&
      85       59493 :         results.location_valid) {
      86       59493 :         const Vector3p posNED = orgn.get_distance_NED_postype(results.location);
      87       59493 :         results.position_NE = posNED.xy();
      88       59493 :         results.position_NE_valid = true;
      89       59493 :         results.position_D = posNED.z;
      90       59493 :         results.position_D_valid = true;
      91             :     }
      92             : 
      93             :     // hagl is not supplied:
      94             :     // results.hagl_valid = false;
      95             :     // results.hagl = 0;
      96             : 
      97             :     /*
      98             :      * air data estimates
      99             :      */
     100             :     // wind estimate is not supplied:
     101             :     // results.wind = {};
     102             :     // results.wind_valid = false;
     103             : 
     104             :     /*
     105             :      * Sensor-related information
     106             :      */
     107             :     // true if the estimator will use GPS data in creating its
     108             :     // estimate when the data is good:
     109       59493 :     results.configured_to_use_gps = true;  // massive assumption here
     110             :     // true if GPS is configured as the horizontal position source
     111             :     // for this estimator.  Used to decide whether GPS will set
     112             :     // the navigation origin.
     113       59493 :     results.configured_to_use_gps_for_pos_XY = true;
     114             : 
     115             :     // are we consuming yaw from an external (e.g. vision-based) source?
     116             :     // this relates only to external sources being passed in via mavlink
     117             :     // results.using_extnav_for_yaw = false;
     118             : 
     119             :     // are we consuming yaw from a source which is *not* a compass
     120             :     // results.using_noncompass_for_yaw = false;
     121             : 
     122             : #if AP_AHRS_GET_MAG_DATA_ENABLED
     123             :     // estimators can provide their predicted magnetic fields:
     124             :     // ... but External does not, and probably should not as the
     125             :     // external unit will be experiencing a different magnetic
     126             :     // environment to the autopilot.
     127             :     // results.mag_field_NED = {};
     128             :     // results.mag_field_NED_valid = false;
     129             :     // results.mag_field_corrections = {};
     130             :     // results.mag_field_corrections_valid = false;
     131             : #endif  // AP_AHRS_GET_MAG_DATA_ENABLED
     132             : 
     133             :     /*
     134             :      * filter status and estimates quality values:
     135             :      */
     136       59493 :     AP::externalAHRS().get_filter_status(results.filter_status);
     137       59493 :     results.filter_status_valid = true;
     138             : 
     139             :     // provides the innovations normalised between 0 and 1:
     140       59493 :     results.variances_valid = AP::externalAHRS().get_variances(results.velVar, results.posVar, results.hgtVar, results.magVar, results.tasVar);
     141             : 
     142       59493 :     results.terrain_alt_variance = 0;
     143       59493 :     results.terrain_alt_variance_valid = true;
     144             : }
     145             : 
     146       66599 : bool AP_AHRS_External::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
     147             : {
     148       66599 :     return AP::externalAHRS().pre_arm_check(failure_msg, failure_msg_len);
     149             : }
     150             : 
     151      176762 : bool AP_AHRS_External::get_origin(Location &ret) const
     152             : {
     153      176762 :     return AP::externalAHRS().get_origin(ret);
     154             : }
     155             : 
     156         842 : bool AP_AHRS_External::set_origin(const Location &loc)
     157             : {
     158         842 :     return AP::externalAHRS().set_origin(loc);
     159             : }
     160             : 
     161             : #endif