From 961da9deb8fd59bc15d3f07de85a761fe92f3351 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 15 Apr 2017 21:20:51 +1000 Subject: [PATCH] HAL_SITL: use quaterion instead of eulers --- libraries/AP_HAL_SITL/SITL_State.cpp | 11 +++------ libraries/AP_HAL_SITL/SITL_State.h | 7 ++---- libraries/AP_HAL_SITL/sitl_compass.cpp | 9 +------ libraries/AP_HAL_SITL/sitl_gps.cpp | 4 +-- libraries/AP_HAL_SITL/sitl_ins.cpp | 34 +++----------------------- 5 files changed, 11 insertions(+), 54 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 6bfe93c6c6..3303827185 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -88,8 +88,8 @@ void SITL_State::_sitl_setup(const char *home_str) // setup some initial values #ifndef HIL_MODE _update_barometer(100); - _update_ins(0, 0, 0, 0, 0, 0, 0, 0, -9.8, 0, 100); - _update_compass(0, 0, 0); + _update_ins(0); + _update_compass(); _update_gps(0, 0, 0, 0, 0, 0, false); #endif if (enable_gimbal) { @@ -167,12 +167,9 @@ void SITL_State::_fdm_input_step(void) _sitl->state.altitude, _sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD, !_sitl->gps_disable); - _update_ins(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg, - _sitl->state.rollRate, _sitl->state.pitchRate, _sitl->state.yawRate, - _sitl->state.xAccel, _sitl->state.yAccel, _sitl->state.zAccel, - _sitl->state.airspeed, _sitl->state.altitude); + _update_ins(_sitl->state.airspeed); _update_barometer(_sitl->state.altitude); - _update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg); + _update_compass(); if (_sitl->adsb_plane_count >= 0 && adsb == nullptr) { diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 75d313d2a6..adf5e4dd67 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -84,7 +84,7 @@ private: void set_height_agl(void); void _update_barometer(float height); - void _update_compass(float rollDeg, float pitchDeg, float yawDeg); + void _update_compass(void); void _set_signal_handlers(void) const; @@ -123,10 +123,7 @@ private: void _update_gps(double latitude, double longitude, float altitude, double speedN, double speedE, double speedD, bool have_lock); - void _update_ins(float roll, float pitch, float yaw, // Relative to earth - double rollRate, double pitchRate,double yawRate, // Local to plane - double xAccel, double yAccel, double zAccel, // Local to plane - float airspeed, float altitude); + void _update_ins(float airspeed); void _check_rc_input(void); void _fdm_input_local(void); void _output_to_flightgear(void); diff --git a/libraries/AP_HAL_SITL/sitl_compass.cpp b/libraries/AP_HAL_SITL/sitl_compass.cpp index 3b031ed780..b10d76643f 100644 --- a/libraries/AP_HAL_SITL/sitl_compass.cpp +++ b/libraries/AP_HAL_SITL/sitl_compass.cpp @@ -26,7 +26,7 @@ using namespace HALSITL; setup the compass with new input all inputs are in degrees */ -void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg) +void SITL_State::_update_compass(void) { static uint32_t last_update; @@ -34,13 +34,6 @@ void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg) // no compass in this sketch return; } - yawDeg += _sitl->mag_error; - if (yawDeg > 180.0f) { - yawDeg -= 360.0f; - } - if (yawDeg < -180.0f) { - yawDeg += 360.0f; - } // 100Hz uint32_t now = AP_HAL::millis(); diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 75600dfd0d..352addf462 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -1041,9 +1041,7 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, if (!posRelOffsetBF.is_zero()) { // get a rotation matrix following DCM conventions (body to earth) Matrix3f rotmat; - rotmat.from_euler(radians(_sitl->state.rollDeg), - radians(_sitl->state.pitchDeg), - radians(_sitl->state.yawDeg)); + _sitl->state.quaternion.rotation_matrix(rotmat); // rotate the antenna offset into the earth frame Vector3f posRelOffsetEF = rotmat * posRelOffsetBF; diff --git a/libraries/AP_HAL_SITL/sitl_ins.cpp b/libraries/AP_HAL_SITL/sitl_ins.cpp index 2a6383ef4a..aaf1bb402f 100644 --- a/libraries/AP_HAL_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_SITL/sitl_ins.cpp @@ -93,9 +93,8 @@ uint16_t SITL_State::_ground_sonar(void) if (!relPosSensorBF.is_zero()) { // get a rotation matrix following DCM conventions (body to earth) Matrix3f rotmat; - rotmat.from_euler(radians(_sitl->state.rollDeg), - radians(_sitl->state.pitchDeg), - radians(_sitl->state.yawDeg)); + _sitl->state.quaternion.rotation_matrix(rotmat); + // rotate the offset into earth frame Vector3f relPosSensorEF = rotmat * relPosSensorBF; // correct the altitude at the sensor @@ -124,35 +123,8 @@ uint16_t SITL_State::_ground_sonar(void) /* setup the INS input channels with new input - - Note that this uses roll, pitch and yaw only as inputs. The - simulator rollrates are instantaneous, whereas we need to use - average rates to cope with slow update rates. - - inputs are in degrees - - phi - roll - theta - pitch - psi - true heading - alpha - angle of attack - beta - side slip - phidot - roll rate - thetadot - pitch rate - psidot - yaw rate - v_north - north velocity in local/body frame - v_east - east velocity in local/body frame - v_down - down velocity in local/body frame - A_X_pilot - X accel in body frame - A_Y_pilot - Y accel in body frame - A_Z_pilot - Z accel in body frame - - Note: doubles on high prec. stuff are preserved until the last moment - */ -void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative to earth - double rollRate, double pitchRate,double yawRate, // Local to plane - double xAccel, double yAccel, double zAccel, // Local to plane - float airspeed, float altitude) +void SITL_State::_update_ins(float airspeed) { if (_ins == nullptr) { // no inertial sensor in this sketch