Vector3f cTrackball::ballVector(Vector2f screen) const { // normalize and centre the screen coordinates first screen -= m_center; screen /= m_radius; float lsqared = screen.squaredNorm(); Vector3f ball; // if we are grabbing outside the bounds of the virtual hemisphere, // take a point on the edge if (lsqared > 1.0) { screen.normalize(); ball = Vector3f(screen[0], screen[1], 0.0f); } // otherwise we are on the protruding hemisphere else { float z = sqrtf(1.0f - lsqared); ball = Vector3f(screen[0], screen[1], z); } // return the ball vector, taking into account the camera's orientation return m_inverseCamera * ball; }
// produce a yaw error value. The returned value is proportional // to sin() of the current heading error in earth frame float AP_AHRS_DCM::yaw_error_compass(void) { const Vector3f &mag = _compass->get_field(); // get the mag vector in the earth frame Vector2f rb = _dcm_matrix.mulXY(mag); if (rb.length() < FLT_EPSILON) { return 0.0f; } rb.normalize(); if (rb.is_inf()) { // not a valid vector return 0.0f; } // update vector holding earths magnetic field (if required) if( !is_equal(_last_declination,_compass->get_declination()) ) { _last_declination = _compass->get_declination(); _mag_earth.x = cosf(_last_declination); _mag_earth.y = sinf(_last_declination); } // calculate the error term in earth frame // calculate the Z component of the cross product of rb and _mag_earth return rb % _mag_earth; }
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; } }
void ExampleExperimentFields::DrawVectorField() { viewer->clear(); //Load the vector field VectorField2 field; if (!field.load(VectorfieldFilename)) { output << "Error loading field file " << VectorfieldFilename << "\n"; return; } //Draw vector directions (constant length) for(float32 x=field.boundMin()[0]; x<=field.boundMax()[0]; x+=0.2) { for(float32 y=field.boundMin()[1]; y<=field.boundMax()[1]; y+=0.2) { Vector2f vec = field.sample(x,y); vec.normalize(); viewer->addLine(x, y, x + ArrowScale*vec[0], y + ArrowScale*vec[1]); } } viewer->refresh(); }
void PolygonBody::calculateNormals(std::vector<Vector2f>& outputNormals) const { for (size_t i = 0; i < _verticesWorldSpace.size(); ++i) { Vector2f edge = _verticesWorldSpace[i] - _verticesWorldSpace[(i + 1) % _verticesWorldSpace.size()]; edge.normalize(); outputNormals.push_back(edge.crossProduct(-1.0f)); } }
float getAngle(Vector2f v1, Vector2f v2) { auto l1 = v1.lengthSq(); auto l2 = v2.lengthSq(); // Make sure we don't divide by zero if (std::abs(l1) < EPSILON || std::abs(l2) < EPSILON) return 0; v1.normalize(); v2.normalize(); auto angle = std::acos(Vector2f::dot(v1, v2)); auto orientation = (v1.x * v2.y) - (v2.x * v1.y); if (orientation > 0) angle = -angle; // Radians to degrees return static_cast<float>(angle * (180.0 / M_PI)); }
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_dcm_matrix(); Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x); yawVect.normalize(); float gndSpdFwd = yawVect * gndVel; groundspeed_undershoot = (g.min_gndspeed_cm > 0) ? (g.min_gndspeed_cm - gndSpdFwd*100) : 0; } }
int PxController::bound(Vector2f &n, Vector2f &n2) { if(dbound_lock_cnt > DBOUNDCNT) { std::cout << "----double bound unlocked "<< std::endl; dbound_lock_cnt = 0; n += n2; n.normalize(); return bound(n); }else{ std::cout << "----double bound locked "<< std::endl; std::cout << " count:" << dbound_lock_cnt << std::endl; dbound_lock_cnt++; return 1; } }
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); }
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; } } }
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); }
bool Sprite::manageCollision(Sprite *obj) { if ( !strategy->execute(*this, *obj) ) return false; int widthSum = frame->getWidth()/2 + obj->getFrame()->getWidth()/2; if ( getDistance(obj) <= widthSum ) { Vector2f velocity = getVelocity() - obj->getVelocity(); Vector2f distance = getPosition() - obj->getPosition(); if ( velocity.dot(distance) < 0 ) { distance = distance.normalize(); Vector2f v1 = getVelocity(); v1 -= 2 * distance * getVelocity().dot(distance); setVelocity(v1); Vector2f v2 = obj->getVelocity(); v2 -= 2 * distance * obj->getVelocity().dot(distance); obj->setVelocity(v2); } // end of inner if } // end of outer if return true; }
//Calculates either 1 or 2 contact points for a collision depending on //how the polygons overlap std::vector<Vector2f> PolygonBody::calculateContactPoints(PolygonBody& polygon, const Vector2f& collisionNormal) const { Edge edgeA = calculateCollisionEdge(collisionNormal); Edge edgeB = polygon.calculateCollisionEdge(-collisionNormal); const Edge* referenceEdge = 0; const Edge* incidentEdge = 0; float dotA = (edgeA.end - edgeA.start).dotProduct(collisionNormal); float dotB = (edgeB.end - edgeB.start).dotProduct(collisionNormal); //The reference edge is the most perpendicular to the collision normal (dot product is closest to zero) if (fabsf(dotA) <= fabsf(dotB)) { referenceEdge = &edgeA; incidentEdge = &edgeB; } else { referenceEdge = &edgeB; incidentEdge = &edgeA; } Vector2f referenceVector = referenceEdge->end - referenceEdge->start; referenceVector.normalize(); std::vector<Vector2f> clippedPoints = clipPoints(incidentEdge->start, incidentEdge->end, referenceVector, referenceVector.dotProduct(referenceEdge->start)); if (clippedPoints.size() < 2) return std::vector<Vector2f>(); //something went wrong, return an empty vector clippedPoints = clipPoints(clippedPoints[0], clippedPoints[1], -referenceVector, -(referenceVector.dotProduct(referenceEdge->end))); if (clippedPoints.size() < 2) return std::vector<Vector2f>(); //something went wrong, return an empty vector Vector2f referenceNormal = referenceVector.crossProduct(-1.0f); float max = referenceNormal.dotProduct(referenceEdge->vertex); if (referenceNormal.dotProduct(clippedPoints[1]) > max) clippedPoints.erase(clippedPoints.begin() + 1); if (referenceNormal.dotProduct(clippedPoints[0]) > max) clippedPoints.erase(clippedPoints.begin()); return clippedPoints; }
void AssignmentFour::DrawVectorFieldHelper() { //Draw vector directions (constant length) for (float32 x = Field.boundMin()[0]; x <= Field.boundMax()[0]; x += 0.2) { for (float32 y = Field.boundMin()[1]; y <= Field.boundMax()[1]; y += 0.2) { Vector2f vec = Field.sample(x, y); if (NormalizeVectorField) { vec.normalize(); } float32 x2 = x + ArrowScale*vec[0]; float32 y2 = y + ArrowScale*vec[1]; viewer->addLine(x, y, x2, y2); if (ShowPoints) { viewer->addPoint(Point2D(x2, y2)); } } } }
// produce a yaw error value. The returned value is proportional // to sin() of the current heading error in earth frame float AP_AHRS_DCM::yaw_error_compass(void) { Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z); // get the mag vector in the earth frame Vector2f rb = _dcm_matrix.mulXY(mag); rb.normalize(); if (rb.is_inf()) { // not a valid vector return 0.0; } // update vector holding earths magnetic field (if required) if( _last_declination != _compass->get_declination() ) { _last_declination = _compass->get_declination(); _mag_earth.x = cosf(_last_declination); _mag_earth.y = sinf(_last_declination); } // calculate the error term in earth frame // calculate the Z component of the cross product of rb and _mag_earth return rb % _mag_earth; }
void CPlanetDot::update(float dt) { /** Assume circular orbit */ /** x^2 + y^2 = orbitRadius */ /** position = (cos(t), sin(t)) */ /* time +=dt * orbitSpeed;; mVecPosition.x = cosf(time); mVecPosition.y = sinf(time); */ float a = mVecVelocity.lengthSq() / this->orbitRadius; Vector2f aVec = mVecCenter - mVecPosition; aVec.normalize(); aVec *= a; mVecVelocity += aVec; if(mVecVelocity.length() > orbitSpeed) { mVecVelocity.normalize(); mVecVelocity *= orbitSpeed; } mVecPosition += mVecVelocity; }
void testNormalize() { v5.normalize(); CPPUNIT_ASSERT(std::abs(v5.length() - 1.0) < EPSILON); }
float scoreForWidget(GuiWidget const &widget, ui::Direction dir) const { if (!widget.canBeFocused() || &widget == thisPublic) { return -1; } Rectanglef const viewRect = self().root().viewRule().rect(); Rectanglef const selfRect = self().hitRule().rect(); Rectanglef const otherRect = widget.hitRule().rect(); Vector2f const otherMiddle = (dir == ui::Up? otherRect.midBottom() : dir == ui::Down? otherRect.midTop() : dir == ui::Left? otherRect.midRight() : otherRect.midLeft() ); //otherRect.middle(); if (!viewRect.contains(otherMiddle)) { return -1; } bool const axisOverlap = (isHorizontal(dir) && !selfRect.vertical() .intersection(otherRect.vertical()) .isEmpty()) || (isVertical(dir) && !selfRect.horizontal().intersection(otherRect.horizontal()).isEmpty()); // Check for contacting edges. float edgeDistance = 0; // valid only if axisOverlap if (axisOverlap) { switch (dir) { case ui::Left: edgeDistance = selfRect.left() - otherRect.right(); break; case ui::Up: edgeDistance = selfRect.top() - otherRect.bottom(); break; case ui::Right: edgeDistance = otherRect.left() - selfRect.right(); break; default: edgeDistance = otherRect.top() - selfRect.bottom(); break; } // Very close edges are considered contacting. if (edgeDistance >= 0 && edgeDistance < toDevicePixels(5)) { return edgeDistance; } } Vector2f const middle = (dir == ui::Up? selfRect.midTop() : dir == ui::Down? selfRect.midBottom() : dir == ui::Left? selfRect.midLeft() : selfRect.midRight() ); Vector2f const delta = otherMiddle - middle; Vector2f const dirVector = directionVector(dir); auto dotProd = delta.normalize().dot(dirVector); if (dotProd <= 0) { // On the wrong side. return -1; } float distance = delta.length(); if (axisOverlap) { dotProd = 1.0; if (edgeDistance > 0) { distance = de::min(distance, edgeDistance); } } float favorability = 1; if (widget.parentWidget() == self().parentWidget()) { favorability = .1f; // Siblings are much preferred. } else if (self().hasAncestor(widget) || widget.hasAncestor(self())) { favorability = .2f; // Ancestry is also good. } // Prefer widgets that are nearby, particularly in the specified direction. return distance * (.5f + acos(dotProd)) * favorability; }
Vector2f Vector2f::GetNormalized() { Vector2f temp = *this; temp.normalize(); return temp; }
Vector2f AssignmentFour::FieldValue(Vector2f xi) { StaticVector<float, 2U> vec = Field.sample(xi[0], xi[1]); Vector2f v = makeVector2f(vec[0], vec[1]); if (DirectionFieldOnly) v.normalize(); return IntegrateBackwards ? -v : v; }
//We use a modified Separating Axis Test to test for polygon-circle collisions bool PolygonBody::checkCollision(CircleBody& circle, Collision& collision) { float radiusSum = _radius + circle.radius(); if ((circle.position() - _position).lengthSquared() > radiusSum * radiusSum) return false; std::vector<Vector2f> axes; this->calculateNormals(axes); //We need the axis from the center of the circle to the closest vertex on the polygon Vector2f additionalAxis = _verticesWorldSpace[0] - circle.position(); for (size_t i = 1; i < _verticesWorldSpace.size(); ++i) { Vector2f v = _verticesWorldSpace[i] - circle.position(); if (v.lengthSquared() < additionalAxis.lengthSquared()) additionalAxis = v; } additionalAxis.normalize(); axes.push_back(additionalAxis); float minOverlap = std::numeric_limits<float>::max(); Vector2f minOverlapAxis; for (size_t i = 0; i < axes.size(); ++i) { Vector2f& axis = axes[i]; float projection = axis.dotProduct(_verticesWorldSpace.front()); float aMinProjection = projection; float aMaxProjection = projection; for (size_t j = 1; j < _verticesWorldSpace.size(); ++j) { projection = axis.dotProduct(_verticesWorldSpace[j]); aMinProjection = std::min(aMinProjection, projection); aMaxProjection = std::max(aMaxProjection, projection); } projection = axis.dotProduct(circle.position()); float bMinProjection = projection - circle.radius(); float bMaxProjection = projection + circle.radius(); if (bMinProjection > bMaxProjection) std::swap(bMinProjection, bMaxProjection); if ((aMaxProjection < bMinProjection) || (bMaxProjection < aMinProjection)) return false; //This axis has no overlap, a collision is not possible float lowerOverlap = aMaxProjection - bMinProjection; float upperOverlap = bMaxProjection - aMinProjection; if (lowerOverlap < minOverlap) { minOverlap = lowerOverlap; minOverlapAxis = axis; } if (upperOverlap < minOverlap) { minOverlap = upperOverlap; minOverlapAxis = -axis; } } collision.bodyA = this; collision.bodyB = &circle; collision.normal = minOverlapAxis; collision.depth = minOverlap; collision.contactPoints.push_back(circle.position() - collision.normal * (circle.radius() - collision.depth)); return true; }
// update L1 control for waypoint navigation // this function costs about 3.5 milliseconds on a AVR2560 void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP) { struct Location _current_loc; float Nu; float xtrackVel; float ltrackVel; // Calculate L1 gain required for specified damping float K_L1 = 4.0f * _L1_damping * _L1_damping; // Get current position and velocity _ahrs.get_position(_current_loc); Vector2f _groundspeed_vector = _ahrs.groundspeed_vector(); // update _target_bearing_cd _target_bearing_cd = get_bearing_cd(_current_loc, next_WP); //Calculate groundspeed float groundSpeed = _groundspeed_vector.length(); if (groundSpeed < 0.1f) { // use a small ground speed vector in the right direction, // allowing us to use the compass heading at zero GPS velocity groundSpeed = 0.1f; _groundspeed_vector = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw)) * groundSpeed; } // Calculate time varying control parameters // Calculate the L1 length required for specified period // 0.3183099 = 1/1/pipi _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed; // Calculate the NE position of WP B relative to WP A Vector2f AB = location_diff(prev_WP, next_WP); // Check for AB zero length and track directly to the destination // if too small if (AB.length() < 1.0e-6f) { AB = location_diff(_current_loc, next_WP); } AB.normalize(); // Calculate the NE position of the aircraft relative to WP A Vector2f A_air = location_diff(prev_WP, _current_loc); // calculate distance to target track, for reporting _crosstrack_error = AB % A_air; //Determine if the aircraft is behind a +-135 degree degree arc centred on WP A //and further than L1 distance from WP A. Then use WP A as the L1 reference point //Otherwise do normal L1 guidance float WP_A_dist = A_air.length(); float alongTrackDist = A_air * AB; if (WP_A_dist > _L1_dist && alongTrackDist/max(WP_A_dist, 1.0f) < -0.7071f) { //Calc Nu to fly To WP A Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line Nu = atan2f(xtrackVel,ltrackVel); _prevent_indecision(Nu); _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point } else { //Calc Nu to fly along AB line //Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints) xtrackVel = _groundspeed_vector % AB; // Velocity cross track ltrackVel = _groundspeed_vector * AB; // Velocity along track float Nu2 = atan2f(xtrackVel,ltrackVel); //Calculate Nu1 angle (Angle to L1 reference point) float xtrackErr = A_air % AB; float sine_Nu1 = xtrackErr/max(_L1_dist, 0.1f); //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f); float Nu1 = asinf(sine_Nu1); Nu = Nu1 + Nu2; _nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point } _last_Nu = Nu; //Limit Nu to +-pi Nu = constrain_float(Nu, -1.5708f, +1.5708f); _latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu); // Waypoint capture status is always false during waypoint following _WPcircle = false; _bearing_error = Nu; // bearing error angle (radians), +ve to left of track }
// update L1 control for waypoint navigation void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP) { struct Location _current_loc; float Nu; float xtrackVel; float ltrackVel; uint32_t now = AP_HAL::micros(); float dt = (now - _last_update_waypoint_us) * 1.0e-6f; if (dt > 0.1) { dt = 0.1; } _last_update_waypoint_us = now; // Calculate L1 gain required for specified damping float K_L1 = 4.0f * _L1_damping * _L1_damping; // Get current position and velocity _ahrs.get_position(_current_loc); Vector2f _groundspeed_vector = _ahrs.groundspeed_vector(); // update _target_bearing_cd _target_bearing_cd = get_bearing_cd(_current_loc, next_WP); //Calculate groundspeed float groundSpeed = _groundspeed_vector.length(); if (groundSpeed < 0.1f) { // use a small ground speed vector in the right direction, // allowing us to use the compass heading at zero GPS velocity groundSpeed = 0.1f; _groundspeed_vector = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw)) * groundSpeed; } // Calculate time varying control parameters // Calculate the L1 length required for specified period // 0.3183099 = 1/1/pipi _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed; // Calculate the NE position of WP B relative to WP A Vector2f AB = location_diff(prev_WP, next_WP); // Check for AB zero length and track directly to the destination // if too small if (AB.length() < 1.0e-6f) { AB = location_diff(_current_loc, next_WP); if (AB.length() < 1.0e-6f) { AB = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw)); } } AB.normalize(); // Calculate the NE position of the aircraft relative to WP A Vector2f A_air = location_diff(prev_WP, _current_loc); // calculate distance to target track, for reporting _crosstrack_error = A_air % AB; //Determine if the aircraft is behind a +-135 degree degree arc centred on WP A //and further than L1 distance from WP A. Then use WP A as the L1 reference point //Otherwise do normal L1 guidance float WP_A_dist = A_air.length(); float alongTrackDist = A_air * AB; if (WP_A_dist > _L1_dist && alongTrackDist/MAX(WP_A_dist, 1.0f) < -0.7071f) { //Calc Nu to fly To WP A Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line Nu = atan2f(xtrackVel,ltrackVel); _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point } else { //Calc Nu to fly along AB line //Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints) xtrackVel = _groundspeed_vector % AB; // Velocity cross track ltrackVel = _groundspeed_vector * AB; // Velocity along track float Nu2 = atan2f(xtrackVel,ltrackVel); //Calculate Nu1 angle (Angle to L1 reference point) float sine_Nu1 = _crosstrack_error/MAX(_L1_dist, 0.1f); //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f); float Nu1 = asinf(sine_Nu1); // compute integral error component to converge to a crosstrack of zero when traveling // straight but reset it when disabled or if it changes. That allows for much easier // tuning by having it re-converge each time it changes. if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain, _L1_xtrack_i_gain_prev)) { _L1_xtrack_i = 0; _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain; } else if (fabsf(Nu1) < radians(5)) { _L1_xtrack_i += Nu1 * _L1_xtrack_i_gain * dt; // an AHRS_TRIM_X=0.1 will drift to about 0.08 so 0.1 is a good worst-case to clip at _L1_xtrack_i = constrain_float(_L1_xtrack_i, -0.1f, 0.1f); } // to converge to zero we must push Nu1 harder Nu1 += _L1_xtrack_i; Nu = Nu1 + Nu2; _nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point } _prevent_indecision(Nu); _last_Nu = Nu; //Limit Nu to +-pi Nu = constrain_float(Nu, -1.5708f, +1.5708f); _latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu); // Waypoint capture status is always false during waypoint following _WPcircle = false; _bearing_error = Nu; // bearing error angle (radians), +ve to left of track }
// rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target rate in radians/second void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(const Vector3f &rate_rads, float rate_roll_target_rads, float rate_pitch_target_rads) { float roll_pd, roll_i, roll_ff; // used to capture pid values float pitch_pd, pitch_i, pitch_ff; // used to capture pid values float rate_roll_error_rads, rate_pitch_error_rads; // simply target_rate - current_rate float roll_out, pitch_out; // calculate error rate_roll_error_rads = rate_roll_target_rads - rate_rads.x; rate_pitch_error_rads = rate_pitch_target_rads - rate_rads.y; // pass error to PID controller _pid_rate_roll.set_input_filter_all(rate_roll_error_rads); _pid_rate_roll.set_desired_rate(rate_roll_target_rads); _pid_rate_pitch.set_input_filter_all(rate_pitch_error_rads); _pid_rate_pitch.set_desired_rate(rate_pitch_target_rads); // call p and d controllers roll_pd = _pid_rate_roll.get_p() + _pid_rate_roll.get_d(); pitch_pd = _pid_rate_pitch.get_p() + _pid_rate_pitch.get_d(); // get roll i term roll_i = _pid_rate_roll.get_integrator(); // update i term as long as we haven't breached the limits or the I term will certainly reduce if (!_flags_heli.limit_roll || ((roll_i>0&&rate_roll_error_rads<0)||(roll_i<0&&rate_roll_error_rads>0))){ if (_flags_heli.leaky_i){ roll_i = _pid_rate_roll.get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); }else{ roll_i = _pid_rate_roll.get_i(); } } // get pitch i term pitch_i = _pid_rate_pitch.get_integrator(); // update i term as long as we haven't breached the limits or the I term will certainly reduce if (!_flags_heli.limit_pitch || ((pitch_i>0&&rate_pitch_error_rads<0)||(pitch_i<0&&rate_pitch_error_rads>0))){ if (_flags_heli.leaky_i) { pitch_i = _pid_rate_pitch.get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); }else{ pitch_i = _pid_rate_pitch.get_i(); } } // For legacy reasons, we convert to centi-degrees before inputting to the feedforward roll_ff = roll_feedforward_filter.apply(_pid_rate_roll.get_ff(rate_roll_target_rads), _dt); pitch_ff = pitch_feedforward_filter.apply(_pid_rate_pitch.get_ff(rate_pitch_target_rads), _dt); // add feed forward and final output roll_out = roll_pd + roll_i + roll_ff; pitch_out = pitch_pd + pitch_i + pitch_ff; // constrain output and update limit flags if (fabsf(roll_out) > AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX) { roll_out = constrain_float(roll_out,-AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX,AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX); _flags_heli.limit_roll = true; }else{ _flags_heli.limit_roll = false; } if (fabsf(pitch_out) > AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX) { pitch_out = constrain_float(pitch_out,-AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX,AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX); _flags_heli.limit_pitch = true; }else{ _flags_heli.limit_pitch = false; } // output to motors _motors.set_roll(roll_out); _motors.set_pitch(pitch_out); // Piro-Comp, or Pirouette Compensation is a pre-compensation calculation, which basically rotates the Roll and Pitch Rate I-terms as the // helicopter rotates in yaw. Much of the built-up I-term is needed to tip the disk into the incoming wind. Fast yawing can create an instability // as the built-up I-term in one axis must be reduced, while the other increases. This helps solve that by rotating the I-terms before the error occurs. // It does assume that the rotor aerodynamics and mechanics are essentially symmetrical about the main shaft, which is a generally valid assumption. if (_piro_comp_enabled){ int32_t piro_roll_i, piro_pitch_i; // used to hold I-terms while doing piro comp piro_roll_i = roll_i; piro_pitch_i = pitch_i; Vector2f yawratevector; yawratevector.x = cosf(-rate_rads.z * _dt); yawratevector.y = sinf(-rate_rads.z * _dt); yawratevector.normalize(); roll_i = piro_roll_i * yawratevector.x - piro_pitch_i * yawratevector.y; pitch_i = piro_pitch_i * yawratevector.x + piro_roll_i * yawratevector.y; _pid_rate_pitch.set_integrator(pitch_i); _pid_rate_roll.set_integrator(roll_i); } }
void NPCFreefall_DZ::onUpdate(float dt) { // obtain spinal cord SpinalCord* spinalCord = getNPC()->getSpinalCord(); spinalCord->reset(); // update target position Matrix4f catToyPose = getNPC()->getCatToy()->getCurrentPose(); _targetPos.set( catToyPose[3][0], catToyPose[3][1], catToyPose[3][2] ); // jump only when the wind is right Vector2f pos = Vector2f(catToyPose[3][0], catToyPose[3][2]); pos.normalize(); NxVec3 wind = getNPC()->getJumper()->getScene()->getWindAtPoint(NxVec3(catToyPose[3][0], catToyPose[3][1], catToyPose[3][2])); wind.normalize(); Vector2f wind2(wind.x, wind.z); float wind_angle = 0.0; if (wind.magnitude() < 1.5f) { wind_angle = 1.0f; } else { wind2.normalize(); wind_angle = pos.dot(wind2); } // if my character is still roaming if (wind_angle >= 0.8f && catToyPose[3][1] >= 300.0f) _timeUntilJump -= dt; //getCore()->logMessage("%s has %2.4f seconds to jump", getNPC()->getNPCName(), _timeUntilJump); if( _timeUntilJump <= 0.0f && getNPC()->getJumper()->getPhase() == ::jpRoaming ) { Vector3f pos = Vector3f(catToyPose[3][0], catToyPose[3][1], catToyPose[3][2]); // if npc on airplane if( getNPC()->getJumper()->getAirplane() != NULL ) { // just jump! spinalCord->phase = true; return; } // if npc at the exit point and ready to jump else if( _positionIsSucceed && _directionIsSucceed ) { // jump! spinalCord->phase = true; return; } // if npc at the exit point and not surely ready to clear jump else { if( !_positionIsSucceed ) { // move to abyss Matrix4f jumpPose = getNPC()->getCatToy()->getJumpPose(); Vector3f jumpPos( jumpPose[3][0], jumpPose[3][1], jumpPose[3][2] ); call( new NPCMove( getNPC(), jumpPos ) ); _positionIsSucceed = true; return; } if( !_directionIsSucceed ) { // jumper absolute orientation Vector3f jumperAt = getNPC()->getJumper()->getClump()->getFrame()->getAt(); jumperAt.normalize(); // direction of cat toy jump Matrix4f jumpPose = getNPC()->getCatToy()->getJumpPose(); Vector3f targetDir( jumpPose[2][0], 0.0f, jumpPose[2][2] ); targetDir.normalize(); // angle to target Vector3f atH = jumperAt; atH[1] = 0; atH.normalize(); Vector3f dirH = targetDir; dirH[1] = 0; dirH.normalize(); float targetAngle = ::calcAngle( dirH, atH, Vector3f( 0,1,0 ) ); if( fabs( targetAngle ) < 1.0f ) { _directionIsSucceed = true; return; } else { // turn jumper by AI algo float aiRotationVel = 180.0f; float aiRotationAngle = sgn( targetAngle ) * aiRotationVel * dt; if( fabs( aiRotationAngle ) > fabs( targetAngle ) ) aiRotationAngle = targetAngle; getNPC()->getJumper()->getClump()->getFrame()->rotateRelative( Vector3f( 0,1,0 ), aiRotationAngle ); } } } } // else, turn & track towards the target else if( getNPC()->getJumper()->getPhase() == ::jpFreeFalling ) { // jumper absolute orientation Vector3f jumperAt = getNPC()->getJumper()->getClump()->getFrame()->getAt(); jumperAt.normalize(); Vector3f jumperRight = getNPC()->getJumper()->getClump()->getFrame()->getRight(); jumperRight.normalize(); Vector3f jumperUp = getNPC()->getJumper()->getClump()->getFrame()->getUp(); jumperUp.normalize(); // retrieve current jumper position Vector3f jumperPos = getNPC()->getJumper()->getClump()->getFrame()->getPos(); jumperPos += Vector3f( 0, jumperRoamingSphereSize, 0 ); // wingsuit bool wingsuit = database::Suit::getRecord(getNPC()->getJumper()->getVirtues()->equipment.suit.id)->wingsuit; // leveling // wlo - when npc higher //if (!wingsuit) { // if (jumperPos[1] - _targetPos[1] > 1500.0f) { // spinalCord->wlo = true; // } else if (jumperPos[1] - _targetPos[1] < -1500.0f) { // spinalCord->hook = true; // } //} // direction to target Vector3f targetDir = _targetPos - jumperPos; float distanceToTarget = Vector3f( targetDir[0], 0, targetDir[2] ).length(); targetDir.normalize(); // landing too far away? let's deploy in order to return safely // glide ratio is based on canopy size database::Canopy *canopy = database::Canopy::getRecord(getNPC()->getJumper()->getVirtues()->equipment.canopy.id); Vector3f pos = getNPC()->getJumper()->getClump()->getFrame()->getPos(); float glide = canopy->square / 150.0f; float alt = pos[1]; pos[1] = 0; bool farEnough = pos.length() >= alt*glide; //getCore()->logMessage("alt: %2.5f; dst: %2.5f; coverage: %2.5f", alt, pos.length(), alt*glide); // deploy now? if ((alt <= 85000.0f || farEnough) && alt <= 150000.0f) { if (getNPC()->getJumper()->getCanopySimulator()) { //getCore()->logMessage("npc pull. Far enough: %d", (int)farEnough); spinalCord->phase = true; spinalCord->modifier = wingsuit; } else { //getCore()->logMessage("npc pull reserve. Far enough: %d", (int)farEnough); spinalCord->pullReserve = true; spinalCord->modifier = wingsuit; } } // if toy tracking modifier is on if( wingsuit /* getNPC()->getCatToy()->getModifier()*/ ) { // sum up target direction Vector3f targetFlatAt( getNPC()->getCatToy()->getCurrentPose()[2][0], 0, getNPC()->getCatToy()->getCurrentPose()[2][2] ); /*getCore()->logMessage( "POS: %3.2f %3.2f %3.2f AT : %3.2f 0.0 %3.2f", getNPC()->getCatToy()->getCurrentPose()[3][0], getNPC()->getCatToy()->getCurrentPose()[3][1], getNPC()->getCatToy()->getCurrentPose()[3][2], targetFlatAt[0], targetFlatAt[2] );*/ targetFlatAt.normalize(); targetDir += targetFlatAt * 3; targetDir.normalize(); } // horizontal steering float minAngle = 5.0f; float minValue = 0.0f; float maxAngle = 45.0f; float maxValue = 1.0f; // angle to target Vector3f atH = jumperAt; // headdown if (getNPC()->getJumper()->getDefaultPose() == 1) { atH = jumperUp; minAngle = 25.0f; maxValue = 0.1f; // sitfly } else if (getNPC()->getJumper()->getDefaultPose() == 2) { atH = jumperUp; atH[2] *= -1; //minAngle = 25.0f; maxValue = 0.1f; } atH[1] = 0; atH.normalize(); Vector3f dirH = targetDir; dirH[1] = 0; dirH.normalize(); float targetAngle = ::calcAngle( dirH, atH, Vector3f( 0,1,0 ) ); // inclination angle relative to the horizont float horizontalAngle = -1 * ::calcAngle( atH, jumperUp, jumperRight ); // horizontal steering float factor = ( fabs( targetAngle ) - minAngle ) / ( maxAngle - minAngle ); factor = ( factor < 0 ) ? 0 : ( ( factor > 1 ) ? 1 : factor ); float impulse = minValue * ( 1 - factor ) + maxValue * factor; if (wingsuit) impulse = 0.0f; // smooth impulse impulse = pow( impulse, 1.25f ); if (alt <= 130000.0f) impulse = 0.0f; // apply impulse if( targetAngle < 0 ) { spinalCord->right = impulse; } else { spinalCord->left = impulse; } // relation btw desired inclination and target angle minAngle = 5.0f; minValue = 25.0f; maxAngle = 90.0f; maxValue = 0.0f; factor = ( fabs( targetAngle ) - minAngle ) / ( maxAngle - minAngle ); factor = ( factor < 0 ) ? 0 : ( ( factor > 1 ) ? 1 : factor ); float desiredInclination = minValue * ( 1 - factor ) + maxValue * factor; // inclination difference float inclinationDifference = desiredInclination - horizontalAngle; // vertical steering minAngle = 0.0f; minValue = 0.0f; maxAngle = 30.0f; maxValue = 1.0f; factor = ( fabs( inclinationDifference ) - minAngle ) / ( maxAngle - minAngle ); factor = ( factor < 0 ) ? 0 : ( ( factor > 1 ) ? 1 : factor ); impulse = minValue * ( 1 - factor ) + maxValue * factor; if (!wingsuit && distanceToTarget > 2500.0f && alt > 130000.0f) { if( inclinationDifference < 0 ) { spinalCord->down += impulse; if (spinalCord->down > 1.0f) spinalCord->down = 1.0f; } else { spinalCord->up += impulse; if (spinalCord->up > 1.0f) spinalCord->up = 1.0f; } } // tracking modifier if( fabs( targetAngle ) < 90.0f && distanceToTarget > 2500.0f ) { spinalCord->modifier = 0.0f; spinalCord->trigger_modifier = false; } else { spinalCord->modifier = 0.0f; spinalCord->trigger_modifier = false; } if (wingsuit || alt <= 130000.0f) spinalCord->modifier = 1.0f; if (wingsuit || alt <= 130000.0f) spinalCord->trigger_modifier = true; return; // force tracking in several condition if( getNPC()->getCatToy()->getModifier() ) { // not a skydiving? if( !getNPC()->getJumper()->getCanopySimulator()->getGearRecord()->skydiving ) { // imitate cat toy modifier spinalCord->modifier = getNPC()->getCatToy()->getModifier(); } else { Vector3f cattoyFaceH( catToyPose[2][0], 0, catToyPose[2][2] ); cattoyFaceH.normalize(); Vector3f npcFaceH = getNPC()->getJumper()->getClump()->getFrame()->getAt(); npcFaceH[1] = 0; npcFaceH.normalize(); //if( Vector3f::dot( dirH, cattoyFaceH ) > 0.85f ) spinalCord->modifier = true; } } } }