Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Compass: add motor compensation to HIL library

Browse Source
This commit is contained in:
Randy Mackay 2013-03-01 23:58:55 +09:00 committed by rmackay9
parent 6b2b69535f
commit 8093c98cd3
2 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -18,6 +18,17 @@ extern const AP_HAL::HAL& hal;
bool AP_Compass_HIL::read()
{
// get offsets
Vector3f ofs = _offset.get();
// get motor compensation
_motor_offset = _motor_compensation.get() * _throttle_pct;
// return last values provided by setHIL function
mag_x = _hil_mag.x + ofs.x + _motor_offset.x;
mag_y = _hil_mag.y + ofs.y + _motor_offset.y;
mag_z = _hil_mag.z + ofs.z + _motor_offset.z;
// values set by setHIL function
last_update = hal.scheduler->micros(); // record time of update
return true;
@ -27,10 +38,9 @@ bool AP_Compass_HIL::read()
//
void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
{
Vector3f ofs = _offset.get();
mag_x = _mag_x + ofs.x;
mag_y = _mag_y + ofs.y;
mag_z = _mag_z + ofs.z;
_hil_mag.x = _mag_x;
_hil_mag.y = _mag_y;
_hil_mag.z = _mag_z;
healthy = true;
}

2
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -13,6 +13,8 @@ public:
bool read(void);
void accumulate(void);
void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
private:
Vector3f _hil_mag;
};
#endif