// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas)
{
// The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
// The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
// A positive X rate is produced by a positive sensor rotation about the X axis
// A positive Y rate is produced by a positive sensor rotation about the Y axis
// This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
// negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate
flowMeaTime_ms = imuSampleTime_ms;
// calculate bias errors on flow sensor gyro rates, but protect against spikes in data
// reset the accumulated body delta angle and time
// don't do the calculation if not enough time lapsed for a reliable body rate measurement
if (delTimeOF > 0.01f) {
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
delAngBodyOF.zero();
delTimeOF = 0.0f;
}
// check for takeoff if relying on optical flow and zero measurements until takeoff detected
// if we haven't taken off - constrain position and velocity states
if (frontend->_fusionModeGPS == 3) {
detectOptFlowTakeoff();
}
// calculate rotation matrices at mid sample time for flow observations
stateStruct.quat.rotation_matrix(Tbn_flow);
Tnb_flow = Tbn_flow.transposed();
// don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
// correct flow sensor rates for bias
omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x;
omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y;
// write uncorrected flow rate measurements that will be used by the focal length scale factor estimator
// note correction for different axis and sign conventions used by the px4flow sensor
ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
// write flow rate measurements corrected for body rates
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + omegaAcrossFlowTime.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + omegaAcrossFlowTime.y;
// record time last observation was received so we can detect loss of data elsewhere
flowValidMeaTime_ms = imuSampleTime_ms;
// estimate sample time of the measurement
ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
// Correct for the average intersampling delay due to the filter updaterate
ofDataNew.time_ms -= localFilterTimeStep_ms/2;
// Prevent time delay exceeding age of oldest IMU data in the buffer
ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms);
// Save data to buffer
storedOF.push(ofDataNew);
// Check for data at the fusion time horizon
flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms);
}
}
//----------------------------------------------------------------------------
void BSplineCurveExamples::OnDisplay ()
{
ClearScreen();
ColorRGB lightGray(224, 224, 224);
ColorRGB mediumGray(192, 192, 192);
ColorRGB darkGray(128, 128, 128);
ColorRGB black(0, 0, 0);
// draw axes
int i;
for (i = mSize/16; i < mSize; ++i)
{
SetPixel(mSize/16, mSize - 1 - i, lightGray);
SetPixel(i, mSize - 1 - mSize/16, lightGray);
}
// draw control points
int imax = mSpline->GetNumCtrlPoints();
int x, y;
for (i = 0; i < imax; ++i)
{
const Vector2f& ctrl = mSpline->GetControlPoint(i);
x = (int)(ctrl.X() + 0.5f);
y = mSize - 1 - (int)(ctrl.Y() + 0.5f);
SetThickPixel(x, y, 2, darkGray);
}
// draw spline
imax = 2048;
for (i = 0; i <= imax; ++i)
{
// draw point
float u = i/(float)imax;
Vector2f pos = mSpline->GetPosition(u);
x = (int)(pos.X() + 0.5f);
y = mSize - 1 - (int)(pos.Y() + 0.5f);
if (mModified
&& mLocCtrlMin[mCurveType] <= u && u <= mLocCtrlMax[mCurveType])
{
SetPixel(x, y, mediumGray);
}
else
{
SetPixel(x, y, black);
}
}
WindowApplication2::OnDisplay();
}
void Camera::rotate2D(float x, float y) {
Vector2f rot = Vector2f(x, y);
AngleAxis<float> rotX(rot.x(), Vector3f::UnitY()); // look left right
AngleAxis<float> rotY(rot.y(), Vector3f::UnitX()); // look up down
rotation_future_ = (rotX * rotY) * rotation_current_;
if(roll_correction_){
rotation_future_ = rollcorrect(rotation_future_);
}
emit modified();
}
float fractalsum(const Vector2f& p, float freq)
{
float t;
float vec[2];
for (t = 0.0f; freq >= 1.0f; freq *= 0.5f)
{
vec[0] = freq * p.x();
vec[1] = freq * p.y();
t += noise2(vec) / freq;
}
return t;
}
void Game::OnInput()
{
if(_input->IsKeyDown(Keys::KEY_A))
printf("Button %d pressed! \n", Keys::KEY_A);
if(_input->IsKeyDown(Keys::KEY_B))
printf("Button %d pressed! \n", Keys::KEY_B);
if(_input->IsMouseClicked())
{
Vector2f mousePosition = _input->GetMousePosition();
int x = mousePosition.GetX();
int y = mousePosition.GetY();
printf("Mouse clicked on x : %d, y : %d \n", x, y);
}
}
float turbulence(const Vector2f& p, float freq)
{
float t;
float vec[2];
for (t = 0.0f; freq >= 1.0f; freq *= 0.5f)
{
vec[0] = freq * p.x();
vec[1] = freq * p.y();
t += (float) fabs(noise2(vec)) / freq;
}
return t;
}
//----------------------------------------------------------------------------
void MapTextureToQuad::SelectVertex (const Vector2f& rkPos)
{
// identify vertex within 5 pixels of mouse click
const float fPixels = 5.0f;
m_iSelected = -1;
for (int i = 0; i < 4; i++)
{
Vector2f kDiff = rkPos - m_akVertex[i];
if ( kDiff.Length() <= fPixels )
{
m_iSelected = i;
break;
}
}
}
float Attitude::getDip() const
{
Vector2f north = Vector2f::UnitX(); // parallel to x axis
Vector2f dip = getDipDirectionVector().head(
2); // he third element is 0 (working on horizontal plane)
float angle = acos(north.dot(dip)) * 180 / M_PI;
if (dip(1)
> 0.0) // if y component is postive we are in the 180 to 360 half-space
angle = 360 - angle;
return angle;
}
//----------------------------------------------------------------------------
void MapTextureToQuad::SelectVertex (const Vector2f& position)
{
// Identify vertex within 5 pixels of mouse click.
const float pixelRange = 5.0f;
mSelected = -1;
for (int i = 0; i < 4; ++i)
{
Vector2f diff = position - mVertex[i];
if (diff.Length() <= pixelRange)
{
mSelected = i;
break;
}
}
}
Vector4f LIPStateEstimator::measure(SupportFoot supportFoot, const Vector2f& LIPOrigin) const
{
const Pose3f& supportFootToTorso = supportFoot == SupportFoot::left ? theRobotModel.soleLeft : theRobotModel.soleRight;
const Quaternionf& torsoToWorld = theInertialData.orientation2D;
const Pose3f originToTorso = supportFootToTorso + Vector3f(LIPOrigin.x(), LIPOrigin.y(), 0.f);
const Vector3f comInOrigin = originToTorso.inverse() * theRobotModel.centerOfMass;
const Vector3f com = (torsoToWorld * originToTorso.rotation) * comInOrigin;
PLOT("module:ZmpWalkingEngine:LIPStateEstimator:Estimate:measuredComHeight", com.z());
const Vector2f accInWorld = (torsoToWorld * theInertialData.acc * 1000.f).head<2>();
const Vector2f zmp = com.head<2>().array() - (accInWorld.array() / LIP3D(LIPHeights).getK().square());
return (Vector4f() << com.head<2>(), zmp).finished();
}
bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v)
{
if (PX4_ISFINITE(v.length()) && v.length() > SIGMA_NORM) {
v.normalize();
// To find yaw: take dot product of x = (1,0) and v
// and multiply by the sign given of cross product of x and v.
// Dot product: (x(0)*v(0)+(x(1)*v(1)) = v(0)
// Cross product: x(0)*v(1) - v(0)*x(1) = v(1)
heading = math::sign(v(1)) * wrap_pi(acosf(v(0)));
return true;
}
// heading unknown and therefore do not change heading
return false;
}
float AP_Landing_Deepstall::update_steering()
{
Location current_loc;
if ((!landing.ahrs.get_position(current_loc) || !landing.ahrs.healthy()) && !hold_level) {
// panic if no position source is available
// continue the stall but target just holding the wings held level as deepstall should be a minimal
// energy configuration on the aircraft, and if a position isn't available aborting would be worse
gcs().send_text(MAV_SEVERITY_CRITICAL, "Deepstall: Invalid data from AHRS. Holding level");
hold_level = true;
}
float desired_change = 0.0f;
if (!hold_level) {
uint32_t time = AP_HAL::millis();
float dt = constrain_float(time - last_time, (uint32_t)10UL, (uint32_t)200UL) * 1e-3;
last_time = time;
Vector2f ab = location_diff(arc_exit, extended_approach);
ab.normalize();
Vector2f a_air = location_diff(arc_exit, current_loc);
crosstrack_error = a_air % ab;
float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f);
float nu1 = asinf(sine_nu1);
if (L1_i > 0) {
L1_xtrack_i += nu1 * L1_i / dt;
L1_xtrack_i = constrain_float(L1_xtrack_i, -0.5f, 0.5f);
nu1 += L1_xtrack_i;
}
desired_change = wrap_PI(radians(target_heading_deg) + nu1 - landing.ahrs.yaw) / time_constant;
}
float yaw_rate = landing.ahrs.get_gyro().z;
float yaw_rate_limit_rps = radians(yaw_rate_limit);
float error = wrap_PI(constrain_float(desired_change, -yaw_rate_limit_rps, yaw_rate_limit_rps) - yaw_rate);
#ifdef DEBUG_PRINTS
gcs().send_text(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f",
(double)crosstrack_error,
(double)error,
(double)degrees(yaw_rate),
(double)location_diff(current_loc, landing_point).length());
#endif // DEBUG_PRINTS
return ds_PID.get_pid(error);
}
// intersect lines with plane
void Slicer::CalcCuttingPlane(float where, CuttingPlane &plane, const Matrix4f &T)
{
#if CUTTING_PLANE_DEBUG
cout << "intersect with z " << where << "\n";
#endif
plane.Clear();
plane.SetZ(where);
Vector3f min = T*Min;
Vector3f max = T*Max;
plane.Min.x = min.x;
plane.Min.y = min.y;
plane.Max.x = max.x;
plane.Max.y = max.y;
Vector2f lineStart;
Vector2f lineEnd;
bool foundOne = false;
int num_cutpoints;
for (size_t i = 0; i < triangles.size(); i++)
{
foundOne=false;
CuttingPlane::Segment line(-1,-1);
num_cutpoints = triangles[i].CutWithPlane(where, T, lineStart, lineEnd);
if (num_cutpoints>0)
line.start = plane.RegisterPoint(lineStart);
if (num_cutpoints>1)
line.end = plane.RegisterPoint(lineEnd);
// Check segment normal against triangle normal. Flip segment, as needed.
if (line.start != -1 && line.end != -1 && line.end != line.start)
// if we found a intersecting triangle
{
Vector3f Norm = triangles[i].Normal;
Vector2f triangleNormal = Vector2f(Norm.x, Norm.y);
Vector2f segmentNormal = (lineEnd - lineStart).normal();
triangleNormal.normalise();
segmentNormal.normalise();
if( (triangleNormal-segmentNormal).lengthSquared() > 0.2f)
// if normals does not align, flip the segment
line.Swap();
plane.AddLine(line);
}
}
}
//----------------------------------------------------------------------------------------------------
Vector2f Vector2f::operator/ ( float fScalar ) const
{
Vector2f quot;
if ( fScalar != 0.0f )
{
float fInvScalar = 1.0f / fScalar;
quot.x = x * fInvScalar;
quot.y = y * fInvScalar;
}
else
{
quot.MakeZero();
}
return( quot );
}
float MT::evaluate(Warp warp)
{
rect_t rect = window(warp.t);
float fine_cell = sqrt(rectArea(rect) / cell_n);
feature.set_cell(fine_cell);
feature.set_step(1);
float E = 0.0f;
for (int i = 0; i < fine_samples.size(); ++i) {
Vector2f p = warp.transform2(fine_samples[i]);
Vector32f F;
feature.descriptor4(p.x(), p.y(), F.data());
E = E + sigmoid((F - fine_model.col(i)).squaredNorm());
}
return E / fine_samples.size();
}
Vector2f LIP3D::requiredVelocity(const Vector2f& pos, float time)
{
const Array2f sinhkt(std::sinh(k.x() * time), std::sinh(k.y() * time));
const Array2f coshkt(std::cosh(k.x() * time), std::cosh(k.y() * time));
return k.array() * (pos.array() - position.array() * coshkt) / sinhkt;
}
void Ball::Bounce(Vector2f normal)
{
normal.normalize();
Vector2f normal_component = normal * mVelocity.dotProduct(normal);
mVelocity -= normal_component * 2;
//Now constrain angles - two cones of 30 degrees left/right should be invalid
Vector2f v_norm = mVelocity;
v_norm.normalize();
float dot = v_norm.dotProduct(Vector2f(1, 0));
if(dot > cos(DEG2RAD(30)) || dot < -cos(DEG2RAD(30)))
{
float magnitude = mVelocity.length();
if(mVelocity.x < 0 && mVelocity.y < 0)
mVelocity = Vector2f(-cos(DEG2RAD(30)), -sin(DEG2RAD(30))) * magnitude;
else if(mVelocity.x > 0 && mVelocity.y < 0)
mVelocity = Vector2f(cos(DEG2RAD(30)), -sin(DEG2RAD(30))) * magnitude;
else if(mVelocity.x < 0 && mVelocity.y > 0)
mVelocity = Vector2f(-cos(DEG2RAD(30)), sin(DEG2RAD(30))) * magnitude;
else
mVelocity = Vector2f(cos(DEG2RAD(30)), sin(DEG2RAD(30))) * magnitude;
}
}
/* PRIVATE MEMBER FUNCTIONS */
void FFTSplineControl::expandButton_Dragged(const Vector2f &v)
{
assert(modelview != NULL);
assert(expandButton != NULL);
assert(exitButton != NULL);
Point2f p(v.getX(), 0.0f);
modelview->unScalePoint(p);
float newWidth = width + p.getX();
float beatWidth = InterfaceManager::getBeatLength();
if(newWidth >= (beatWidth * MIN_BEAT_WIDTH) && newWidth <= (beatWidth * MAX_BEAT_WIDTH))
{
// expand/shrink the control
float factor = newWidth / width;
stretchControlPoints(factor);
setWidth(newWidth);
// move the handles
expandButton->getPosition().setX(width + BORDER_DIM);
exitButton->getPosition().setX(width + BORDER_DIM);
dirty = true;
fftDirty = true;
}
}
/*
* Adjusts the desired velocity for the polygon fence.
*/
void AC_Avoid::adjust_velocity_polygon_fence(float kP, float accel_cmss, Vector2f &desired_vel)
{
// exit if the polygon fence is not enabled
if ((_fence.get_enabled_fences() & AC_FENCE_TYPE_POLYGON) == 0) {
return;
}
// exit if the polygon fence has already been breached
if ((_fence.get_breaches() & AC_FENCE_TYPE_POLYGON) != 0) {
return;
}
// exit immediately if no desired velocity
if (desired_vel.is_zero()) {
return;
}
// get polygon boundary
// Note: first point in list is the return-point (which copter does not use)
uint16_t num_points;
Vector2f* boundary = _fence.get_polygon_points(num_points);
// adjust velocity using polygon
adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points, true);
}
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
// indicates prefect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accpeted by the filter
// boolean false is returned if variances are not available
bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar, Vector2f &offset) const
{
switch (ekf_type()) {
case EKF_TYPE_NONE:
// We are not using an EKF so no data
return false;
#if AP_AHRS_WITH_EKF1
case EKF_TYPE1:
// use EKF to get variance
EKF1.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
return true;
#endif
case EKF_TYPE2:
default:
// use EKF to get variance
EKF2.getVariances(-1,velVar, posVar, hgtVar, magVar, tasVar, offset);
return true;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
velVar = 0;
posVar = 0;
hgtVar = 0;
magVar.zero();
tasVar = 0;
offset.zero();
return true;
#endif
}
}
Triangle::Triangle(Vertex* a_first, Vertex* a_second, Vertex* a_third)
: m_first(a_first), m_second(a_second), m_third(a_third)
{
Vector2f centroid;
centroid.x = a_first->m_position.x + ((2 / 3)*a_second->m_position.x - a_first->m_position.x);
centroid.y = a_first->m_position.y + ((2 / 3)*a_second->m_position.y - a_first->m_position.y);
float distToFirst = centroid.Distance(a_first->m_position);
float distToSecond = centroid.Distance(a_second->m_position);
float distToThird = centroid.Distance(a_third->m_position);
float totalDist = distToFirst + distToSecond + distToThird;
r = (m_first->r * distToFirst + m_second->r * distToSecond + m_third->r * distToThird) / totalDist;
g = (m_first->g * distToFirst + m_second->g * distToSecond + m_third->g * distToThird) / totalDist;
b = (m_first->b * distToFirst + m_second->b * distToSecond + m_third->b * distToThird) / totalDist;
}
QuadMesh::QuadMesh( const Vector3f& pos, const Vector2f& dim,
const MeshVertexData::AttributeType type ) {
size_t nvertices = 4, nindices = 6;
std::vector< MeshVertexData > vdata;
size_t nbytes_per_vert = vdata_from_attributes( type, vdata );
unsigned char* data = (unsigned char*)allocate_data( nvertices, vdata );
unsigned short* indices = (unsigned short*)
allocate_index_data<unsigned short>(nindices);
Vector3f off[2];
for( unsigned i = 0; i < 2; ++i ) off[i].index(i) = dim.index(i);
unsigned char* p = data;
if( type & MeshVertexData::VERTEX ) {
write_vector( pos, (float*)(p+0) );
write_vector( pos + off[0], (float*)(p+1*nbytes_per_vert) );
write_vector( pos + off[0] + off[1], (float*)(p+2*nbytes_per_vert));
write_vector( pos + off[1], (float*)(p+3*nbytes_per_vert) );
p += sizeof(float)*3;
}
if( type& MeshVertexData::TEX_COORD0 ) {
write_vector( Vector2f(), (float*)(p+0) );
write_vector( Vector2f(1., 0.), (float*)(p+1*nbytes_per_vert) );
write_vector( Vector2f(1., 1.), (float*)(p+2*nbytes_per_vert) );
write_vector( Vector2f(0., 1.), (float*)(p+3*nbytes_per_vert) );
p += sizeof(float)*2;
}
indices[0] = 0; indices[1] = 1; indices[2] = 2;
indices[3] = 0; indices[4] = 2; indices[5] = 3;
}
bool SideConfidenceProvider::ballModelCanBeUsed(const BallModel& ballModel, const Pose2f& robotPose) const
{
// Is the ball model still "fresh"?
if(theFrameInfo.getTimeSince(ballModel.timeWhenLastSeen) > maxBallAge)
return false;
// We do not want to consider fast rolling balls for the SideConfidence:
if(ballModel.estimate.velocity.norm() > maxBallVelocity)
return false;
// Close to the field center, the ball information does not help:
const Vector2f globalPos = robotPose * ballModel.estimate.position;
if(globalPos.norm() < centerBanZoneRadius)
return false;
// As the perception of the new ball might be unreliable, we only consider stuff away from lines:
if(ballIsCloseToPenaltyMarkOrLine(globalPos))
return false;
return true;
}
float DistancetoLineSegment(const Vector2f& a, const Vector2f& b, const Vector2f& point)
{
Vector2f dist = b - a;
float length = dist.Length();
float t = (point - a).Dot(dist);
if(t < 0.0f)
return (point - a).Length();
if(t > length)
return (point - b).Length();
dist.Normalize();
if(dist == Vector2f::Zero)
return (point - a).Length();
return (point - (a + dist * t)).Length();
}
void GMExperiment6_1::tangent_check(int32 start_idx, int32 end_idx, vector<int> & knots, vector<Vector2f> &smoothData)
{
Vector2f tangent_begin = firstDerivative(smoothData,start_idx);
tangent_begin.normalize();
//tangent_begin.normalise();
for(int i=start_idx+1;i<end_idx; i++)
{
Vector2f tangent = firstDerivative(smoothData,i);
tangent.normalize();
if( (tangent[0]*tangent_begin[0]+tangent[1]*tangent_begin[1]) < cos(tangent_filter_angle*M_PI/180.0) )//if tangents off by more than 90 deg
{
knots.push_back(i);
tangent_check(i,end_idx,knots,smoothData);
break;
}
}
}
MTV getMTV_circ_circ(Game_Object &circle0, Game_Object &circle1){
MTV result;
float overlap = 5000.0f;
Vector2f smallest;
const Vector2f dist_vec = (circle0.get_position() + 0.5f * circle0.get_size()) -
(circle1.get_position() + 0.5f * circle1.get_size());
const float dist2 = dist_vec * dist_vec;
const float radius_sum = circle0.get_radius() + circle1.get_radius();
if (dist2 < radius_sum * radius_sum) {
overlap = radius_sum - sqrt(dist2);
smallest = dist_vec.normalized();
result = MTV(true, smallest, overlap);
}
return result;
}
float AP_Landing_Deepstall::update_steering()
{
Location current_loc;
if (!landing.ahrs.get_position(current_loc)) {
// panic if no position source is available
// continue the but target just holding the wings held level as deepstall should be a minimal energy
// configuration on the aircraft, and if a position isn't available aborting would be worse
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Deepstall: No position available. Attempting to hold level");
memcpy(¤t_loc, &landing_point, sizeof(Location));
}
uint32_t time = AP_HAL::millis();
float dt = constrain_float(time - last_time, (uint32_t)10UL, (uint32_t)200UL) / 1000.0;
last_time = time;
Vector2f ab = location_diff(arc_exit, extended_approach);
ab.normalize();
Vector2f a_air = location_diff(arc_exit, current_loc);
crosstrack_error = a_air % ab;
float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f);
float nu1 = asinf(sine_nu1);
if (L1_i > 0) {
L1_xtrack_i += nu1 * L1_i / dt;
L1_xtrack_i = constrain_float(L1_xtrack_i, -0.5f, 0.5f);
nu1 += L1_xtrack_i;
}
float desired_change = wrap_PI(radians(target_heading_deg) + nu1 - landing.ahrs.yaw);
float yaw_rate = landing.ahrs.get_gyro().z;
float yaw_rate_limit_rps = radians(yaw_rate_limit);
float error = wrap_PI(constrain_float(desired_change / time_constant,
-yaw_rate_limit_rps, yaw_rate_limit_rps) - yaw_rate);
#ifdef DEBUG_PRINTS
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f",
(double)crosstrack_error,
(double)error,
(double)degrees(yaw_rate),
(double)location_diff(current_loc, landing_point).length());
#endif // DEBUG_PRINTS
return ds_PID.get_pid(error);
}
Matrix3f NNUtilities::calcInverseTransformation(const Vector2i& center, const Vector2i& inSize, const Vector2i& outSize, const Angle rotation)
{
const Vector2i upperLeft = (center.array() - inSize.array() / 2).matrix();
const Vector2f transformationFactor = (inSize.cast<float>().array() / outSize.cast<float>().array()).matrix();
//float sin = std::sin(rotation);
//float cos = std::cos(rotation);
Matrix3f inverseTransformation; //TODO check rotation
inverseTransformation(0, 0) = transformationFactor.x();// *cos;
inverseTransformation(0, 1) = 0;// transformationFactor.x() * (-sin);
inverseTransformation(1, 0) = 0;// transformationFactor.y() * sin;
inverseTransformation(1, 1) = transformationFactor.y();// *cos;
inverseTransformation(0, 2) = upperLeft.x();// +outSize.x() * (1 - cos + sin) / 2 * transformationFactor.x();
inverseTransformation(1, 2) = upperLeft.y();// +outSize.y() * (1 - sin - cos) / 2 * transformationFactor.y();
return inverseTransformation;
}
void Plane::calc_gndspeed_undershoot()
{
// Use the component of ground speed in the forward direction
// This prevents flyaway if wind takes plane backwards
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
Vector2f gndVel = ahrs.groundspeed_vector();
const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x);
if (!yawVect.is_zero()) {
yawVect.normalize();
float gndSpdFwd = yawVect * gndVel;
groundspeed_undershoot = (aparm.min_gndspeed_cm > 0) ? (aparm.min_gndspeed_cm - gndSpdFwd*100) : 0;
}
} else {
groundspeed_undershoot = 0;
}
}
//----------------------------------------------------------------------------
Vector2f Renderer::PointWorldToViewPort (const APoint &point, bool *isInBack)
{
HMatrix matProjView = GetProjectionMatrix() * GetViewMatrix();
HPoint hPoint(point.X(), point.Y(), point.Z(), point.W());
HPoint tempPoint = matProjView * hPoint;
if (isInBack)
{
if (tempPoint.Z() <= 0)
*isInBack = true;
else
*isInBack = false;
}
float wInv = 1.0f / tempPoint.W();
//投影坐标范围为[-1,1]要变成[0,1]
Vector2f screenPoint;
screenPoint.X() = (1.0f + tempPoint.X()*wInv)/2.0f;
screenPoint.Y() = (1.0f + tempPoint.Y()*wInv)/2.0f;
//投影坐标范围为[0,1]要变成视口内坐标
int viewX, viewY, viewW, viewH;
GetViewport(viewX, viewY, viewW, viewH);
screenPoint.X() = viewX + screenPoint.X()*viewW;
screenPoint.Y() = viewY + screenPoint.Y()*viewH;
return screenPoint;
}
