// 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]
}
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]
}
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]
}
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
}