mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-02 10:23:25 +08:00
Peter BarkerandClaude Opus 5
4ea77b7b2d
AP_Compass_SITL held the compass sensor model -- the noise, the delay buffer, the elliptical correction and the failure injection -- inline. Move it to SITL::CompassSim so it sits alongside the rest of the sensor simulation and beside the transformation it shares with get_mag_offsets(). The consumer owns its own instance, so sampling stays where it was and the 100Hz rate at which the sensor is simulated is unchanged. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
61 lines
1.6 KiB
C++
61 lines
1.6 KiB
C++
#include "AP_Compass_SITL.h"
|
|
|
|
#if AP_COMPASS_SITL_ENABLED
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
AP_Compass_SITL::AP_Compass_SITL(uint8_t _sitl_instance) :
|
|
_sitl(AP::sitl()),
|
|
sitl_instance(_sitl_instance)
|
|
{
|
|
if (sitl_instance > ARRAY_SIZE(_sitl->mag_devid)) {
|
|
return;
|
|
}
|
|
|
|
const uint32_t dev_id = _sitl->mag_devid[sitl_instance];
|
|
if (dev_id == 0) {
|
|
return;
|
|
}
|
|
if (!register_compass(dev_id)) {
|
|
return;
|
|
}
|
|
if (_sitl->mag_save_ids) {
|
|
// save so the compass always comes up configured in SITL
|
|
save_dev_id();
|
|
}
|
|
set_rotation(ROTATION_NONE);
|
|
|
|
// Scroll through the registered compasses, and set the offsets
|
|
if (_compass.get_offsets(instance).is_zero()) {
|
|
_compass.set_offsets(instance, _sitl->mag_ofs[sitl_instance]);
|
|
}
|
|
|
|
// we want to simulate a calibrated compass by default, so set
|
|
// scale to 1
|
|
AP_Param::set_default_by_name("COMPASS_SCALE", 1);
|
|
AP_Param::set_default_by_name("COMPASS_SCALE2", 1);
|
|
AP_Param::set_default_by_name("COMPASS_SCALE3", 1);
|
|
|
|
// make first compass external
|
|
if (sitl_instance == 0) {
|
|
set_external(true);
|
|
}
|
|
|
|
hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &AP_Compass_SITL::_timer, void));
|
|
}
|
|
|
|
|
|
void AP_Compass_SITL::_timer()
|
|
{
|
|
Vector3f f;
|
|
if (!_compass_sim.update(f)) {
|
|
return;
|
|
}
|
|
|
|
accumulate_sample(f, 10);
|
|
}
|
|
|
|
#endif // AP_COMPASS_SITL_ENABLED
|