HAL_SITL: use quaterion instead of eulers

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_ins(0);
-        _update_compass(0, 0, 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,
+        _update_ins(_sitl->state.airspeed);
-                    _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_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) {

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
+    void _update_ins(float airspeed);
-                     double rollRate, 	double pitchRate,double yawRate,	// Local to plane
-                     double xAccel, 	double yAccel, 	double zAccel,		// Local to plane
-                     float airspeed,	float altitude);
     void _check_rc_input(void);
     void _fdm_input_local(void);
     void _output_to_flightgear(void);

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();

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),
+        _sitl->state.quaternion.rotation_matrix(rotmat);
-                          radians(_sitl->state.pitchDeg),
-                          radians(_sitl->state.yawDeg));
 
         // rotate the antenna offset into the earth frame
         Vector3f posRelOffsetEF = rotmat * posRelOffsetBF;

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),
+        _sitl->state.quaternion.rotation_matrix(rotmat);
-                          radians(_sitl->state.pitchDeg),
+
-                          radians(_sitl->state.yawDeg));
         // 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
+void SITL_State::_update_ins(float airspeed)
-                             double rollRate, 	double pitchRate,double yawRate,	// Local to plane
-                             double xAccel, 	double yAccel, 	double zAccel,		// Local to plane
-                             float airspeed,	float altitude)
 {
     if (_ins == nullptr) {
         // no inertial sensor in this sketch