aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: use new HIL compass API

Browse Source
This commit is contained in:
Andrew Tridgell 2013-05-02 15:00:07 +10:00
parent 5594e4d9bc
commit 172d9724df
1 changed files with 1 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduCopter/GCS_Mavlink.pde
View File

@ -1925,24 +1925,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
y *= 95446.0;
barometer.setHIL(Temp, y);
Vector3f Bearth, m;
Matrix3f R;
// Bearth is the magnetic field in Canberra. We need to adjust
// it for inclination and declination
Bearth(400, 0, 0);
R.from_euler(0, 0, 0);
Bearth = R * Bearth;
// create a rotation matrix for the given attitude
R.from_euler(packet.roll, packet.pitch, packet.yaw);
// convert the earth frame magnetic vector to body frame, and
// apply the offsets
m = R.transposed() * Bearth - Vector3f(0, 0, 0);
compass.setHIL(m.x,m.y,m.z);
compass.setHIL(packet.roll, packet.pitch, packet.yaw);
#if HIL_MODE == HIL_MODE_ATTITUDE
// set AHRS hil sensor