mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 08:38:36 -04:00
88 lines
2.2 KiB
C++
88 lines
2.2 KiB
C++
/*
|
|
SITL handling
|
|
|
|
This simulates a optical flow sensor
|
|
|
|
Andrew Tridgell November 2011
|
|
*/
|
|
|
|
#include <AP_HAL.h>
|
|
#include <AP_Math.h>
|
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
|
|
|
#include "AP_HAL_AVR_SITL.h"
|
|
|
|
using namespace AVR_SITL;
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#include <unistd.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <math.h>
|
|
|
|
/*
|
|
update the optical flow with new data
|
|
*/
|
|
void SITL_State::_update_flow(void)
|
|
{
|
|
double p, q, r;
|
|
Vector3f gyro;
|
|
|
|
if (!_optical_flow ||
|
|
!_terrain ||
|
|
!_sitl->flow_enable) {
|
|
return;
|
|
}
|
|
|
|
// convert roll rates to body frame
|
|
SITL::convert_body_frame(radians(_sitl->state.rollDeg),
|
|
radians(_sitl->state.pitchDeg),
|
|
radians(_sitl->state.rollRate),
|
|
radians(_sitl->state.pitchRate),
|
|
radians(_sitl->state.yawRate),
|
|
&p, &q, &r);
|
|
gyro(p, q, r);
|
|
|
|
OpticalFlow::OpticalFlow_state state;
|
|
|
|
// get height above terrain from AP_Terrain. This assumes
|
|
// AP_Terrain is working
|
|
float terrain_height_amsl;
|
|
struct Location location;
|
|
location.lat = _sitl->state.latitude*1.0e7;
|
|
location.lng = _sitl->state.longitude*1.0e7;
|
|
|
|
if (!_terrain->height_amsl(location, terrain_height_amsl)) {
|
|
// no terrain height available
|
|
return;
|
|
}
|
|
|
|
float height_agl = _sitl->state.altitude - terrain_height_amsl;
|
|
|
|
// 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 = 0;
|
|
|
|
// rubbish calculation for Paul to fill in
|
|
state.flowRate = Vector2f(gyro.x, gyro.y) * height_agl * velocity.length();
|
|
state.bodyRate = Vector2f(gyro.x, gyro.y) * height_agl;
|
|
|
|
_optical_flow->setHIL(state);
|
|
}
|
|
|
|
#endif
|