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

AP_HAL_SITL: move SITL Compass to standard sensor backend model

Browse Source
This commit is contained in:
Miguel Arroyo 2017-06-21 13:39:07 -04:00 committed by Andrew Tridgell
parent ee328c9683
commit 86b9942d51
3 changed files with 0 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -88,7 +88,6 @@ void SITL_State::_sitl_setup(const char *home_str)
// setup some initial values
#ifndef HIL_MODE
_update_airspeed(0);
_update_compass();
_update_gps(0, 0, 0, 0, 0, 0, false);
_update_rangefinder(0);
#endif
@ -167,7 +166,6 @@ void SITL_State::_fdm_input_step(void)
_sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
!_sitl->gps_disable);
_update_airspeed(_sitl->state.airspeed);
_update_compass();
_update_rangefinder(_sitl->state.range);
if (_sitl->adsb_plane_count >= 0 &&
@ -431,20 +429,6 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
current_pin_value = ((_current / 17.0f) / 5.0f) * 1024;
}
// generate a random Vector3f of size 1
Vector3f SITL_State::_rand_vec3f(void)
{
Vector3f v = Vector3f(rand_float(),
rand_float(),
rand_float());
if (v.length() != 0.0f) {
v.normalize();
}
return v;
}
void SITL_State::init(int argc, char * const argv[])
{
pwm_input[0] = pwm_input[1] = pwm_input[3] = 1500;

2
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -83,7 +83,6 @@ private:
void _setup_adc(void);
void set_height_agl(void);
void _update_compass(void);
void _update_rangefinder(float range_value);
void _set_signal_handlers(void) const;
@ -131,7 +130,6 @@ private:
void _simulator_output(bool synthetic_clock_mode);
uint16_t _airspeed_sensor(float airspeed);
uint16_t _ground_sonar();
Vector3f _rand_vec3f(void);
void _fdm_input_step(void);
void wait_clock(uint64_t wait_time_usec);

87
libraries/AP_HAL_SITL/sitl_compass.cpp
View File

@ -1,87 +0,0 @@
/*
SITL handling
This simulates a compass
Andrew Tridgell November 2011
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include "AP_HAL_SITL.h"
#include "AP_HAL_SITL_Namespace.h"
#include "HAL_SITL_Class.h"
#include <AP_Math/AP_Math.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_Declination/AP_Declination.h>
#include <SITL/SITL.h>
extern const AP_HAL::HAL& hal;
using namespace HALSITL;
/*
setup the compass with new input
all inputs are in degrees
*/
void SITL_State::_update_compass(void)
{
static uint32_t last_update;
if (_compass == nullptr) {
// no compass in this sketch
return;
}
// 100Hz
uint32_t now = AP_HAL::millis();
if ((now - last_update) < 10) {
return;
}
last_update = now;
// calculate sensor noise and add to 'truth' field in body frame
// units are milli-Gauss
Vector3f noise = _rand_vec3f() * _sitl->mag_noise;
Vector3f new_mag_data = _sitl->state.bodyMagField + noise;
// add delay
uint32_t best_time_delta_mag = 1000; // initialise large time representing buffer entry closest to current time - delay.
uint8_t best_index_mag = 0; // initialise number representing the index of the entry in buffer closest to delay.
// storing data from sensor to buffer
if (now - last_store_time_mag >= 10) { // store data every 10 ms.
last_store_time_mag = now;
if (store_index_mag > mag_buffer_length-1) { // reset buffer index if index greater than size of buffer
store_index_mag = 0;
}
buffer_mag[store_index_mag].data = new_mag_data; // add data to current index
buffer_mag[store_index_mag].time = last_store_time_mag; // add time to current index
store_index_mag = store_index_mag + 1; // increment index
}
// return delayed measurement
delayed_time_mag = now - _sitl->mag_delay; // get time corresponding to delay
// find data corresponding to delayed time in buffer
for (uint8_t i=0; i<=mag_buffer_length-1; i++) {
// find difference between delayed time and time stamp in buffer
time_delta_mag = abs((int32_t)(delayed_time_mag - buffer_mag[i].time));
// if this difference is smaller than last delta, store this time
if (time_delta_mag < best_time_delta_mag) {
best_index_mag = i;
best_time_delta_mag = time_delta_mag;
}
}
if (best_time_delta_mag < 1000) { // only output stored state if < 1 sec retrieval error
new_mag_data = buffer_mag[best_index_mag].data;
}
new_mag_data -= _sitl->mag_ofs.get();
_compass->setHIL(0, new_mag_data, AP_HAL::micros());
_compass->setHIL(1, new_mag_data, AP_HAL::micros());
}
#endif