From 70d37d2cf66d8cd4bff670ee15759fb5f9a4b3c0 Mon Sep 17 00:00:00 2001 From: Ben Nizette Date: Tue, 26 Nov 2013 16:42:43 +1100 Subject: [PATCH] SITL: Wire in basic support for SITL downward-looking sonar sensors --- libraries/AP_HAL_AVR_SITL/SITL_State.cpp | 4 ++-- libraries/AP_HAL_AVR_SITL/SITL_State.h | 3 ++- libraries/AP_HAL_AVR_SITL/sitl_ins.cpp | 30 ++++++++++++++++++++++-- libraries/SITL/SITL.cpp | 1 + libraries/SITL/SITL.h | 2 ++ 5 files changed, 35 insertions(+), 5 deletions(-) diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp index 1405c9df1d..317f751f5d 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.cpp @@ -178,7 +178,7 @@ void SITL_State::_sitl_setup(void) if (_sitl != NULL) { // setup some initial values _update_barometer(_initial_height); - _update_ins(0, 0, 0, 0, 0, 0, 0, 0, -9.8, 0); + _update_ins(0, 0, 0, 0, 0, 0, 0, 0, -9.8, 0, _initial_height); _update_compass(0, 0, 0); _update_gps(0, 0, 0, 0, 0, 0, false); } @@ -294,7 +294,7 @@ void SITL_State::_timer_handler(int signum) _update_ins(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg, _sitl->state.rollRate, _sitl->state.pitchRate, _sitl->state.yawRate, _sitl->state.xAccel, _sitl->state.yAccel, _sitl->state.zAccel, - _sitl->state.airspeed); + _sitl->state.airspeed, _sitl->state.altitude); _update_barometer(_sitl->state.altitude); _update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg); } diff --git a/libraries/AP_HAL_AVR_SITL/SITL_State.h b/libraries/AP_HAL_AVR_SITL/SITL_State.h index fdd76a09a2..440805a9bf 100644 --- a/libraries/AP_HAL_AVR_SITL/SITL_State.h +++ b/libraries/AP_HAL_AVR_SITL/SITL_State.h @@ -89,11 +89,12 @@ private: static void _update_ins(float roll, float pitch, float yaw, // Relative to earth double rollRate, double pitchRate,double yawRate, // Local to plane double xAccel, double yAccel, double zAccel, // Local to plane - float airspeed); + float airspeed, float altitude); static void _fdm_input(void); static void _simulator_output(void); static void _apply_servo_filter(float deltat); static uint16_t _airspeed_sensor(float airspeed); + static uint16_t _ground_sonar(float altitude); static float _gyro_drift(void); static float _rand_float(void); static Vector3f _rand_vec3f(void); diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp index 5d4b09eb58..1e3486badb 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp @@ -17,12 +17,14 @@ #include #include "../AP_Compass/AP_Compass.h" #include "../AP_Declination/AP_Declination.h" +#include "../AP_RangeFinder/AP_RangeFinder.h" #include "../SITL/SITL.h" #include "Scheduler.h" #include #include "../AP_ADC/AP_ADC.h" #include + using namespace AVR_SITL; /* @@ -54,6 +56,25 @@ float SITL_State::_gyro_drift(void) } +uint16_t SITL_State::_ground_sonar(float altitude) +{ + static float home_alt = -1; + // TODO Find the current sonar object and load these params from it + // rather than assuming XL type + float scaler = AP_RANGEFINDER_MAXSONARXL_SCALER; + + if (home_alt == -1) + home_alt = altitude; + + altitude = altitude - home_alt; + altitude = constrain_float(altitude, + AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE / 100.0f, + AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE / 100.0f); + + // Altitude in in m, scaler relative to cm + return (uint16_t)(altitude * 100.0f / scaler); +} + /* setup the INS input channels with new input @@ -84,7 +105,7 @@ float SITL_State::_gyro_drift(void) void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative to earth double rollRate, double pitchRate,double yawRate, // Local to plane double xAccel, double yAccel, double zAccel, // Local to plane - float airspeed) + float airspeed, float altitude) { double p, q, r; @@ -128,7 +149,12 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative _ins->set_gyro(Vector3f(p, q, r) + _ins->get_gyro_offsets()); _ins->set_accel(Vector3f(xAccel, yAccel, zAccel) + _ins->get_accel_offsets()); - airspeed_pin_value = _airspeed_sensor(airspeed + (_sitl->aspd_noise * _rand_float())); + // Airspeed and Sonar share the same analog pin. Connection type is + // manually selected. Crude.. + if(_sitl->sonar_connected) + airspeed_pin_value = _ground_sonar(altitude); + else + airspeed_pin_value = _airspeed_sensor(airspeed + (_sitl->aspd_noise * _rand_float())); } #endif diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index e036a12784..2b8055541d 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -48,6 +48,7 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = { AP_GROUPINFO("ASPD_RND", 20, SITL, aspd_noise, 0.5), AP_GROUPINFO("ACCEL_FAIL", 21, SITL, accel_fail, 0), AP_GROUPINFO("BARO_DRIFT", 22, SITL, baro_drift, 0), + AP_GROUPINFO("SONAR_CON", 23, SITL, sonar_connected, 0), AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index d43bc217e8..5da6bb4e0e 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -55,6 +55,8 @@ public: AP_Float mag_error; // in degrees AP_Float servo_rate; // servo speed in degrees/second + AP_Float sonar_connected; // Analogue sonar connected to AnalogPin 0 (else airspeed) + AP_Float drift_speed; // degrees/second/minute AP_Float drift_time; // period in minutes AP_Float engine_mul; // engine multiplier