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