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