void
EdgePosition::advance(Vector2f& adv, Node*& next_node)
{
// FIXME: This might be optimizable
Vector2f p1 = edge->get_node1()->get_pos();
Vector2f p2 = edge->get_node2()->get_pos();
Vector2f edge_v = p2 - p1;
Vector2f proj = adv.project(edge_v);
float angle = atan2f(edge_v.y, edge_v.x) - atan2f(proj.y, proj.x);
// Check if we are going forward or backward
float advf;
if (angle > M_PI/2 || angle < -M_PI/2)
advf = -proj.length();
else
advf = proj.length();
// Move forward
advance(advf, next_node);
// Calculate the rest Vector2f
// Calculate the rest Vector2f
if (advf == 0.0f)
adv = Vector2f(0,0);
else
adv -= (proj * ((proj.length() - advf)/proj.length()));
}
void Cannon::aimAtPoint(float timeElapsed, Vector2f pos, Vector2f vel)
{
float myPosX = myStation->getPos().x + ((cos(myStation->getRotation()*M_PI/180)*position.x)-(sin(myStation->getRotation()*M_PI/180)*position.y));
float myPosY = myStation->getPos().y + ((sin(myStation->getRotation()*M_PI/180)*position.x)+(cos(myStation->getRotation()*M_PI/180)*position.y));
Vector2f distBetween = Vector2f(pos.x-myPosX,pos.y-myPosY);
float time = 826.0f/distBetween.length();
Vector2f fDist = (myStation->getVelocity()/time)*-1;
Vector2f fDist2 = (vel/time);
float rot = rotation;
if(rot < 0)
{
rot+=360;
}
if(rot>=360)
{
rot -= 360;
}
float dir = 0;
distBetween = Vector2f((pos.x+fDist.x+fDist2.x)-(myPosX),(pos.y+fDist.y+fDist2.y)-(myPosY));
distBetween /= distBetween.length();
dir = ( ( SDL_cos((rot)*M_PI/180)*180/M_PI) * (distBetween.y) - ( SDL_sin((rot)*M_PI/180)*180/M_PI) * (distBetween.x));
if(dir <-2)
{
rotation -=20*timeElapsed;
}
else if(dir > 2)
{
rotation += 20*timeElapsed;
}
if( dir <=2 && dir >= -2)
{
action(timeElapsed);
}
}
// 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;
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
ofDataNew.time_ms = roundToNearest(ofDataNew.time_ms, frontend->fusionTimeStep_ms);
// 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
StoreOF();
// Check for data at the fusion time horizon
newDataFlow = RecallOF();
}
}
void FlightTaskAuto::_set_heading_from_mode()
{
Vector2f v; // Vector that points towards desired location
switch (MPC_YAW_MODE.get()) {
case 0: // Heading points towards the current waypoint.
v = Vector2f(_target) - Vector2f(_position);
break;
case 1: // Heading points towards home.
if (_sub_home_position->get().valid_hpos) {
v = Vector2f(&_sub_home_position->get().x) - Vector2f(_position);
}
break;
case 2: // Heading point away from home.
if (_sub_home_position->get().valid_hpos) {
v = Vector2f(_position) - Vector2f(&_sub_home_position->get().x);
}
break;
case 3: // Along trajectory.
// The heading depends on the kind of setpoint generation. This needs to be implemented
// in the subclasses where the velocity setpoints are generated.
v.setAll(NAN);
break;
}
if (PX4_ISFINITE(v.length())) {
// We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance
// radius, lock yaw to current yaw.
// This prevents excessive yawing.
if (v.length() > _target_acceptance_radius) {
_compute_heading_from_2D_vector(_yaw_setpoint, v);
_yaw_lock = false;
} else {
if (!_yaw_lock) {
_yaw_setpoint = _yaw;
_yaw_lock = true;
}
}
} else {
_yaw_lock = false;
_yaw_setpoint = NAN;
}
}
// 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;
}
// by definition if this function is called, then flow measurements have been provided so we
// need to run the optical flow takeoff detection
detectOptFlowTakeoff();
// calculate rotation matrices at mid sample time for flow observations
stateStruct.quat.rotation_matrix(Tbn_flow);
// 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);
}
}
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;
}
// calculate vehicle stopping point using current location, velocity and maximum acceleration
bool AR_WPNav::get_stopping_location(Location& stopping_loc)
{
Location current_loc;
if (!AP::ahrs().get_position(current_loc)) {
return false;
}
// get current velocity vector and speed
const Vector2f velocity = AP::ahrs().groundspeed_vector();
const float speed = velocity.length();
// avoid divide by zero
if (!is_positive(speed)) {
stopping_loc = current_loc;
return true;
}
// get stopping distance in meters
const float stopping_dist = _atc.get_stopping_distance(speed);
// calculate stopping position from current location in meters
const Vector2f stopping_offset = velocity.normalized() * stopping_dist;
stopping_loc = current_loc;
stopping_loc.offset(stopping_offset.x, stopping_offset.y);
return true;
}
// 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;
}
bool Bullet::outOfBounds() {
Vector2f camv = Vector2f();
camv.x = cam.getX();
camv.y = cam.getY();
Vector2f diff = getShortestDiffVectorBullet(camv, position);
return diff.length() > CHUNK_SIZE * 1.4;
}
void RigidBody::addForce(Vector2f _force, Vector2f point) {
Vector2f dir = (point - position);
forces += _force;
aTorque += dir.length() * _force.scalarProjectAt(dir.rightNormal());
}
void Cannon::crewControl(float timeElapsed,vector<Entity*> *nightmares)
{
float dist = 2000*((Unit*)controller)->getHatStrength();
float num = -1;
for(int j = 0; j < nightmares->size(); j++)
{
if(((Station*)nightmares->at(j))!=myStation)
{
if(((Station*)nightmares->at(j))->getAssignedUnits()->size()>0&&((Station*)nightmares->at(j))->getAssignedCrew()->size()==0)
{
if(num==-1)
{
Vector2f ePos = ((Station*)nightmares->at(j))->getPos();
float myPosX = myStation->getPos().x + ((cos(myStation->getRotation()*M_PI/180)*position.x)-(sin(myStation->getRotation()*M_PI/180)*position.y));
float myPosY = myStation->getPos().y + ((sin(myStation->getRotation()*M_PI/180)*position.x)+(cos(myStation->getRotation()*M_PI/180)*position.y));
Vector2f distBetween = Vector2f(ePos.x-myPosX,ePos.y-myPosY);
if(distBetween.length()<dist)
{
num=j;
dist=distBetween.length();
}
}
else
{
Vector2f ePos = ((Station*)nightmares->at(j))->getPos();
float myPosX = myStation->getPos().x + ((cos(myStation->getRotation()*M_PI/180)*position.x)-(sin(myStation->getRotation()*M_PI/180)*position.y));
float myPosY = myStation->getPos().y + ((sin(myStation->getRotation()*M_PI/180)*position.x)+(cos(myStation->getRotation()*M_PI/180)*position.y));
Vector2f distBetween = Vector2f(ePos.x-myPosX,ePos.y-myPosY);
if(distBetween.length()<dist)
{
num=j;
dist=distBetween.length();
}
}
}
}
}
if(num != -1)
{
Vector2f ePos = ((Station*)nightmares->at(num))->getPos();
Vector2f vel = ((Station*)nightmares->at(num))->getVelocity();
aimAtPoint(timeElapsed,ePos,vel);
}
}
void CPlayerDot::update(float dt, vector<CPlanetDot*> * planetList)
{
if(isGravityOn)
{
for(vector<CPlanetDot*>::iterator it = planetList->begin(); it != planetList->end(); it++)
{
/** (*it) is the planet dot */
CPlanetDot * pPlanet = (CPlanetDot*)(*it);
/** distance = planet.position - myposition */
Vector2f distanceVector = pPlanet->mVecPosition - this->mVecPosition + pPlanet->radius + this->radius;
float distance = distanceVector.length();
/** Lets say the planet is 500 units away from us or less */
//if(distance < 5000)
{
/** force = distanceVector * planetRadius / (distance squared) */
double G = 6.67e-11;
//Vector2f forceVector = distanceVector * pPlanet->radius / (distance * distance);
float force = (G*((pPlanet->mass * this->mass)/(distance*distance)));
//distanceVector.normalize();
Vector2f forceVector = distanceVector * force;
/** Make the vector of length 1 */
//forceVector.normalize();
mVecAcceleration += forceVector;
}
}
//if(mVecAcceleration.length() > 0.0001)
//mVecAcceleration.normalize();
}
else
{
}
/** try not to delete the stuff below */
/** add acceleration impulse to velocity */
mVecVelocity += mVecAcceleration;
/** if velocity > our maximum velocity, then normalize the velocity vector making it length
/** 1, and then multiply it by our max speed making it equal to the max speed */
if(mVecVelocity.length() > maxSpeed)
{
mVecVelocity.normalize();
mVecVelocity *= maxSpeed;
}
/** add velocity to position to update position */
mVecPosition += mVecVelocity*dt;
/** reset acceleration impulse */
mVecAcceleration.x = 0;
mVecAcceleration.y = 0;
}
void AP_OSD_Screen::draw_speed_vector(uint8_t x, uint8_t y,Vector2f v, int32_t yaw)
{
float v_length = v.length();
char arrow = SYM_ARROW_START;
if (v_length > 1.0f) {
int32_t angle = wrap_360_cd(DEGX100 * atan2f(v.y, v.x) - yaw);
int32_t interval = 36000 / SYM_ARROW_COUNT;
arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT;
}
backend->write(x, y, false, "%c%3d%c", arrow, (int)u_scale(SPEED, v_length), u_icon(SPEED));
}
void AP_OSD_Screen::draw_eff(uint8_t x, uint8_t y)
{
AP_BattMonitor &battery = AP::battery();
AP_AHRS &ahrs = AP::ahrs();
Vector2f v = ahrs.groundspeed_vector();
float speed = u_scale(SPEED,v.length());
if (speed > 2.0){
backend->write(x, y, false, "%c%3d%c", SYM_EFF,int(1000*battery.current_amps()/speed),SYM_MAH);
} else {
backend->write(x, y, false, "%c---%c", SYM_EFF,SYM_MAH);
}
}
/*
* Adjusts the desired velocity for the circular fence.
*/
void AC_Avoid::adjust_velocity_circle(const float kP, const float accel_cmss, Vector2f &desired_vel)
{
// exit if circular fence is not enabled
if ((_fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) == 0) {
return;
}
// exit if the circular fence has already been breached
if ((_fence.get_breaches() & AC_FENCE_TYPE_CIRCLE) != 0) {
return;
}
// get position as a 2D offset in cm from ahrs home
const Vector2f position_xy = get_position();
float speed = desired_vel.length();
// get the fence radius in cm
const float fence_radius = _fence.get_radius() * 100.0f;
// get the margin to the fence in cm
const float margin = get_margin();
if (!is_zero(speed) && position_xy.length() <= fence_radius) {
// Currently inside circular fence
Vector2f stopping_point = position_xy + desired_vel*(get_stopping_distance(kP, accel_cmss, speed)/speed);
float stopping_point_length = stopping_point.length();
if (stopping_point_length > fence_radius - margin) {
// Unsafe desired velocity - will not be able to stop before fence breach
// Project stopping point radially onto fence boundary
// Adjusted velocity will point towards this projected point at a safe speed
Vector2f target = stopping_point * ((fence_radius - margin) / stopping_point_length);
Vector2f target_direction = target - position_xy;
float distance_to_target = target_direction.length();
float max_speed = get_max_speed(kP, accel_cmss, distance_to_target);
desired_vel = target_direction * (MIN(speed,max_speed) / distance_to_target);
}
}
}
bool AC_Fence::check_fence_circle()
{
if (!(_enabled_fences & AC_FENCE_TYPE_CIRCLE)) {
// not enabled; no breach
return false;
}
Vector2f home;
if (_ahrs.get_relative_position_NE_home(home)) {
// we (may) remain breached if we can't update home
_home_distance = home.length();
}
// check if we are outside the fence
if (_home_distance >= _circle_radius) {
// record distance outside the fence
_circle_breach_distance = _home_distance - _circle_radius;
// check for a new breach or a breach of the backup fence
if (!(_breached_fences & AC_FENCE_TYPE_CIRCLE) ||
(!is_zero(_circle_radius_backup) && _home_distance >= _circle_radius_backup)) {
// new breach
// create a backup fence 20m further out
record_breach(AC_FENCE_TYPE_CIRCLE);
_circle_radius_backup = _home_distance + AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE;
return true;
}
return false;
}
// not currently breached
// clear circle breach if present
if (_breached_fences & AC_FENCE_TYPE_CIRCLE) {
clear_breach(AC_FENCE_TYPE_CIRCLE);
_circle_radius_backup = 0.0f;
_circle_breach_distance = 0.0f;
}
return false;
}
void ChannelBase<Vector2f>::set(Vector2f newValue) {
if (isDigital) {
newValue.x = newValue.x >= actPoint ? 1.0f : 0.0f;
newValue.y = newValue.y >= actPoint ? 1.0f : 0.0f;
} else {
float length = newValue.length();
if (deadZone > 0) {
if (length <= deadZone) {
newValue = Vector2f::ZERO;
} else {
newValue *= (length - deadZone) / (length * (1 - deadZone));
}
}
if (hasBound) {
newValue.x = newValue.x >= bound ? 1.0f : newValue.x;
newValue.y = newValue.y >= bound ? 1.0f : newValue.y;
newValue.x = newValue.x <= -bound ? -1.0f : newValue.x;
newValue.y = newValue.y <= -bound ? -1.0f : newValue.y;
}
newValue *= sensitivity;
}
if (alwaysNotifyListener or !newValue.fuzzyEqual(value, 0.0001)) {
value = newValue;
if (not listeners.empty()) {
notifyListeners();
}
} else {
return;
}
if (false) {
value = Vector2f(0.0f);
}
}
// calculate vehicle stopping point using current location, velocity and maximum acceleration
void Mode::calc_stopping_location(Location& stopping_loc)
{
// default stopping location
stopping_loc = rover.current_loc;
// get current velocity vector and speed
const Vector2f velocity = ahrs.groundspeed_vector();
const float speed = velocity.length();
// avoid divide by zero
if (!is_positive(speed)) {
stopping_loc = rover.current_loc;
return;
}
// get stopping distance in meters
const float stopping_dist = attitude_control.get_stopping_distance(speed);
// calculate stopping position from current location in meters
const Vector2f stopping_offset = velocity.normalized() * stopping_dist;
location_offset(stopping_loc, stopping_offset.x, stopping_offset.y);
}
void ParticleState::updateParticle(
Particle &particle
)
{
Vector2f vel = particle.m_state.m_velocity;
float speed = vel.length();
particle.m_position += vel;
float alpha = min(1.0f, min(particle.m_percentLife * 2, speed * 1.0f));
alpha *= alpha;
particle.m_color.a = alpha;
if(particle.m_state.m_type == kBullet)
{
particle.m_scale.x = particle.m_state.m_lengthMultiplier
* min(min(1.0f, 0.1f * speed + 0.1f), alpha);
}
else
{
particle.m_scale.x = particle.m_state.m_lengthMultiplier
* min(min(1.0f, 0.2f * speed + 0.1f), alpha);
}
particle.m_orientation = Extensions::toAngle(vel);
Vector2f pos = particle.m_position;
int width = (int) constants::WINDOW_WIDTH;
int height = (int) constants::WINDOW_HEIGHT;
if(pos.x < 0)
{
vel.x = (float) fabs(vel.x);
}
else if(pos.x > width)
{
vel.x = (float) -fabs(vel.x);
}
if(pos.y < 0)
{
vel.y = (float) fabs(vel.y);
}
else if(pos.x > width)
{
vel.y = (float) -fabs(vel.y);
}
if(particle.m_state.m_type != kIgnoreGravity)
{
for(std::list<BlackHole*>::iterator j = EntityManager::getInstance()->m_blackHoles.begin();
j != EntityManager::getInstance()->m_blackHoles.end();
j++)
{
Vector2f pos = (*j)->getPosition() - pos;
float distance = pos.length();
Vector2f n = pos / distance;
vel += 10000.0f * n / (distance * distance + 10000.0f);
if(distance < 400)
{
vel += 45.0f * Vector2f(n.y, -n.x) / (distance + 100.0f);
}
}
}
if(fabs(vel.x) + fabs(vel.y) < 0.00000000001f)
{
vel = Vector2f(0, 0);
}
else if(particle.m_state.m_type == kEnemy)
{
vel *= 0.94f;
}
else
{
vel *= 0.96f + (float) fmod(fabs(pos.x), 0.94f);
}
particle.m_state.m_velocity = vel;
}
int ModelViewController::handle(int event)
{
Vector2f Clickpoint; // NEW: Current Mouse Point
static float Click_y;
static float old_zoom;
Clickpoint.x = (GLfloat)Fl::event_x();;
Clickpoint.y = (GLfloat)Fl::event_y();;
switch(event) {
case FL_PUSH: //mouse down event position in Fl::event_x() and Fl::event_y()
{
switch(Fl::event_button()) {
case FL_LEFT_MOUSE:
MousePt.T[0] = (GLfloat)Clickpoint.x;
MousePt.T[1] = (GLfloat)Clickpoint.y;
ArcBall->click(&MousePt); // Update Start Vector And Prepare For Dragging
break;
case FL_MIDDLE_MOUSE:
downPoint = Clickpoint;
/*
Matrix3fSetIdentity(&LastRot); // Reset Rotation
Matrix3fSetIdentity(&ThisRot); // Reset Rotation
Matrix4fSetRotationFromMatrix3f(&Transform, &ThisRot); // Reset Rotation
*/
break;
case FL_RIGHT_MOUSE:
Click_y = Clickpoint.y;
old_zoom = zoom;
break;
}
LastRot = ThisRot; // Set Last Static Rotation To Last Dynamic One
redraw();
return 1;
}
case FL_DRAG: //mouse moved while down event ...
switch(Fl::event_button())
{
case FL_LEFT_MOUSE:
Quat4fT ThisQuat;
MousePt.T[0] = Clickpoint.x;
MousePt.T[1] = Clickpoint.y;
ArcBall->drag(&MousePt, &ThisQuat); // Update End Vector And Get Rotation As Quaternion
Matrix3fSetRotationFromQuat4f(&ThisRot, &ThisQuat); // Convert Quaternion Into Matrix3fT
Matrix3fMulMatrix3f(&ThisRot, &LastRot); // Accumulate Last Rotation Into This One
Matrix4fSetRotationFromMatrix3f(&Transform, &ThisRot); // Set Our Final Transform's Rotation From This One
redraw();
break;
case FL_MIDDLE_MOUSE:
{
Vector2f dragp = Clickpoint;
Vector2f delta = downPoint-dragp;
Matrix4f matrix;
memcpy(&matrix.m00, &Transform.M[0], sizeof(Matrix4f));
Vector3f X(delta.x,0,0);
X = matrix * X;
Vector3f Y(0,-delta.y,0);
Y = matrix * Y;
ProcessControl.Center += X*delta.length()*0.01f;
ProcessControl.Center += Y*delta.length()*0.01f;
redraw();
downPoint=Clickpoint;
}
break;
case FL_RIGHT_MOUSE:
float y = Click_y - Clickpoint.y;
zoom = old_zoom + y*0.1;
redraw();
break;
}
return 1;
case FL_RELEASE: //mouse up event ...
MousePt.T[0] = (GLfloat)Fl::event_x();
MousePt.T[1] = (GLfloat)Fl::event_y();
redraw();
return 1;
case FL_MOUSEWHEEL: { //mouse scroll event
int mwscrolled = Fl::event_dy();
zoom += mwscrolled*1;
redraw();
return 1;
}
case FL_FOCUS :
case FL_UNFOCUS : // Return 1 if you want keyboard events, 0 otherwise
return 0;
case FL_KEYBOARD: //keypress, key is in Fl::event_key(), ascii in Fl::event_text() .. Return 1 if you understand/use the keyboard event, 0 otherwise...
return 1;
case FL_SHORTCUT: // shortcut, key is in Fl::event_key(), ascii in Fl::event_text() ... Return 1 if you understand/use the shortcut event, 0 otherwise...
return 1;
case FL_CLOSE:
if (fl_choice("Save settings ?", "Exit", "Save then exit", NULL))
{
// int a=0;
// ProcessControl.SaveXML();
}
break;
default:
break;
}
// pass other events to the base class...
return Fl_Gl_Window::handle(event);
}
inline Vector2f normalize(const Vector2f &v) {
return v / v.length();
}
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;
}
void testNormalize()
{
v5.normalize();
CPPUNIT_ASSERT(std::abs(v5.length() - 1.0) < EPSILON);
}
// 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
}
