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