mirror of https://github.com/ArduPilot/ardupilot
123 lines
4.2 KiB
C++
123 lines
4.2 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
/*
|
|
* AP_OpticalFlow_SITL.cpp - SITL emulation of optical flow sensor.
|
|
*/
|
|
|
|
#include "AP_OpticalFlow_SITL.h"
|
|
|
|
#if AP_OPTICALFLOW_SITL_ENABLED
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <SITL/SITL.h>
|
|
|
|
void AP_OpticalFlow_SITL::update(void)
|
|
{
|
|
auto *_sitl = AP::sitl();
|
|
|
|
if (!_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;
|
|
|
|
Vector3f gyro(radians(_sitl->state.rollRate),
|
|
radians(_sitl->state.pitchRate),
|
|
radians(_sitl->state.yawRate));
|
|
|
|
AP_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.surface_quality = 51;
|
|
|
|
// sensor position offset in body frame
|
|
Vector3f posRelSensorBF = _sitl->optflow_pos_offset;
|
|
|
|
// estimate range to centre of image
|
|
float range;
|
|
if (rotmat.c.z > 0.05f && _sitl->state.height_agl > 0) {
|
|
Vector3f relPosSensorEF = rotmat * posRelSensorBF;
|
|
range = (_sitl->state.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
|
|
relVelSensor += gyro % posRelSensorBF;
|
|
|
|
// scaling based on parameters
|
|
const Vector2f flowScaler = _flowScaler();
|
|
const float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
|
|
const float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
|
|
|
|
// 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 + _sitl->flow_noise * rand_float()) * flowScaleFactorX;
|
|
state.flowRate.y = (relVelSensor.x/range + gyro.y + _sitl->flow_noise * rand_float()) * flowScaleFactorY;
|
|
|
|
// 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 > 0 &&
|
|
(uint8_t)(_sitl->flow_delay) > ARRAY_SIZE(optflow_data)) {
|
|
_sitl->flow_delay.set(ARRAY_SIZE(optflow_data));
|
|
}
|
|
optflow_delay = _sitl->flow_delay;
|
|
for (uint8_t i=0; i<optflow_delay; i++) {
|
|
optflow_data[i] = state;
|
|
}
|
|
}
|
|
|
|
_applyYaw(state.flowRate);
|
|
|
|
// copy results to front end
|
|
_update_frontend(state);
|
|
}
|
|
|
|
#endif // AP_OPTICALFLOW_SITL_ENABLED
|