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

Plane: added HIL_ERR_LIMIT

Browse Source 
this allows DCM to recover if HIL attitude drifts too far from true attitude
This commit is contained in:
Andrew Tridgell 2013-11-23 18:27:20 +11:00
parent 17c9cedac1
commit 490a061c6f
3 changed files with 18 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/GCS_Mavlink.pde
View File

@ -2067,9 +2067,10 @@ mission_failed:
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)) {
if (g.hil_err_limit > 0 &&
(fabsf(packet.roll - ahrs.roll) > ToRad(g.hil_err_limit) ||
fabsf(packet.pitch - ahrs.pitch) > ToRad(g.hil_err_limit) ||
wrap_PI(fabsf(packet.yaw - ahrs.yaw)) > ToRad(g.hil_err_limit))) {
ahrs.reset_attitude(packet.roll, packet.pitch, packet.yaw);
}
break;

5
ArduPlane/Parameters.h
View File

@ -97,6 +97,7 @@ public:
k_param_ground_steer_alt,
k_param_ground_steer_dps,
k_param_rally_limit_km,
k_param_hil_err_limit,
// 110: Telemetry control
//
@ -273,6 +274,10 @@ public:
AP_Int8 serial3_baud;
AP_Int8 telem_delay;
#if HIL_MODE != HIL_MODE_DISABLED
AP_Float hil_err_limit;
#endif
// Feed-forward gains
//
AP_Float kff_rudder_mix;

9
ArduPlane/Parameters.pde
View File

@ -739,6 +739,15 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
GSCALAR(hil_servos, "HIL_SERVOS", 0),
// @Param: HIL_ERR_LIMIT
// @DisplayName: Limit of error in HIL attitude before reset
// @Description: This controls the maximum error in degrees on any axis before HIL will reset the DCM attitude to match the HIL_STATE attitude. This limit will prevent poor timing on HIL from causing a major attitude error. If the value is zero then no limit applies.
// @Units: degrees
// @Range: 0 90
// @Increment: 0.1
// @User: Advanced
GSCALAR(hil_err_limit, "HIL_ERR_LIMIT", 5),
#endif
// barometer ground calibration. The GND_ prefix is chosen for