HAL_SITL: removed old optflow simulation

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

libraries/AP_HAL_SITL/SITL_State.h

@@ -18,7 +18,6 @@
 #include <AP_Baro/AP_Baro.h>
 #include <AP_InertialSensor/AP_InertialSensor.h>
 #include <AP_Compass/AP_Compass.h>
-#include <AP_OpticalFlow/AP_OpticalFlow.h>
 #include <AP_Terrain/AP_Terrain.h>
 #include <SITL/SITL.h>
 #include <SITL/SIM_Gimbal.h>
@@ -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

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;

libraries/AP_HAL_SITL/sitl_optical_flow.cpp

@@ -1,132 +0,0 @@
-/*
-  SITL handling
-
-  This simulates a optical flow sensor
-
-  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>
-
-#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; i<optflow_delay; i++) {
-            optflow_data[i] = state;
-        }
-    }
-
-    _optical_flow->setHIL(state);
-}
-
-#endif