You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
46 lines
971 B
46 lines
971 B
/* |
|
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; |
|
_compass->set_field(_compass->get_field() + noise); |
|
} |
|
|
|
#endif
|
|
|