Ejemplo n.º 1
0
// approach control, 
void approach_control(double time, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){

	double phi   = navData_ptr->phi;						// roll angle
	double theta = navData_ptr->the - base_pitch_cmd; 		// subtract theta trim value to convert to delta coordinates
	double p     = sensorData_ptr->imuData_ptr->p; 			// roll rate
	double q     = sensorData_ptr->imuData_ptr->q; 			// pitch rate
	double ias   = sensorData_ptr->adData_ptr->ias_filt;	// filtered airspeed
	double ias_cmd, phi_cmd, theta_cmd;
	
	controlData_ptr->theta_cmd 	= approach_theta - base_pitch_cmd;	// delta theta command [rad]
	controlData_ptr->ias_cmd 	= approach_speed;					// absolute airspeed command [m/s]
	controlData_ptr->phi_cmd	= pilot_phi(sensorData_ptr);		// phi from the pilot stick
	
	theta_cmd 	= controlData_ptr->theta_cmd;
	ias_cmd		= controlData_ptr->ias_cmd;
	phi_cmd		= controlData_ptr->phi_cmd;
	
	controlData_ptr->dthr 	= speed_control(ias_cmd, ias, TIMESTEP);			// Throttle [ND 0-1]
	controlData_ptr->l1   	= 0;												// L1 [rad]
    controlData_ptr->l2   	= roll_control(phi_cmd, phi, p, TIMESTEP, 0);		// L2 [rad]
	controlData_ptr->l3   	= pitch_control(theta_cmd, theta, q, TIMESTEP, 0);	// L3 [rad]
	controlData_ptr->l4   	= controlData_ptr->l3 + controlData_ptr->l2; 		// L4 [rad]
    controlData_ptr->r1   	= 0; 												// R1 [rad]
	controlData_ptr->r2   	= -1*controlData_ptr->l2; 							// R2 [rad]
	controlData_ptr->r3   	= controlData_ptr->l3;								// R3 [rad]
	controlData_ptr->r4   	= controlData_ptr->r3 + controlData_ptr->r2; 		// R4 [rad]
}
Ejemplo n.º 2
0
void pilot_flying_inner(double time, double ias_cmd, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){
	double phi   = navData_ptr->phi;						// roll angle
	double theta = navData_ptr->the - base_pitch_cmd; 		// subtract theta trim value to convert to delta coordinates
	double p     = sensorData_ptr->imuData_ptr->p; 			// roll rate
	double q     = sensorData_ptr->imuData_ptr->q; 			// pitch rate
	double ias   = sensorData_ptr->adData_ptr->ias_filt;	// filtered airspeed
	double phi_cmd, theta_cmd;
	
	controlData_ptr->phi_cmd	= pilot_phi(sensorData_ptr) + controlData_ptr->phi_cmd;		// phi from the pilot stick + guidance
	controlData_ptr->theta_cmd	= pilot_theta(sensorData_ptr) + controlData_ptr->theta_cmd;	// theta from the pilot stick + guidance
	controlData_ptr->ias_cmd = ias_cmd;
	
	theta_cmd 	= controlData_ptr->theta_cmd;
	phi_cmd		= controlData_ptr->phi_cmd;
	
	controlData_ptr->dthr 	= speed_control(ias_cmd, ias, TIMESTEP);			// Throttle [ND 0-1]
	controlData_ptr->l1   	= 0;												// L1 [rad]
    controlData_ptr->l2   	= roll_control(phi_cmd, phi, p, TIMESTEP, 1);		// L2 [rad]
	controlData_ptr->l3   	= pitch_control(theta_cmd, theta, q, TIMESTEP, 1);	// L3 [rad]
	controlData_ptr->l4   	= 0; 												// L4 [rad]
    controlData_ptr->r1   	= 0; 												// R1 [rad]
	controlData_ptr->r2   	= -1*controlData_ptr->l2; 							// R2 [rad]
	controlData_ptr->r3   	= controlData_ptr->l3;								// R3 [rad]
	controlData_ptr->r4   	= 0; 												// R4 [rad]
}
Ejemplo n.º 3
0
void flare_control(double time, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){
	double ramp_time = 6.0;
	double min_speed = 8;
	double cut_time  = 5;
	double phi   = navData_ptr->phi;						// roll angle
	double theta = navData_ptr->the - base_pitch_cmd; 		// subtract theta trim value to convert to delta coordinates
	double p     = sensorData_ptr->imuData_ptr->p; 			// roll rate
	double q     = sensorData_ptr->imuData_ptr->q; 			// pitch rate
	double ias   = sensorData_ptr->adData_ptr->ias_filt;	// filtered airspeed
	static int t1_latched = FALSE;	// time latching
	static double t1 = 0;
	double ias_cmd, phi_cmd, theta_cmd;
	
	if(time <= ramp_time){ // ramp commands
		ias_cmd = approach_speed - time*((approach_speed - flare_speed)/ramp_time);
		theta_cmd = approach_theta + time*((flare_theta - approach_theta)/ramp_time);
	}
	else{ // final values
		ias_cmd = flare_speed;
		theta_cmd = flare_theta;
	}
	
	if(ias < min_speed){ // engine cutoff
		if(t1_latched == FALSE){
			t1 = time;
			t1_latched = TRUE;
		}
		if((time-t1) > cut_time){
			ias_cmd = -100;	
		}
	}
	else{
		t1_latched = FALSE;
	}
				
	controlData_ptr->theta_cmd 	= theta_cmd - base_pitch_cmd;	// delta theta command [rad]
	controlData_ptr->ias_cmd 	= ias_cmd;						// absolute airspeed command [m/s]
	controlData_ptr->phi_cmd	= 0;							// wings level
	
	theta_cmd 	= controlData_ptr->theta_cmd;
	ias_cmd		= controlData_ptr->ias_cmd;
	phi_cmd		= controlData_ptr->phi_cmd;
	
	controlData_ptr->dthr 	= speed_control(ias_cmd, ias, TIMESTEP);			// Throttle [ND 0-1]
	controlData_ptr->l1   	= 0;												// L1 [rad]
    controlData_ptr->l2   	= roll_control(phi_cmd, phi, p, TIMESTEP, 0);		// L2 [rad]
	controlData_ptr->l3   	= pitch_control(theta_cmd, theta, q, TIMESTEP, 0);	// L3 [rad]
	controlData_ptr->l4   	= controlData_ptr->l3 + controlData_ptr->l2; 		// L4 [rad]
    controlData_ptr->r1   	= 0; 												// R1 [rad]
	controlData_ptr->r2   	= -1*controlData_ptr->l2; 							// R2 [rad]
	controlData_ptr->r3   	= controlData_ptr->l3;								// R3 [rad]
	controlData_ptr->r4   	= controlData_ptr->r3 + controlData_ptr->r2; 		// R4 [rad]
}
Ejemplo n.º 4
0
extern void get_control(double time, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr, struct mission *missionData_ptr) {
/// Return control outputs based on references and feedback signals.

#ifdef AIRCRAFT_THOR
	double base_pitch_cmd= 0.0872664;  // (Thor Trim value) use 5 deg (0.0872664 rad) for flight, use 3.082 deg (0.0537910 rad) in sim
#endif
#ifdef AIRCRAFT_TYR
	double base_pitch_cmd= 0.0872664;  // (Thor Trim value) use 5 deg (0.0872664 rad) for flight, use 3.082 deg (0.0537910 rad) in sim
#endif
#ifdef AIRCRAFT_FASER
	double base_pitch_cmd= 0.0872664;  // (Faser Trim value) use 5 deg (0.0872664  rad) for flight, use 4.669 deg (0.0814990 rad) in sim
#endif
#ifdef AIRCRAFT_IBIS
	double base_pitch_cmd= 0.0872664;  // (Faser Trim value) use 5 deg (0.0872664  rad) for flight, use 4.669 deg (0.0814990 rad) in sim
#endif
#ifdef AIRCRAFT_BALDR
	double base_pitch_cmd= 0.0872664;  // (Faser Trim value) use 5 deg (0.0872664  rad) for flight, use 4.669 deg (0.0814990 rad) in sim
#endif
#ifdef HIL_SIM
	double base_pitch_cmd= 0.0872664;  // (Thor Trim value) use 5 deg (0.0872664 rad) for flight, use 3.082 deg (0.0537910 rad) in sim
#endif

	double phi   = navData_ptr->phi;
	double theta = navData_ptr->the - base_pitch_cmd; //subtract theta trim value to convert to delta coordinates
	double p     = sensorData_ptr->imuData_ptr->p; // Roll rate
	double q     = sensorData_ptr->imuData_ptr->q; // Pitch rate
	double r     = sensorData_ptr->imuData_ptr->r; // Yaw rate

    double phi_cmd = controlData_ptr->phi_cmd;
    double theta_cmd = controlData_ptr->theta_cmd;

    controlData_ptr->de = pitch_control(theta_cmd, theta, q, TIMESTEP); // Elevator deflection [rad]
    controlData_ptr->dr = yaw_damper(r); 								// Rudder deflection [rad]
    controlData_ptr->da_r = roll_control(phi_cmd, phi, p, TIMESTEP); 		// Right Aileron deflection [rad]
	controlData_ptr->da_l = -controlData_ptr->da_r; 					// Left aileron deflection [rad]
	controlData_ptr->dthr = 0; // throttle
	controlData_ptr->df_l = 0; // left flap
	controlData_ptr->df_r = 0; // right flap
}