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

Plane: reset HIL attitude if off by more than 5 degrees

Browse Source
This commit is contained in:
Andrew Tridgell 2013-11-23 12:37:05 +11:00
parent 8bfe59cea5
commit 1e36ebc31b
1 changed files with 7 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/GCS_Mavlink.pde
View File

@ -2065,6 +2065,13 @@ mission_failed:
barometer.setHIL(packet.alt*0.001f);
compass.setHIL(packet.roll, packet.pitch, packet.yaw);
airspeed.disable();
// cope with DCM getting badly off due to HIL lag
if (fabsf(packet.roll - ahrs.roll) > ToRad(5) ||
fabsf(packet.pitch - ahrs.pitch) > ToRad(5) ||
fabsf(packet.yaw - ahrs.yaw) > ToRad(5)) {
ahrs.reset_attitude(packet.roll, packet.pitch, packet.yaw);
}
break;
}
#endif // HIL_MODE