mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-26 01:33:56 -04:00
HAL_SITL: moved SITL baro to AP_Baro_SITL
This commit is contained in:
Andrew Tridgell
parent
ee4161fa62
commit
ac82d1a65d
@ -87,7 +87,6 @@ void SITL_State::_sitl_setup(const char *home_str)
|
||||
if (_sitl != nullptr) {
|
||||
// setup some initial values
|
||||
#ifndef HIL_MODE
|
||||
_update_barometer(100);
|
||||
_update_ins(0);
|
||||
_update_compass();
|
||||
_update_gps(0, 0, 0, 0, 0, 0, false);
|
||||
@ -156,7 +155,6 @@ void SITL_State::_fdm_input_step(void)
|
||||
|
||||
if (_update_count == 0 && _sitl != nullptr) {
|
||||
_update_gps(0, 0, 0, 0, 0, 0, false);
|
||||
_update_barometer(0);
|
||||
_scheduler->timer_event();
|
||||
_scheduler->sitl_end_atomic();
|
||||
return;
|
||||
@ -168,7 +166,6 @@ void SITL_State::_fdm_input_step(void)
|
||||
_sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
|
||||
!_sitl->gps_disable);
|
||||
_update_ins(_sitl->state.airspeed);
|
||||
_update_barometer(_sitl->state.altitude);
|
||||
_update_compass();
|
||||
|
||||
if (_sitl->adsb_plane_count >= 0 &&
|
||||
@ -433,18 +430,12 @@ void SITL_State::_simulator_servos(SITL::Aircraft::sitl_input &input)
|
||||
}
|
||||
|
||||
|
||||
// generate a random float between -1 and 1
|
||||
float SITL_State::_rand_float(void)
|
||||
{
|
||||
return ((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6;
|
||||
}
|
||||
|
||||
// generate a random Vector3f of size 1
|
||||
Vector3f SITL_State::_rand_vec3f(void)
|
||||
{
|
||||
Vector3f v = Vector3f(_rand_float(),
|
||||
_rand_float(),
|
||||
_rand_float());
|
||||
Vector3f v = Vector3f(rand_float(),
|
||||
rand_float(),
|
||||
rand_float());
|
||||
if (v.length() != 0.0f) {
|
||||
v.normalize();
|
||||
}
|
||||
|
||||
@ -83,7 +83,6 @@ private:
|
||||
void _setup_adc(void);
|
||||
|
||||
void set_height_agl(void);
|
||||
void _update_barometer(float height);
|
||||
void _update_compass(void);
|
||||
|
||||
void _set_signal_handlers(void) const;
|
||||
@ -132,7 +131,6 @@ private:
|
||||
void _simulator_output(bool synthetic_clock_mode);
|
||||
uint16_t _airspeed_sensor(float airspeed);
|
||||
uint16_t _ground_sonar();
|
||||
float _rand_float(void);
|
||||
Vector3f _rand_vec3f(void);
|
||||
void _fdm_input_step(void);
|
||||
|
||||
@ -173,7 +171,6 @@ private:
|
||||
// delay buffer variables
|
||||
static const uint8_t mag_buffer_length = 250;
|
||||
static const uint8_t wind_buffer_length = 50;
|
||||
static const uint8_t baro_buffer_length = 50;
|
||||
|
||||
// magnetometer delay buffer variables
|
||||
struct readings_mag {
|
||||
@ -197,17 +194,6 @@ private:
|
||||
uint32_t time_delta_wind;
|
||||
uint32_t delayed_time_wind;
|
||||
|
||||
// barometer delay buffer variables
|
||||
struct readings_baro {
|
||||
uint32_t time;
|
||||
float data;
|
||||
};
|
||||
uint8_t store_index_baro;
|
||||
uint32_t last_store_time_baro;
|
||||
VectorN<readings_baro,baro_buffer_length> buffer_baro;
|
||||
uint32_t time_delta_baro;
|
||||
uint32_t delayed_time_baro;
|
||||
|
||||
// internal SITL model
|
||||
SITL::Aircraft *sitl_model;
|
||||
|
||||
|
||||
@ -1,95 +0,0 @@
|
||||
/*
|
||||
SITL handling
|
||||
|
||||
This simulates a barometer
|
||||
|
||||
Andrew Tridgell November 2011
|
||||
*/
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
|
||||
#include "AP_HAL_SITL.h"
|
||||
|
||||
using namespace HALSITL;
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <cmath>
|
||||
|
||||
/*
|
||||
setup the barometer with new input
|
||||
altitude is in meters
|
||||
*/
|
||||
void SITL_State::_update_barometer(float altitude)
|
||||
{
|
||||
static uint32_t last_update;
|
||||
|
||||
float sim_alt = altitude;
|
||||
|
||||
if (_barometer == nullptr) {
|
||||
// this sketch doesn't use a barometer
|
||||
return;
|
||||
}
|
||||
|
||||
if (_sitl->baro_disable) {
|
||||
// barometer is disabled
|
||||
return;
|
||||
}
|
||||
|
||||
// 80Hz
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if ((now - last_update) < 12) {
|
||||
return;
|
||||
}
|
||||
last_update = now;
|
||||
|
||||
sim_alt += _sitl->baro_drift * now / 1000;
|
||||
sim_alt += _sitl->baro_noise * _rand_float();
|
||||
|
||||
// add baro glitch
|
||||
sim_alt += _sitl->baro_glitch;
|
||||
|
||||
// add delay
|
||||
uint32_t best_time_delta_baro = 200; // initialise large time representing buffer entry closest to current time - delay.
|
||||
uint8_t best_index_baro = 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_baro >= 10) { // store data every 10 ms.
|
||||
last_store_time_baro = now;
|
||||
if (store_index_baro > baro_buffer_length-1) { // reset buffer index if index greater than size of buffer
|
||||
store_index_baro = 0;
|
||||
}
|
||||
buffer_baro[store_index_baro].data = sim_alt; // add data to current index
|
||||
buffer_baro[store_index_baro].time = last_store_time_baro; // add time_stamp_baro to current index
|
||||
store_index_baro = store_index_baro + 1; // increment index
|
||||
}
|
||||
|
||||
// return delayed measurement
|
||||
delayed_time_baro = now - _sitl->baro_delay; // get time corresponding to delay
|
||||
|
||||
// find data corresponding to delayed time in buffer
|
||||
for (uint8_t i=0; i<=baro_buffer_length-1; i++) {
|
||||
// find difference between delayed time and time stamp in buffer
|
||||
time_delta_baro = abs(
|
||||
(int32_t)(delayed_time_baro - buffer_baro[i].time));
|
||||
// if this difference is smaller than last delta, store this time
|
||||
if (time_delta_baro < best_time_delta_baro) {
|
||||
best_index_baro = i;
|
||||
best_time_delta_baro = time_delta_baro;
|
||||
}
|
||||
}
|
||||
if (best_time_delta_baro < 200) { // only output stored state if < 200 msec retrieval error
|
||||
sim_alt = buffer_baro[best_index_baro].data;
|
||||
}
|
||||
|
||||
_barometer->setHIL(sim_alt);
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -107,13 +107,13 @@ uint16_t SITL_State::_ground_sonar(void)
|
||||
// adjust for apparent altitude with roll
|
||||
altitude /= cosf(radians(_sitl->state.rollDeg)) * cosf(radians(_sitl->state.pitchDeg));
|
||||
|
||||
altitude += _sitl->sonar_noise * _rand_float();
|
||||
altitude += _sitl->sonar_noise * rand_float();
|
||||
|
||||
// Altitude in in m, scaler in meters/volt
|
||||
voltage = altitude / _sitl->sonar_scale;
|
||||
voltage = constrain_float(voltage, 0, 5.0f);
|
||||
|
||||
if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f) {
|
||||
if (_sitl->sonar_glitch >= (rand_float() + 1.0f)/2.0f) {
|
||||
voltage = 5.0f;
|
||||
}
|
||||
}
|
||||
@ -133,7 +133,7 @@ void SITL_State::_update_ins(float airspeed)
|
||||
|
||||
sonar_pin_value = _ground_sonar();
|
||||
float airspeed_simulated = (fabsf(_sitl->arspd_fail) > 1.0e-6f) ? _sitl->arspd_fail : airspeed;
|
||||
airspeed_pin_value = _airspeed_sensor(airspeed_simulated + (_sitl->arspd_noise * _rand_float()));
|
||||
airspeed_pin_value = _airspeed_sensor(airspeed_simulated + (_sitl->arspd_noise * rand_float()));
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
Loading…
Reference in New Issue
Block a user