From def6e014cfece09db111e7432f8b75c33d578e7d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 19 Nov 2016 17:21:54 +1100 Subject: [PATCH] HAL_SITL: removed old optflow simulation --- libraries/AP_HAL_SITL/SITL_State.cpp | 13 +- libraries/AP_HAL_SITL/SITL_State.h | 5 +- libraries/AP_HAL_SITL/sitl_ins.cpp | 2 +- libraries/AP_HAL_SITL/sitl_optical_flow.cpp | 132 -------------------- 4 files changed, 9 insertions(+), 143 deletions(-) delete mode 100644 libraries/AP_HAL_SITL/sitl_optical_flow.cpp diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 67e07df524..07fe738374 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -83,7 +83,6 @@ void SITL_State::_sitl_setup(const char *home_str) #if AP_TERRAIN_AVAILABLE _terrain = (AP_Terrain *)AP_Param::find_object("TERRAIN_"); #endif - _optical_flow = (OpticalFlow *)AP_Param::find_object("FLOW"); if (_sitl != nullptr) { // setup some initial values @@ -171,7 +170,6 @@ void SITL_State::_fdm_input_step(void) _sitl->state.airspeed, _sitl->state.altitude); _update_barometer(_sitl->state.altitude); _update_compass(_sitl->state.rollDeg, _sitl->state.pitchDeg, _sitl->state.yawDeg); - _update_flow(); if (_sitl->adsb_plane_count >= 0 && adsb == nullptr) { @@ -309,6 +307,8 @@ void SITL_State::_fdm_input_local(void) hal.scheduler->stop_clock(AP_HAL::micros64()+100); } + set_height_agl(); + _synthetic_clock_mode = true; _update_count++; } @@ -463,9 +463,9 @@ void SITL_State::init(int argc, char * const argv[]) } /* - return height above the ground in meters + set height above the ground in meters */ -float SITL_State::height_agl(void) +void SITL_State::set_height_agl(void) { static float home_alt = -1; @@ -485,13 +485,14 @@ float SITL_State::height_agl(void) location.lng = _sitl->state.longitude*1.0e7; if (_terrain->height_amsl(location, terrain_height_amsl, false)) { - return _sitl->state.altitude - terrain_height_amsl; + _sitl->height_agl = _sitl->state.altitude - terrain_height_amsl; + return; } } #endif // fall back to flat earth model - return _sitl->state.altitude - home_alt; + _sitl->height_agl = _sitl->state.altitude - home_alt; } #endif diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 5f6ca3afa0..d585ee65db 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -83,10 +82,9 @@ private: void _setup_timer(void); void _setup_adc(void); - float height_agl(void); + void set_height_agl(void); void _update_barometer(float height); void _update_compass(float rollDeg, float pitchDeg, float yawDeg); - void _update_flow(void); void _set_signal_handlers(void) const; @@ -155,7 +153,6 @@ private: AP_InertialSensor *_ins; Scheduler *_scheduler; Compass *_compass; - OpticalFlow *_optical_flow; #if AP_TERRAIN_AVAILABLE AP_Terrain *_terrain; #endif diff --git a/libraries/AP_HAL_SITL/sitl_ins.cpp b/libraries/AP_HAL_SITL/sitl_ins.cpp index ef03101ad3..f76ef3d127 100644 --- a/libraries/AP_HAL_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_SITL/sitl_ins.cpp @@ -84,7 +84,7 @@ uint16_t SITL_State::_airspeed_sensor(float airspeed) */ uint16_t SITL_State::_ground_sonar(void) { - float altitude = height_agl(); + float altitude = _sitl->height_agl; // sensor position offset in body frame Vector3f relPosSensorBF = _sitl->rngfnd_pos_offset; diff --git a/libraries/AP_HAL_SITL/sitl_optical_flow.cpp b/libraries/AP_HAL_SITL/sitl_optical_flow.cpp deleted file mode 100644 index f3455db895..0000000000 --- a/libraries/AP_HAL_SITL/sitl_optical_flow.cpp +++ /dev/null @@ -1,132 +0,0 @@ -/* - SITL handling - - This simulates a optical flow sensor - - Andrew Tridgell November 2011 - */ - -#include -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - -#include "AP_HAL_SITL.h" - -using namespace HALSITL; - -extern const AP_HAL::HAL& hal; - -#include -#include -#include -#include -#include - -#define MAX_OPTFLOW_DELAY 20 -static uint8_t next_optflow_index; -static uint8_t optflow_delay; -static OpticalFlow::OpticalFlow_state optflow_data[MAX_OPTFLOW_DELAY]; - -/* - update the optical flow with new data - */ -void SITL_State::_update_flow(void) -{ - Vector3f gyro; - static uint32_t last_flow_ms; - - if (!_optical_flow || - !_sitl->flow_enable) { - return; - } - - // update at the requested rate - uint32_t now = AP_HAL::millis(); - if (now - last_flow_ms < 1000*(1.0f/_sitl->flow_rate)) { - return; - } - last_flow_ms = now; - - gyro(radians(_sitl->state.rollRate), - radians(_sitl->state.pitchRate), - radians(_sitl->state.yawRate)); - - OpticalFlow::OpticalFlow_state state; - - // NED velocity vector in m/s - Vector3f velocity(_sitl->state.speedN, - _sitl->state.speedE, - _sitl->state.speedD); - - // a rotation matrix following DCM conventions - Matrix3f rotmat; - rotmat.from_euler(radians(_sitl->state.rollDeg), - radians(_sitl->state.pitchDeg), - radians(_sitl->state.yawDeg)); - - - state.device_id = 1; - state.surface_quality = 51; - - // sensor position offset in body frame - Vector3f posRelSensorBF = _sitl->optflow_pos_offset; - - // for effiency only do offset correction if offset is non-zero - bool correctForOffset = !posRelSensorBF.is_zero(); - - // estimate range to centre of image - float range; - if (rotmat.c.z > 0.05f && height_agl() > 0) { - if (!correctForOffset) { - range = height_agl() / rotmat.c.z; - } else { - Vector3f relPosSensorEF = rotmat * posRelSensorBF; - range = (height_agl() - relPosSensorEF.z) / rotmat.c.z; - } - } else { - range = 1e38f; - } - - // Calculate relative velocity in sensor frame assuming no misalignment between sensor and vehicle body axes - Vector3f relVelSensor = rotmat.mul_transpose(velocity); - - // correct relative velocity for rotation rates and sensor offset - if (correctForOffset) { - relVelSensor += gyro % posRelSensorBF; - } - - // Divide velocity by range and add body rates to get predicted sensed angular - // optical rates relative to X and Y sensor axes assuming no misalignment or scale - // factor error. Note - these are instantaneous values. The sensor sums these values across the interval from the last - // poll to provide a delta angle across the interface - state.flowRate.x = -relVelSensor.y/range + gyro.x; - state.flowRate.y = relVelSensor.x/range + gyro.y; - - // The flow sensors body rates are assumed to be the same as the vehicle body rates (ie no misalignment) - // Note - these are instantaneous values. The sensor sums these values across the interval from the last - // poll to provide a delta angle across the interface. - state.bodyRate = Vector2f(gyro.x, gyro.y); - - optflow_data[next_optflow_index++] = state; - if (next_optflow_index >= optflow_delay+1) { - next_optflow_index = 0; - } - - state = optflow_data[next_optflow_index]; - - if (_sitl->flow_delay != optflow_delay) { - // cope with updates to the delay control - if (_sitl->flow_delay > MAX_OPTFLOW_DELAY) { - _sitl->flow_delay = MAX_OPTFLOW_DELAY; - } - optflow_delay = _sitl->flow_delay; - for (uint8_t i=0; isetHIL(state); -} - -#endif