Line data    Source code

       1             : #include "AP_AHRS_SIM.h"
       2             : 
       3             : #if AP_AHRS_SIM_ENABLED
       4             : 
       5             : #include "AP_AHRS.h"
       6             : 
       7    41686716 : bool AP_AHRS_SIM::get_location(Location &loc) const
       8             : {
       9    41686716 :     if (_sitl == nullptr) {
      10           0 :         return false;
      11             :     }
      12             : 
      13    41686716 :     const struct SITL::sitl_fdm &fdm = _sitl->state;
      14    41686716 :     loc = {};
      15    41686716 :     loc.lat = fdm.latitude * 1e7;
      16    41686716 :     loc.lng = fdm.longitude * 1e7;
      17    41686716 :     loc.alt = fdm.altitude*100;
      18             : 
      19    41686716 :     return true;
      20             : }
      21             : 
      22         598 : bool AP_AHRS_SIM::airspeed_EAS(float &airspeed_ret) const
      23             : {
      24         598 :     if (_sitl == nullptr) {
      25           0 :         return false;
      26             :     }
      27             : 
      28         598 :     airspeed_ret = _sitl->state.airspeed;
      29             : 
      30         598 :     return true;
      31             : }
      32             : 
      33           0 : bool AP_AHRS_SIM::airspeed_EAS(uint8_t index, float &airspeed_ret) const
      34             : {
      35           0 :     return airspeed_EAS(airspeed_ret);
      36             : }
      37             : 
      38    41686716 : bool AP_AHRS_SIM::get_filter_status(nav_filter_status &status) const
      39             : {
      40    41686716 :     memset(&status, 0, sizeof(status));
      41    41686716 :     status.flags.attitude = true;
      42    41686716 :     status.flags.horiz_vel = true;
      43    41686716 :     status.flags.vert_vel = true;
      44    41686716 :     status.flags.horiz_pos_rel = true;
      45    41686716 :     status.flags.horiz_pos_abs = true;
      46    41686716 :     status.flags.vert_pos = true;
      47    41686716 :     status.flags.pred_horiz_pos_rel = true;
      48    41686716 :     status.flags.pred_horiz_pos_abs = true;
      49    41686716 :     status.flags.initalized = true;
      50    41686716 :     status.flags.using_gps = true;
      51    41686716 :     status.flags.terrain_alt = true;
      52             : 
      53    41686716 :     return true;
      54             : }
      55             : 
      56    45502473 : bool AP_AHRS_SIM::get_origin(Location &ret) const
      57             : {
      58    45502473 :     if (_sitl == nullptr) {
      59           0 :         return false;
      60             :     }
      61             : 
      62    45502473 :     ret = _sitl->state.home;
      63             : 
      64    45502473 :     return true;
      65             : }
      66             : 
      67             : #if AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
      68             : /*
      69             :   return the ideal offsets for a compass instance, in body frame,
      70             :   milligauss.  The simulation subtracts SIM_MAGn_OFS from the field it
      71             :   reports and Compass adds COMPASS_OFS back when correcting, so the
      72             :   offset the compass wants is SIM_MAGn_OFS put through the same
      73             :   transformation the simulated sensor applies after subtracting it.
      74             :   SITL::SIM::get_mag_offsets_for_devid() does that, so the maths lives in one
      75             :   place rather than being duplicated here.
      76             :  */
      77           3 : bool AP_AHRS_SIM::get_mag_offsets(uint8_t mag_idx, Vector3f &magOffsets) const
      78             : {
      79           3 :     if (_sitl == nullptr) {
      80           0 :         return false;
      81             :     }
      82             :     // the Compass may rotate the reading before adding COMPASS_OFS, and
      83             :     // the simulated offset is in the body frame; only offer it for an
      84             :     // instance whose field arrives unrotated.  The rotation which
      85             :     // matters is this instance's, not the board's - an external compass
      86             :     // carries its own COMPASS_ORIENT:
      87           3 :     if (!AP::compass().instance_is_unrotated(mag_idx)) {
      88           0 :         return false;
      89             :     }
      90             : 
      91             :     // mag_idx is a priority index; the simulated sensors are indexed
      92             :     // in detection order, and COMPASS_PRIO*_ID can reorder one
      93             :     // against the other.  Go via the device id:
      94           3 :     return _sitl->get_mag_offsets_for_devid(AP::compass().get_dev_id(mag_idx), magOffsets);
      95             : }
      96             : #endif  // AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
      97             : 
      98             : // return the innovations for the specified instance
      99             : // An out of range instance (eg -1) returns data for the primary instance
     100       69084 : bool AP_AHRS_SIM::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const
     101             : {
     102       69084 :     velInnov.zero();
     103       69084 :     posInnov.zero();
     104       69084 :     magInnov.zero();
     105       69084 :     tasInnov = 0.0f;
     106       69084 :     yawInnov = 0.0f;
     107             : 
     108       69084 :     return true;
     109             : }
     110             : 
     111    41686716 : void AP_AHRS_SIM::get_results(AP_AHRS_Backend::Estimates &results)
     112             : {
     113    41686716 :     if (_sitl == nullptr) {
     114        2086 :         _sitl = AP::sitl();
     115        2086 :         if (_sitl == nullptr) {
     116           0 :             return;
     117             :         }
     118             :     }
     119             : 
     120             :     // always initialised once sitl pointer is good
     121    41686716 :     results.initialised = true;
     122             : 
     123             :     // always healthy
     124    41686716 :     results.healthy = true;
     125             : 
     126             :     // not using a specific sensor:
     127    41686716 :     results.primary_gyro = AP::ins().get_first_usable_gyro();
     128    41686716 :     results.primary_accel = AP::ins().get_first_usable_accel();
     129             : 
     130    41686716 :     const struct SITL::sitl_fdm &fdm = _sitl->state;
     131    41686716 :     const AP_InertialSensor &_ins = AP::ins();
     132             : 
     133    41686716 :     results.attitude_valid = true;
     134             : 
     135             :     // populate vehicle body attitude:
     136    41686716 :     results.quaternion = fdm.quaternion;
     137    41686716 :     results.quaternion.rotate(-AP::ahrs().get_trim());
     138             : 
     139             :     // Apply offsets
     140    41686716 :     Quaternion offsets;
     141    41686716 :     offsets.from_euler(Vector3f{_sitl->sim_ahrs_offset.roll, _sitl->sim_ahrs_offset.pitch, _sitl->sim_ahrs_offset.yaw} * radians(1));
     142    41686716 :     results.quaternion *= offsets;
     143             : 
     144             :     // update derived attitude values:
     145    41686716 :     results.quaternion.rotation_matrix(results.dcm_matrix);
     146    41686716 :     results.quaternion.to_euler(results.roll_rad, results.pitch_rad, results.yaw_rad);
     147             : 
     148    41686716 :     results.gyro_estimate = _ins.get_gyro();
     149    41686716 :     results.gyro_drift.zero();
     150             : 
     151             :     /*
     152             :      * acceleration estimates
     153             :      */
     154             :     // SIM exactly estimates accel bias:
     155    41686716 :     results.accel_bias = AP::sitl()->accel_bias[results.primary_accel].get();
     156             : 
     157    41686716 :     const Vector3f &accel = _ins.get_accel();
     158    41686716 :     results.accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel;
     159             : 
     160    41686716 :     results.velocity_NED = Vector3f(fdm.speedN, fdm.speedE, fdm.speedD);
     161    41686716 :     results.velocity_NED_valid = true;
     162             : 
     163             :     // ground velocity estimate in meters/second, in North/East order
     164    41686716 :     results.velocity_NE = results.velocity_NED.xy();
     165             : 
     166             :     // a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
     167             :     // 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.
     168    41686716 :     results.vert_pos_rate_D_valid = true;
     169    41686716 :     results.vert_pos_rate_D = _sitl->state.speedD;
     170             : 
     171             :     /*
     172             :      * position estimates
     173             :      */
     174    41686716 :     results.location_valid = get_location(results.location);
     175             : 
     176             :     // origin-relative functions
     177             :     // results.provides_common_origin = false;
     178             : 
     179             :     // origin-relative position:
     180             :     {
     181    41686716 :         Location orgn;
     182    41686716 :         if (get_origin(orgn)) {
     183    41686716 :             results.position_D = -(fdm.altitude - orgn.alt*0.01f);
     184    41686716 :             results.position_D_valid = true;
     185             : 
     186    41686716 :             if (results.location_valid) {
     187    41686716 :                 results.position_NE = orgn.get_distance_NE_postype(results.location);
     188    41686716 :                 results.position_NE_valid = true;
     189             :             }
     190             :         }
     191             :     }
     192             : 
     193    41686716 :     results.hagl_valid = true;
     194    41686716 :     results.hagl = _sitl->state.altitude - AP::ahrs().get_home().alt*0.01f;
     195             : 
     196             :     /*
     197             :      * air data estimates
     198             :      */
     199    41686716 :     results.wind = _sitl->state.wind_ef;
     200    41686716 :     results.wind_valid = true;
     201             : 
     202             :     /*
     203             :      * Sensor-related information
     204             :      */
     205             :     // true if the estimator will use GPS data in creating its
     206             :     // estimate when the data is good:
     207    41686716 :     results.configured_to_use_gps = true;
     208             :     // true if GPS is configured as the horizontal position source
     209             :     // for this estimator.  Used to decide whether GPS will set
     210             :     // the navigation origin:
     211    41686716 :     results.configured_to_use_gps_for_pos_XY = true;
     212             : 
     213             :     // are we consuming yaw from an external (e.g. vision-based) source?
     214             :     // results.using_extnav_for_yaw = false;
     215             : 
     216             :     // are we consuming yaw from a source which is *not* a compass
     217             :     // (e.g. the GSF)
     218             :     // results.using_noncompass_for_yaw = false;
     219             : 
     220             : #if AP_AHRS_GET_MAG_DATA_ENABLED
     221             :     // estimators can provide their predicted magnetic fields:
     222             :     // ... but SIM does not (and probably should!):
     223             :     // results.mag_field_NED = {};
     224             :     // results.mag_field_NED_valid = false;
     225             :     // results.mag_field_corrections = {};
     226             :     // results.mag_field_corrections_valid = false;
     227             : #endif  // AP_AHRS_GET_MAG_DATA_ENABLED
     228             : 
     229             :     /*
     230             :      * filter status and estimates quality values:
     231             :      */
     232    41686716 :     results.filter_status_valid = get_filter_status(results.filter_status);
     233             : 
     234             :     // provides the innovations normalised between 0 and 1:
     235             :     // velVar = 0;
     236             :     // posVar = 0;
     237             :     // hgtVar = 0;
     238             :     // magVar.zero();
     239             :     // tasVar = 0;
     240    41686716 :     results.variances_valid = true;
     241             : 
     242             :     // terrain_alt_variance = 0;
     243    41686716 :     results.terrain_alt_variance_valid = true;
     244             : 
     245             :     // very loose limits on velocities and no gain scaling:
     246    41686716 :     results.control_ground_speed_limit_ms = 400.0;
     247    41686716 :     results.control_gain_scaler_XY = 1;
     248    41686716 :     results.control_gain_scaler_Z = 1;
     249             : 
     250             :     // control height is never limited:
     251             :     // results.control_height_limit_valid = false;
     252             :     // results.control_height_limit_m = 0;
     253             : 
     254             : #if HAL_NAVEKF3_AVAILABLE
     255    41686716 :     if (_sitl->odom_enable) {
     256             :         // use SITL states to write body frame odometry data at 20Hz
     257           0 :         uint32_t timeStamp_ms = AP_HAL::millis();
     258           0 :         if (timeStamp_ms - _last_body_odm_update_ms > 50) {
     259           0 :             const float quality = 100.0f;
     260           0 :             const Vector3f posOffset(0.0f, 0.0f, 0.0f);
     261           0 :             const float delTime = 0.001f * (timeStamp_ms - _last_body_odm_update_ms);
     262           0 :             _last_body_odm_update_ms = timeStamp_ms;
     263           0 :             timeStamp_ms -= (timeStamp_ms - _last_body_odm_update_ms)/2; // correct for first order hold average delay
     264           0 :             Vector3f delAng = _ins.get_gyro();
     265             : 
     266           0 :             delAng *= delTime;
     267             :             // rotate earth velocity into body frame and calculate delta position
     268           0 :             Matrix3f Tbn;
     269           0 :             Tbn.from_euler(radians(fdm.rollDeg),radians(fdm.pitchDeg),radians(fdm.yawDeg));
     270           0 :             const Vector3f earth_vel(fdm.speedN,fdm.speedE,fdm.speedD);
     271           0 :             const Vector3f delPos = Tbn.transposed() * (earth_vel * delTime);
     272             :             // write to EKF
     273           0 :             EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, 0, posOffset);
     274             :         }
     275             :     }
     276             : #endif // HAL_NAVEKF3_AVAILABLE
     277             : }
     278             : 
     279             : #endif // AP_AHRS_SIM_ENABLED