ardupilot/libraries/AP_HAL_AVR_SITL/sitl_compass.cpp

48 lines
1021 B
C++

/*
SITL handling
This simulates a compass
Andrew Tridgell November 2011
*/
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
#include <AP_HAL_AVR.h>
#include <AP_HAL_AVR_SITL.h>
#include "AP_HAL_AVR_SITL_Namespace.h"
#include "HAL_AVR_SITL_Class.h"
#include <AP_Math.h>
#include "../AP_Compass/AP_Compass.h"
#include "../AP_Declination/AP_Declination.h"
#include "../SITL/SITL.h"
using namespace AVR_SITL;
/*
setup the compass with new input
all inputs are in degrees
*/
void SITL_State::_update_compass(float rollDeg, float pitchDeg, float yawDeg)
{
if (_compass == NULL) {
// no compass in this sketch
return;
}
yawDeg += _sitl->mag_error;
if (yawDeg > 180.0f) {
yawDeg -= 360.0f;
}
if (yawDeg < -180.0f) {
yawDeg += 360.0f;
}
_compass->setHIL(radians(rollDeg), radians(pitchDeg), radians(yawDeg));
Vector3f noise = _rand_vec3f() * _sitl->mag_noise;
Vector3f motor = _sitl->mag_mot.get() * _current;
_compass->setHIL(_compass->getHIL() + noise+motor);
}
#endif