void View::keyPressEvent(QKeyEvent *e)
{
const qreal c_keyFactor = 2.0;
switch(e->key())
{
case Qt::Key_Space: setAnimated( !isAnimated() ); break;
case Qt::Key_Up :
if(e->modifiers() & Qt::ControlModifier )
setRoll( roll() + c_keyFactor );
else
setRotate( rotate() + QVector3D(-c_keyFactor , 0, 0) );
break;
case Qt::Key_Down :
if(e->modifiers() & Qt::ControlModifier )
setRoll( roll() - c_keyFactor );
else
setRotate( rotate() + QVector3D(c_keyFactor , 0, 0) );
break;
case Qt::Key_Left : setRotate( rotate() + QVector3D(0 , 0, -c_keyFactor) ); break;
case Qt::Key_Right: setRotate( rotate() + QVector3D(0 , 0, c_keyFactor) ); break;
case Qt::Key_Plus : setScale( scale() * 1.2 ); break;
case Qt::Key_Minus: setScale( scale() / 1.2 ); break;
case Qt::Key_A : if(e->modifiers() & Qt::ControlModifier )
toggleVisualHint( VH_ShowAxis);
break;
case Qt::Key_P : if(e->modifiers() & Qt::ControlModifier )
toggleVisualHint( VH_ShowPlane);
break;
default:
break;
}
}
void View::gestureEvent(QTouchEvent *touchEvent)
{
if(touchEvent->type() == QEvent::TouchBegin)
{
m_scaleTouched = m_camera->scale();
m_rollTouched = m_camera->roll();
}
QList<QTouchEvent::TouchPoint> touchPoints = touchEvent->touchPoints();
if (touchPoints.count() != 2)
{
m_isTouchEvent = false;
return;
}
// determine scale factor
const QTouchEvent::TouchPoint &touchPoint0 = touchPoints.first();
const QTouchEvent::TouchPoint &touchPoint1 = touchPoints.last();
QLineF current(touchPoint0.pos(), touchPoint1.pos());
QLineF start(touchPoint0.startPos(), touchPoint1.startPos());
qreal currentScaleFactor = current.length() / start.length();
const qreal c_coeffScaled = 0.3;
currentScaleFactor = 1.0 + (currentScaleFactor - 1.0)*c_coeffScaled;
setScale( m_scaleTouched * currentScaleFactor );
// determine roll camera
qreal currentRoll = start.angleTo( current );
setRoll( m_rollTouched + currentRoll );
m_isTouchEvent = touchEvent->type() != QEvent::TouchEnd;
}
void Navigator_Dcm::updateFast(float dt) {
if (_board->getMode() != AP_Board::MODE_LIVE)
return;
setTimeStamp(micros()); // if running in live mode, record new time stamp
// use range finder if attached and close to the ground
if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695) {
setAlt(_rangeFinderDown->distance);
// otherwise if you have a baro attached, use it
} else if (_board->baro) {
/**
* The altitued is read off the barometer by implementing the following formula:
* altitude (in m) = 44330*(1-(p/po)^(1/5.255)),
* where, po is pressure in Pa at sea level (101325 Pa).
* See http://www.sparkfun.com/tutorials/253 or type this formula
* in a search engine for more information.
* altInt contains the altitude in meters.
*
* pressure input is in pascals
* temp input is in deg C *10
*/
_board->baro->Read(); // Get new data from absolute pressure sensor
float reference = 44330 * (1.0 - (pow(_groundPressure.get()/101325.0,0.190295)));
setAlt(_baroLowPass.update((44330 * (1.0 - (pow((_board->baro->Press/101325.0),0.190295)))) - reference,dt));
//_board->debug->printf_P(PSTR("Ground Pressure %f\tAltitude = %f\tGround Temperature = %f\tPress = %ld\tTemp = %d\n"),_groundPressure.get(),getAlt(),_groundTemperature.get(),_board->baro->Press,_board->baro->Temp);
// last resort, use gps altitude
} else if (_board->gps && _board->gps->fix) {
setAlt_intM(_board->gps->altitude * 10); // gps in cm, intM in mm
}
// update dcm calculations and navigator data
//
_dcm.update_DCM_fast();
setRoll(_dcm.roll);
setPitch(_dcm.pitch);
setYaw(_dcm.yaw);
setRollRate(_dcm.get_gyro().x);
setPitchRate(_dcm.get_gyro().y);
setYawRate(_dcm.get_gyro().z);
setXAccel(_dcm.get_accel().x);
setYAccel(_dcm.get_accel().y);
setZAccel(_dcm.get_accel().z);
/*
* accel/gyro debug
*/
/*
Vector3f accel = _board->imu->get_accel();
Vector3f gyro = _board->imu->get_gyro();
Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"),
accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z);
*/
}
void Camera::init(const QVector3D &pos, const QVector3D &target, qreal roll)
{
m_cameraPos = pos;
m_targetCamera = target;
m_sceneRadius = (pos - target).length();
calcCameraAxis();
m_roll = 0.0; /// NOTE: clean, because setRoll use difference
setRoll( roll );
}
/**
* update attitude
*
* @param
* void
*
* @return
* void
*
*/
void attitudeUpdate(){
float yrpAttitude[3];
float pryRate[3];
float xyzAcc[3];
float xComponent[3];
float yComponent[3];
float zComponent[3];
float xyzMagnet[3];
#if CHECK_ATTITUDE_UPDATE_LOOP_TIME
struct timeval tv_c;
static struct timeval tv_l;
unsigned long timeDiff=0;
gettimeofday(&tv_c,NULL);
timeDiff=GET_USEC_TIMEDIFF(tv_c,tv_l);
_DEBUG(DEBUG_NORMAL,"attitude update duration=%ld us\n",timeDiff);
UPDATE_LAST_TIME(tv_c,tv_l);
#endif
getYawPitchRollInfo(yrpAttitude, pryRate, xyzAcc, xComponent, yComponent, zComponent,xyzMagnet);
setYaw(yrpAttitude[0]);
setRoll(yrpAttitude[1]);
setPitch(yrpAttitude[2]);
setYawGyro(-pryRate[2]);
setPitchGyro(pryRate[0]);
setRollGyro(-pryRate[1]);
setXAcc(xyzAcc[0]);
setYAcc(xyzAcc[1]);
setZAcc(xyzAcc[2]);
setXGravity(zComponent[0]);
setYGravity(zComponent[1]);
setZGravity(zComponent[2]);
setVerticalAcceleration(deadband((getXAcc() * zComponent[0] + getYAcc() * zComponent[1]
+ getZAcc() * zComponent[2] - 1.f) * 100.f,3.f) );
setXAcceleration(deadband((getXAcc() * xComponent[0] + getYAcc() * xComponent[1]
+ getZAcc() * xComponent[2]) * 100.f,3.f) );
setYAcceleration(deadband((getXAcc() * yComponent[0] + getYAcc() * yComponent[1]
+ getZAcc() * yComponent[2]) * 100.f,3.f) );
_DEBUG(DEBUG_ATTITUDE,
"(%s-%d) ATT: Roll=%3.3f Pitch=%3.3f Yaw=%3.3f\n", __func__,
__LINE__, getRoll(), getPitch(), getYaw());
_DEBUG(DEBUG_GYRO,
"(%s-%d) GYRO: Roll=%3.3f Pitch=%3.3f Yaw=%3.3f\n",
__func__, __LINE__, getRollGyro(), getPitchGyro(),
getYawGyro());
_DEBUG(DEBUG_ACC, "(%s-%d) ACC: x=%3.3f y=%3.3f z=%3.3f\n",
__func__, __LINE__, getXAcc(), getYAcc(), getZAcc());
}
/** Sets the pitch and the roll of this vector to follow the normal given. The
* heading is taken from this vector.
* \param normal The normal vector to which pitch and roll should be aligned.
*/
void Vec3::setPitchRoll(const Vec3 &normal)
{
const float X = sin(getHeading());
const float Z = cos(getHeading());
// Compute the angle between the normal of the plane and the line to
// (x,0,z). (x,0,z) is normalised, so are the coordinates of the plane,
// which simplifies the computation of the scalar product.
float pitch = ( normal.getX()*X + normal.getZ()*Z ); // use ( x,0,z)
float roll = (-normal.getX()*Z + normal.getZ()*X ); // use (-z,0,x)
// The actual angle computed above is between the normal and the (x,y,0)
// line, so to compute the actual angles 90 degrees must be subtracted.
setPitch(-acosf(pitch) + NINETY_DEGREE_RAD);
setRoll (-acosf(roll) + NINETY_DEGREE_RAD);
} // setPitchRoll
void View::mouseMoveEvent(QMouseEvent *event)
{
if(m_isTouchEvent)
return;
const qreal c_mouseFactor = 0.2;
QPoint newPos = event->pos();
if((event->buttons() & Qt::LeftButton) && (event->modifiers() & Qt::ControlModifier))
{
QPoint center(width()/2, height()/2);
QLineF oldLine( center, m_mousePos);
QLineF newLine( center, newPos);
qreal angle = oldLine.angleTo(newLine);
if(angle > 180.0)
angle = angle - 360.0;
setRoll( roll() + angle*c_mouseFactor*2 );
}
else if(event->buttons() & Qt::LeftButton)
{
QVector3D diffRotate = QVector3D(newPos.y() - m_mousePos.y(),
0,
newPos.x() - m_mousePos.x()) * c_mouseFactor;
setRotate( rotate() + diffRotate );
const qreal c_treshold = 15.0;
const qreal c_speedFactor = 0.5;
if(diffRotate.lengthSquared() > c_treshold)
camera()->setAnimatedRotation( -diffRotate*c_speedFactor );
else
camera()->setAnimatedRotation( QVector3D() );
}
else if((event->buttons() & Qt::MidButton) && isFreeMode())
{
QVector3D diffShift = c_mouseFactor* QVector3D(newPos.x() - m_mousePos.x(),
newPos.y() - m_mousePos.y(),
0 );//newPos.x() - m_mousePos.x()) * c_mouseFactor;
shift(diffShift);
}
m_mousePos = newPos;
}
void Matrix::setRotation(float rotation) {
setRoll(rotation);
}
// ---
void QGAMES::Background::initialize ()
{
// The initial values...
int fadeGrade = 255;
bool autMove = false;
QGAMES::Vector movDirection (__BD 1, __BD 0, __BD 0);
int speed = 0, pixels = 0;
QGAMES::Background* linkedBackground = NULL;
int linkedSpeed = 0;
bool roll = false;
bool fix = false;
// Has the fasdegrade property been defined?
if (existObjProperty (_FADEGRADEPROPERTY))
fadeGrade = std::stoi (objectProperty (_FADEGRADEPROPERTY));
// Has the automatic movement property been defined?
if (existObjProperty (_AUTOMATICMOVEPROPERTY))
autMove = (objectProperty (_AUTOMATICMOVEPROPERTY) ==
std::string (__YES_STRING__)) ? true : false ;
// Has the movement direction property been defined?
if (existObjProperty (_MOVDIRECTIONPROPERTY))
{
std::vector <int> cV;
std::string m = objectProperty (_MOVDIRECTIONPROPERTY);
while (m != std::string (__NULL_STRING__))
{
int cP = m.find_first_of (','); // The vector dta is separated by comma!
if (cP != -1)
{
cV.push_back (std::stoi (m.substr (0, cP)));
m = (cP != m.length () - 1)
? m.substr (cP + 1) : std::string (__NULL_STRING__);
}
else
{
cV.push_back (std::stoi (m));
m = std::string (__NULL_STRING__);
}
}
movDirection = QGAMES::Vector ((cV.size () > 0) ? __BD cV [0] : __BD 0,
(cV.size () > 1) ? __BD cV [1] : __BD 0, (cV.size () > 2) ? __BD cV [2] : __BD 0);
}
// Has the speed property been defined?
if (existObjProperty (_SPEEDPROPERTY))
speed = std::stoi (objectProperty (_SPEEDPROPERTY));
// Has the pixels property been defined?
if (existObjProperty (_PIXELSPROPERTY))
pixels = std::stoi (objectProperty (_PIXELSPROPERTY));
// Has the background linked been defined?
if (existObjProperty (_LINKEDBKPROPERTY))
{
assert (game ()); // The game has to have been defined...
linkedBackground = (QGAMES::Background*) game () -> objectBuilder () ->
object (std::stoi (objectProperty (_LINKEDBKPROPERTY)));
linkedBackground -> initialize ();
}
// Has the speed of the background linked been defined?
if (existObjProperty (_SPEEDLINKEDBKPROPERTY))
linkedSpeed = std::stoi (objectProperty (_SPEEDLINKEDBKPROPERTY));
// Has to roll?
if (existObjProperty (_ROLLPROPERTY))
roll = (objectProperty (_ROLLPROPERTY) ==
std::string (__YES_STRING__)) ? true : false ;
// The position is fixed?
if (existObjProperty (_FIXPOSITIONPROPERTY))
fix = (objectProperty (_FIXPOSITIONPROPERTY) ==
std::string (__YES_STRING__)) ? true : false ;
// The background is a mandatory attribute...
// Sets the fadegrade
setFade (fadeGrade);
// Sets the movement
setMove (autMove, movDirection);
// Sets the movement data
setMovementData (speed, pixels);
// Sets the linked background if any...
setLinkedBackground (linkedSpeed, linkedBackground);
// Sets whether it rolls or not!
setRoll (roll);
// Sets whether the position is or not fix...
setFixPosition (fix);
// Restats the counters...
reStartAllCounters ();
// Initially the position of the background is to 0...
setPosition (QGAMES::Position::_cero);
}
void Kompas::on()
{
timer->stop();
QSerialPortInfo *info = new QSerialPortInfo(*port);
if(!(port->isOpen() && info->portName() == settings->m_name_COM))
{
if(port->isOpen())
port->close();
qDebug()<<settings->m_name_COM;
port->setPortName(settings->m_name_COM);
if (port->open(QIODevice::ReadWrite))
{
QFile file("dataRead.dat");
if (file.open(QIODevice::ReadOnly | QIODevice::Text))
{
QTextStream stream(&file);
m_skl = stream.readLine().toInt();
m_coef_A = stream.readLine().toInt();
file.close();
}
updateSettings();
QSerialPortInfo *info = new QSerialPortInfo(*port);
qDebug() << "Name : " << info->portName();
qDebug() << "Description : " << info->description();
qDebug() << "Manufacturer: " << info->manufacturer();
qDebug() << "BaudRate: " << port->baudRate();
qDebug() << "Parity: " << port->parity();
qDebug() << "Data bits: " << port->dataBits();
qDebug() << "Stop Bits: " << port->stopBits();
delete info;
m_state=1;
emit stateChanged();
m_connect_state=1;
emit connect_stateChanged();
qDebug()<<"state = 1 ON";
qDebug()<<settings->m_name_COM<<"opened";
}
else
{
if(port->isOpen())
port->close();
qDebug()<<"Error while opening";
}
}
if(port->isOpen() && port->waitForReadyRead(1000))
{
QString data;
QByteArray ByteArray;
m_state = 1;
while(m_state)
{
qint64 byteAvail = port->bytesAvailable();
qApp->processEvents();
if(byteAvail >=13)
{
ByteArray = port->readAll();
data = data.fromLocal8Bit(ByteArray).trimmed();
if(ByteArray[3]=='p')
{
QBitArray bitdata(104),two_bytes(16);
for(int i = 0,j; i < 104; ++i)
{
j=i/8;
if(j<=13)
bitdata[i] = ByteArray[j] & (1 << i%8);
else
break;
}
for(int i=40,j=15;i<56&&j>=0;i++,j--){two_bytes[j]=bitdata[i];} //Roll
setRoll(Round(toDec(two_bytes,1)*1.41,1));
for(int i=56,j=15;i<72&&j>=0;i++,j--){two_bytes[j]=bitdata[i];} //Pitch
setPitch(Round(toDec(two_bytes,1)*1.41,1));
for(int i=72,j=15;i<88&&j>=0;i++,j--){two_bytes[j]=bitdata[i];} //Azimuth
setAngle(Round(toDec(two_bytes,0)*1.41,1));
m_state=0;
qApp->processEvents();
}
}
}
}
else
{
qDebug()<<"WaitForReadyRead failed";
qDebug()<<port->error();
}
kompasThread1->quit();
timer->start(10);
}
void PlaneState::updateState(const Basic::Component* const actor)
{
const Simulation::AirVehicle* airVehicle = dynamic_cast<const Simulation::AirVehicle*>(actor);
setAlive(false);
if (airVehicle != nullptr && airVehicle->isActive()) {
setAltitude(airVehicle->getAltitude());
setAlive(airVehicle->getMode() == Simulation::Player::ACTIVE);
setHeading(airVehicle->getHeading());
setPitch(airVehicle->getPitch());
setRoll(airVehicle->getRoll());
osg::Vec3d angularVels = airVehicle->getAngularVelocities();
setRollRate(angularVels.x());
setPitchRate(angularVels.y());
setYawRate(angularVels.z());
setTracking(false);
setTargetTrack(MAX_TRACKS); // 0 is a valid target track, use MAX_TRACKS to
// signal "no tgt track"
setSpeed(airVehicle->getCalibratedAirspeed());
setNumEngines(airVehicle->getNumberOfEngines());
setIncomingMissile(false);
setMissileFired(false);
// determine if we have a missile to fire
#if 1
const Simulation::StoresMgr* stores = airVehicle->getStoresManagement();
if (stores == nullptr || stores->getNextMissile() == nullptr) {
// either we have no SMS, or we have no more missile
setMissileFired(true);
}
else {
// we have an sms, and we have a missile available
// loop through player list and attempt to find out if one of our missiles is active
// if there is an active missile, then for the time being, we do not have a missile to fire
const Simulation::Simulation* sim = airVehicle->getSimulation();
const Basic::PairStream* players = sim->getPlayers();
bool finished = false;
for (const Basic::List::Item* item = players->getFirstItem(); item != nullptr && !finished; item = item->getNext()) {
// Get the pointer to the target player
const Basic::Pair* pair = static_cast<const Basic::Pair*>(item->getValue());
const Simulation::Player* player = static_cast<const Simulation::Player*>(pair->object());
if (player->isMajorType(Simulation::Player::WEAPON) && (player->isActive() || player->isMode(Simulation::Player::PRE_RELEASE)) && (player->getSide() == airVehicle->getSide())) {
// our side has a weapon on-the-way/in-the-air;
setMissileFired(true);
finished=true;
}
}
}
#else
// this state class has no way to determine whether we've fired a missile other than checking to see if sms is out of missiles to fire.
// which means, it will fire all its missiles at first target.
const Simulation::StoresMgr* stores = airVehicle->getStoresManagement();
if (stores != 0) {
const Simulation::Missile* wpn = stores->getNextMissile();
if (!wpn)
setMissileFired(true);
}
else {
// we have no SMS, we can't fire a missile;
setMissileFired(true);
}
#endif
//const Basic::String* playerName = airVehicle->getName();
// DH - DOES NOT COMPILE WITH CONST -- ????
Simulation::AirVehicle* airVehicleX = const_cast<Simulation::AirVehicle*>(airVehicle);
const Basic::Pair* sensorPair = airVehicleX->getSensorByType(typeid(Simulation::Radar));
if (sensorPair != nullptr) {
const Simulation::Radar* radar = static_cast<const Simulation::Radar*>(sensorPair->object());
if (radar != nullptr) {
const Simulation::TrackManager* trackManager = radar->getTrackManager();
Basic::safe_ptr<Simulation::Track> trackList[50];
unsigned int nTracks = trackManager->getTrackList(trackList, 50);
for (int trackIndex = nTracks -1; trackIndex >= 0; trackIndex--) {
setHeadingToTracked(trackIndex, trackList[trackIndex]->getRelAzimuth());
setPitchToTracked(trackIndex, trackList[trackIndex]->getElevation());
setDistanceToTracked(trackIndex, trackList[trackIndex]->getRange());
// do we have a live "target track"? (shootlist is 1-based)
if (getTargetTrack()==MAX_TRACKS && (trackList[trackIndex]->getShootListIndex() == 1) && trackList[trackIndex]->getTarget()->isActive() ) {
setTargetTrack(trackIndex);
}
setTracking(true);
setNumTracks(nTracks);
// hack to implement "missile warning"
if (isIncomingMissile() == false) {
// is this track a weapon, and if so, is it targeting me?
Simulation::Player* target = trackList[trackIndex]->getTarget();
Simulation::Weapon* weapon = dynamic_cast<Simulation::Weapon*> (target);
if (weapon!=nullptr && !weapon->isDead()) {
Simulation::Player* wpntgt = weapon->getTargetPlayer();
if (wpntgt == airVehicle) {
setIncomingMissile(true);
}
}
}
}
}
}
const Simulation::OnboardComputer* oc = airVehicle->getOnboardComputer();
if (oc != nullptr) {
const Simulation::TrackManager* rtm = oc->getTrackManagerByType(typeid(Simulation::RwrTrkMgr));
if(rtm !=nullptr) {
Basic::safe_ptr<Simulation::Track> trackList[50];
unsigned int nTracks = rtm->getTrackList(trackList, 50);
int newTracks = 0;
for (unsigned int trackIndex = 0; trackIndex < nTracks; trackIndex++) {
Simulation::Player* target = trackList[trackIndex]->getTarget();
bool alreadyTracked = false;
for (unsigned int currTracks = 0; currTracks>getNumTracks(); currTracks++) {
// tracks are the same if the associated players are the same
if(trackList[currTracks]->getTarget()==target) {
alreadyTracked = true;
break;
}
}
if (!alreadyTracked && (getNumTracks() + newTracks) < MAX_TRACKS) {
int newTrackIndex = getNumTracks() + newTracks;
newTracks++;
setHeadingToTracked(newTrackIndex, trackList[trackIndex]->getRelAzimuth());
setPitchToTracked(newTrackIndex, trackList[trackIndex]->getElevation());
setDistanceToTracked(newTrackIndex, trackList[trackIndex]->getRange());
setTracking(true);
// update numTracks to new sum of radar + rwr tracks
setNumTracks(getNumTracks()+newTracks);
}
// do we have a live "target track"? (shootlist is 1-based)
if (getTargetTrack()==MAX_TRACKS && (trackList[trackIndex]->getShootListIndex() == 1) && trackList[trackIndex]->getTarget()->isActive() ) {
setTargetTrack(trackIndex);
}
// hack to implement "missile warning"
if (isIncomingMissile() == false) {
// is this track a weapon, and if so, is it targeting me?
Simulation::Weapon* weapon = dynamic_cast<Simulation::Weapon*> (target);
if (weapon!=nullptr && !weapon->isDead()) {
Simulation::Player* wpntgt = weapon->getTargetPlayer();
if (wpntgt == airVehicle) {
setIncomingMissile(true);
}
}
}
}
}
}
}
BaseClass::updateState(actor);
}
void View::roll(float _droll) {
setRoll(m_roll + _droll);
}
