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;
}
}
}
}
