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