Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-03 14:38:44 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: use quaterion instead of eulers

Browse Source
This commit is contained in:
Andrew Tridgell 2017-04-15 21:20:51 +10:00
parent 4de0daa7a1
commit 961da9deb8
5 changed files with 11 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -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) {

7
libraries/AP_HAL_SITL/SITL_State.h
View File

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

9
libraries/AP_HAL_SITL/sitl_compass.cpp
View File

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

4
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

@ -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;

34
libraries/AP_HAL_SITL/sitl_ins.cpp
View File

@ -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