void TransformState::getProjMatrix(mat4& projMatrix) const { double halfFov = std::atan(0.5 / getAltitude()); double topHalfSurfaceDistance = std::sin(halfFov) * getAltitude() / std::sin(M_PI / 2.0f - getPitch() - halfFov); // Calculate z value of the farthest fragment that should be rendered. double farZ = std::cos(M_PI / 2.0f - getPitch()) * topHalfSurfaceDistance + getAltitude(); matrix::perspective(projMatrix, 2.0f * std::atan((size.height / 2.0f) / getAltitude()), double(size.width) / size.height, 0.1, farZ); matrix::translate(projMatrix, projMatrix, 0, 0, -getAltitude()); // After the rotateX, z values are in pixel units. Convert them to // altitude unites. 1 altitude unit = the screen height. const bool flippedY = viewportMode == ViewportMode::FlippedY; matrix::scale(projMatrix, projMatrix, 1, flippedY ? 1 : -1, 1.0f / (rotatedNorth() ? size.width : size.height)); using NO = NorthOrientation; switch (getNorthOrientation()) { case NO::Rightwards: matrix::rotate_y(projMatrix, projMatrix, getPitch()); break; case NO::Downwards: matrix::rotate_x(projMatrix, projMatrix, -getPitch()); break; case NO::Leftwards: matrix::rotate_y(projMatrix, projMatrix, -getPitch()); break; default: matrix::rotate_x(projMatrix, projMatrix, getPitch()); break; } matrix::rotate_z(projMatrix, projMatrix, getAngle() + getNorthOrientationAngle()); matrix::translate(projMatrix, projMatrix, pixel_x() - size.width / 2.0f, pixel_y() - size.height / 2.0f, 0); }
void testGetAltitude() { int altitude; printf("***Testing getAltitude... ***\n\n"); printf("TEST 1: When prompted, input a number between 1 and 9999, inclusive. You should not see an error.\n\n"); altitude = getAltitude(); printf("\nAltitude returned was: %d\n", altitude); printf("\nTEST 2: When prompted, input a number less than 1. You should see an error and then be prompted again.\n"); printf("Then enter a number greater than 9999. You should see the same error message and be prompted again.\n"); printf("\nFinally, enter a number between 1 and 9999, inclusive.\n"); altitude = getAltitude(); printf("\nAltitude returned was: %d\n", altitude); }
void copyGPSData(){ if (isNewGPSDataAvailable()){ interchip_send_buffer.pm_data.time = gps_data.utc_time; interchip_send_buffer.pm_data.longitude = gps_data.longitude; interchip_send_buffer.pm_data.latitude = gps_data.latitude; interchip_send_buffer.pm_data.heading = gps_data.heading; interchip_send_buffer.pm_data.speed = gps_data.ground_speed; interchip_send_buffer.pm_data.satellites = (char)gps_data.num_satellites; interchip_send_buffer.pm_data.positionFix = (char)gps_data.fix_status; checkForFirstGPSLock(); } interchip_send_buffer.pm_data.gps_communication_error_count = getGPSCommunicationErrors(); interchip_send_buffer.pm_data.batteryLevel1 = getMainBatteryLevel(); interchip_send_buffer.pm_data.batteryLevel2 = getExtBatteryLevel(); interchip_send_buffer.pm_data.airspeed = getCurrentAirspeed(); interchip_send_buffer.pm_data.altitude = getAltitude(); //want to get altitude regardless of if there is new GPS data interchip_send_buffer.pm_data.pmOrbitGain = k_gain[ORBIT]; interchip_send_buffer.pm_data.pmPathGain = k_gain[PATH]; interchip_send_buffer.pm_data.waypointCount = pathCount; interchip_send_buffer.pm_data.waypointChecksum = getWaypointChecksum(); interchip_send_buffer.pm_data.pathFollowing = followPath; }
float Landscape::getAdjustedAltitude(int worldX, int worldZ) { // if ((150<worldX) && (worldX < 250) && (150<worldZ) && (worldZ < 250)) { // return 20; // } // return 0; //return worldX/10; return getAltitude(worldX, worldZ) * SIZES::MAX_TERRAIN_HEIGHT; }
boolean FlightComputer::haveWeLanded() { float newReading = getAltitude(); if(fabs(newReading-groundAltitude)<ALT_TOLERANCE) //Near ground { if(fabs(lastReading-newReading)<FALL_TOLERANCE) return true; } return false; }
boolean FlightComputer::hasAscentStarted() { float newReading = getAltitude(); boolean status = (newReading-groundAltitude) > ALT_TOLERANCE; if (newReading>apogee) { apogee = newReading; } return status; }
const float Himmel::setAltitude(const float altitude) { osg::Vec4f temp; u_common->get(temp); // Clamp altitude into non uniform atmosphere. (min alt is 1m) temp[0] = _clamp(0.001f, Earth::atmosphereThicknessNonUniform(), altitude); u_common->set(temp); return getAltitude(); }
GPSPosition::WaypointStruct GPSPosition::getWaypointStructData() { WaypointStruct tempWaypoint; memset(&tempWaypoint,0,sizeof(WaypointStruct)); tempWaypoint.Position.Latitude = (quint32) (getLatitude()*(double)10000000); tempWaypoint.Position.Longitude = (quint32) (getLongitude()*(double)10000000); tempWaypoint.Position.Altitude = (qint32) (getAltitude()*(double)1000); tempWaypoint.Position.Status = GPS_NEWDATA; tempWaypoint.ToleranceRadius = GPS_TOLERANCE_RADIUS_DEFAULT; tempWaypoint.HoldTime = GPS_HOLD_TIME_DEFAULT; tempWaypoint.Index = 0; tempWaypoint.pointType = GPS_POINT_TYPE_WPT; /*tempWaypoint.Name[0] = getName()[0].toLatin1(); tempWaypoint.Name[1] = getName()[1].toLatin1(); tempWaypoint.Name[2] = getName()[2].toLatin1(); tempWaypoint.Name[3] = getName()[3].toLatin1();*/ return tempWaypoint; }
//------------------------------------------------------------------------------ //Find ILS Glideslope By Freq - ALWAYS FIND GLIDESLOPE FIRST THEN LOCALIZER!!! //------------------------------------------------------------------------------ bool IlsRadio::findILSGlideslopeByFreq(LCreal freq) { //Reset glideSlopeValid so only the first valid glide slope is returned... glideSlopeValid = false; if (getAirportLoader() != 0 && freq > 0) { if (getAirportLoader()->requestDbInUse()) { //Set the active area: getAirportLoader()->setArea(getLatitude(), getLongitude(), getMaxDetectRange()); //See what the results are: int found = getAirportLoader()->queryIlsByFreq(static_cast<float>(freq)); //Sort through the results and check the type - assume the closest ones are correct //"found" list is already sorted from closest to farthest away: for (int i = 0; i < found; i++) { Dafif::Ils* p = getAirportLoader()->getIls(i); //Debug Prints: //p->printRecord(std::cout); //Get Glideslope data here: if((!glideSlopeValid)&&(p->isIlsType(Dafif::Ils::GLIDESLOPE))){ //Glideslope should not affect the bearing data from the localizer currentMagVar = p->magVariance(); float ilsGS(0), acGS(0), delGS(0); p->getGlideSlopeData(getLatitude(),getLongitude(),getAltitude(),&ilsGS,&acGS,&delGS); ilsGlideSlope = ilsGS; acGlideSlope= acGS; deltaGlideSlope = delGS; glideSlopeValid = true; } //Toast the object: p->unref(); } getAirportLoader()->clearDbInUse(); } } return glideSlopeValid; }
/** * Using the current calculated altitude, compute the pressure * at sea level in Pascals. update() must have been called * prior to calling this function. * * @returns The computed sea level pressure in Pascals. */ int getSealevelPressure() { return getSealevelPressure(getAltitude()); }
// -------------------------------------------------------------------------- // ARDrone::followLine() // Description : Send command to the AR Drone according to the line position // Return value : True if a line was found, false elsewhere // -------------------------------------------------------------------------- bool ARDrone::followLine(IplImage* image) { //Get the image of the bottom camera const cv::Mat image_bottom (image, cv::Rect(0, 0, 85, 70)); //Useful parameters to move the drone double offsetPix = 0.0, angleDeg = 0.0; double vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0; const bool found = findLine(image_bottom, offsetPix, angleDeg, 43); //If a line was found in the image if (found) { //Ajust altitude if (getAltitude() > 1.1) vz = -0.5; //Ajust angle if (angleDeg < -10 || angleDeg > 10) { //Turn left if (angleDeg < -10) vr = 0.5; //Turn right else vr = -0.5; } //Ajust according the line offset else if (offsetPix < -20 || offsetPix > 20) { //Move left if (offsetPix < -20) vy = 0.25; //Move right else vy = -0.25; } //If no ajustment is necessary, move on ! else vx = 0.6; //Send the command move3D(vx, vy, vz, vr); //sucess return true; } //if no line was found, increase altitude to find a line else if (getAltitude() < 1.2) vz = 0.5; //Send the command move3D(vx, vy, vz, vr); //Fail return false; }
boolean FlightComputer::hasDescentStarted() { float newReading = getAltitude(); boolean status = (apogee-newReading) > ALT_TOLERANCE; lastReading = newReading; return status; }
void BasicSubmarine::UI::updateUI(double dt, SubWindow& subWindow) { auto vessel = mVessel.lock(); if (vessel) { auto vesselPosition = vessel->getState().getLocation(); double lat = vesselPosition.getLatitude(); double lng = vesselPosition.getLongitude(); double alt = vesselPosition.getAltitude(); std::stringstream locationText; locationText << "Position and depth: " << std::endl; locationText << std::setprecision(6) << std::fixed << lat << " deg" << std::endl << std::setprecision(6) << std::fixed << lng << " deg" << std::endl; locationText << "Depth: " << std::setprecision(2) << alt << " m"; mLocation->SetText(locationText.str()); auto ocean = Ocean::getOcean(); ocean->lockAccess(); vessel->setHeading(mHeading->GetValue()); vessel->setPitch(mPitch->GetValue()); vessel->setVelocity(mVelocity->GetValue() * 0.514444444); //Convert to m/s. ocean->unlockAccess(); BroadbandState state(0.3f, 0.3f, 5.0f); std::shared_ptr<GameManager> gameManager = GameManager::getCurrent().lock(); BOOST_ASSERT_MSG(gameManager, "GameManager null"); auto eigenrayMap = gameManager->getUsmlManager()->getEigenrayMap(); //Iterate through all contacts we can hear. for (auto& contactKV : eigenrayMap) { //If the azimuth angle is NaN, that means that there were no eigenrays for this contact. if (!isnan(contactKV.second.source_az)) { //Average the intensities across all frequencies. //TODO: don't weight them all the same, and figure out what the numbers USML gives us even mean. double intensitySum = 0; for (auto intensity : contactKV.second.intensity) { if (intensity > 299.9999f) continue; //TODO: What causes this? intensitySum += intensity; } double intensityAverage = intensitySum / contactKV.second.intensity.size(); float adjustedIntensity = (float)(intensityAverage / 300); state.pushContact(BroadbandState::BroadbandContact((float)contactKV.second.source_az, adjustedIntensity, 5.0f)); } } mWaterfall->SetState(state); } else { subWindow.switchToScreen<MainMenu>(); } }
void pathManagerInit(void) { initTimer4(); initLED(0); //Communication with GPS initGPS(); initBatterySensor(); initAirspeedSensor(); initSPI(IC_DMA_PORT, DMA_CLOCK_KHZ, SPI_MODE1, SPI_BYTE, SPI_MASTER); initInterchip(DMA_CHIP_ID_PATH_MANAGER); //Communication with Altimeter if (initAltimeter()){ float initialValue = 0; while (initialValue == 0) initialValue = getAltitude(); calibrateAltimeter(initialValue); } //Initialize Home Location home.altitude = 400; home.latitude = RELATIVE_LATITUDE; home.longitude = RELATIVE_LONGITUDE; home.radius = 1; home.id = -1; //Initialize first path nodes // PathData* node = initializePathNode(); // node->altitude = 10; // node->latitude = 43.473662; // node->longitude = -80.540019; // node->radius = 5; // appendPathNode(node); // node = initializePathNode(); // node->altitude = 10; // node->latitude = 43.473479; // node->longitude = -80.540601; // node->radius = 5; // appendPathNode(node); // node = initializePathNode(); // node->altitude = 10; // node->latitude = 43.473718; // node->longitude = -80.540837; // node->radius = 5; // appendPathNode(node); // node = initializePathNode(); // node->altitude = 10; // node->latitude = 43.473946; // node->longitude = -80.540261; // node->radius = 5; // appendPathNode(node); // node = initializePathNode(); // node->altitude = 10; // node->latitude = 43.473685; // node->longitude = -80.540073; // node->radius = 5; // appendPathNode(node); #if DEBUG char str[20]; sprintf(str, "PM:%d, AM:%d", sizeof(PMData),sizeof(AMData)); debug(str); #endif }
int main(void) { TFT_Setup(); //Set up the TFT LCD setupSensorian(); //Set up all the sensors on the Sensorian Shield orange_led_on(); //Turn on the Sensorian Orange LED printf("Light: %f\n", getAmbientLight()); //Print the current light level pollMPL(); //Poll the sensor for the current temperature, altitude and pressure printf("Temperature: %d\n", getTemperature()); //Print the last polled temperature printf("Altitude: %d\n", getAltitude()); //Print the last polled altitude printf("Pressure: %d\n", getBarometricPressure()); //Print the last polled pressure pollFXOS(); //Poll the sensor for the current accelerometer and magnetometer values printf("Magnetometer X: %d, Y: %d, Z: %d\n", getMagX(), getMagY(), getMagZ()); //Print last polled magnet values printf("Accelerometer X: %d, Y: %d, Z: %d\n", getAccelX(), getAccelY(), getAccelZ()); //Print last accel values poll_rtcc(); //Poll the real time clock to get the current date and time printf("Date (DD/MM/YY): %d, %d, %d\n", get_rtcc_date(), get_rtcc_month(), get_rtcc_year()); //Print the date printf("Time: %d:%d:%d\n", get_rtcc_hour(), get_rtcc_minute(), get_rtcc_second()); //Print the last polled time //Create a sample string/char array to print to the screen char s[] = "This is a long string that will wrap with the display to fit if possible."; printf("Print Test\n"); TFT_Printer_Print(s); //Print the string to the screen with the default settings sleep(1); printf("Print Color Test\n"); TFT_Printer_PrintColor(RED, GREEN, s); //Print the string to the screen, changing the color sleep(1); printf("Print Size Test\n"); TFT_Printer_PrintSize(s, 2); //Print the string to the screen, changing the size sleep(1); printf("Print Both Test\n"); TFT_Printer_PrintBoth(YELLOW, BLUE, s, 3); //Print the string to the screen changing the color and size sleep(1); printf("Print All Test\n"); TFT_Printer_PrintAll(PORTRAIT, WHITE, BLACK, s, 2); //Print the string to the screen, changing color, sleep(1); //size and the orientation printf("Print Test 2\n"); TFT_Printer_Print(s); //Print the string to the screen again, using the last settings applied sleep(1); char ip_address[16] = {0}; //Creates a buffer in which to store the interface IP pi_get_interface_ip(ip_address); //Passes the buffer to the function which gets the interface IP char public_ip[16] = {0}; //Creates a buffer in which to store the public IP cloud_get_public_ip(public_ip); //Passes the buffer to the function which gets the public IP char ips_printed[57]; //Create a char array in which to store the string to be printed to the LCD strcpy(ips_printed, "Interface IP: "); //Start the new string off with the label Interface IP strcat(ips_printed, ip_address); //Add the string containing the temperature to the end of the string strcat(ips_printed, " Public IP: "); //Add the label Public IP to the end of the string strcat(ips_printed, public_ip); //Add the string containing the public ip to the end of the string TFT_Printer_PrintAll(LANDSCAPE_INV, WHITE, BLACK, ips_printed, 2); //Prints both IP addresses to the LCD sleep(1); char *key = "YourIFTTTMakerChannelKey"; //Create a string for your IFTTT Maker Channel Key char *event = "YourRecipeEventName"; //Create a string for your IFTTT Maker Channel Event long timeout = 5; //Create a long for the number of seconds to wait before timing out the request cloud_ifttt_trigger(key, event, timeout); //Send a HTTP request to trigger an IFTTT Maker Channel Recipe //Send a HTTP request to trigger an IFTTT Maker Channel Recipe along with 3 values in a JSON cloud_ifttt_trigger_values(key, event, timeout, "Value1", "2", "3.0"); float cpu_temp = pi_get_cpu_temperature(); //Store the float of the CPU temperature returned by the function char cpu_temp_str[10]; //Create a char array in which to store the string equivalent of the temperature sprintf(cpu_temp_str, "%f", cpu_temp); //Convert the CPU temperature float to a char array and store it char cpu_temp_printed[20]; //Create a char array in which to store the string to be printed to the LCD strcpy(cpu_temp_printed, "CPU TEMP: "); //Start the new string off with the label CPU TEMP strcat(cpu_temp_printed, cpu_temp_str); //Add the string containing the temperature to the end of the string TFT_Printer_PrintAll(LANDSCAPE_INV, WHITE, BLACK, cpu_temp_printed, 2); //Prints CPU Temperature to the LCD sleep(1); char cpu_serial[17] = {0}; //Creates a buffer in which to store the CPU serial pi_get_cpu_serial(cpu_serial); //Passes the buffer to the function which gets the CPU Serial char cpu_serial_printed[29]; //Create a char array in which to store the string to be printed to the LCD strcpy(cpu_serial_printed, "CPU SERIAL: "); //Start the new string off with the label CPU SERIAL strcat(cpu_serial_printed, cpu_serial); //Add the string containing the serial to the end of the string TFT_Printer_PrintAll(LANDSCAPE_INV, WHITE, BLACK, cpu_serial_printed, 1); //Prints CPU serial to the LCD sleep(1); orange_led_off(); //Turn off the Sensorian Orange LED //printf("Rebooting\n"); //pi_reboot(5); //Reboots the Raspberry Pi in the given number of seconds without blocking program execution return 0; //End the program }
float CTerrain::getAltitude(float x, float y) { float vec[2]={x,y}; return getAltitude(vec); }