Exemplo n.º 1
0
int main (int argc, char **argv)
{
  ros::init (argc, argv, "autopilot");
  ros::NodeHandle nh;
  ros::NodeHandle nh_private("~");
  asctec::AutoPilot autopilot(nh, nh_private);
  ros::spin ();
  return 0;
}
Exemplo n.º 2
0
void numerical_dynamics (void)
  // This is the function that performs the numerical integration to update the
  // lander's pose. The time step is delta_t (global variable).
{
  // INSERT YOUR CODE HERE
	if(infinite_fuel) fuel = 1;
	double lander_mass = UNLOADED_LANDER_MASS + (fuel * FUEL_CAPACITY * FUEL_DENSITY);
	vector3d acceleration = -GRAVITY * MARS_MASS * (position.norm() / position.abs2());
	vector3d lander_drag = -0.5*DRAG_COEF_LANDER*atmospheric_density(position)*M_PI*LANDER_SIZE*LANDER_SIZE*velocity.abs2()*velocity.norm();
	if (parachute_status == DEPLOYED)
	{
		vector3d parachute_drag = -0.5*DRAG_COEF_CHUTE*atmospheric_density(position)*5.0*2.0*LANDER_SIZE*2.0*LANDER_SIZE*velocity.abs2()*velocity.norm();
		lander_drag = lander_drag + parachute_drag;
	}
	acceleration = acceleration + ((lander_drag + thrust_wrt_world()) / lander_mass);

	if (!update_first_pos) 
	{
		previous_position = position;
		position = previous_position + velocity * delta_t;
		velocity = velocity + acceleration * delta_t;
		update_first_pos = true;
	}
	else
	{
		vector3d temp = position;
		position = (2 * position) - previous_position + (delta_t * delta_t * acceleration);
		previous_position = temp;
		velocity = (1 / delta_t) * (position - previous_position);
	}
  // Here we can apply an autopilot to adjust the thrust, parachute and attitude
	if (autopilot_enabled)  autopilot();

  // Here we can apply 3-axis stabilization to ensure the base is always pointing downwards
  if (stabilized_attitude) attitude_stabilization();
}
Exemplo n.º 3
0
TestFlightResult
run_flight(TestFlightComponents components, TaskManager &task_manager,
           const AutopilotParameters &parms, const int n_wind,
           const double speed_factor)
{
  AircraftStateFilter *aircraft_filter = components.aircraft_filter;
  Airspaces *airspaces = components.airspaces;

  TestFlightResult result;

  TaskAccessor ta(task_manager, fixed_300);
  PrintTaskAutoPilot autopilot(parms);
  AircraftSim aircraft;

  autopilot.SetDefaultLocation(GeoPoint(Angle::Degrees(1), Angle::Degrees(0)));

  unsigned print_counter=0;
  if (n_wind)
    aircraft.SetWind(wind_to_mag(n_wind), wind_to_dir(n_wind));

  autopilot.SetSpeedFactor(fixed(speed_factor));

  Directory::Create(_T("output/results"));
  std::ofstream f4("output/results/res-sample.txt");
  std::ofstream f5("output/results/res-sample-filtered.txt");

  bool do_print = verbose;
  bool first = true;

  static const fixed fixed_10(10);

  const AirspaceAircraftPerformance perf(task_manager.GetGlidePolar());

  if (aircraft_filter)
    aircraft_filter->Reset(aircraft.GetState());

  autopilot.Start(ta);
  aircraft.Start(autopilot.location_start, autopilot.location_previous,
                 parms.start_alt);

  AirspaceWarningManager *airspace_warnings;
  if (airspaces) {
    airspace_warnings = new AirspaceWarningManager(*airspaces);
    airspace_warnings->Reset(aircraft.GetState());
  } else {
    airspace_warnings = NULL;
  }

  do {

    if ((task_manager.GetActiveTaskPointIndex() == 1) &&
        first && (task_manager.GetStats().total.time_elapsed > fixed_10)) {
      result.time_remaining = (double)task_manager.GetStats().total.time_remaining_now;
      first = false;

      result.time_planned = (double)task_manager.GetStats().total.time_planned;

      if (verbose > 1) {
        printf("# time remaining %g\n", result.time_remaining);
        printf("# time planned %g\n", result.time_planned);
      }
    }

    if (do_print) {
      PrintHelper::taskmanager_print(task_manager, aircraft.GetState());

      const AircraftState state = aircraft.GetState();
      f4 << state.time << " "
         <<  state.location.longitude << " "
         <<  state.location.latitude << " "
         <<  state.altitude << "\n";

      f4.flush();
      if (aircraft_filter) {
        f5 << aircraft_filter->GetSpeed() << " " 
           << aircraft_filter->GetBearing() << " " 
           << aircraft_filter->GetClimbRate() << "\n"; 
        f5.flush();
      }
    }

    if (airspaces) {
      scan_airspaces(aircraft.GetState(), *airspaces, perf,
                     do_print, 
                     autopilot.GetTarget(ta));
    }
    if (airspace_warnings) {
      if (verbose > 1) {
        bool warnings_updated = airspace_warnings->Update(aircraft.GetState(),
                                                          task_manager.GetGlidePolar(),
                                                          task_manager.GetStats(),
                                                          false, 1);
        if (warnings_updated) {
          printf("# airspace warnings updated, size %d\n",
                 (int)airspace_warnings->size());
          print_warnings(*airspace_warnings);
          WaitPrompt();
        }
      }
    }

    n_samples++;

    do_print = (++print_counter % output_skip == 0) && verbose;

    if (aircraft_filter)
      aircraft_filter->Update(aircraft.GetState());

    autopilot.UpdateState(ta, aircraft.GetState());
    aircraft.Update(autopilot.heading);

    {
      const AircraftState state = aircraft.GetState();
      const AircraftState state_last = aircraft.GetLastState();
      task_manager.Update(state, state_last);
      task_manager.UpdateIdle(state);
      task_manager.UpdateAutoMC(state, fixed(0));
    }

  } while (autopilot.UpdateAutopilot(ta, aircraft.GetState()));

  if (verbose) {
    PrintHelper::taskmanager_print(task_manager, aircraft.GetState());

    const AircraftState state = aircraft.GetState();
    f4 << state.time << " "
       <<  state.location.longitude << " "
       <<  state.location.latitude << " "
       <<  state.altitude << "\n";
    f4 << "\n";
    f4.flush();
    task_report(task_manager, "end of task\n");
  }
  WaitPrompt();

  result.time_elapsed = (double)task_manager.GetStats().total.time_elapsed;
  result.time_planned = (double)task_manager.GetStats().total.time_planned;
  result.calc_cruise_efficiency = (double)task_manager.GetStats().cruise_efficiency;
  result.calc_effective_mc = (double)task_manager.GetStats().effective_mc;

  if (verbose)
    PrintDistanceCounts();

  if (airspace_warnings)
    delete airspace_warnings;

  result.result = true;
  return result;
}
Exemplo n.º 4
0
// It all starts here:
int main(void) {
    // start with default user settings in case there's nothing in eeprom
    default_user_settings();
    // try to load settings from eeprom
    read_user_settings_from_eeprom();
   
    // set our LED as a digital output
    lib_digitalio_initpin(LED1_OUTPUT,DIGITALOUTPUT);

    //initialize the libraries that require initialization
    lib_timers_init();
    lib_i2c_init();

    // pause a moment before initializing everything. To make sure everything is powered up
    lib_timers_delaymilliseconds(100);
   
    // initialize all other modules
    init_rx();
    init_outputs();
    serial_init();
    init_gyro();
    init_acc();
    init_baro();
    init_compass();
    init_gps();
    init_imu();
   
    // set the default i2c speed to 400 KHz.  If a device needs to slow it down, it can, but it should set it back.
    lib_i2c_setclockspeed(I2C_400_KHZ);

    // initialize state
    global.state.armed=0;
    global.state.calibratingCompass=0;
    global.state.calibratingAccAndGyro=0;
    global.state.navigationMode=NAVIGATION_MODE_OFF;
    global.failsafeTimer=lib_timers_starttimer();

    // run loop
    for(;;) {
      // check to see what switches are activated
      check_checkbox_items();
      
      // check for config program activity
      serial_check_for_action();   
      
      calculate_timesliver();
      
      // run the imu to estimate the current attitude of the aircraft
      imu_calculate_estimated_attitude();

      // arm and disarm via rx aux switches
      if (global.rxValues[THROTTLE_INDEX]<FPSTICKLOW) { // see if we want to change armed modes
          if (!global.state.armed) {
             if (global.activeCheckboxItems & CHECKBOX_MASK_ARM) {
                 global.state.armed=1;
                #if (GPS_TYPE!=NO_GPS)
                 navigation_set_home_to_current_location();
                #endif
                 global.home.heading=global.currentEstimatedEulerAttitude[YAW_INDEX];
                 global.home.location.altitude=global.baroRawAltitude;
             }
          } else if (!(global.activeCheckboxItems & CHECKBOX_MASK_ARM)) global.state.armed=0;
      }

      #if (GPS_TYPE!=NO_GPS)
      // turn on or off navigation when appropriate
      if (global.state.navigationMode==NAVIGATION_MODE_OFF) {
          if (global.activeCheckboxItems & CHECKBOX_MASK_RETURNTOHOME) { // return to home switch turned on
              navigation_set_destination(global.home.location.latitude,global.home.location.longitude);
              global.state.navigationMode=NAVIGATION_MODE_RETURN_TO_HOME;
          } else if (global.activeCheckboxItems & CHECKBOX_MASK_POSITIONHOLD) { // position hold turned on
              navigation_set_destination(global.gps.currentLatitude,global.gps.currentLongitude);
              global.state.navigationMode=NAVIGATION_MODE_POSITION_HOLD;
          }
      } else { // we are currently navigating
          // turn off navigation if desired
          if ((global.state.navigationMode==NAVIGATION_MODE_RETURN_TO_HOME && !(global.activeCheckboxItems & CHECKBOX_MASK_RETURNTOHOME)) || (global.state.navigationMode==NAVIGATION_MODE_POSITION_HOLD && !(global.activeCheckboxItems & CHECKBOX_MASK_POSITIONHOLD))) {
              global.state.navigationMode=NAVIGATION_MODE_OFF;
            
              // we will be turning control back over to the pilot.
              reset_pilot_control();
          }
      }
        #endif
      
       // read the receiver
       read_rx();
      
       // turn on the LED when we are stable and the gps has 5 satelites or more
      #if (GPS_TYPE==NO_GPS)
       lib_digitalio_setoutput(LED1_OUTPUT, (global.state.stable==0)==LED1_ON);
      #else
       lib_digitalio_setoutput(LED1_OUTPUT, (!(global.state.stable && global.gps.numSatelites>=5))==LED1_ON);
      #endif
      
       // get the angle error.  Angle error is the difference between our current attitude and our desired attitude.
       // It can be set by navigation, or by the pilot, etc.
       fixedpointnum angleError[3];
      
       // let the pilot control the aircraft.
       get_angle_error_from_pilot_input(angleError);
      
#if (GPS_TYPE!=NO_GPS)
       // read the gps
       unsigned char gotNewGpsReading=read_gps();

       // if we are navigating, use navigation to determine our desired attitude (tilt angles)
       if (global.state.navigationMode!=NAVIGATION_MODE_OFF) { // we are navigating
           navigation_set_angle_error(gotNewGpsReading,angleError);
       }
#endif

       if (global.rxValues[THROTTLE_INDEX]<FPSTICKLOW) {
           // We are probably on the ground. Don't accumnulate error when we can't correct it
           reset_pilot_control();
         
           // bleed off integrated error by averaging in a value of zero
           lib_fp_lowpassfilter(&global.integratedAngleError[ROLL_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
           lib_fp_lowpassfilter(&global.integratedAngleError[PITCH_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
           lib_fp_lowpassfilter(&global.integratedAngleError[YAW_INDEX],0L,global.timesliver>>TIMESLIVEREXTRASHIFT,FIXEDPOINTONEOVERONEFOURTH,0);
       }

#ifndef NO_AUTOTUNE
       // let autotune adjust the angle error if the pilot has autotune turned on
       if (global.activeCheckboxItems & CHECKBOX_MASK_AUTOTUNE) {
           if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOTUNE)) {
               autotune(angleError,AUTOTUNE_STARTING); // tell autotune that we just started autotuning
           } else {
               autotune(angleError,AUTOTUNE_TUNING); // tell autotune that we are in the middle of autotuning
           }
       } else if (global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOTUNE) {
           autotune(angleError,AUTOTUNE_STOPPING); // tell autotune that we just stopped autotuning
       }
#endif

        // This gets reset every loop cycle
        // keep a flag to indicate whether we shoud apply altitude hold.  The pilot can turn it on or
        // uncrashability/autopilot mode can turn it on.
        global.state.altitudeHold=0;
        
        if (global.activeCheckboxItems & CHECKBOX_MASK_ALTHOLD) {
            global.state.altitudeHold=1;
            if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_ALTHOLD)) {
                // we just turned on alt hold.  Remember our current alt. as our target
                global.altitudeHoldDesiredAltitude=global.altitude;
                global.integratedAltitudeError=0;
            }
        }
        
        fixedpointnum throttleOutput;
        
#ifndef NO_AUTOPILOT
        // autopilot is available
        if (global.activeCheckboxItems & CHECKBOX_MASK_AUTOPILOT) {
            if (!(global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOPILOT)) {
                // let autopilot know to transition to the starting state
                autopilot(AUTOPILOT_STARTING);
            } else {
                // autopilot normal run state
                autopilot(AUTOPILOT_RUNNING);
            }
        } else if (global.previousActiveCheckboxItems & CHECKBOX_MASK_AUTOPILOT) {
            // tell autopilot that we just stopped autotuning
            autopilot(AUTOPILOT_STOPPING);
        } else {
            // get the pilot's throttle component
            // convert from fixedpoint -1 to 1 to fixedpoint 0 to 1
            throttleOutput=(global.rxValues[THROTTLE_INDEX]>>1)+FIXEDPOINTCONSTANT(.5)+FPTHROTTLETOMOTOROFFSET;
        }
#else

       // get the pilot's throttle component
       // convert from fixedpoint -1 to 1 to fixedpoint 0 to 1
       throttleOutput=(global.rxValues[THROTTLE_INDEX]>>1)+FIXEDPOINTCONSTANT(.5)+FPTHROTTLETOMOTOROFFSET;
#endif

#ifndef NO_UNCRASHABLE
        uncrashable(gotNewGpsReading,angleError,&throttleOutput);
#endif
        
#if (BAROMETER_TYPE!=NO_BAROMETER)
       // check for altitude hold and adjust the throttle output accordingly
       if (global.state.altitudeHold) {
           global.integratedAltitudeError+=lib_fp_multiply(global.altitudeHoldDesiredAltitude-global.altitude,global.timesliver);
           lib_fp_constrain(&global.integratedAltitudeError,-INTEGRATED_ANGLE_ERROR_LIMIT,INTEGRATED_ANGLE_ERROR_LIMIT); // don't let the integrated error get too high
         
           // do pid for the altitude hold and add it to the throttle output
           throttleOutput+=lib_fp_multiply(global.altitudeHoldDesiredAltitude-global.altitude,settings.pid_pgain[ALTITUDE_INDEX])-lib_fp_multiply(global.altitudeVelocity,settings.pid_dgain[ALTITUDE_INDEX])+lib_fp_multiply(global.integratedAltitudeError,settings.pid_igain[ALTITUDE_INDEX]);
       }
#endif

#ifndef NO_AUTOTHROTTLE
       if ((global.activeCheckboxItems & CHECKBOX_MASK_AUTOTHROTTLE) || global.state.altitudeHold) {
           if (global.estimatedDownVector[Z_INDEX]>FIXEDPOINTCONSTANT(.3)) {
               // Divide the throttle by the throttleOutput by the z component of the down vector
               // This is probaly the slow way, but it's a way to do fixed point division
               fixedpointnum recriprocal=lib_fp_invsqrt(global.estimatedDownVector[Z_INDEX]);
               recriprocal=lib_fp_multiply(recriprocal,recriprocal);
         
               throttleOutput=lib_fp_multiply(throttleOutput-AUTOTHROTTLE_DEAD_AREA,recriprocal)+AUTOTHROTTLE_DEAD_AREA;
           }
       }
#endif

#ifndef NO_FAILSAFE
       // if we don't hear from the receiver for over a second, try to land safely
       if (lib_timers_gettimermicroseconds(global.failsafeTimer)>1000000L) {
           throttleOutput=FPFAILSAFEMOTOROUTPUT;

           // make sure we are level!
           angleError[ROLL_INDEX]=-global.currentEstimatedEulerAttitude[ROLL_INDEX];
           angleError[PITCH_INDEX]=-global.currentEstimatedEulerAttitude[PITCH_INDEX];
       }
#endif
        
       // calculate output values.  Output values will range from 0 to 1.0
       // calculate pid outputs based on our angleErrors as inputs
       fixedpointnum pidOutput[3];
      
       // Gain Scheduling essentialy modifies the gains depending on
       // throttle level. If GAIN_SCHEDULING_FACTOR is 1.0, it multiplies PID outputs by 1.5 when at full throttle,
       // 1.0 when at mid throttle, and .5 when at zero throttle.  This helps
       // eliminate the wobbles when decending at low throttle.
       fixedpointnum gainSchedulingMultiplier=lib_fp_multiply(throttleOutput-FIXEDPOINTCONSTANT(.5),FIXEDPOINTCONSTANT(GAIN_SCHEDULING_FACTOR))+FIXEDPOINTONE;
      
       for (int x=0;x<3;++x) {
           global.integratedAngleError[x]+=lib_fp_multiply(angleError[x],global.timesliver);
         
           // don't let the integrated error get too high (windup)
           lib_fp_constrain(&global.integratedAngleError[x],-INTEGRATED_ANGLE_ERROR_LIMIT,INTEGRATED_ANGLE_ERROR_LIMIT);
         
           // do the attitude pid
           pidOutput[x]=lib_fp_multiply(angleError[x],settings.pid_pgain[x])-lib_fp_multiply(global.gyrorate[x],settings.pid_dgain[x])+(lib_fp_multiply(global.integratedAngleError[x],settings.pid_igain[x])>>4);
            
           // add gain scheduling.
           pidOutput[x]=lib_fp_multiply(gainSchedulingMultiplier,pidOutput[x]);
       }

       lib_fp_constrain(&throttleOutput,0,FIXEDPOINTONE); // Keep throttle output between 0 and 1

       compute_mix(throttleOutput, pidOutput); // aircraft type dependent mixes
       
#if (NUM_SERVOS>0)
       // do not update servos during unarmed calibration of sensors which are sensitive to vibration
       if (global.state.armed || (!global.state.calibratingAccAndGyro)) write_servo_outputs();
#endif
       
       write_motor_outputs();
   }
Exemplo n.º 5
0
int main(void)
{
	int sock, sock2;
	struct sockaddr_in echoclient,from, ctrlclient, ctrlinclient;
	unsigned int echolen, clientlen;
	int received = 0;

	FGNetFDM buf;
	char* buffer = (char*)&buf;

	FGNetCtrls bufctrl;
	char* buffer_ctrl = (char*)&bufctrl; 

	struct known_state state; 
	struct known_state old_state; 

	struct known_state_ints state_ints; 
	struct known_state_ints old_state_ints; 
	
	srand ( time(NULL) );
	memset(&state,		0, sizeof(state));
	memset(&old_state,	0, sizeof(state));

	memset(&state_ints,		0, sizeof(state_ints));
	memset(&old_state_ints,	0, sizeof(state_ints));


	/* Create the UDP socket */
	if ((sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
		std::cerr << "Failed to create socket" << std::endl;
		perror("socket");
		return -1;
	}

	if ((sock2 = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) {
		std::cerr << "Failed to create socket" << std::endl;
		perror("socket");
		return -1;
	}

	/// the port where net fdm is received
	memset(&echoclient, 0, sizeof(echoclient));       /* Clear struct */
	echoclient.sin_family = AF_INET;                  /* Internet/IP */
	echoclient.sin_addr.s_addr = inet_addr("127.0.0.1");  /* IP address */
	echoclient.sin_port = htons(5500);       /* server port */

	/// the port where the modified net ctrl is sent to flightgear
	memset(&ctrlclient, 0, sizeof(ctrlclient));       /* Clear struct */
	ctrlclient.sin_family = AF_INET;                  /* Internet/IP */
	ctrlclient.sin_addr.s_addr = inet_addr("127.0.0.1");  /* IP address */
	ctrlclient.sin_port = htons(5600);       /* server port */

	/// this is the port where the net ctrl comes from flight gear
	memset(&ctrlinclient, 0, sizeof(ctrlinclient));       /* Clear struct */
	ctrlinclient.sin_family = AF_INET;                  /* Internet/IP */
	ctrlinclient.sin_addr.s_addr = inet_addr("127.0.0.1");  /* IP address */
	ctrlinclient.sin_port = htons(5700);       /* server port */

	int length = sizeof(buf);

	std::cout << "packet length should be " << length << " bytes" << std::endl;

	echolen = length;

	char errorbuf[100];

	/// need this?
	int ret  = bind(sock,  (struct sockaddr*)&echoclient, sizeof(echoclient) );
	int ret2 = bind(sock2, (struct sockaddr*)&ctrlinclient, sizeof(ctrlinclient) );
	
	std::cout << "bind " << ret << " " << ret2 << std::endl;
	///////////////////////////////////////////////////////////////////

	std::string file = "telemetry.csv";
	std::ofstream telem(file.c_str(), std::ios_base::out );
	if (!telem) {
		std::cout << "File \"" << file << "\" not found.\n";
		throw fileNotFound();
		return 2;
	}
	telem.precision(10);

    telem <<
	        "longitude, latitude, altitude" <<
			        "p, q, r," <<
					"elevator, rudder, aileron" <<
							            std::endl;

	//std::string binfile = "sim_telemetry.bin";
	//std::ofstream myFile(binfile, std::ios::out | std::ios::binary);

	double time = 0.0; 

	float dist = 40e3; //state.altitude;
	float div = 3e7;


	double target_longitude = -2.137; 
	double target_latitude  = .658;
	double target_altitude  = 50.0; //meters 
	std::cout << "struct size " << sizeof(state) << std::endl;

	FILE* telem_file;
	telem_file = fopen("telem.bin","wb");

	int i = 0;
	while(1) {
	 
	 	i++;
		// I think this clobbers echoclient
		/// receive net_fdm over udp
		received = recvfrom(sock, buffer, sizeof(buf), 0,
						(struct sockaddr *) &from,
						&echolen);
	
/// -extract accelerations A_X_pilot etc, add noise
/// - are phidot, thetadot, psidot in body frame? Probably 
/// (I thought someone told me there was no difference, but that doesn't 
/// make sense- what if the vehicle was 90 degrees up from normal, how is
/// roll still roll?)
/// - take position lat long alt, but filter so it is only updated at a few Hz

/// and that's all we'll know

		FGNetFDM2Props( &buf);

		if (i == 1) {
			target_longitude = buf.longitude + (float)(rand()%(int)dist)/div - dist/div*0.5; 
			target_latitude  = buf.latitude  + (float)(rand()%(int)dist)/div - dist/div*0.5; 
			std::cout << "t longitude=" << target_longitude << ", t latitude=" << target_latitude << std::endl;
		}

		if (i%10 == 0) {
			state.longitude= (float)((int)(180e4/M_PI*buf.longitude))/(180e4/M_PI);;
			state.latitude = (float)((int)(180e4/M_PI*buf.latitude))/(180e4/M_PI);
			state.altitude = buf.altitude*M2FT;
		} else {
			state.longitude= old_state.longitude;
			state.latitude = old_state.latitude;
			state.altitude = old_state.altitude;
		}

		state.p = FPFIX(buf.p);
		state.q = FPFIX(buf.q);
		state.r = FPFIX(buf.r);

		state.A_X_pilot = FPFIX(buf.A_X_pilot);
		state.A_Y_pilot = FPFIX(buf.A_Y_pilot);
		state.A_Z_pilot = FPFIX(buf.A_Z_pilot);

		state.target_long= target_longitude;
		state.target_lat = target_latitude;
		state.target_alt = target_altitude;
		
		#if 0
		/// fixed point version
		if (j%10 == 0) {
		    /// convert to arc-minutes
			state_ints.longitude = float_to_fixed(180.0*60.0/M_PI*buf.longitude);
			state_ints.latitude  = float_to_fixed(180.0*60.0/M_PI*buf.latitude);
			/// convert to feet later?
			state_ints.altitude  = (int)(buf.altitude);

			/// TEMP
			/// alternate way of getting velocity- this one matches the sim 
			double x1 = (EARTH_RADIUS_METERS+state.altitude)*cos(state.latitude)*cos(state.longitude);
			double y1 = (EARTH_RADIUS_METERS+state.altitude)*cos(state.latitude)*sin(state.longitude);
			double z1 = (EARTH_RADIUS_METERS+state.altitude)*sin(state.latitude);

			double x2 = (EARTH_RADIUS_METERS+old_state.altitude)*cos(old_state.latitude)*cos(old_state.longitude);
			double y2 = (EARTH_RADIUS_METERS+old_state.altitude)*cos(old_state.latitude)*sin(old_state.longitude);
			double z2 = (EARTH_RADIUS_METERS+old_state.altitude)*sin(old_state.latitude);

			// delta xyz to target
			double xt = (EARTH_RADIUS_METERS+target_altitude)*cos(target_latitude)*cos(target_longitude);
			double yt = (EARTH_RADIUS_METERS+target_altitude)*cos(target_latitude)*sin(target_longitude);
			double zt = (EARTH_RADIUS_METERS+target_altitude)*sin(target_latitude);

			double dx = (x1-x2);
			double dy = (y1-y2);
			double dz = (z1-z2);

			// length of vector pointing from old location to current
			float l1 = sqrtf(dx*dx + dy*dy + dz*dz);

			/// length of vector point straight at center of earth
			double l2 = sqrtf(x2*x2 + y2*y2 + z2*z2);
		
			std::cout << "velocity correct " << l1
				<< ", dx " << (dx);
			//	<< ", radius " << EARTH_RADIUS_METERS 
			//	<< ", coslat " << cos(state.latitude)*cos(state.longitude);
				//<< ", dy " << (y1)
				//<< ", dz " << (z1);
			/*
			double dotprod = ( 
								(dx/l1 * (-x2)/l2) +
								(dy/l1 * (-y2)/l2) +
								(dz/l1 * (-z2)/l2)  );
			
			state.pitch = acos(dotprod)/M_PI -0.5;
			*/
		}
		j++;

		state_ints.target_long= float_to_fixed(target_longitude);
		state_ints.target_lat = float_to_fixed(target_latitude);
		state_ints.target_alt = float_to_fixed(target_altitude);

		state_ints.p = float_to_fixed(buf.p);
		state_ints.q = float_to_fixed(buf.q);
		state_ints.r = float_to_fixed(buf.r);

		state_ints.A_X_pilot = float_to_fixed(buf.A_X_pilot);
		state_ints.A_Y_pilot = float_to_fixed(buf.A_Y_pilot);
		state_ints.A_Z_pilot = float_to_fixed(buf.A_Z_pilot);
#endif

		if (received < 0) {
			perror("recvfrom");
	//	} else if (received != echolen) {
	//		std::cerr << "wrong sized packet " << received << std::endl;
		} else {

			/// get the ctrls that are properly filled out with environmental
			/// data, so we don't clobber that when we modify the flap positions
			received = recvfrom(sock2, buffer_ctrl, sizeof(bufctrl), 0,
						(struct sockaddr *) &ctrlinclient,
						&echolen);

			FGProps2NetCtrls(&bufctrl);

			float dt =1.0/10.0;
			autopilot(state, old_state,dt);
			bufctrl.elevator = FPFIX(state.elevator);
			bufctrl.aileron  = FPFIX(state.aileron);
			bufctrl.rudder   = FPFIX(state.rudder);

			//autopilot_ints(state_ints, old_state_ints, buf, bufctrl);
			//state_fixed_to_float(state_ints,state);

			/// output to file and stdout 
			{
				/// save telemetry to file
				telem << 
					buf.longitude << "," << buf.latitude << "," << buf.altitude*M2FT << "," <<
					state.p << "," << state.q << "," << state.r << "," <<
					bufctrl.elevator << "," << bufctrl.rudder << "," << bufctrl.aileron << "," <<
					state.dq << "," << state.iq << "," << 
					state.error_heading << "," << state.derror_heading << "," << state.ierror_heading << "," << 
					state.error_pitch << "," << state.dpitch << "," << state.ipitch << "," <<
					state.tpitch << "," << state.speed << "," << 
					state.tdx << "," << state.tdy << "," << 
					bufctrl.wind_speed_kt << "," << bufctrl.wind_dir_deg << "," << bufctrl.press_inhg << "," <<  
					state.dr << "," << state.ir << "," << state.pitch << 
					state.A_X_pilot << "," << state.A_Y_pilot << "," << state.A_Z_pilot << "," << 
					state.longitude << "," << state.latitude << "," << state.altitude << "," <<
					std::endl;


				/// output some of the state to stdout every few seconds
				static int i = 0;
				i++;
				std::cout.precision(2);
				//if ((state.longitude != old_state.longitude ) || (state.latitude != old_state.latitude)) {
				if (i%30==0) {
					std::cout <<
						", hor dist=" << sqrtf(state.tdist*state.tdist - (state.altitude-target_altitude)*(state.altitude-target_altitude)) <<
						//", agl=" << buf.agl << ", alt=" << state.altitude << ", vel=" << state.speed << 
						//		", fdm v= " << sqrtf(buf.v_north*buf.v_north + buf.v_east*buf.v_east + buf.v_down*buf.v_down)*0.3048 <<
						//	" tdx=" << state.tdx << ", tdy=" << state.tdy <<
						//		" heading= " << heading << " target heading= " << theading <<
						", err_head= " << state.error_heading << ", rud=" << bufctrl.rudder <<
						//			", lat= " << state.latitude << ", long= " << state.longitude << ", alt= " << state.altitude <<
						", pitch= " << state.pitch << ", tpitch= " << state.tpitch << ", elev=" << bufctrl.elevator <<
						//	", p=" << state.p <<  
						//		", q=" << state.q << ", elev=" << bufctrl.elevator <<
						", r=" << state.r << ", ail= " << bufctrl.aileron << 
						", ax=" << state.A_X_pilot << ", ay=" << state.A_Y_pilot << ", az=" << state.A_Z_pilot << 
						", orient=" << state.acc_orientation <<
						std::endl;
				}
			}

			fwrite((void*)&state, 1, sizeof(state), telem_file);

			//std::cout << old_state.altitude << " " << state.altitude << std::endl;
			memcpy(&old_state, &state, sizeof(state));
			
			//memcpy(&old_state_ints, &state_ints, sizeof(state_ints));


			////////////////////////////////////////////////////////////////////////

			received = sendto(sock, buffer_ctrl, sizeof(bufctrl), 0,
			(struct sockaddr *) &ctrlclient, sizeof(ctrlclient));

			
			if (received <0) {
				perror("sendto");
			}

			time += 0.1;	
		
		}

		usleep(3000);
	}

	fclose(telem_file);

return 0;
}
Exemplo n.º 6
0
bool run_flight(TaskManager &task_manager,
                const AutopilotParameters &parms,
                const int n_wind,
                const double speed_factor) 
{
  DirectTaskAccessor ta(task_manager);
  PrintTaskAutoPilot autopilot(parms);
  AircraftSim aircraft;

  autopilot.set_default_location(GeoPoint(Angle::degrees(fixed(1.0)), Angle::degrees(fixed(0.0))));

  unsigned print_counter=0;
  if (n_wind)
    aircraft.set_wind(wind_to_mag(n_wind), wind_to_dir(n_wind));

  autopilot.set_speed_factor(fixed(speed_factor));

  std::ofstream f4("results/res-sample.txt");
  std::ofstream f5("results/res-sample-filtered.txt");

  bool do_print = verbose;
  bool first = true;

  time_elapsed = 0.0;
  time_planned = 1.0;
  time_remaining = 0;
  calc_cruise_efficiency = 1.0;
  calc_effective_mc = 1.0;

  static const fixed fixed_10(10);

  AirspaceAircraftPerformanceGlide perf(task_manager.get_glide_polar());

  if (aircraft_filter)
    aircraft_filter->Reset(aircraft.get_state());

  autopilot.Start(ta);
  aircraft.Start(autopilot.location_start, autopilot.location_previous,
                 parms.start_alt);

  if (airspaces) {
    airspace_warnings =
        new AirspaceWarningManager(*airspaces, task_manager);
    airspace_warnings->reset(aircraft.get_state());
  }

  do {

    if ((task_manager.getActiveTaskPointIndex() == 1) &&
        first && (task_manager.get_stats().total.time_elapsed > fixed_10)) {
      time_remaining = task_manager.get_stats().total.time_remaining;
      first = false;

      time_planned = task_manager.get_stats().total.time_planned;

      if (verbose > 1) {
        printf("# time remaining %g\n", time_remaining);
        printf("# time planned %g\n", time_planned);
      }
    }

    if (do_print) {
      PrintHelper::taskmanager_print(task_manager, aircraft.get_state());

      const AircraftState state = aircraft.get_state();
      f4 << state.time << " "
         <<  state.location.Longitude << " "
         <<  state.location.Latitude << " "
         <<  state.altitude << "\n";

      f4.flush();
      if (aircraft_filter) {
        f5 << aircraft_filter->GetSpeed() << " " 
           << aircraft_filter->GetBearing() << " " 
           << aircraft_filter->GetClimbRate() << "\n"; 
        f5.flush();
      }
    }

    if (airspaces) {
      scan_airspaces(aircraft.get_state(), *airspaces, perf,
                     do_print, 
                     autopilot.target(ta));
    }
    if (airspace_warnings) {
      if (verbose > 1) {
        bool warnings_updated = airspace_warnings->update(aircraft.get_state(),
                                                          false, 1);
        if (warnings_updated) {
          printf("# airspace warnings updated, size %d\n",
                 (int)airspace_warnings->size());
          print_warnings();
          wait_prompt();
        }
      }
    }

    n_samples++;

    do_print = (++print_counter % output_skip == 0) && verbose;

    if (aircraft_filter)
      aircraft_filter->Update(aircraft.get_state());

    autopilot.update_state(ta, aircraft.get_state());
    aircraft.Update(autopilot.heading);

    {
      const AircraftState state = aircraft.get_state();
      const AircraftState state_last = aircraft.get_state_last();
      task_manager.update(state, state_last);
      task_manager.update_idle(state);
      task_manager.update_auto_mc(state, fixed_zero);
    }

  } while (autopilot.update_autopilot(ta, aircraft.get_state(), aircraft.get_state_last()));

  aircraft.Stop();
  autopilot.Stop();

  if (verbose) {
    PrintHelper::taskmanager_print(task_manager, aircraft.get_state());

    const AircraftState state = aircraft.get_state();
    f4 << state.time << " "
       <<  state.location.Longitude << " "
       <<  state.location.Latitude << " "
       <<  state.altitude << "\n";
    f4 << "\n";
    f4.flush();
    task_report(task_manager, "end of task\n");
  }
  wait_prompt();

  time_elapsed = task_manager.get_stats().total.time_elapsed;
  time_planned = task_manager.get_stats().total.time_planned;
  calc_cruise_efficiency = task_manager.get_stats().cruise_efficiency;
  calc_effective_mc = task_manager.get_stats().effective_mc;

  if (verbose)
    distance_counts();

  if (airspace_warnings)
    delete airspace_warnings;

  return true;
}