Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: make quaternion state available outside SITL

Browse Source
This commit is contained in:
Andrew Tridgell 2017-04-15 21:20:28 +10:00
parent e440d22003
commit 4de0daa7a1
4 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/SITL/SIM_Aircraft.cpp
View File

@ -351,6 +351,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm)
fdm.rollDeg = degrees(r);
fdm.pitchDeg = degrees(p);
fdm.yawDeg = degrees(y);
fdm.quaternion.from_rotation_matrix(dcm);
fdm.airspeed = airspeed_pitot;
fdm.battery_voltage = battery_voltage;
fdm.battery_current = battery_current;

4
libraries/SITL/SIM_Gimbal.cpp
View File

@ -39,7 +39,7 @@ Gimbal::Gimbal(const struct sitl_fdm &_fdm) :
mav_socket(false)
{
memset(&mavlink, 0, sizeof(mavlink));
dcm.from_euler(radians(fdm.rollDeg), radians(fdm.pitchDeg), radians(fdm.yawDeg));
fdm.quaternion.rotation_matrix(dcm);
}
@ -55,7 +55,7 @@ void Gimbal::update(void)
last_update_us = now_us;
Matrix3f vehicle_dcm;
vehicle_dcm.from_euler(radians(fdm.rollDeg), radians(fdm.pitchDeg), radians(fdm.yawDeg));
fdm.quaternion.rotation_matrix(vehicle_dcm);
Vector3f vehicle_gyro = Vector3f(radians(fdm.rollRate),
radians(fdm.pitchRate),

6
libraries/SITL/SITL.cpp
View File

@ -139,7 +139,11 @@ void SITL::Log_Write_SIMSTATE(DataFlash_Class *DataFlash)
yaw : (uint16_t)(wrap_360_cd(yaw*100)),
alt : (float)state.altitude,
lat : (int32_t)(state.latitude*1.0e7),
lng : (int32_t)(state.longitude*1.0e7)
lng : (int32_t)(state.longitude*1.0e7),
q1 : state.quaternion.q1,
q2 : state.quaternion.q2,
q3 : state.quaternion.q3,
q4 : state.quaternion.q4,
};
DataFlash->WriteBlock(&pkt, sizeof(pkt));
}

1
libraries/SITL/SITL.h
View File

@ -16,6 +16,7 @@ struct sitl_fdm {
double xAccel, yAccel, zAccel; // m/s/s in body frame
double rollRate, pitchRate, yawRate; // degrees/s/s in body frame
double rollDeg, pitchDeg, yawDeg; // euler angles, degrees
Quaternion quaternion;
double airspeed; // m/s
double battery_voltage; // Volts
double battery_current; // Amps