CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_AHRS/AP_AHRS_SIM.cpp
Views: 1798
1
#include "AP_AHRS_SIM.h"

2


3
#if AP_AHRS_SIM_ENABLED

4


5
#include "AP_AHRS.h"

6


7
bool AP_AHRS_SIM::get_location(Location &loc) const

8
{

9
    if (_sitl == nullptr) {

10
        return false;

11
    }

12


13
    const struct SITL::sitl_fdm &fdm = _sitl->state;

14
    loc = {};

15
    loc.lat = fdm.latitude * 1e7;

16
    loc.lng = fdm.longitude * 1e7;

17
    loc.alt = fdm.altitude*100;

18


19
    return true;

20
}

21


22
bool AP_AHRS_SIM::get_velocity_NED(Vector3f &vec) const

23
{

24
    if (_sitl == nullptr) {

25
        return false;

26
    }

27


28
    const struct SITL::sitl_fdm &fdm = _sitl->state;

29
    vec = Vector3f(fdm.speedN, fdm.speedE, fdm.speedD);

30


31
    return true;

32
}

33


34
bool AP_AHRS_SIM::wind_estimate(Vector3f &wind) const

35
{

36
    if (_sitl == nullptr) {

37
        return false;

38
    }

39


40
    wind = _sitl->state.wind_ef;

41
    return true;

42
}

43


44
bool AP_AHRS_SIM::airspeed_EAS(float &airspeed_ret) const

45
{

46
    if (_sitl == nullptr) {

47
        return false;

48
    }

49


50
    airspeed_ret = _sitl->state.airspeed;

51


52
    return true;

53
}

54


55
bool AP_AHRS_SIM::airspeed_EAS(uint8_t index, float &airspeed_ret) const

56
{

57
    return airspeed_EAS(airspeed_ret);

58
}

59


60
bool AP_AHRS_SIM::get_quaternion(Quaternion &quat) const

61
{

62
    if (_sitl == nullptr) {

63
        return false;

64
    }

65


66
    const struct SITL::sitl_fdm &fdm = _sitl->state;

67
    quat = fdm.quaternion;

68


69
    return true;

70
}

71


72
Vector2f AP_AHRS_SIM::groundspeed_vector(void)

73
{

74
    if (_sitl == nullptr) {

75
        return Vector2f{};

76
    }

77


78
    const struct SITL::sitl_fdm &fdm = _sitl->state;

79


80
    return Vector2f(fdm.speedN, fdm.speedE);

81
}

82


83
bool AP_AHRS_SIM::get_vert_pos_rate_D(float &velocity) const

84
{

85
    if (_sitl == nullptr) {

86
        return false;

87
    }

88


89
    velocity = _sitl->state.speedD;

90


91
    return true;

92
}

93


94
bool AP_AHRS_SIM::get_hagl(float &height) const

95
{

96
    if (_sitl == nullptr) {

97
        return false;

98
    }

99


100
    height = _sitl->state.altitude - AP::ahrs().get_home().alt*0.01f;

101


102
    return true;

103
}

104


105
bool AP_AHRS_SIM::get_relative_position_NED_origin(Vector3f &vec) const

106
{

107
    if (_sitl == nullptr) {

108
        return false;

109
    }

110


111
    Location loc, orgn;

112
    if (!get_location(loc) ||

113
        !get_origin(orgn)) {

114
        return false;

115
    }

116


117
    const Vector2f diff2d = orgn.get_distance_NE(loc);

118
    const struct SITL::sitl_fdm &fdm = _sitl->state;

119
    vec = Vector3f(diff2d.x, diff2d.y,

120
                   -(fdm.altitude - orgn.alt*0.01f));

121


122
    return true;

123
}

124


125
bool AP_AHRS_SIM::get_relative_position_NE_origin(Vector2f &posNE) const

126
{

127
    Location loc, orgn;

128
    if (!get_location(loc) ||

129
        !get_origin(orgn)) {

130
        return false;

131
    }

132
    posNE = orgn.get_distance_NE(loc);

133


134
    return true;

135
}

136


137
bool AP_AHRS_SIM::get_relative_position_D_origin(float &posD) const

138
{

139
    if (_sitl == nullptr) {

140
        return false;

141
    }

142
    const struct SITL::sitl_fdm &fdm = _sitl->state;

143
    Location orgn;

144
    if (!get_origin(orgn)) {

145
        return false;

146
    }

147
    posD = -(fdm.altitude - orgn.alt*0.01f);

148


149
    return true;

150
}

151


152
bool AP_AHRS_SIM::get_filter_status(nav_filter_status &status) const

153
{

154
    memset(&status, 0, sizeof(status));

155
    status.flags.attitude = true;

156
    status.flags.horiz_vel = true;

157
    status.flags.vert_vel = true;

158
    status.flags.horiz_pos_rel = true;

159
    status.flags.horiz_pos_abs = true;

160
    status.flags.vert_pos = true;

161
    status.flags.pred_horiz_pos_rel = true;

162
    status.flags.pred_horiz_pos_abs = true;

163
    status.flags.using_gps = true;

164


165
    return true;

166
}

167


168
void AP_AHRS_SIM::get_control_limits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const

169
{

170
    // same as EKF2 for no optical flow

171
    ekfGndSpdLimit = 400.0f;

172
    ekfNavVelGainScaler = 1.0f;

173
}

174


175
void AP_AHRS_SIM::send_ekf_status_report(GCS_MAVLINK &link) const

176
{

177
#if HAL_GCS_ENABLED

178
    // send status report with everything looking good

179
    const uint16_t flags =

180
        EKF_ATTITUDE | /* Set if EKF's attitude estimate is good. | */

181
        EKF_VELOCITY_HORIZ | /* Set if EKF's horizontal velocity estimate is good. | */

182
        EKF_VELOCITY_VERT | /* Set if EKF's vertical velocity estimate is good. | */

183
        EKF_POS_HORIZ_REL | /* Set if EKF's horizontal position (relative) estimate is good. | */

184
        EKF_POS_HORIZ_ABS | /* Set if EKF's horizontal position (absolute) estimate is good. | */

185
        EKF_POS_VERT_ABS | /* Set if EKF's vertical position (absolute) estimate is good. | */

186
        EKF_POS_VERT_AGL | /* Set if EKF's vertical position (above ground) estimate is good. | */

187
        //EKF_CONST_POS_MODE | /* EKF is in constant position mode and does not know it's absolute or relative position. | */

188
        EKF_PRED_POS_HORIZ_REL | /* Set if EKF's predicted horizontal position (relative) estimate is good. | */

189
        EKF_PRED_POS_HORIZ_ABS; /* Set if EKF's predicted horizontal position (absolute) estimate is good. | */

190
    mavlink_msg_ekf_status_report_send(link.get_chan(), flags, 0, 0, 0, 0, 0, 0);

191
#endif // HAL_GCS_ENABLED

192
}

193


194
bool AP_AHRS_SIM::get_origin(Location &ret) const

195
{

196
    if (_sitl == nullptr) {

197
        return false;

198
    }

199


200
    ret = _sitl->state.home;

201


202
    return true;

203
}

204


205
// return the innovations for the specified instance

206
// An out of range instance (eg -1) returns data for the primary instance

207
bool AP_AHRS_SIM::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const

208
{

209
    velInnov.zero();

210
    posInnov.zero();

211
    magInnov.zero();

212
    tasInnov = 0.0f;

213
    yawInnov = 0.0f;

214


215
    return true;

216
}

217


218
bool AP_AHRS_SIM::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const

219
{

220
    velVar = 0;

221
    posVar = 0;

222
    hgtVar = 0;

223
    magVar.zero();

224
    tasVar = 0;

225


226
    return true;

227
}

228


229
void AP_AHRS_SIM::get_results(AP_AHRS_Backend::Estimates &results)

230
{

231
    if (_sitl == nullptr) {

232
        _sitl = AP::sitl();

233
        if (_sitl == nullptr) {

234
            return;

235
        }

236
    }

237


238
    const struct SITL::sitl_fdm &fdm = _sitl->state;

239
    const AP_InertialSensor &_ins = AP::ins();

240


241
    fdm.quaternion.rotation_matrix(results.dcm_matrix);

242
    results.dcm_matrix = results.dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body();

243
    results.dcm_matrix.to_euler(&results.roll_rad, &results.pitch_rad, &results.yaw_rad);

244


245
    results.gyro_estimate = _ins.get_gyro();

246
    results.gyro_drift.zero();

247


248
    const Vector3f &accel = _ins.get_accel();

249
    results.accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel;

250


251
    results.location_valid = get_location(results.location);

252


253
#if HAL_NAVEKF3_AVAILABLE

254
    if (_sitl->odom_enable) {

255
        // use SITL states to write body frame odometry data at 20Hz

256
        uint32_t timeStamp_ms = AP_HAL::millis();

257
        if (timeStamp_ms - _last_body_odm_update_ms > 50) {

258
            const float quality = 100.0f;

259
            const Vector3f posOffset(0.0f, 0.0f, 0.0f);

260
            const float delTime = 0.001f * (timeStamp_ms - _last_body_odm_update_ms);

261
            _last_body_odm_update_ms = timeStamp_ms;

262
            timeStamp_ms -= (timeStamp_ms - _last_body_odm_update_ms)/2; // correct for first order hold average delay

263
            Vector3f delAng = _ins.get_gyro();

264


265
            delAng *= delTime;

266
            // rotate earth velocity into body frame and calculate delta position

267
            Matrix3f Tbn;

268
            Tbn.from_euler(radians(fdm.rollDeg),radians(fdm.pitchDeg),radians(fdm.yawDeg));

269
            const Vector3f earth_vel(fdm.speedN,fdm.speedE,fdm.speedD);

270
            const Vector3f delPos = Tbn.transposed() * (earth_vel * delTime);

271
            // write to EKF

272
            EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, 0, posOffset);

273
        }

274
    }

275
#endif // HAL_NAVEKF3_AVAILABLE

276
}

277


278
#endif // AP_AHRS_SIM_ENABLED

279


280