mirror of https://github.com/ArduPilot/ardupilot
HAL_SITL: removed old optflow simulation
This commit is contained in:
parent
42435e700b
commit
def6e014cf
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
Loading…
Reference in New Issue