Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_SITL: rename sitl_ins & _update_ins to sitl_airspeed & _update_airspeed

This commit is contained in:
Pierre Kancir 2017-05-11 15:38:32 +02:00 committed by Andrew Tridgell
parent b176ba53d7
commit 0afc907b0a
3 changed files with 13 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -87,7 +87,7 @@ void SITL_State::_sitl_setup(const char *home_str)
if (_sitl != nullptr) { if (_sitl != nullptr) {
// setup some initial values // setup some initial values
#ifndef HIL_MODE #ifndef HIL_MODE
_update_ins(0); _update_airspeed(0);
_update_compass(); _update_compass();
_update_gps(0, 0, 0, 0, 0, 0, false); _update_gps(0, 0, 0, 0, 0, 0, false);
_update_rangefinder(0); _update_rangefinder(0);
@ -166,7 +166,7 @@ void SITL_State::_fdm_input_step(void)
_sitl->state.altitude, _sitl->state.altitude,
_sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD, _sitl->state.speedN, _sitl->state.speedE, _sitl->state.speedD,
!_sitl->gps_disable); !_sitl->gps_disable);
_update_ins(_sitl->state.airspeed); _update_airspeed(_sitl->state.airspeed);
_update_compass(); _update_compass();
_update_rangefinder(_sitl->state.range); _update_rangefinder(_sitl->state.range);

2
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -122,7 +122,7 @@ private:
void _update_gps(double latitude, double longitude, float altitude, void _update_gps(double latitude, double longitude, float altitude,
double speedN, double speedE, double speedD, bool have_lock); double speedN, double speedE, double speedD, bool have_lock);
void _update_ins(float airspeed); void _update_airspeed(float airspeed);
void _update_gps_instance(SITL::SITL::GPSType gps_type, const struct gps_data *d, uint8_t instance); void _update_gps_instance(SITL::SITL::GPSType gps_type, const struct gps_data *d, uint8_t instance);
void _check_rc_input(void); void _check_rc_input(void);
void _fdm_input_local(void); void _fdm_input_local(void);

28
libraries/AP_HAL_SITL/sitl_ins.cpp → libraries/AP_HAL_SITL/sitl_airspeed.cpp
View File

@ -1,7 +1,7 @@
/* /*
SITL handling SITL handling
This emulates the ADS7844 ADC This simulates an analog airspeed sensor
Andrew Tridgell November 2011 Andrew Tridgell November 2011
*/ */
@ -31,16 +31,22 @@ using namespace HALSITL;
/* /*
convert airspeed in m/s to an airspeed sensor value convert airspeed in m/s to an airspeed sensor value
*/ */
uint16_t SITL_State::_airspeed_sensor(float airspeed) void SITL_State::_update_airspeed(float airspeed)
{ {
const float airspeed_ratio = 1.9936f; const float airspeed_ratio = 1.9936f;
const float airspeed_offset = 2013; const float airspeed_offset = 2013;
float airspeed_pressure, airspeed_raw; float airspeed_pressure, airspeed_raw;
// Check sensor failure
airspeed = is_zero(_sitl->arspd_fail) ? airspeed : _sitl->arspd_fail;
// Add noise
airspeed = airspeed + (_sitl->arspd_noise * rand_float());
airspeed_pressure = (airspeed*airspeed) / airspeed_ratio; airspeed_pressure = (airspeed*airspeed) / airspeed_ratio;
airspeed_raw = airspeed_pressure + airspeed_offset; airspeed_raw = airspeed_pressure + airspeed_offset;
if (airspeed_raw/4 > 0xFFFF) { if (airspeed_raw/4 > 0xFFFF) {
return 0xFFFF; airspeed_pin_value = 0xFFFF;
return;
} }
// add delay // add delay
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
@ -75,21 +81,7 @@ uint16_t SITL_State::_airspeed_sensor(float airspeed)
airspeed_raw = buffer_wind[best_index_wind].data; airspeed_raw = buffer_wind[best_index_wind].data;
} }
return airspeed_raw/4; airspeed_pin_value = airspeed_raw/4;
}
/*
setup the INS input channels with new input
*/
void SITL_State::_update_ins(float airspeed)
{
if (_ins == nullptr) {
// no inertial sensor in this sketch
return;
}
const float airspeed_simulated = is_zero(_sitl->arspd_fail) ? airspeed : _sitl->arspd_fail;
airspeed_pin_value = _airspeed_sensor(airspeed_simulated + (_sitl->arspd_noise * rand_float()));
} }
#endif #endif