void LifeForm::move(const LCreal fwd, const LCreal sdws)
{
if (getDamage() < 1) {
osg::Vec3 old = getEulerAngles();
LCreal hdg = old.z();
LCreal tempFwd = fwd, tempSdws = sdws;
// our deadband (if we are barely moving, just stop)
if (lcAbs(tempFwd) < 0.9f) tempFwd = 0;
if (lcAbs(tempSdws) < 0.9f) tempSdws = 0;
const double xVel = tempFwd * (lcCos(hdg));
const double yVel = tempFwd * (lcSin(hdg));
// now calculate our sideways velocity
const double xxVel = tempSdws * (lcCos((hdg + (90 * static_cast<LCreal>(Basic::Angle::D2RCC)))));
const double yyVel = tempSdws * (lcSin((hdg + (90 * static_cast<LCreal>(Basic::Angle::D2RCC)))));
// now add the vectors
const double newXVel = xVel + xxVel;
const double newYVel = yVel + yyVel;
LCreal zVel = 0.0;
setVelocity(static_cast<LCreal>(newXVel), static_cast<LCreal>(newYVel), zVel);
}
else setVelocity(0, 0, 0);
}
void PlaceableObject::mouseDrag(Point<int> mousePos)
{
/* mouse pos in screen coordinates (-1, 1) */
float mx = -1 + 2 * (mousePos.getX() / viewport->W);
float my = +1 - 2 * (mousePos.getY() / viewport->H);
glm::vec4 c0;
glm::vec4 e0;
getCentroid (c0[0], c0[1], c0[2]);
getEulerAngles(e0[0], e0[1], e0[2]);
glm::vec4 vp = viewport->V * c0;
glm::vec4 d0(mx, my, viewport->N, 1); /* 3d device coordinates of pick-ray endpoints */
glm::vec4 d1(mx, my, viewport->F, 1);
glm::vec4 x0 = viewport->inversePV * d0; x0 /= x0.w; /* 3d world coordinates of pick-ray endpoints */
glm::vec4 x1 = viewport->inversePV * d1; x1 /= x1.w;
//glm::mat4 R0 = glm::yawPitchRoll(e0[0], e0[1], e0[2]);
//glm::vec4 zh = glm::inverse(glm::transpose(R0)) * glm::vec4(0,0,1,0);
glm::vec4 zh;
getDraggingPlane(zh[0], zh[1], zh[2]);
/* parameter along the pick-ray where it intersects the model plane */
float s = glm::dot(zh, c0 - x0) / glm::dot(zh, x1 - x0);
glm::vec4 c1 = x0 + (x1 - x0) * s;
setCentroid(c1[0], c1[1], c1[2]);
}
// Check the alignmnent status of the tilt and yaw attitude
// Used during initial bootstrap alignment of the filter
void NavEKF2_core::checkAttitudeAlignmentStatus()
{
// Check for tilt convergence - used during initial alignment
float alpha = 1.0f*dtIMUavg;
float temp=tiltErrVec.length();
tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
tiltAlignComplete = true;
hal.console->printf("EKF tilt alignment complete\n");
}
// Once tilt has converged, align yaw using magnetic field measurements
if (tiltAlignComplete && !yawAlignComplete) {
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
StoreQuatReset();
yawAlignComplete = true;
hal.console->printf("EKF yaw alignment complete\n");
}
}
/* Monitor GPS data to see if quality is good enough to initialise the EKF
Monitor magnetometer innovations to to see if the heading is good enough to use GPS
Return true if all criteria pass for 10 seconds
We also record the failure reason so that prearm_failure_reason()
can give a good report to the user on why arming is failing
*/
bool NavEKF2_core::calcGpsGoodToAlign(void)
{
// fail if reported speed accuracy greater than threshold
bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f) && (frontend._gpsCheck & MASK_GPS_SPD_ERR);
// Report check result as a text string and bitmask
if (gpsSpdAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS speed error %.1f", (double)gpsSpdAccuracy);
gpsCheckStatus.bad_sAcc = true;
} else {
gpsCheckStatus.bad_sAcc = false;
}
// fail if not enough sats
bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (frontend._gpsCheck & MASK_GPS_NSATS);
// Report check result as a text string and bitmask
if (numSatsFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats());
gpsCheckStatus.bad_sats = true;
} else {
gpsCheckStatus.bad_sats = false;
}
// fail if satellite geometry is poor
bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (frontend._gpsCheck & MASK_GPS_HDOP);
// Report check result as a text string and bitmask
if (hdopFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS HDOP %.1f (needs 2.5)", (double)(0.01f * _ahrs->get_gps().get_hdop()));
gpsCheckStatus.bad_hdop = true;
} else {
gpsCheckStatus.bad_hdop = false;
}
// fail if horiziontal position accuracy not sufficient
float hAcc = 0.0f;
bool hAccFail;
if (_ahrs->get_gps().horizontal_accuracy(hAcc)) {
hAccFail = (hAcc > 5.0f) && (frontend._gpsCheck & MASK_GPS_POS_ERR);
} else {
hAccFail = false;
}
// Report check result as a text string and bitmask
if (hAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS horiz error %.1f", (double)hAcc);
gpsCheckStatus.bad_hAcc = true;
} else {
gpsCheckStatus.bad_hAcc = false;
}
// If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states
// This enables us to handle large changes to the external magnetic field environment that occur before arming
if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f) || !consistentMagData) {
magYawResetTimer_ms = imuSampleTime_ms;
}
if (imuSampleTime_ms - magYawResetTimer_ms > 5000) {
// reset heading and field states
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
// reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds
magYawResetTimer_ms = imuSampleTime_ms;
}
// fail if magnetometer innovations are outside limits indicating bad yaw
// with bad yaw we are unable to use GPS
bool yawFail;
if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f) && (frontend._gpsCheck & MASK_GPS_YAW_ERR)) {
yawFail = true;
} else {
yawFail = false;
}
// Report check result as a text string and bitmask
if (yawFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"Mag yaw error x=%.1f y=%.1f",
(double)magTestRatio.x,
(double)magTestRatio.y);
gpsCheckStatus.bad_yaw = true;
} else {
gpsCheckStatus.bad_yaw = false;
}
// Check for significant change in GPS position if disarmed which indicates bad GPS
// This check can only be used when the vehicle is stationary
const struct Location &gpsloc = _ahrs->get_gps().location(); // Current location
const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
// calculate time lapsesd since last update and limit to prevent numerical errors
float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
// Sum distance moved
gpsDriftNE += location_diff(gpsloc_prev, gpsloc).length();
gpsloc_prev = gpsloc;
// Decay distance moved exponentially to zero
gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst);
// Clamp the fiter state to prevent excessive persistence of large transients
gpsDriftNE = min(gpsDriftNE,10.0f);
// Fail if more than 3 metres drift after filtering whilst on-ground
// This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m
bool gpsDriftFail = (gpsDriftNE > 3.0f) && onGround && (frontend._gpsCheck & MASK_GPS_POS_DRIFT);
// Report check result as a text string and bitmask
if (gpsDriftFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS drift %.1fm (needs 3.0)", (double)gpsDriftNE);
gpsCheckStatus.bad_horiz_drift = true;
} else {
gpsCheckStatus.bad_horiz_drift = false;
}
// Check that the vertical GPS vertical velocity is reasonable after noise filtering
bool gpsVertVelFail;
if (_ahrs->get_gps().have_vertical_velocity() && onGround) {
// check that the average vertical GPS velocity is close to zero
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f) && (frontend._gpsCheck & MASK_GPS_VERT_SPD);
} else if ((frontend._fusionModeGPS == 0) && !_ahrs->get_gps().have_vertical_velocity()) {
// If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
gpsVertVelFail = true;
} else {
gpsVertVelFail = false;
}
// Report check result as a text string and bitmask
if (gpsVertVelFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS vertical speed %.2fm/s (needs 0.30)", (double)fabsf(gpsVertVelFilt));
gpsCheckStatus.bad_vert_vel = true;
} else {
gpsCheckStatus.bad_vert_vel = false;
}
// Check that the horizontal GPS vertical velocity is reasonable after noise filtering
// This check can only be used if the vehicle is stationary
bool gpsHorizVelFail;
if (onGround) {
gpsHorizVelFilt = 0.1f * pythagorous2(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt;
gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f) && (frontend._gpsCheck & MASK_GPS_HORIZ_SPD);
} else {
gpsHorizVelFail = false;
}
// Report check result as a text string and bitmask
if (gpsHorizVelFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS horizontal speed %.2fm/s (needs 0.30)", (double)gpsDriftNE);
gpsCheckStatus.bad_horiz_vel = true;
} else {
gpsCheckStatus.bad_horiz_vel = false;
}
// record time of fail - assume fail first time called
if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail || lastGpsVelFail_ms == 0) {
lastGpsVelFail_ms = imuSampleTime_ms;
}
// continuous period without fail required to return a healthy status
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
return true;
} else {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF checking GPS");
return false;
}
}
void LifeForm::look(const LCreal up, const LCreal sdws)
{
if (getDamage() < 1) {
if (lockMode != LOCKED) {
lockMode = SEARCHING;
// our up and sideways come in as -5 to 5, which is a rate to adjust heading
const osg::Vec3 old = getEulerAngles();
LCreal hdg = old.z();
LCreal ptc = lookAngle;
LCreal tempSdws = sdws;
LCreal tempUp = up;
if (lcAbs(tempSdws) < 0.00005) tempSdws = 0;
if (lcAbs(tempUp) < 0.05) tempUp = 0;
hdg += tempSdws;
hdg = lcAepcRad(hdg);
// we don't change our pitch when we look up and down, we only change our look angle, so we have to keep
// that separate. WE do, however, change our heading based on where we are looking, so that is correct
ptc += tempUp;
if (ptc > 90) ptc = 90;
else if (ptc < -90) ptc = -90;
//std::cout << "HEADING = " << hdg << std::endl;
setLookAngle(ptc);
osg::Vec3 eul(0, 0, hdg);
setEulerAngles(eul);
// now based on this we need to know if we have a target in our crosshairs...
tgtAquired = false;
if (tgtPlayer != nullptr) tgtPlayer->unref();
tgtPlayer = nullptr;
const osg::Vec3 myPos = getPosition();
osg::Vec3 tgtPos;
osg::Vec3 vecPos;
LCreal az = 0.0, el = 0.0, range = 0.0, diffAz = 0.0, diffEl = 0.0;
const LCreal maxAz = (0.7f * static_cast<LCreal>(Basic::Angle::D2RCC));
const LCreal maxEl = (0.7f * static_cast<LCreal>(Basic::Angle::D2RCC));
//LCreal maxRange = 1500.0f; // long range right now
const LCreal la = lookAngle * static_cast<LCreal>(Basic::Angle::D2RCC);
Simulation* sim = getSimulation();
if (sim != nullptr) {
Basic::PairStream* players = sim->getPlayers();
if (players != nullptr) {
Basic::List::Item* item = players->getFirstItem();
while (item != nullptr && !tgtAquired) {
Basic::Pair* pair = static_cast<Basic::Pair*>(item->getValue());
if (pair != nullptr) {
Player* player = dynamic_cast<Player*>(pair->object());
if (player != nullptr && player != this && !player->isMajorType(WEAPON) && !player->isDestroyed()) {
// ok, calculate our position from this guy
tgtPos = player->getPosition();
vecPos = tgtPos - myPos;
az = lcAtan2(vecPos.y(), vecPos.x());
range = (vecPos.x() * vecPos.x() + vecPos.y() * vecPos.y());
range = std::sqrt(range);
// now get our elevation
el = lcAtan2(-vecPos.z(), range);
diffAz = lcAbs(lcAepcRad(az - static_cast<LCreal>(getHeadingR())));
diffEl = lcAbs(lcAepcRad(la - el));
if ((diffAz <= maxAz) && (diffEl <= maxEl)) {
lockMode = TGT_IN_SIGHT;
tgtAquired = true;
if (tgtPlayer != player) {
if (tgtPlayer != nullptr) tgtPlayer->unref();
tgtPlayer = player;
tgtPlayer->ref();
}
}
}
}
item = item->getNext();
}
players->unref();
players = nullptr;
}
}
}
// else we are locking on target, and need to follow our target player
else {
if (tgtPlayer == nullptr) lockMode = SEARCHING;
else {
const osg::Vec3 vecPos = tgtPlayer->getPosition() - getPosition();
const LCreal az = lcAtan2(vecPos.y(), vecPos.x());
LCreal range = (vecPos.x() * vecPos.x() + vecPos.y() * vecPos.y());
range = std::sqrt(range);
// now get our elevation
const LCreal el = lcAtan2(-vecPos.z(), range);
// now force that on us
setLookAngle(el * static_cast<LCreal>(Basic::Angle::R2DCC));
setEulerAngles(0, 0, az);
}
}
}
}
