From fbee3d10f0aafd4178fc1313edb8593c156b2874 Mon Sep 17 00:00:00 2001 From: curt Date: Tue, 16 Mar 2004 04:01:38 +0000 Subject: [PATCH] Rob Deters: UIUC updates from March 1, 2004. --- src/FDM/LaRCsim/LaRCsim.cxx | 2 +- src/FDM/UIUCModel/Makefile.am | 3 + src/FDM/UIUCModel/uiuc_aerodeflections.cpp | 28 +- src/FDM/UIUCModel/uiuc_aircraft.h | 210 +++++++-- src/FDM/UIUCModel/uiuc_alh_ap.cpp | 16 +- src/FDM/UIUCModel/uiuc_alh_ap.h | 2 + src/FDM/UIUCModel/uiuc_auto_pilot.cpp | 166 +++++++ src/FDM/UIUCModel/uiuc_auto_pilot.h | 16 + src/FDM/UIUCModel/uiuc_coef_lift.cpp | 4 +- src/FDM/UIUCModel/uiuc_coefficients.cpp | 43 +- src/FDM/UIUCModel/uiuc_coefficients.h | 3 +- src/FDM/UIUCModel/uiuc_hh_ap.cpp | 139 ++++++ src/FDM/UIUCModel/uiuc_hh_ap.h | 12 + src/FDM/UIUCModel/uiuc_icing_demo.cpp | 52 ++- src/FDM/UIUCModel/uiuc_map_controlSurface.cpp | 11 +- src/FDM/UIUCModel/uiuc_map_ice.cpp | 10 +- src/FDM/UIUCModel/uiuc_map_init.cpp | 2 +- src/FDM/UIUCModel/uiuc_map_misc.cpp | 4 +- src/FDM/UIUCModel/uiuc_map_record1.cpp | 3 + src/FDM/UIUCModel/uiuc_map_record3.cpp | 7 +- src/FDM/UIUCModel/uiuc_map_record5.cpp | 12 +- src/FDM/UIUCModel/uiuc_map_record6.cpp | 21 +- src/FDM/UIUCModel/uiuc_menu_CL.cpp | 16 +- .../UIUCModel/uiuc_menu_controlSurface.cpp | 90 ++++ src/FDM/UIUCModel/uiuc_menu_ice.cpp | 136 +++++- src/FDM/UIUCModel/uiuc_menu_init.cpp | 6 +- src/FDM/UIUCModel/uiuc_menu_misc.cpp | 18 +- src/FDM/UIUCModel/uiuc_pah_ap.cpp | 24 +- src/FDM/UIUCModel/uiuc_rah_ap.cpp | 137 ++++++ src/FDM/UIUCModel/uiuc_rah_ap.h | 12 + src/FDM/UIUCModel/uiuc_recorder.cpp | 410 +++++++++++++----- src/FDM/UIUCModel/uiuc_wrapper.cpp | 24 +- 32 files changed, 1339 insertions(+), 300 deletions(-) create mode 100644 src/FDM/UIUCModel/uiuc_auto_pilot.cpp create mode 100644 src/FDM/UIUCModel/uiuc_auto_pilot.h create mode 100644 src/FDM/UIUCModel/uiuc_hh_ap.cpp create mode 100644 src/FDM/UIUCModel/uiuc_hh_ap.h create mode 100644 src/FDM/UIUCModel/uiuc_rah_ap.cpp create mode 100644 src/FDM/UIUCModel/uiuc_rah_ap.h diff --git a/src/FDM/LaRCsim/LaRCsim.cxx b/src/FDM/LaRCsim/LaRCsim.cxx index f69bd9939..708b60952 100644 --- a/src/FDM/LaRCsim/LaRCsim.cxx +++ b/src/FDM/LaRCsim/LaRCsim.cxx @@ -325,7 +325,7 @@ void FGLaRCsim::update( double dt ) { // flaps with transition occuring in uiuc_aerodeflections.cpp if (use_flaps) { - fgSetDouble("/surface-positions/flight/flap-pos-norm", flap_pos_pct); + fgSetDouble("/surface-positions/flight/flap-pos-norm", flap_pos_norm); } // spoilers with transition occurring in uiuc_aerodeflections.cpp diff --git a/src/FDM/UIUCModel/Makefile.am b/src/FDM/UIUCModel/Makefile.am index 914721ae7..56ccf9a85 100644 --- a/src/FDM/UIUCModel/Makefile.am +++ b/src/FDM/UIUCModel/Makefile.am @@ -11,6 +11,7 @@ libUIUCModel_a_SOURCES = \ uiuc_aerodeflections.cpp uiuc_aerodeflections.h \ uiuc_aircraftdir.h uiuc_aircraft.h \ uiuc_alh_ap.cpp uiuc_alh_ap.h \ + uiuc_auto_pilot.cpp uiuc_auto_pilot.h \ uiuc_betaprobe.cpp uiuc_betaprobe.h \ uiuc_coef_drag.cpp uiuc_coef_drag.h \ uiuc_coef_lift.cpp uiuc_coef_lift.h \ @@ -28,6 +29,7 @@ libUIUCModel_a_SOURCES = \ uiuc_gear.cpp uiuc_gear.h\ uiuc_get_flapper.cpp uiuc_get_flapper.h \ uiuc_getwind.cpp uiuc_getwind.h \ + uiuc_hh_ap.cpp uiuc_hh_ap.h \ uiuc_ice.cpp uiuc_ice.h \ uiuc_iceboot.cpp uiuc_iceboot.h \ uiuc_iced_nonlin.cpp uiuc_iced_nonlin.h \ @@ -75,6 +77,7 @@ libUIUCModel_a_SOURCES = \ uiuc_menu_functions.cpp uiuc_menu_functions.h \ uiuc_pah_ap.cpp uiuc_pah_ap.h \ uiuc_parsefile.cpp uiuc_parsefile.h \ + uiuc_rah_ap.cpp uiuc_rah_ap.h \ uiuc_recorder.cpp uiuc_recorder.h \ uiuc_warnings_errors.cpp uiuc_warnings_errors.h \ uiuc_wrapper.cpp uiuc_wrapper.h diff --git a/src/FDM/UIUCModel/uiuc_aerodeflections.cpp b/src/FDM/UIUCModel/uiuc_aerodeflections.cpp index 7293a9b0b..63b1d21fe 100644 --- a/src/FDM/UIUCModel/uiuc_aerodeflections.cpp +++ b/src/FDM/UIUCModel/uiuc_aerodeflections.cpp @@ -30,6 +30,9 @@ 03/03/2003 (RD) changed flap code to call uiuc_find_position to determine flap position + 08/20/2003 (RD) changed spoiler variables and code + to match flap conventions. Changed + flap_pos_pct to flap_pos_norm ---------------------------------------------------------------------- @@ -299,26 +302,19 @@ void uiuc_aerodeflections( double dt ) // determine flap position [rad] with respect to flap command flap_pos = uiuc_find_position(flap_cmd,flap_increment_per_timestep,flap_pos); // get the normalized position - flap_pos_pct = flap_pos/(flap_max * DEG_TO_RAD); + flap_pos_norm = flap_pos/(flap_max * DEG_TO_RAD); } if(use_spoilers) { - // angle of spoilers desired [deg] - spoiler_cmd_deg = Spoiler_handle; - // amount spoilers move per time step [deg] - spoiler_increment_per_timestep = spoiler_rate * dt; - // determine spoiler position with respect to spoiler command - if (spoiler_pos_deg < spoiler_cmd_deg) { - spoiler_pos_deg += spoiler_increment_per_timestep; - if (spoiler_pos_deg > spoiler_cmd_deg) - spoiler_pos_deg = spoiler_cmd_deg; - } else if (spoiler_pos_deg > spoiler_cmd_deg) { - spoiler_pos_deg -= spoiler_increment_per_timestep; - if (spoiler_pos_deg < spoiler_cmd_deg) - spoiler_pos_deg = spoiler_cmd_deg; - } - spoiler_pos = spoiler_pos_deg * DEG_TO_RAD; + // angle of spoilers desired [rad] + spoiler_cmd = Spoiler_handle * DEG_TO_RAD; + // amount spoilers move per time step [rad/sec] + spoiler_increment_per_timestep = spoiler_rate * dt * DEG_TO_RAD; + // determine spoiler position [rad] with respect to spoiler command + spoiler_pos = uiuc_find_position(spoiler_cmd,spoiler_increment_per_timestep,spoiler_pos); + // get the normailized position + spoiler_pos_norm = spoiler_pos/(spoiler_max * DEG_TO_RAD); } diff --git a/src/FDM/UIUCModel/uiuc_aircraft.h b/src/FDM/UIUCModel/uiuc_aircraft.h index c517c47e9..bf40f5cc3 100644 --- a/src/FDM/UIUCModel/uiuc_aircraft.h +++ b/src/FDM/UIUCModel/uiuc_aircraft.h @@ -73,6 +73,13 @@ 09/18/2002 (MSS) Added downwash options 03/03/2003 (RD) Changed flap_cmd_deg to flap_cmd (rad) 03/16/2003 (RD) Added trigger variables + 08/20/2003 (RD) Removed old_flap_routine. Changed spoiler + variables to match flap convention. Changed + flap_pos_pct to flap_pos_norm + 10/31/2003 (RD) Added variables and keywords for pah and alh + autopilots + 11/04/2003 (RD) Added variables and keywords for rah and hh + autopilots ---------------------------------------------------------------------- @@ -196,7 +203,6 @@ enum {Dx_pilot_flag = 2000, ignore_unknown_keywords_flag, trim_case_2_flag, use_uiuc_network_flag, - old_flap_routine_flag, icing_demo_flag, outside_control_flag}; @@ -225,7 +231,9 @@ enum {de_flag = 4000, da_flag, dr_flag, rudder_stick_gain_flag, use_elevator_sas_type1_flag, use_aileron_sas_type1_flag, - use_rudder_sas_type1_flag}; + use_rudder_sas_type1_flag, + ap_pah_flag, ap_alh_flag, ap_Theta_ref_flag, ap_alt_ref_flag, + ap_rah_flag, ap_Phi_ref_flag, ap_hh_flag, ap_Psi_ref_flag}; // controlsMixer == Controls mixer enum {nomix_flag = 5000}; @@ -349,8 +357,11 @@ enum {iceTime_flag = 15000, transientTime_flag, eta_ice_final_flag, demo_eps_airspeed_max_flag, demo_eps_airspeed_min_flag, demo_boot_cycle_tail_flag, demo_boot_cycle_wing_left_flag, demo_boot_cycle_wing_right_flag, demo_eps_pitch_input_flag, - tactilefadef_flag, tactile_pitch_flag, demo_ap_Theta_ref_deg_flag, - demo_ap_pah_on_flag, demo_tactile_flag, demo_ice_tail_flag, + tactilefadef_flag, tactile_pitch_flag, demo_ap_pah_on_flag, + demo_ap_alh_on_flag, demo_ap_rah_on_flag, demo_ap_hh_on_flag, + demo_ap_Theta_ref_flag, demo_ap_alt_ref_flag, + demo_ap_Phi_ref_flag, demo_ap_Psi_ref_flag, + demo_tactile_flag, demo_ice_tail_flag, demo_ice_left_flag, demo_ice_right_flag}; // record ===== Record desired quantites to file @@ -370,7 +381,8 @@ enum {Simtime_record = 16000, dt_record, Dx_cg_record, Dy_cg_record, Dz_cg_record, Lat_geocentric_record, Lon_geocentric_record, Radius_to_vehicle_record, Latitude_record, Longitude_record, Altitude_record, - Phi_record, Theta_record, Psi_record, + Phi_record, Theta_record, Psi_record, + Phi_deg_record, Theta_deg_record, Psi_deg_record, // added to uiuc_map_record1.cpp V_dot_north_record, V_dot_east_record, V_dot_down_record, @@ -425,10 +437,11 @@ enum {Simtime_record = 16000, dt_record, elevator_record, elevator_deg_record, Lat_control_record, aileron_record, aileron_deg_record, Rudder_pedal_record, rudder_record, rudder_deg_record, - Flap_handle_record, flap_record, flap_cmd_record, flap_cmd_deg_record, - flap_pos_record, flap_pos_deg_record, + Flap_handle_record, flap_cmd_record, flap_cmd_deg_record, + flap_pos_record, flap_pos_deg_record, flap_pos_norm_record, Spoiler_handle_record, spoiler_cmd_deg_record, spoiler_pos_deg_record, spoiler_pos_norm_record, spoiler_pos_record, + spoiler_cmd_record, // added to uiuc_map_record4.cpp CD_record, CDfaI_record, CDfCLI_record, CDfadeI_record, CDfdfI_record, @@ -492,6 +505,7 @@ enum {Simtime_record = 16000, dt_record, M_l_engine_record, M_m_engine_record, M_n_engine_record, M_l_gear_record, M_m_gear_record, M_n_gear_record, M_l_rp_record, M_m_rp_record, M_n_rp_record, + M_l_cg_record, M_m_cg_record, M_n_cg_record, // added to uiuc_map_record5.cpp flapper_freq_record, flapper_phi_record, @@ -524,6 +538,10 @@ enum {Simtime_record = 16000, dt_record, debug4_record, debug5_record, debug6_record, + debug7_record, + debug8_record, + debug9_record, + debug10_record, // added to uiuc_map_record6.cpp CL_clean_record, CL_iced_record, @@ -563,13 +581,23 @@ enum {Simtime_record = 16000, dt_record, tactilefadefI_record, // added to uiuc_map_record6.cpp - ap_Theta_ref_deg_record, ap_pah_on_record, trigger_on_record, - trigger_num_record, trigger_toggle_record, trigger_counter_record}; + ap_pah_on_record, ap_alh_on_record, ap_Theta_ref_deg_record, + ap_Theta_ref_rad_record, ap_alt_ref_ft_record, trigger_on_record, + ap_rah_on_record, ap_Phi_ref_rad_record, ap_Phi_ref_deg_record, + ap_hh_on_record, ap_Psi_ref_rad_record, ap_Psi_ref_deg_record, + trigger_num_record, trigger_toggle_record, trigger_counter_record, + + // added to uiuc_map_record6.cpp + T_local_to_body_11_record, T_local_to_body_12_record, + T_local_to_body_13_record, T_local_to_body_21_record, + T_local_to_body_22_record, T_local_to_body_23_record, + T_local_to_body_31_record, T_local_to_body_32_record, + T_local_to_body_33_record}; // misc ======= Miscellaneous inputs // added to uiuc_map_misc.cpp -enum {simpleHingeMomentCoef_flag = 17000, dfTimefdf_flag, flapper_flag, +enum {simpleHingeMomentCoef_flag = 17000, flapper_flag, flapper_phi_init_flag}; //321654 @@ -742,8 +770,6 @@ struct AIRCRAFT #define use_uiuc_network aircraft_->use_uiuc_network #define server_IP aircraft_->server_IP #define port_num aircraft_->port_num - bool old_flap_routine; -#define old_flap_routine aircraft_->old_flap_routine bool icing_demo; #define icing_demo aircraft_->icing_demo bool outside_control; @@ -785,8 +811,8 @@ struct AIRCRAFT #define aileron aircraft_->aileron #define elevator aircraft_->elevator #define rudder aircraft_->rudder - double flap; -#define flap aircraft_->flap + // double flap; + //#define flap aircraft_->flap bool set_Long_trim, zero_Long_trim; double Long_trim_constant; @@ -2631,18 +2657,90 @@ struct AIRCRAFT #define demo_ap_pah_on_daArray aircraft_->demo_ap_pah_on_daArray #define demo_ap_pah_on_ntime aircraft_->demo_ap_pah_on_ntime #define demo_ap_pah_on_startTime aircraft_->demo_ap_pah_on_startTime - bool demo_ap_Theta_ref_deg; - string demo_ap_Theta_ref_deg_file; - double demo_ap_Theta_ref_deg_timeArray[10]; - double demo_ap_Theta_ref_deg_daArray[10]; - int demo_ap_Theta_ref_deg_ntime; - double demo_ap_Theta_ref_deg_startTime; -#define demo_ap_Theta_ref_deg aircraft_->demo_ap_Theta_ref_deg -#define demo_ap_Theta_ref_deg_file aircraft_->demo_ap_Theta_ref_deg_file -#define demo_ap_Theta_ref_deg_timeArray aircraft_->demo_ap_Theta_ref_deg_timeArray -#define demo_ap_Theta_ref_deg_daArray aircraft_->demo_ap_Theta_ref_deg_daArray -#define demo_ap_Theta_ref_deg_ntime aircraft_->demo_ap_Theta_ref_deg_ntime -#define demo_ap_Theta_ref_deg_startTime aircraft_->demo_ap_Theta_ref_deg_startTime + bool demo_ap_alh_on; + string demo_ap_alh_on_file; + double demo_ap_alh_on_timeArray[10]; + int demo_ap_alh_on_daArray[10]; + int demo_ap_alh_on_ntime; + double demo_ap_alh_on_startTime; +#define demo_ap_alh_on aircraft_->demo_ap_alh_on +#define demo_ap_alh_on_file aircraft_->demo_ap_alh_on_file +#define demo_ap_alh_on_timeArray aircraft_->demo_ap_alh_on_timeArray +#define demo_ap_alh_on_daArray aircraft_->demo_ap_alh_on_daArray +#define demo_ap_alh_on_ntime aircraft_->demo_ap_alh_on_ntime +#define demo_ap_alh_on_startTime aircraft_->demo_ap_alh_on_startTime + bool demo_ap_rah_on; + string demo_ap_rah_on_file; + double demo_ap_rah_on_timeArray[10]; + int demo_ap_rah_on_daArray[10]; + int demo_ap_rah_on_ntime; + double demo_ap_rah_on_startTime; +#define demo_ap_rah_on aircraft_->demo_ap_rah_on +#define demo_ap_rah_on_file aircraft_->demo_ap_rah_on_file +#define demo_ap_rah_on_timeArray aircraft_->demo_ap_rah_on_timeArray +#define demo_ap_rah_on_daArray aircraft_->demo_ap_rah_on_daArray +#define demo_ap_rah_on_ntime aircraft_->demo_ap_rah_on_ntime +#define demo_ap_rah_on_startTime aircraft_->demo_ap_rah_on_startTime + bool demo_ap_hh_on; + string demo_ap_hh_on_file; + double demo_ap_hh_on_timeArray[10]; + int demo_ap_hh_on_daArray[10]; + int demo_ap_hh_on_ntime; + double demo_ap_hh_on_startTime; +#define demo_ap_hh_on aircraft_->demo_ap_hh_on +#define demo_ap_hh_on_file aircraft_->demo_ap_hh_on_file +#define demo_ap_hh_on_timeArray aircraft_->demo_ap_hh_on_timeArray +#define demo_ap_hh_on_daArray aircraft_->demo_ap_hh_on_daArray +#define demo_ap_hh_on_ntime aircraft_->demo_ap_hh_on_ntime +#define demo_ap_hh_on_startTime aircraft_->demo_ap_hh_on_startTime + bool demo_ap_Theta_ref; + string demo_ap_Theta_ref_file; + double demo_ap_Theta_ref_timeArray[10]; + double demo_ap_Theta_ref_daArray[10]; + int demo_ap_Theta_ref_ntime; + double demo_ap_Theta_ref_startTime; +#define demo_ap_Theta_ref aircraft_->demo_ap_Theta_ref +#define demo_ap_Theta_ref_file aircraft_->demo_ap_Theta_ref_file +#define demo_ap_Theta_ref_timeArray aircraft_->demo_ap_Theta_ref_timeArray +#define demo_ap_Theta_ref_daArray aircraft_->demo_ap_Theta_ref_daArray +#define demo_ap_Theta_ref_ntime aircraft_->demo_ap_Theta_ref_ntime +#define demo_ap_Theta_ref_startTime aircraft_->demo_ap_Theta_ref_startTime + bool demo_ap_alt_ref; + string demo_ap_alt_ref_file; + double demo_ap_alt_ref_timeArray[10]; + double demo_ap_alt_ref_daArray[10]; + int demo_ap_alt_ref_ntime; + double demo_ap_alt_ref_startTime; +#define demo_ap_alt_ref aircraft_->demo_ap_alt_ref +#define demo_ap_alt_ref_file aircraft_->demo_ap_alt_ref_file +#define demo_ap_alt_ref_timeArray aircraft_->demo_ap_alt_ref_timeArray +#define demo_ap_alt_ref_daArray aircraft_->demo_ap_alt_ref_daArray +#define demo_ap_alt_ref_ntime aircraft_->demo_ap_alt_ref_ntime +#define demo_ap_alt_ref_startTime aircraft_->demo_ap_alt_ref_startTime + bool demo_ap_Phi_ref; + string demo_ap_Phi_ref_file; + double demo_ap_Phi_ref_timeArray[10]; + double demo_ap_Phi_ref_daArray[10]; + int demo_ap_Phi_ref_ntime; + double demo_ap_Phi_ref_startTime; +#define demo_ap_Phi_ref aircraft_->demo_ap_Phi_ref +#define demo_ap_Phi_ref_file aircraft_->demo_ap_Phi_ref_file +#define demo_ap_Phi_ref_timeArray aircraft_->demo_ap_Phi_ref_timeArray +#define demo_ap_Phi_ref_daArray aircraft_->demo_ap_Phi_ref_daArray +#define demo_ap_Phi_ref_ntime aircraft_->demo_ap_Phi_ref_ntime +#define demo_ap_Phi_ref_startTime aircraft_->demo_ap_Phi_ref_startTime + bool demo_ap_Psi_ref; + string demo_ap_Psi_ref_file; + double demo_ap_Psi_ref_timeArray[10]; + double demo_ap_Psi_ref_daArray[10]; + int demo_ap_Psi_ref_ntime; + double demo_ap_Psi_ref_startTime; +#define demo_ap_Psi_ref aircraft_->demo_ap_Psi_ref +#define demo_ap_Psi_ref_file aircraft_->demo_ap_Psi_ref_file +#define demo_ap_Psi_ref_timeArray aircraft_->demo_ap_Psi_ref_timeArray +#define demo_ap_Psi_ref_daArray aircraft_->demo_ap_Psi_ref_daArray +#define demo_ap_Psi_ref_ntime aircraft_->demo_ap_Psi_ref_ntime +#define demo_ap_Psi_ref_startTime aircraft_->demo_ap_Psi_ref_startTime bool demo_tactile; string demo_tactile_file; double demo_tactile_timeArray[1500]; @@ -2807,14 +2905,14 @@ struct AIRCRAFT double simpleHingeMomentCoef; #define simpleHingeMomentCoef aircraft_->simpleHingeMomentCoef - string dfTimefdf; - double dfTimefdf_dfArray[100]; - double dfTimefdf_TimeArray[100]; - int dfTimefdf_ndf; -#define dfTimefdf aircraft_->dfTimefdf -#define dfTimefdf_dfArray aircraft_->dfTimefdf_dfArray -#define dfTimefdf_TimeArray aircraft_->dfTimefdf_TimeArray -#define dfTimefdf_ndf aircraft_->dfTimefdf_ndf + //string dfTimefdf; + //double dfTimefdf_dfArray[100]; + //double dfTimefdf_TimeArray[100]; + //int dfTimefdf_ndf; + //#define dfTimefdf aircraft_->dfTimefdf + //#define dfTimefdf_dfArray aircraft_->dfTimefdf_dfArray + //#define dfTimefdf_TimeArray aircraft_->dfTimefdf_TimeArray + //#define dfTimefdf_ndf aircraft_->dfTimefdf_ndf FlapData *flapper_data; #define flapper_data aircraft_->flapper_data @@ -2855,21 +2953,18 @@ struct AIRCRAFT #define dfArray aircraft_->dfArray #define TimeArray aircraft_->TimeArray - double flap_percent, flap_increment_per_timestep, flap_cmd, flap_pos, flap_pos_pct; + double flap_percent, flap_increment_per_timestep, flap_cmd, flap_pos, flap_pos_norm; #define flap_percent aircraft_->flap_percent #define flap_increment_per_timestep aircraft_->flap_increment_per_timestep #define flap_cmd aircraft_->flap_cmd - //#define flap_cmd_deg aircraft_->flap_cmd_deg #define flap_pos aircraft_->flap_pos - //#define flap_pos_deg aircraft_->flap_pos_deg -#define flap_pos_pct aircraft_->flap_pos_pct +#define flap_pos_norm aircraft_->flap_pos_norm - double Spoiler_handle, spoiler_increment_per_timestep, spoiler_cmd_deg; - double spoiler_pos_norm, spoiler_pos_deg, spoiler_pos; + double Spoiler_handle, spoiler_increment_per_timestep, spoiler_cmd; + double spoiler_pos_norm, spoiler_pos; #define Spoiler_handle aircraft_->Spoiler_handle #define spoiler_increment_per_timestep aircraft_->spoiler_increment_per_timestep -#define spoiler_cmd_deg aircraft_->spoiler_cmd_deg -#define spoiler_pos_deg aircraft_->spoiler_pos_deg +#define spoiler_cmd aircraft_->spoiler_cmd #define spoiler_pos_norm aircraft_->spoiler_pos_norm #define spoiler_pos aircraft_->spoiler_pos @@ -2951,15 +3046,31 @@ struct AIRCRAFT int ap_pah_on; #define ap_pah_on aircraft_->ap_pah_on - double ap_Theta_ref_deg, ap_Theta_ref_rad; -#define ap_Theta_ref_deg aircraft_->ap_Theta_ref_deg + double ap_pah_start_time; +#define ap_pah_start_time aircraft_->ap_pah_start_time + double ap_Theta_ref_rad; #define ap_Theta_ref_rad aircraft_->ap_Theta_ref_rad int ap_alh_on; #define ap_alh_on aircraft_->ap_alh_on - double ap_alt_ref_ft, ap_alt_ref_m; + double ap_alh_start_time; +#define ap_alh_start_time aircraft_->ap_alh_start_time + double ap_alt_ref_ft; #define ap_alt_ref_ft aircraft_->ap_alt_ref_ft -#define ap_alt_ref_m aircraft_->ap_alt_ref_m + + int ap_rah_on; +#define ap_rah_on aircraft_->ap_rah_on + double ap_rah_start_time; +#define ap_rah_start_time aircraft_->ap_rah_start_time + double ap_Phi_ref_rad; +#define ap_Phi_ref_rad aircraft_->ap_Phi_ref_rad + + int ap_hh_on; +#define ap_hh_on aircraft_->ap_hh_on + double ap_hh_start_time; +#define ap_hh_start_time aircraft_->ap_hh_start_time + double ap_Psi_ref_rad; +#define ap_Psi_ref_rad aircraft_->ap_Psi_ref_rad int pitch_trim_up, pitch_trim_down; #define pitch_trim_up aircraft_->pitch_trim_up @@ -2981,6 +3092,13 @@ struct AIRCRAFT #define trigger_num aircraft_->trigger_num #define trigger_toggle aircraft_->trigger_toggle #define trigger_counter aircraft_->trigger_counter + + // temp debug values + double debug7, debug8, debug9, debug10; +#define debug7 aircraft_->debug7 +#define debug8 aircraft_->debug8 +#define debug9 aircraft_->debug9 +#define debug10 aircraft_->debug10 }; extern AIRCRAFT *aircraft_; // usually defined in the first program that includes uiuc_aircraft.h diff --git a/src/FDM/UIUCModel/uiuc_alh_ap.cpp b/src/FDM/UIUCModel/uiuc_alh_ap.cpp index ab908ab40..c7e5a8253 100644 --- a/src/FDM/UIUCModel/uiuc_alh_ap.cpp +++ b/src/FDM/UIUCModel/uiuc_alh_ap.cpp @@ -40,7 +40,7 @@ #include "uiuc_alh_ap.h" double alh_ap(double pitch, double pitchrate, double H_ref, double H, - double V, double sample_t, int init) + double V, double sample_time, int init) { // changes by RD so function keeps previous values static double u2prev; @@ -49,8 +49,6 @@ double alh_ap(double pitch, double pitchrate, double H_ref, double H, static double x3prev; static double ubarprev; - double pi = 3.14159; - if (init == 0) { u2prev = 0; @@ -69,12 +67,12 @@ double alh_ap(double pitch, double pitchrate, double H_ref, double H, Ktheta = -0.0004*V*V + 0.0479*V - 2.409; Kq = -0.0005*V*V + 0.054*V - 1.5931; Ki = 0.5; - Kh = -0.25*pi/180 + (((-0.15 + 0.25)*pi/180)/(20))*(V-60); + Kh = -0.25*LS_PI/180 + (((-0.15 + 0.25)*LS_PI/180)/(20))*(V-60); Kd = -0.0025*V + 0.2875; double u1,u2,u3,ubar; - ubar = (1-Kd*sample_t)*ubarprev + Ktheta*pitchrate*sample_t; + ubar = (1-Kd*sample_time)*ubarprev + Ktheta*pitchrate*sample_time; u1 = Kh*(H_ref-H) - ubar; - u2 = u2prev + Ki*(Kh*(H_ref-H)-ubar)*sample_t; + u2 = u2prev + Ki*(Kh*(H_ref-H)-ubar)*sample_time; u3 = Kq*pitchrate; double totalU; totalU = u1 + u2 - u3; @@ -83,10 +81,10 @@ double alh_ap(double pitch, double pitchrate, double H_ref, double H, // the following is using the actuator dynamics given in Beaver. // the actuator dynamics for Twin Otter are still unavailable. x1 = x1prev +(-10.951*x1prev + 7.2721*x2prev + 20.7985*x3prev + -25.1568*totalU)*sample_t; - x2 = x2prev + x3prev*sample_t; +25.1568*totalU)*sample_time; + x2 = x2prev + x3prev*sample_time; x3 = x3prev + (7.3446*x1prev - 668.6713*x2prev - 16.8697*x3prev + -5.8694*totalU)*sample_t; +5.8694*totalU)*sample_time; deltae = 57.2958*x2; x1prev = x1; x2prev = x2; diff --git a/src/FDM/UIUCModel/uiuc_alh_ap.h b/src/FDM/UIUCModel/uiuc_alh_ap.h index f305314a8..98916dcdd 100644 --- a/src/FDM/UIUCModel/uiuc_alh_ap.h +++ b/src/FDM/UIUCModel/uiuc_alh_ap.h @@ -2,6 +2,8 @@ #ifndef _ALH_AP_H_ #define _ALH_AP_H_ +#include + double alh_ap(double pitch, double pitchrate, double H_ref, double H, double V, double sample_t, int init); diff --git a/src/FDM/UIUCModel/uiuc_auto_pilot.cpp b/src/FDM/UIUCModel/uiuc_auto_pilot.cpp new file mode 100644 index 000000000..5019d1c55 --- /dev/null +++ b/src/FDM/UIUCModel/uiuc_auto_pilot.cpp @@ -0,0 +1,166 @@ +/********************************************************************** + + FILENAME: uiuc_auto_pilot.cpp + +---------------------------------------------------------------------- + + DESCRIPTION: calls autopilot routines + +---------------------------------------------------------------------- + + STATUS: alpha version + +---------------------------------------------------------------------- + + REFERENCES: + +---------------------------------------------------------------------- + + HISTORY: 09/04/2003 initial release (with PAH) + 10/31/2003 added ALH autopilot + 11/04/2003 added RAH and HH autopilots + +---------------------------------------------------------------------- + + AUTHOR(S): Robert Deters + +---------------------------------------------------------------------- + + VARIABLES: + +---------------------------------------------------------------------- + + INPUTS: -V_rel_wind (or U_body) + -dyn_on_speed + -ice on/off + -phugoid on/off + +---------------------------------------------------------------------- + + OUTPUTS: -CL + -CD + -Cm + -CY + -Cl + -Cn + +---------------------------------------------------------------------- + + CALLED BY: uiuc_coefficients + +---------------------------------------------------------------------- + + CALLS TO: uiuc_coef_lift + uiuc_coef_drag + +---------------------------------------------------------------------- + + COPYRIGHT: (C) 2003 by Michael Selig + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License + as published by the Free Software Foundation. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, + USA or view http://www.gnu.org/copyleft/gpl.html. + +**********************************************************************/ + +#include "uiuc_auto_pilot.h" +//#include +void uiuc_auto_pilot(double dt) +{ + double V_rel_wind_ms; + double Altitude_m; + //static bool ap_pah_on_prev = false; + static int ap_pah_init = 0; + //static bool ap_alh_on_prev = false; + static int ap_alh_init = 0; + static int ap_rah_init = 0; + static int ap_hh_init = 0; + double ap_alt_ref_m; + double bw_m; + double ail_rud[2]; + V_rel_wind_ms = V_rel_wind * 0.3048; + Altitude_m = Altitude * 0.3048; + + if (ap_pah_on && icing_demo==false && Simtime>=ap_pah_start_time) + { + //ap_Theta_ref_rad = ap_Theta_ref_deg * DEG_TO_RAD; + //if (ap_pah_on_prev == false) { + //ap_pah_init = 1; + //ap_pah_on_prev = true; + //} + elevator = pah_ap(Theta, Theta_dot, ap_Theta_ref_rad, V_rel_wind_ms, dt, + ap_pah_init); + if (elevator*RAD_TO_DEG <= -1*demax) + elevator = -1*demax * DEG_TO_RAD; + if (elevator*RAD_TO_DEG >= demin) + elevator = demin * DEG_TO_RAD; + pilot_elev_no=true; + ap_pah_init=1; + //printf("elv1=%f\n",elevator); + } + + if (ap_alh_on && icing_demo==false && Simtime>=ap_alh_start_time) + { + ap_alt_ref_m = ap_alt_ref_ft * 0.3048; + //if (ap_alh_on_prev == false) + //ap_alh_init = 0; + elevator = alh_ap(Theta, Theta_dot, ap_alt_ref_m, Altitude_m, + V_rel_wind_ms, dt, ap_alh_init); + if (elevator*RAD_TO_DEG <= -1*demax) + elevator = -1*demax * DEG_TO_RAD; + if (elevator*RAD_TO_DEG >= demin) + elevator = demin * DEG_TO_RAD; + pilot_elev_no=true; + ap_alh_init = 1; + } + + if (ap_rah_on && icing_demo==false && Simtime>=ap_rah_start_time) + { + bw_m = bw * 0.3048; + rah_ap(Phi, Phi_dot, ap_Phi_ref_rad, V_rel_wind_ms, dt, + bw_m, Psi_dot, ail_rud, ap_rah_init); + aileron = ail_rud[0]; + rudder = ail_rud[1]; + if (aileron*RAD_TO_DEG <= -1*damax) + aileron = -1*damax * DEG_TO_RAD; + if (aileron*RAD_TO_DEG >= damin) + aileron = damin * DEG_TO_RAD; + if (rudder*RAD_TO_DEG <= -1*drmax) + rudder = -1*drmax * DEG_TO_RAD; + if (rudder*RAD_TO_DEG >= drmin) + rudder = drmin * DEG_TO_RAD; + pilot_ail_no=true; + pilot_rud_no=true; + ap_rah_init = 1; + } + + if (ap_hh_on && icing_demo==false && Simtime>=ap_hh_start_time) + { + bw_m = bw * 0.3048; + hh_ap(Phi, Psi, Phi_dot, ap_Psi_ref_rad, V_rel_wind_ms, dt, + bw_m, Psi_dot, ail_rud, ap_hh_init); + aileron = ail_rud[0]; + rudder = ail_rud[1]; + if (aileron*RAD_TO_DEG <= -1*damax) + aileron = -1*damax * DEG_TO_RAD; + if (aileron*RAD_TO_DEG >= damin) + aileron = damin * DEG_TO_RAD; + if (rudder*RAD_TO_DEG <= -1*drmax) + rudder = -1*drmax * DEG_TO_RAD; + if (rudder*RAD_TO_DEG >= drmin) + rudder = drmin * DEG_TO_RAD; + pilot_ail_no=true; + pilot_rud_no=true; + ap_hh_init = 1; + } +} diff --git a/src/FDM/UIUCModel/uiuc_auto_pilot.h b/src/FDM/UIUCModel/uiuc_auto_pilot.h new file mode 100644 index 000000000..b2ef4ce02 --- /dev/null +++ b/src/FDM/UIUCModel/uiuc_auto_pilot.h @@ -0,0 +1,16 @@ +#ifndef _AUTO_PILOT_H_ +#define _AUTO_PILOT_H_ + +#include "uiuc_aircraft.h" +#include "uiuc_pah_ap.h" +#include "uiuc_alh_ap.h" +#include "uiuc_rah_ap.h" +#include "uiuc_hh_ap.h" +#include +#include /* RAD_TO_DEG, DEG_TO_RAD*/ + +extern double Simtime; + +void uiuc_auto_pilot(double dt); + +#endif // _AUTO_PILOT_H_ diff --git a/src/FDM/UIUCModel/uiuc_coef_lift.cpp b/src/FDM/UIUCModel/uiuc_coef_lift.cpp index 6f0549668..5047caacd 100644 --- a/src/FDM/UIUCModel/uiuc_coef_lift.cpp +++ b/src/FDM/UIUCModel/uiuc_coef_lift.cpp @@ -247,7 +247,7 @@ void uiuc_coef_lift() CLfdfI = uiuc_1Dinterpolation(CLfdf_dfArray, CLfdf_CLArray, CLfdf_ndf, - flap); + flap_pos); CL += CLfdfI; break; } @@ -259,7 +259,7 @@ void uiuc_coef_lift() CLfadf_nAlphaArray, CLfadf_ndf, Std_Alpha, - flap); + flap_pos); CL += CLfadfI; break; } diff --git a/src/FDM/UIUCModel/uiuc_coefficients.cpp b/src/FDM/UIUCModel/uiuc_coefficients.cpp index 737e5f7d1..bd196278a 100644 --- a/src/FDM/UIUCModel/uiuc_coefficients.cpp +++ b/src/FDM/UIUCModel/uiuc_coefficients.cpp @@ -33,7 +33,7 @@ Added pilot_elev_no, pilot_ail_no, and pilot_rud_no. 07/05/2001 (RD) Added pilot_(elev,ail,rud)_no=false - 01/11/2002 (AP) Added call to uiuc_bootTime() + 01/11/2002 (AP) Added call to uiuc_iceboot() 12/02/2002 (RD) Moved icing demo interpolations to its own function @@ -104,10 +104,6 @@ void uiuc_coefficients(double dt) static string uiuc_coefficients_error = " (from uiuc_coefficients.cpp) "; double l_trim, l_defl; double V_rel_wind_dum, U_body_dum; - static bool ap_pah_on_prev = false; - int ap_pah_init = 1; - static bool ap_alh_on_prev = false; - int ap_alh_init = 1; if (Alpha_init_true && Simtime==0) Std_Alpha = Alpha_init; @@ -223,38 +219,15 @@ void uiuc_coefficients(double dt) uiuc_controlInput(); } - if (ap_pah_on && icing_demo==false) - { - double V_rel_wind_ms; - V_rel_wind_ms = V_rel_wind * 0.3048; - ap_Theta_ref_rad = ap_Theta_ref_deg * DEG_TO_RAD; - if (ap_pah_on_prev == false) - ap_pah_init = 0; - elevator = pah_ap(Theta, Theta_dot, ap_Theta_ref_rad, V_rel_wind_ms, dt, - ap_pah_init); - if (elevator*RAD_TO_DEG <= -1*demax) - elevator = -1*demax * DEG_TO_RAD; - if (elevator*RAD_TO_DEG >= demin) - elevator = demin * DEG_TO_RAD; - pilot_elev_no=true; - } + //if (Simtime >=10.0) + // { + // ap_hh_on = 1; + // ap_Psi_ref_rad = 0*DEG_TO_RAD; + // } - if (ap_alh_on && icing_demo==false) + if (ap_pah_on || ap_alh_on || ap_rah_on || ap_hh_on) { - double V_rel_wind_ms; - double Altitude_m; - V_rel_wind_ms = V_rel_wind * 0.3048; - ap_alt_ref_m = ap_alt_ref_ft * 0.3048; - Altitude_m = Altitude * 0.3048; - if (ap_alh_on_prev == false) - ap_alh_init = 0; - elevator = alh_ap(Theta, Theta_dot, ap_alt_ref_m, Altitude_m, - V_rel_wind_ms, dt, ap_alh_init); - if (elevator*RAD_TO_DEG <= -1*demax) - elevator = -1*demax * DEG_TO_RAD; - if (elevator*RAD_TO_DEG >= demin) - elevator = demin * DEG_TO_RAD; - pilot_elev_no=true; + uiuc_auto_pilot(dt); } CD = CX = CL = CZ = Cm = CY = Cl = Cn = 0.0; diff --git a/src/FDM/UIUCModel/uiuc_coefficients.h b/src/FDM/UIUCModel/uiuc_coefficients.h index ed56fccf4..1be46f97f 100644 --- a/src/FDM/UIUCModel/uiuc_coefficients.h +++ b/src/FDM/UIUCModel/uiuc_coefficients.h @@ -12,8 +12,7 @@ #include "uiuc_iceboot.h" //Anne's code #include "uiuc_iced_nonlin.h" #include "uiuc_icing_demo.h" -#include "uiuc_pah_ap.h" -#include "uiuc_alh_ap.h" +#include "uiuc_auto_pilot.h" #include "uiuc_1Dinterpolation.h" #include "uiuc_3Dinterpolation.h" #include "uiuc_warnings_errors.h" diff --git a/src/FDM/UIUCModel/uiuc_hh_ap.cpp b/src/FDM/UIUCModel/uiuc_hh_ap.cpp new file mode 100644 index 000000000..d66f562f9 --- /dev/null +++ b/src/FDM/UIUCModel/uiuc_hh_ap.cpp @@ -0,0 +1,139 @@ +// * * +// * hh_ap.C * +// * * +// * Heading Hold autopilot function. takes in * +// * the state * +// * variables and reference angle as arguments * +// * (there are other variable too as arguments * +// * as listed below) * +// * and returns the aileron and rudder deflection * +// * angle at every time step * +// * * +// * * +// * Written 2/14/03 by Vikrant Sharma * +// * * +// ***************************************************** + +//#include +//#include + +// define u2prev,x1prev,x2prev,x3prev,x4prev,x5prev and x6prev in the main +// function +// that uses this autopilot function. give them initial values at origin. +// Pass these values to the A/P function as an argument and pass by +// reference +// Parameters passed as arguments to the function: +// yaw - current yaw angle difference from the trim value +// phi - Current roll angle ,,,,,,,,,,,,,,,,,,,, +// rollrate - current rate of change of roll angle +// yawrate - current rate of change of yaw angle +// b - the wingspan +// yaw_ref - reference yaw angle to be tracked (amount of increase/decrease desired from the trim) +// sample_t - sampling time +// V - aircraft's current velocity +// u2prev - u2 value at the previous time step. +// x1prev - x1 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +// x2prev - x2 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +// x3prev - x3 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +// x4prev - x4 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +// x5prev - x5 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +// x6prev - x6 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +// the autpilot function (rah_ap) changes these values at every time step. +// so the simulator guys don't have to do it. Since these values are +// passed by reference to the function. + +// Another important thing to do and for you simulator guys to check is the +// way I return the the deltaa (aileron deflection) and deltar (rudder deflection). +// I expect you guys to define an array that holds two float values. The first entry +// is for deltaa and the second for deltar and the pointer to this array (ctr_ptr) is also +// one of the arguments for the function rah_ap and then this function updates the value for +// deltae and deltaa every time its called. PLEASE CHECK IF THE SYNTAX IS RIGHT. Also note that +// these values have to be added to the initial trim values of the control deflection to get the +// entire control deflection. + +#include "uiuc_hh_ap.h" + +// (RD) changed float to double + +void hh_ap(double phi, double yaw, double phirate, double yaw_ref, + double V, double sample_time, double b, double yawrate, + double ctr_defl[2], int init) +{ + + static double u2prev; + static double x1prev; + static double x2prev; + static double x3prev; + static double x4prev; + static double x5prev; + static double x6prev; + + if (init==0) + { + u2prev = 0; + x1prev = 0; + x2prev = 0; + x3prev = 0; + x4prev = 0; + x5prev = 0; + x6prev = 0; + } + + double Kphi, Kyaw; + double Kr; + double Ki; + double drr; + double dar; + double deltar; + double deltaa; + double x1, x2, x3, x4, x5, x6, phi_ref; + Kphi = 0.000975*V*V - 0.108*V + 2.335625; + Kr = -4; + Ki = 0.25; + Kyaw = 0.05*V-1.1; + dar = 0.165; + drr = -0.000075*V*V + 0.0095*V -0.4606; + double u1,u2,u3,u4,u5,u6,u7,ubar,udbar; + phi_ref = Kyaw*(yaw_ref-yaw); + u1 = Kphi*(phi_ref-phi); + u2 = u2prev + Ki*Kphi*(phi_ref-phi)*sample_time; + u3 = dar*yawrate; + u4 = u1 + u2 + u3; + u2prev = u2; + double K1,K2; + K1 = Kr*9.8*sin(phi)/V; + K2 = drr - Kr; + u5 = K2*yawrate; + u6 = K1*phi; + u7 = u5 + u6; + ubar = phirate*b/(2*V); + udbar = yawrate*b/(2*V); +// the following is using the actuator dynamics to get the aileron +// angle, given in Beaver. +// the actuator dynamics for Twin Otter are still unavailable. + x1 = x1prev +(-10.6*x1prev - 2.64*x2prev -7.58*x3prev + +27.46*u4 -0.0008*ubar)*sample_time; + x2 = x2prev + x3prev*sample_time; + x3 = x3prev + (1.09*x1prev - 558.86*x2prev - 23.35*x3prev + +3.02*u4 - 0.164*ubar)*sample_time; + deltaa = 57.3*x2; + x1prev = x1; + x2prev = x2; + x3prev = x3; + +// the following is using the actuator dynamics to get the rudder +// angle, given in Beaver. +// the actuator dynamics for Twin Otter are still unavailable. + x4 = x4prev +(-9.2131*x4prev + 5.52*x5prev + 16*x6prev + +24.571*u7 + 0.0036*udbar)*sample_time; + x5 = x5prev + x6prev*sample_time; + x6 = x6prev + (0.672*x4prev - 919.78*x5prev - 56.453*x6prev + +7.54*u7 - 0.636*udbar)*sample_time; + deltar = 57.3*x5; + x4prev = x4; + x5prev = x5; + x6prev = x6; + ctr_defl[0] = deltaa; + ctr_defl[1] = deltar; +return; +} diff --git a/src/FDM/UIUCModel/uiuc_hh_ap.h b/src/FDM/UIUCModel/uiuc_hh_ap.h new file mode 100644 index 000000000..e1bc521e4 --- /dev/null +++ b/src/FDM/UIUCModel/uiuc_hh_ap.h @@ -0,0 +1,12 @@ + +#ifndef _HH_AP_H_ +#define _HH_AP_H_ + +#include +#include + +void hh_ap(double phi, double yaw, double phirate, double yaw_ref, + double V, double sample_time, double b, double yawrate, + double ctr_defl[2], int init); + +#endif //_HH_AP_H_ diff --git a/src/FDM/UIUCModel/uiuc_icing_demo.cpp b/src/FDM/UIUCModel/uiuc_icing_demo.cpp index bc9ee8816..cc6a46537 100644 --- a/src/FDM/UIUCModel/uiuc_icing_demo.cpp +++ b/src/FDM/UIUCModel/uiuc_icing_demo.cpp @@ -186,11 +186,53 @@ void uiuc_icing_demo() demo_ap_pah_on_ntime, time); } - if (demo_ap_Theta_ref_deg){ - double time = Simtime - demo_ap_Theta_ref_deg_startTime; - ap_Theta_ref_deg = uiuc_1Dinterpolation(demo_ap_Theta_ref_deg_timeArray, - demo_ap_Theta_ref_deg_daArray, - demo_ap_Theta_ref_deg_ntime, + if (demo_ap_alh_on){ + double time = Simtime - demo_ap_alh_on_startTime; + ap_alh_on = uiuc_1Dinterpolation(demo_ap_alh_on_timeArray, + demo_ap_alh_on_daArray, + demo_ap_alh_on_ntime, + time); + } + if (demo_ap_rah_on){ + double time = Simtime - demo_ap_rah_on_startTime; + ap_rah_on = uiuc_1Dinterpolation(demo_ap_rah_on_timeArray, + demo_ap_rah_on_daArray, + demo_ap_rah_on_ntime, + time); + } + if (demo_ap_hh_on){ + double time = Simtime - demo_ap_hh_on_startTime; + ap_hh_on = uiuc_1Dinterpolation(demo_ap_hh_on_timeArray, + demo_ap_hh_on_daArray, + demo_ap_hh_on_ntime, + time); + } + if (demo_ap_Theta_ref){ + double time = Simtime - demo_ap_Theta_ref_startTime; + ap_Theta_ref_rad = uiuc_1Dinterpolation(demo_ap_Theta_ref_timeArray, + demo_ap_Theta_ref_daArray, + demo_ap_Theta_ref_ntime, + time); + } + if (demo_ap_alt_ref){ + double time = Simtime - demo_ap_alt_ref_startTime; + ap_alt_ref_ft = uiuc_1Dinterpolation(demo_ap_alt_ref_timeArray, + demo_ap_alt_ref_daArray, + demo_ap_alt_ref_ntime, + time); + } + if (demo_ap_Phi_ref){ + double time = Simtime - demo_ap_Phi_ref_startTime; + ap_Phi_ref_rad = uiuc_1Dinterpolation(demo_ap_Phi_ref_timeArray, + demo_ap_Phi_ref_daArray, + demo_ap_Phi_ref_ntime, + time); + } + if (demo_ap_Psi_ref){ + double time = Simtime - demo_ap_Psi_ref_startTime; + ap_Psi_ref_rad = uiuc_1Dinterpolation(demo_ap_Psi_ref_timeArray, + demo_ap_Psi_ref_daArray, + demo_ap_Psi_ref_ntime, time); } diff --git a/src/FDM/UIUCModel/uiuc_map_controlSurface.cpp b/src/FDM/UIUCModel/uiuc_map_controlSurface.cpp index e0920750c..39eb3a9a0 100644 --- a/src/FDM/UIUCModel/uiuc_map_controlSurface.cpp +++ b/src/FDM/UIUCModel/uiuc_map_controlSurface.cpp @@ -20,7 +20,7 @@ 06/18/2001 (RD) Added aileron_input, rudder_input, pilot_elev_no, pilot_ail_no, and pilot_rud_no - 11/12/2001 (RD) Added flap_max, flap_rate, and + 11/12/2001 (RD) Added flap_max, flap_rate ---------------------------------------------------------------------- @@ -115,6 +115,15 @@ void uiuc_map_controlSurface() controlSurface_map["use_aileron_sas_type1"] = use_aileron_sas_type1_flag ; controlSurface_map["use_elevator_sas_type1"] = use_elevator_sas_type1_flag ; controlSurface_map["use_rudder_sas_type1"] = use_rudder_sas_type1_flag ; + + controlSurface_map["ap_pah"] = ap_pah_flag ; + controlSurface_map["ap_alh"] = ap_alh_flag ; + controlSurface_map["ap_rah"] = ap_rah_flag ; + controlSurface_map["ap_hh"] = ap_hh_flag ; + controlSurface_map["ap_Theta_ref"] = ap_Theta_ref_flag ; + controlSurface_map["ap_alt_ref"] = ap_alt_ref_flag ; + controlSurface_map["ap_Phi_ref"] = ap_Phi_ref_flag ; + controlSurface_map["ap_Psi_ref"] = ap_Psi_ref_flag ; } // end uiuc_map_controlSurface.cpp diff --git a/src/FDM/UIUCModel/uiuc_map_ice.cpp b/src/FDM/UIUCModel/uiuc_map_ice.cpp index 97420f208..ae32f4806 100644 --- a/src/FDM/UIUCModel/uiuc_map_ice.cpp +++ b/src/FDM/UIUCModel/uiuc_map_ice.cpp @@ -17,11 +17,13 @@ ---------------------------------------------------------------------- HISTORY: 04/08/2000 initial release + --/--/2002 (RD) add SIS icing ---------------------------------------------------------------------- AUTHOR(S): Bipin Sehgal Jeff Scott + Robert Deters ---------------------------------------------------------------------- @@ -159,8 +161,14 @@ void uiuc_map_ice() ice_map["demo_eps_pitch_input"] = demo_eps_pitch_input_flag ; ice_map["tactilefadef"] = tactilefadef_flag ; ice_map["tactile_pitch"] = tactile_pitch_flag ; - ice_map["demo_ap_Theta_ref_deg"]= demo_ap_Theta_ref_deg_flag ; ice_map["demo_ap_pah_on"] = demo_ap_pah_on_flag ; + ice_map["demo_ap_alh_on"] = demo_ap_alh_on_flag ; + ice_map["demo_ap_rah_on"] = demo_ap_rah_on_flag ; + ice_map["demo_ap_hh_on"] = demo_ap_hh_on_flag ; + ice_map["demo_ap_Theta_ref_"] = demo_ap_Theta_ref_flag ; + ice_map["demo_ap_alt_ref_"] = demo_ap_alt_ref_flag ; + ice_map["demo_ap_Phi_ref_"] = demo_ap_Phi_ref_flag ; + ice_map["demo_ap_Psi_ref_"] = demo_ap_Psi_ref_flag ; ice_map["demo_tactile"] = demo_tactile_flag ; ice_map["demo_ice_tail"] = demo_ice_tail_flag ; ice_map["demo_ice_left"] = demo_ice_left_flag ; diff --git a/src/FDM/UIUCModel/uiuc_map_init.cpp b/src/FDM/UIUCModel/uiuc_map_init.cpp index 9548764bf..e3cf7c335 100644 --- a/src/FDM/UIUCModel/uiuc_map_init.cpp +++ b/src/FDM/UIUCModel/uiuc_map_init.cpp @@ -19,6 +19,7 @@ HISTORY: 04/08/2000 initial release 06/18/2001 (RD) Added Alpha, Beta, U_body V_body, and W_body. + 08/20/2003 (RD) Removed old_flap_routine ---------------------------------------------------------------------- @@ -108,7 +109,6 @@ void uiuc_map_init() init_map["ignore_unknown_keywords"] = ignore_unknown_keywords_flag; init_map["trim_case_2"] = trim_case_2_flag ; init_map["use_uiuc_network"] = use_uiuc_network_flag ; - init_map["old_flap_routine"] = old_flap_routine_flag ; init_map["icing_demo"] = icing_demo_flag ; init_map["outside_control"] = outside_control_flag ; } diff --git a/src/FDM/UIUCModel/uiuc_map_misc.cpp b/src/FDM/UIUCModel/uiuc_map_misc.cpp index 48852689f..9b2825832 100644 --- a/src/FDM/UIUCModel/uiuc_map_misc.cpp +++ b/src/FDM/UIUCModel/uiuc_map_misc.cpp @@ -17,11 +17,13 @@ ---------------------------------------------------------------------- HISTORY: 04/08/2000 initial release + --/--/2002 (RD) added flapper ---------------------------------------------------------------------- AUTHOR(S): Bipin Sehgal Jeff Scott + Robert Deters ---------------------------------------------------------------------- @@ -69,7 +71,7 @@ void uiuc_map_misc() { misc_map["simpleHingeMomentCoef"] = simpleHingeMomentCoef_flag ; - misc_map["dfTimefdf"] = dfTimefdf_flag ; + //misc_map["dfTimefdf"] = dfTimefdf_flag ; misc_map["flapper"] = flapper_flag ; misc_map["flapper_phi_init"] = flapper_phi_init_flag ; } diff --git a/src/FDM/UIUCModel/uiuc_map_record1.cpp b/src/FDM/UIUCModel/uiuc_map_record1.cpp index 9d02585b6..a1f70bdcc 100644 --- a/src/FDM/UIUCModel/uiuc_map_record1.cpp +++ b/src/FDM/UIUCModel/uiuc_map_record1.cpp @@ -110,6 +110,9 @@ void uiuc_map_record1() record_map["Phi"] = Phi_record ; record_map["Theta"] = Theta_record ; record_map["Psi"] = Psi_record ; + record_map["Phi_deg"] = Phi_deg_record ; + record_map["Theta_deg"] = Theta_deg_record ; + record_map["Psi_deg"] = Psi_deg_record ; /******************** Accelerations ********************/ diff --git a/src/FDM/UIUCModel/uiuc_map_record3.cpp b/src/FDM/UIUCModel/uiuc_map_record3.cpp index 9a7c3b582..c83d3bd37 100644 --- a/src/FDM/UIUCModel/uiuc_map_record3.cpp +++ b/src/FDM/UIUCModel/uiuc_map_record3.cpp @@ -20,6 +20,8 @@ HISTORY: 06/03/2000 file creation 11/12/2001 (RD) Added flap_cmd_deg and flap_pos 03/03/2003 (RD) Added flap_cmd + 08/20/2003 (RD) Changed spoiler variables to match + flap convention. Added flap_pos_norm ---------------------------------------------------------------------- @@ -139,16 +141,17 @@ void uiuc_map_record3() record_map["rudder"] = rudder_record ; record_map["rudder_deg"] = rudder_deg_record ; record_map["Flap_handle"] = Flap_handle_record ; - record_map["flap"] = flap_record ; record_map["flap_cmd"] = flap_cmd_record ; record_map["flap_cmd_deg"] = flap_cmd_deg_record ; record_map["flap_pos"] = flap_pos_record ; record_map["flap_pos_deg"] = flap_pos_deg_record ; + record_map["flap_pos_norm"] = flap_pos_norm_record ; record_map["Spoiler_handle"] = Spoiler_handle_record ; + record_map["spoiler_cmd"] = spoiler_cmd_record ; record_map["spoiler_cmd_deg"] = spoiler_cmd_deg_record ; + record_map["spoiler_pos"] = spoiler_pos_record ; record_map["spoiler_pos_deg"] = spoiler_pos_deg_record ; record_map["spoiler_pos_norm"] = spoiler_pos_norm_record ; - record_map["spoiler_pos"] = spoiler_pos_record ; } diff --git a/src/FDM/UIUCModel/uiuc_map_record5.cpp b/src/FDM/UIUCModel/uiuc_map_record5.cpp index 197877f5a..bb0a51f10 100644 --- a/src/FDM/UIUCModel/uiuc_map_record5.cpp +++ b/src/FDM/UIUCModel/uiuc_map_record5.cpp @@ -116,10 +116,15 @@ void uiuc_map_record5() record_map["M_m_gear"] = M_m_gear_record ; record_map["M_n_gear"] = M_n_gear_record ; - // total moments + // total moments about reference point record_map["M_l_rp"] = M_l_rp_record ; record_map["M_m_rp"] = M_m_rp_record ; record_map["M_n_rp"] = M_n_rp_record ; + + // total moments about cg + record_map["M_l_cg"] = M_l_cg_record ; + record_map["M_m_cg"] = M_m_cg_record ; + record_map["M_n_cg"] = M_n_cg_record ; /***********************Flapper Data********************/ record_map["flapper_freq"] = flapper_freq_record ; @@ -140,6 +145,11 @@ void uiuc_map_record5() record_map["debug5"] = debug5_record ; record_map["debug6"] = debug6_record ; + record_map["debug7"] = debug7_record ; + record_map["debug8"] = debug8_record ; + record_map["debug9"] = debug9_record ; + record_map["debug10"] = debug10_record ; + /******************** Misc data **********************************/ record_map["V_down_fpm"] = V_down_fpm_record ; record_map["eta_q"] = eta_q_record ; diff --git a/src/FDM/UIUCModel/uiuc_map_record6.cpp b/src/FDM/UIUCModel/uiuc_map_record6.cpp index 5942972d4..234fd0929 100644 --- a/src/FDM/UIUCModel/uiuc_map_record6.cpp +++ b/src/FDM/UIUCModel/uiuc_map_record6.cpp @@ -76,13 +76,32 @@ void uiuc_map_record6() record_map["eps_airspeed_min"] = eps_airspeed_min_record ; record_map["tactilefadefI"] = tactilefadefI_record ; /******************************autopilot****************************/ - record_map["ap_Theta_ref_deg"] = ap_Theta_ref_deg_record ; record_map["ap_pah_on"] = ap_pah_on_record ; + record_map["ap_alh_on"] = ap_alh_on_record ; + record_map["ap_rah_on"] = ap_rah_on_record ; + record_map["ap_hh_on"] = ap_hh_on_record ; + record_map["ap_Theta_ref_deg"] = ap_Theta_ref_deg_record ; + record_map["ap_Theta_ref_rad"] = ap_Theta_ref_rad_record ; + record_map["ap_alt_ref_ft"] = ap_alt_ref_ft_record ; + record_map["ap_Phi_ref_deg"] = ap_Phi_ref_deg_record ; + record_map["ap_Phi_ref_rad"] = ap_Phi_ref_rad_record ; + record_map["ap_Psi_ref_deg"] = ap_Psi_ref_deg_record ; + record_map["ap_Psi_ref_rad"] = ap_Psi_ref_rad_record ; /***********************trigger variables**************************/ record_map["trigger_on"] = trigger_on_record ; record_map["trigger_num"] = trigger_num_record ; record_map["trigger_toggle"] = trigger_toggle_record ; record_map["trigger_counter"] = trigger_counter_record ; + /****************local to body transformation matrix**************/ + record_map["T_local_to_body_11"] = T_local_to_body_11_record ; + record_map["T_local_to_body_12"] = T_local_to_body_12_record ; + record_map["T_local_to_body_13"] = T_local_to_body_13_record ; + record_map["T_local_to_body_21"] = T_local_to_body_21_record ; + record_map["T_local_to_body_22"] = T_local_to_body_22_record ; + record_map["T_local_to_body_23"] = T_local_to_body_23_record ; + record_map["T_local_to_body_31"] = T_local_to_body_31_record ; + record_map["T_local_to_body_32"] = T_local_to_body_32_record ; + record_map["T_local_to_body_33"] = T_local_to_body_33_record ; } // end uiuc_map_record6.cpp diff --git a/src/FDM/UIUCModel/uiuc_menu_CL.cpp b/src/FDM/UIUCModel/uiuc_menu_CL.cpp index 3c7120b1b..dc73e03ab 100644 --- a/src/FDM/UIUCModel/uiuc_menu_CL.cpp +++ b/src/FDM/UIUCModel/uiuc_menu_CL.cpp @@ -285,14 +285,14 @@ void parse_CL( const string& linetoken2, const string& linetoken3, aeroLiftParts -> storeCommands (*command_line); // additional variables to streamline flap routine in aerodeflections - ndf = CLfdf_ndf; - int temp_counter = 1; - while (temp_counter <= ndf) - { - dfArray[temp_counter] = CLfdf_dfArray[temp_counter]; - TimeArray[temp_counter] = dfTimefdf_TimeArray[temp_counter]; - temp_counter++; - } + //ndf = CLfdf_ndf; + //int temp_counter = 1; + //while (temp_counter <= ndf) + // { + // dfArray[temp_counter] = CLfdf_dfArray[temp_counter]; + // TimeArray[temp_counter] = dfTimefdf_TimeArray[temp_counter]; + // temp_counter++; + // } break; } case CLfadf_flag: diff --git a/src/FDM/UIUCModel/uiuc_menu_controlSurface.cpp b/src/FDM/UIUCModel/uiuc_menu_controlSurface.cpp index 5b8208f31..3a4937cbc 100644 --- a/src/FDM/UIUCModel/uiuc_menu_controlSurface.cpp +++ b/src/FDM/UIUCModel/uiuc_menu_controlSurface.cpp @@ -509,6 +509,96 @@ void parse_controlSurface( const string& linetoken2, const string& linetoken3, use_rudder_sas_type1 = true; break; } + case ap_pah_flag: + { + if (check_float(linetoken3)) + token3 >> token_value; + else + uiuc_warnings_errors(1, *command_line); + + ap_pah_start_time=token_value; + ap_pah_on = 1; + break; + } + case ap_alh_flag: + { + if (check_float(linetoken3)) + token3 >> token_value; + else + uiuc_warnings_errors(1, *command_line); + + ap_alh_start_time=token_value; + ap_alh_on = 1; + break; + } + case ap_rah_flag: + { + if (check_float(linetoken3)) + token3 >> token_value; + else + uiuc_warnings_errors(1, *command_line); + + ap_rah_start_time=token_value; + ap_rah_on = 1; + break; + } + case ap_hh_flag: + { + if (check_float(linetoken3)) + token3 >> token_value; + else + uiuc_warnings_errors(1, *command_line); + + ap_hh_start_time=token_value; + ap_hh_on = 1; + break; + } + case ap_Theta_ref_flag: + { + if (check_float(linetoken3)) + token3 >> token_value; + else + uiuc_warnings_errors(1, *command_line); + token4 >> token_value_convert1; + convert_y = uiuc_convert(token_value_convert1); + + ap_Theta_ref_rad = token_value * convert_y; + break; + } + case ap_alt_ref_flag: + { + if (check_float(linetoken3)) + token3 >> token_value; + else + uiuc_warnings_errors(1, *command_line); + + ap_alt_ref_ft = token_value; + break; + } + case ap_Phi_ref_flag: + { + if (check_float(linetoken3)) + token3 >> token_value; + else + uiuc_warnings_errors(1, *command_line); + token4 >> token_value_convert1; + convert_y = uiuc_convert(token_value_convert1); + + ap_Phi_ref_rad = token_value * convert_y; + break; + } + case ap_Psi_ref_flag: + { + if (check_float(linetoken3)) + token3 >> token_value; + else + uiuc_warnings_errors(1, *command_line); + token4 >> token_value_convert1; + convert_y = uiuc_convert(token_value_convert1); + + ap_Psi_ref_rad = token_value * convert_y; + break; + } default: { if (ignore_unknown_keywords) { diff --git a/src/FDM/UIUCModel/uiuc_menu_ice.cpp b/src/FDM/UIUCModel/uiuc_menu_ice.cpp index be5aaa6db..880f35d73 100644 --- a/src/FDM/UIUCModel/uiuc_menu_ice.cpp +++ b/src/FDM/UIUCModel/uiuc_menu_ice.cpp @@ -101,8 +101,10 @@ void parse_ice( const string& linetoken2, const string& linetoken3, LIST command_line ) { double token_value; int token_value_convert1, token_value_convert2, token_value_convert3; + int token_value_convert4; double datafile_xArray[100][100], datafile_yArray[100]; double datafile_zArray[100][100]; + double convert_f; int datafile_nxArray[100], datafile_ny; istringstream token3(linetoken3.c_str()); istringstream token4(linetoken4.c_str()); @@ -1103,22 +1105,6 @@ void parse_ice( const string& linetoken2, const string& linetoken3, demo_eps_pitch_input_startTime = token_value; break; } - case demo_ap_Theta_ref_deg_flag: - { - demo_ap_Theta_ref_deg = true; - demo_ap_Theta_ref_deg_file = aircraft_directory + linetoken3; - token4 >> token_value_convert1; - token5 >> token_value_convert2; - convert_y = uiuc_convert(token_value_convert1); - convert_x = uiuc_convert(token_value_convert2); - uiuc_1DdataFileReader(demo_ap_Theta_ref_deg_file, - demo_ap_Theta_ref_deg_timeArray, - demo_ap_Theta_ref_deg_daArray, - demo_ap_Theta_ref_deg_ntime); - token6 >> token_value; - demo_ap_Theta_ref_deg_startTime = token_value; - break; - } case demo_ap_pah_on_flag: { demo_ap_pah_on = true; @@ -1135,6 +1121,118 @@ void parse_ice( const string& linetoken2, const string& linetoken3, demo_ap_pah_on_startTime = token_value; break; } + case demo_ap_alh_on_flag: + { + demo_ap_alh_on = true; + demo_ap_alh_on_file = aircraft_directory + linetoken3; + token4 >> token_value_convert1; + token5 >> token_value_convert2; + convert_y = uiuc_convert(token_value_convert1); + convert_x = uiuc_convert(token_value_convert2); + uiuc_1DdataFileReader(demo_ap_alh_on_file, + demo_ap_alh_on_timeArray, + demo_ap_alh_on_daArray, + demo_ap_alh_on_ntime); + token6 >> token_value; + demo_ap_alh_on_startTime = token_value; + break; + } + case demo_ap_rah_on_flag: + { + demo_ap_rah_on = true; + demo_ap_rah_on_file = aircraft_directory + linetoken3; + token4 >> token_value_convert1; + token5 >> token_value_convert2; + convert_y = uiuc_convert(token_value_convert1); + convert_x = uiuc_convert(token_value_convert2); + uiuc_1DdataFileReader(demo_ap_rah_on_file, + demo_ap_rah_on_timeArray, + demo_ap_rah_on_daArray, + demo_ap_rah_on_ntime); + token6 >> token_value; + demo_ap_rah_on_startTime = token_value; + break; + } + case demo_ap_hh_on_flag: + { + demo_ap_hh_on = true; + demo_ap_hh_on_file = aircraft_directory + linetoken3; + token4 >> token_value_convert1; + token5 >> token_value_convert2; + convert_y = uiuc_convert(token_value_convert1); + convert_x = uiuc_convert(token_value_convert2); + uiuc_1DdataFileReader(demo_ap_hh_on_file, + demo_ap_hh_on_timeArray, + demo_ap_hh_on_daArray, + demo_ap_hh_on_ntime); + token6 >> token_value; + demo_ap_hh_on_startTime = token_value; + break; + } + case demo_ap_Theta_ref_flag: + { + demo_ap_Theta_ref = true; + demo_ap_Theta_ref_file = aircraft_directory + linetoken3; + token4 >> token_value_convert1; + token5 >> token_value_convert2; + convert_y = uiuc_convert(token_value_convert1); + convert_x = uiuc_convert(token_value_convert2); + uiuc_1DdataFileReader(demo_ap_Theta_ref_file, + demo_ap_Theta_ref_timeArray, + demo_ap_Theta_ref_daArray, + demo_ap_Theta_ref_ntime); + token6 >> token_value; + demo_ap_Theta_ref_startTime = token_value; + break; + } + case demo_ap_alt_ref_flag: + { + demo_ap_alt_ref = true; + demo_ap_alt_ref_file = aircraft_directory + linetoken3; + token4 >> token_value_convert1; + token5 >> token_value_convert2; + convert_y = uiuc_convert(token_value_convert1); + convert_x = uiuc_convert(token_value_convert2); + uiuc_1DdataFileReader(demo_ap_alt_ref_file, + demo_ap_alt_ref_timeArray, + demo_ap_alt_ref_daArray, + demo_ap_alt_ref_ntime); + token6 >> token_value; + demo_ap_alt_ref_startTime = token_value; + break; + } + case demo_ap_Phi_ref_flag: + { + demo_ap_Phi_ref = true; + demo_ap_Phi_ref_file = aircraft_directory + linetoken3; + token4 >> token_value_convert1; + token5 >> token_value_convert2; + convert_y = uiuc_convert(token_value_convert1); + convert_x = uiuc_convert(token_value_convert2); + uiuc_1DdataFileReader(demo_ap_Phi_ref_file, + demo_ap_Phi_ref_timeArray, + demo_ap_Phi_ref_daArray, + demo_ap_Phi_ref_ntime); + token6 >> token_value; + demo_ap_Phi_ref_startTime = token_value; + break; + } + case demo_ap_Psi_ref_flag: + { + demo_ap_Psi_ref = true; + demo_ap_Psi_ref_file = aircraft_directory + linetoken3; + token4 >> token_value_convert1; + token5 >> token_value_convert2; + convert_y = uiuc_convert(token_value_convert1); + convert_x = uiuc_convert(token_value_convert2); + uiuc_1DdataFileReader(demo_ap_Psi_ref_file, + demo_ap_Psi_ref_timeArray, + demo_ap_Psi_ref_daArray, + demo_ap_Psi_ref_ntime); + token6 >> token_value; + demo_ap_Psi_ref_startTime = token_value; + break; + } case tactilefadef_flag: { int tactilefadef_index, i; @@ -1148,14 +1246,16 @@ void parse_ice( const string& linetoken2, const string& linetoken3, if (tactilefadef_index > tactilefadef_nf) tactilefadef_nf = tactilefadef_index; token5 >> flap_value; - tactilefadef_fArray[tactilefadef_index] = flap_value; token6 >> token_value_convert1; token7 >> token_value_convert2; token8 >> token_value_convert3; - token9 >> tactilefadef_nice; + token9 >> token_value_convert4; + token10 >> tactilefadef_nice; convert_z = uiuc_convert(token_value_convert1); convert_x = uiuc_convert(token_value_convert2); convert_y = uiuc_convert(token_value_convert3); + convert_f = uiuc_convert(token_value_convert4); + tactilefadef_fArray[tactilefadef_index] = flap_value * convert_f; /* call 2D File Reader with file name (tactilefadef_file) and conversion factors; function returns array of elevator deflections (deArray) and corresponding diff --git a/src/FDM/UIUCModel/uiuc_menu_init.cpp b/src/FDM/UIUCModel/uiuc_menu_init.cpp index 10cb0ea61..2cec96f27 100644 --- a/src/FDM/UIUCModel/uiuc_menu_init.cpp +++ b/src/FDM/UIUCModel/uiuc_menu_init.cpp @@ -21,6 +21,7 @@ 06/30/2003 (RD) replaced istrstream with istringstream to get rid of the annoying warning about using the strstream header + 08/20/2003 (RD) removed old_flap_routine ---------------------------------------------------------------------- @@ -474,11 +475,6 @@ void parse_init( const string& linetoken2, const string& linetoken3, token4 >> port_num; break; } - case old_flap_routine_flag: - { - old_flap_routine = true; - break; - } case icing_demo_flag: { icing_demo = true; diff --git a/src/FDM/UIUCModel/uiuc_menu_misc.cpp b/src/FDM/UIUCModel/uiuc_menu_misc.cpp index c3f67e274..75c2c512c 100644 --- a/src/FDM/UIUCModel/uiuc_menu_misc.cpp +++ b/src/FDM/UIUCModel/uiuc_menu_misc.cpp @@ -121,19 +121,19 @@ void parse_misc( const string& linetoken2, const string& linetoken3, simpleHingeMomentCoef = token_value; break; } - case dfTimefdf_flag: - { - dfTimefdf = linetoken3; + //case dfTimefdf_flag: + //{ + //dfTimefdf = linetoken3; /* call 1D File Reader with file name (dfTimefdf); function returns array of dfs (dfArray) and corresponding time values (TimeArray) and max number of terms in arrays (ndf) */ - uiuc_1DdataFileReader(dfTimefdf, - dfTimefdf_dfArray, - dfTimefdf_TimeArray, - dfTimefdf_ndf); - break; - } + //uiuc_1DdataFileReader(dfTimefdf, + // dfTimefdf_dfArray, + // dfTimefdf_TimeArray, + // dfTimefdf_ndf); + //break; + //} case flapper_flag: { string flap_file; diff --git a/src/FDM/UIUCModel/uiuc_pah_ap.cpp b/src/FDM/UIUCModel/uiuc_pah_ap.cpp index bc876a0a7..84e4f915c 100644 --- a/src/FDM/UIUCModel/uiuc_pah_ap.cpp +++ b/src/FDM/UIUCModel/uiuc_pah_ap.cpp @@ -43,9 +43,9 @@ // (RD) changed from float to double #include "uiuc_pah_ap.h" - +//#include double pah_ap(double pitch, double pitchrate, double pitch_ref, double V, - double sample_t, int init) + double sample_time, int init) { // changes by RD so function keeps previous values static double u2prev; @@ -72,22 +72,34 @@ double pah_ap(double pitch, double pitchrate, double pitch_ref, double V, Ki = 0.5; double u1,u2,u3; u1 = Ktheta*(pitch_ref-pitch); - u2 = u2prev + Ki*Ktheta*(pitch_ref-pitch)*sample_t; + u2 = u2prev + Ki*Ktheta*(pitch_ref-pitch)*sample_time; u3 = Kq*pitchrate; double totalU; totalU = u1 + u2 - u3; + //printf("\nu1=%f\n",u1); + //printf("u2=%f\n",u2); + //printf("u3=%f\n",u3); + //printf("totalU=%f\n",totalU); u2prev = u2; // the following is using the actuator dynamics given in Beaver. // the actuator dynamics for Twin Otter are still unavailable. x1 = x1prev +(-10.951*x1prev + 7.2721*x2prev + 20.7985*x3prev + - 25.1568*totalU)*sample_t; - x2 = x2prev + x3prev*sample_t; + 25.1568*totalU)*sample_time; + x2 = x2prev + x3prev*sample_time; x3 = x3prev + (7.3446*x1prev - 668.6713*x2prev - 16.8697*x3prev + - 5.8694*totalU)*sample_t; + 5.8694*totalU)*sample_time; deltae = 57.2958*x2; + //deltae = x2; + //printf("x1prev=%f\n",x1prev); + //printf("x2prev=%f\n",x2prev); + //printf("x3prev=%f\n",x3prev); x1prev = x1; x2prev = x2; x3prev = x3; + //printf("x1=%f\n",x1); + //printf("x2=%f\n",x2); + //printf("x3=%f\n",x3); + //printf("deltae=%f\n",deltae); return deltae; } diff --git a/src/FDM/UIUCModel/uiuc_rah_ap.cpp b/src/FDM/UIUCModel/uiuc_rah_ap.cpp new file mode 100644 index 000000000..ac4404e9d --- /dev/null +++ b/src/FDM/UIUCModel/uiuc_rah_ap.cpp @@ -0,0 +1,137 @@ +// * * +// * rah_ap.C * +// * * +// * Roll attitude Hold autopilot function. takes in * +// * the state * +// * variables and reference angle as arguments * +// * (there are other variable too as arguments * +// * as listed below) * +// * and returns the aileron and rudder deflection * +// * angle at every time step * +// * * +// * * +// * Written 2/14/03 by Vikrant Sharma * +// * * +// ***************************************************** + +//#include +//#include + +// define u2prev,x1prev,x2prev,x3prev,x4prev,x5prev and x6prev in the main +// function +// that uses this autopilot function. give them initial values at origin. +// Pass these values to the A/P function as an argument and pass by +// reference +// Parameters passed as arguments to the function: +// phi - Current roll angle difference from the trim +// rollrate - current rate of change of roll angle +// yawrate - current rate of change of yaw angle +// b - the wingspan +// roll_ref - reference roll angle to be tracked (increase or decrease +// desired from the trim value) +// sample_t - sampling time +// V - aircraft's current velocity +// u2prev - u2 value at the previous time step. +// x1prev - x1 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +// x2prev - x2 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +// x3prev - x3 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +// x4prev - x4 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +// x5prev - x5 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +// x6prev - x6 ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,, +// the autpilot function (rah_ap) changes these values at every time step. +// so the simulator guys don't have to do it. Since these values are +// passed by reference to the function. + +// Another important thing to do and for you simulator guys to check is the +// way I return the the deltaa (aileron deflection) and deltar (rudder deflection). +// I expect you guys to define an array that holds two float values. The first entry +// is for deltaa and the second for deltar and the pointer to this array (ctr_ptr) is also +// one of the arguments for the function rah_ap and then this function updates the value for +// deltae and deltaa every time its called. PLEASE CHECK IF THE SYNTAX IS RIGHT. Also note that +// these values have to be added to the initial trim values of the control deflection to get the +// entire control deflection. + +#include "uiuc_rah_ap.h" + +// (RD) changed float to double + +void rah_ap(double phi, double phirate, double phi_ref, double V, + double sample_time, double b, double yawrate, double ctr_defl[2], + int init) +{ + + static double u2prev; + static double x1prev; + static double x2prev; + static double x3prev; + static double x4prev; + static double x5prev; + static double x6prev; + + if (init==0) + { + u2prev = 0; + x1prev = 0; + x2prev = 0; + x3prev = 0; + x4prev = 0; + x5prev = 0; + x6prev = 0; + } + + double Kphi; + double Kr; + double Ki; + double drr; + double dar; + double deltar; + double deltaa; + double x1, x2, x3, x4, x5, x6; + Kphi = 0.000975*V*V - 0.108*V + 2.335625; + Kr = -4; + Ki = 0.25; + dar = 0.165; + drr = -0.000075*V*V + 0.0095*V -0.4606; + double u1,u2,u3,u4,u5,u6,u7,ubar,udbar; + u1 = Kphi*(phi_ref-phi); + u2 = u2prev + Ki*Kphi*(phi_ref-phi)*sample_time; + u3 = dar*yawrate; + u4 = u1 + u2 + u3; + u2prev = u2; + double K1,K2; + K1 = Kr*9.8*sin(phi)/V; + K2 = drr - Kr; + u5 = K2*yawrate; + u6 = K1*phi; + u7 = u5 + u6; + ubar = phirate*b/(2*V); + udbar = yawrate*b/(2*V); +// the following is using the actuator dynamics to get the aileron +// angle, given in Beaver. +// the actuator dynamics for Twin Otter are still unavailable. + x1 = x1prev +(-10.6*x1prev - 2.64*x2prev -7.58*x3prev + +27.46*u4 -0.0008*ubar)*sample_time; + x2 = x2prev + x3prev*sample_time; + x3 = x3prev + (1.09*x1prev - 558.86*x2prev - 23.35*x3prev + +3.02*u4 - 0.164*ubar)*sample_time; + deltaa = 57.3*x2; + x1prev = x1; + x2prev = x2; + x3prev = x3; + +// the following is using the actuator dynamics to get the rudder +// angle, given in Beaver. +// the actuator dynamics for Twin Otter are still unavailable. + x4 = x4prev +(-9.2131*x4prev + 5.52*x5prev + 16*x6prev + +24.571*u7 + 0.0036*udbar)*sample_time; + x5 = x5prev + x6prev*sample_time; + x6 = x6prev + (0.672*x4prev - 919.78*x5prev - 56.453*x6prev + +7.54*u7 - 0.636*udbar)*sample_time; + deltar = 57.3*x5; + x4prev = x4; + x5prev = x5; + x6prev = x6; + ctr_defl[0] = deltaa; + ctr_defl[1] = deltar; +return; +} diff --git a/src/FDM/UIUCModel/uiuc_rah_ap.h b/src/FDM/UIUCModel/uiuc_rah_ap.h new file mode 100644 index 000000000..047fda53e --- /dev/null +++ b/src/FDM/UIUCModel/uiuc_rah_ap.h @@ -0,0 +1,12 @@ + +#ifndef _RAH_AP_H_ +#define _RAH_AP_H_ + +#include +#include + +void rah_ap(double phi, double phirate, double phi_ref, double V, + double sample_time, double b, double yawrate, double ctr_defl[2], + int init); + +#endif //_RAH_AP_H_ diff --git a/src/FDM/UIUCModel/uiuc_recorder.cpp b/src/FDM/UIUCModel/uiuc_recorder.cpp index 8aed92e70..743b78be9 100644 --- a/src/FDM/UIUCModel/uiuc_recorder.cpp +++ b/src/FDM/UIUCModel/uiuc_recorder.cpp @@ -41,6 +41,8 @@ 07/17/2003 (RD) Added error checking code (default routine) since it was removed from uiuc_menu_record() + 08/20/2003 (RD) Changed spoiler variables to match + flap convention. Added flap_pos_norm ---------------------------------------------------------------------- @@ -110,6 +112,16 @@ void uiuc_recorder( double dt ) int modulus = recordStep % recordRate; + //static double lat1; + //static double long1; + //double D_cg_north1; + //double D_cg_east1; + //if (Simtime == 0) + // { + // lat1=Latitude; + // long1=Longitude; + // } + if ((recordStep % recordRate) == 0) { command_list = recordParts->getCommands(); @@ -248,6 +260,21 @@ void uiuc_recorder( double dt ) fout << Psi << " "; break; } + case Phi_deg_record: + { + fout << Phi*RAD_TO_DEG << " "; + break; + } + case Theta_deg_record: + { + fout << Theta*RAD_TO_DEG << " "; + break; + } + case Psi_deg_record: + { + fout << Psi*RAD_TO_DEG << " "; + break; + } /******************** Accelerations ********************/ case V_dot_north_record: @@ -372,6 +399,11 @@ void uiuc_recorder( double dt ) fout << V_down << " "; break; } + case V_down_fpm_record: + { + fout << V_down * 60 << " "; + break; + } case V_north_rel_ground_record: { fout << V_north_rel_ground << " "; @@ -817,6 +849,11 @@ void uiuc_recorder( double dt ) fout << elevator * RAD_TO_DEG << " "; break; } + case elevator_sas_deg_record: + { + fout << elevator_sas * RAD_TO_DEG << " "; + break; + } case Lat_control_record: { fout << Lat_control << " "; @@ -832,6 +869,11 @@ void uiuc_recorder( double dt ) fout << aileron * RAD_TO_DEG << " "; break; } + case aileron_sas_deg_record: + { + fout << aileron_sas * RAD_TO_DEG << " "; + break; + } case Rudder_pedal_record: { fout << Rudder_pedal << " "; @@ -847,16 +889,16 @@ void uiuc_recorder( double dt ) fout << rudder * RAD_TO_DEG << " "; break; } + case rudder_sas_deg_record: + { + fout << rudder_sas * RAD_TO_DEG << " "; + break; + } case Flap_handle_record: { fout << Flap_handle << " "; break; } - case flap_record: - { - fout << flap << " "; - break; - } case flap_cmd_record: { fout << flap_cmd << " "; @@ -877,19 +919,34 @@ void uiuc_recorder( double dt ) fout << flap_pos * RAD_TO_DEG << " "; break; } + case flap_pos_norm_record: + { + fout << flap_pos_norm << " "; + break; + } case Spoiler_handle_record: { fout << Spoiler_handle << " "; break; } + case spoiler_cmd_record: + { + fout << spoiler_cmd << " "; + break; + } case spoiler_cmd_deg_record: { - fout << spoiler_cmd_deg << " "; + fout << spoiler_cmd * RAD_TO_DEG << " "; + break; + } + case spoiler_pos_record: + { + fout << spoiler_pos << " "; break; } case spoiler_pos_deg_record: { - fout << spoiler_pos_deg << " "; + fout << spoiler_pos * RAD_TO_DEG << " "; break; } case spoiler_pos_norm_record: @@ -897,9 +954,21 @@ void uiuc_recorder( double dt ) fout << spoiler_pos_norm << " "; break; } - case spoiler_pos_record: + + /****************** Gear Inputs ************************/ + case Gear_handle_record: { - fout << spoiler_pos << " "; + fout << Gear_handle << " "; + break; + } + case gear_cmd_norm_record: + { + fout << gear_cmd_norm << " "; + break; + } + case gear_pos_norm_record: + { + fout << gear_pos_norm << " "; break; } @@ -1901,23 +1970,63 @@ void uiuc_recorder( double dt ) fout << eps_airspeed_min << " "; break; } - case tactilefadefI_record: - { - fout << tactilefadefI << " "; - break; - } - /*******************Autopilot***************************/ - case ap_Theta_ref_deg_record: - { - fout << ap_Theta_ref_deg << " "; - break; - } + /****************** Autopilot **************************/ case ap_pah_on_record: { fout << ap_pah_on << " "; break; } + case ap_alh_on_record: + { + fout << ap_alh_on << " "; + break; + } + case ap_rah_on_record: + { + fout << ap_rah_on << " "; + break; + } + case ap_hh_on_record: + { + fout << ap_hh_on << " "; + break; + } + case ap_Theta_ref_deg_record: + { + fout << ap_Theta_ref_rad*RAD_TO_DEG << " "; + break; + } + case ap_Theta_ref_rad_record: + { + fout << ap_Theta_ref_rad << " "; + break; + } + case ap_alt_ref_ft_record: + { + fout << ap_alt_ref_ft << " "; + break; + } + case ap_Phi_ref_deg_record: + { + fout << ap_Phi_ref_rad*RAD_TO_DEG << " "; + break; + } + case ap_Phi_ref_rad_record: + { + fout << ap_Phi_ref_rad << " "; + break; + } + case ap_Psi_ref_deg_record: + { + fout << ap_Psi_ref_rad*RAD_TO_DEG << " "; + break; + } + case ap_Psi_ref_rad_record: + { + fout << ap_Psi_ref_rad << " "; + break; + } /************************ Forces ***********************/ case F_X_wind_record: @@ -2072,8 +2181,23 @@ void uiuc_recorder( double dt ) fout << M_n_rp << " "; break; } + case M_l_cg_record: + { + fout << M_l_cg << " "; + break; + } + case M_m_cg_record: + { + fout << M_m_cg << " "; + break; + } + case M_n_cg_record: + { + fout << M_n_cg << " "; + break; + } - /********************* flapper variables *********************/ + /********************* flapper *********************/ case flapper_freq_record: { fout << flapper_freq << " "; @@ -2109,7 +2233,131 @@ void uiuc_recorder( double dt ) fout << flapper_Moment << " "; break; } - /********* MSS debug and other data *******************/ + /****************Other Variables*******************/ + case gyroMomentQ_record: + { + fout << polarInertia * engineOmega * Q_body << " "; + break; + } + case gyroMomentR_record: + { + fout << -polarInertia * engineOmega * R_body << " "; + break; + } + case eta_q_record: + { + fout << eta_q << " "; + break; + } + case rpm_record: + { + fout << (engineOmega * 60 / (2 * LS_PI)) << " "; + break; + } + case w_induced_record: + { + fout << w_induced << " "; + break; + } + case downwashAngle_deg_record: + { + fout << downwashAngle * RAD_TO_DEG << " "; + break; + } + case alphaTail_deg_record: + { + fout << alphaTail * RAD_TO_DEG << " "; + break; + } + case gammaWing_record: + { + fout << gammaWing << " "; + break; + } + case LD_record: + { + fout << V_ground_speed/V_down_rel_ground << " "; + break; + } + case gload_record: + { + fout << -A_Z_cg/32.174 << " "; + break; + } + case tactilefadefI_record: + { + fout << tactilefadefI << " "; + break; + } + /****************Trigger Variables*******************/ + case trigger_on_record: + { + fout << trigger_on << " "; + break; + } + case trigger_num_record: + { + fout << trigger_num << " "; + break; + } + case trigger_toggle_record: + { + fout << trigger_toggle << " "; + break; + } + case trigger_counter_record: + { + fout << trigger_counter << " "; + break; + } + /*********local to body transformation matrix********/ + case T_local_to_body_11_record: + { + fout << T_local_to_body_11 << " "; + break; + } + case T_local_to_body_12_record: + { + fout << T_local_to_body_12 << " "; + break; + } + case T_local_to_body_13_record: + { + fout << T_local_to_body_13 << " "; + break; + } + case T_local_to_body_21_record: + { + fout << T_local_to_body_21 << " "; + break; + } + case T_local_to_body_22_record: + { + fout << T_local_to_body_22 << " "; + break; + } + case T_local_to_body_23_record: + { + fout << T_local_to_body_23 << " "; + break; + } + case T_local_to_body_31_record: + { + fout << T_local_to_body_31 << " "; + break; + } + case T_local_to_body_32_record: + { + fout << T_local_to_body_32 << " "; + break; + } + case T_local_to_body_33_record: + { + fout << T_local_to_body_33 << " "; + break; + } + + /********* MSS debug and other data *******************/ /* debug variables for use in probing data */ /* comment out old lines, and add new */ /* only remove code that you have written */ @@ -2169,7 +2417,11 @@ void uiuc_recorder( double dt ) case debug4_record: { // flapper F_X_aero_flapper - fout << F_X_aero_flapper << " "; + //fout << F_X_aero_flapper << " "; + //ap_pah_on + //fout << ap_pah_on << " "; + //D_cg_north1 = Radius_to_rwy*(Latitude - lat1); + //fout << D_cg_north1 << " "; break; } case debug5_record: @@ -2177,119 +2429,39 @@ void uiuc_recorder( double dt ) // flapper F_Z_aero_flapper //fout << F_Z_aero_flapper << " "; // gear_rate - fout << gear_rate << " "; + //D_cg_east1 = Radius_to_rwy*cos(lat1)*(Longitude - long1); + //fout << D_cg_east1 << " "; break; } case debug6_record: { //gear_max - fout << gear_max << " "; + //fout << gear_max << " "; + //fout << sqrt(D_cg_north1*D_cg_north1+D_cg_east1*D_cg_east1) << " "; break; } - case V_down_fpm_record: + case debug7_record: { - fout << V_down * 60 << " "; + //Debug7 + fout << debug7 << " "; break; } - case eta_q_record: + case debug8_record: { - fout << eta_q << " "; + //Debug8 + fout << debug8 << " "; break; } - case rpm_record: + case debug9_record: { - fout << (engineOmega * 60 / (2 * LS_PI)) << " "; + //Debug9 + fout << debug9 << " "; break; } - case elevator_sas_deg_record: + case debug10_record: { - fout << elevator_sas * RAD_TO_DEG << " "; - break; - } - case aileron_sas_deg_record: - { - fout << aileron_sas * RAD_TO_DEG << " "; - break; - } - case rudder_sas_deg_record: - { - fout << rudder_sas * RAD_TO_DEG << " "; - break; - } - case w_induced_record: - { - fout << w_induced << " "; - break; - } - case downwashAngle_deg_record: - { - fout << downwashAngle * RAD_TO_DEG << " "; - break; - } - case alphaTail_deg_record: - { - fout << alphaTail * RAD_TO_DEG << " "; - break; - } - case gammaWing_record: - { - fout << gammaWing << " "; - break; - } - case LD_record: - { - fout << V_ground_speed/V_down_rel_ground << " "; - break; - } - case gload_record: - { - fout << -A_Z_cg/32.174 << " "; - break; - } - case gyroMomentQ_record: - { - fout << polarInertia * engineOmega * Q_body << " "; - break; - } - case gyroMomentR_record: - { - fout << -polarInertia * engineOmega * R_body << " "; - break; - } - case Gear_handle_record: - { - fout << Gear_handle << " "; - break; - } - case gear_cmd_norm_record: - { - fout << gear_cmd_norm << " "; - break; - } - case gear_pos_norm_record: - { - fout << gear_pos_norm << " "; - break; - } - /****************Trigger Variables*******************/ - case trigger_on_record: - { - fout << trigger_on << " "; - break; - } - case trigger_num_record: - { - fout << trigger_num << " "; - break; - } - case trigger_toggle_record: - { - fout << trigger_toggle << " "; - break; - } - case trigger_counter_record: - { - fout << trigger_counter << " "; + //Debug10 + fout << debug10 << " "; break; } default: diff --git a/src/FDM/UIUCModel/uiuc_wrapper.cpp b/src/FDM/UIUCModel/uiuc_wrapper.cpp index 74aae2460..06be6b61b 100644 --- a/src/FDM/UIUCModel/uiuc_wrapper.cpp +++ b/src/FDM/UIUCModel/uiuc_wrapper.cpp @@ -195,7 +195,6 @@ void uiuc_defaults_inits () W_body_init_true = false; trim_case_2 = false; use_uiuc_network = false; - old_flap_routine = false; icing_demo = false; outside_control = false; set_Long_trim = false; @@ -270,7 +269,9 @@ void uiuc_defaults_inits () demo_eps_pitch_input = false; tactilefadef = false; demo_ap_pah_on = false; - demo_ap_Theta_ref_deg = false; + demo_ap_alh_on = false; + demo_ap_Theta_ref = false; + demo_ap_alt_ref = false; demo_tactile = false; demo_ice_tail = false; demo_ice_left = false; @@ -281,16 +282,17 @@ void uiuc_defaults_inits () Dx_cg = 0.0; Dy_cg = 0.0; Dz_cg = 0.0; + ap_pah_on = 0; + ap_alh_on = 0; - - // Calculates the local velocity (V_north, V_east, V_down) from the body - // velocities. - // Called from uiuc_local_vel_init which is called from ls_step. - // Used during initialization (while Simtime=0) } void uiuc_vel_init () { + // Calculates the local velocity (V_north, V_east, V_down) from the body + // velocities. + // Called from uiuc_local_vel_init which is called from ls_step. + // Used during initialization (while Simtime=0) if (U_body_init_true && V_body_init_true && W_body_init_true) { double det_T_l_to_b, cof11, cof12, cof13, cof21, cof22, cof23, cof31, cof32, cof33; @@ -312,12 +314,12 @@ void uiuc_vel_init () V_east = V_east_rel_ground + OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric); } - // Initializes the UIUC aircraft model. - // Called once from uiuc_init_2_wrapper } void uiuc_init_aeromodel () { + // Initializes the UIUC aircraft model. + // Called once from uiuc_init_2_wrapper SGPath path(globals->get_fg_root()); path.append(aircraft_dir); path.append("aircraft.dat"); @@ -483,12 +485,12 @@ void uiuc_record_routine(double dt) void uiuc_network_recv_routine() { //if (use_uiuc_network) - //uiuc_network(1); + // uiuc_network(1); } void uiuc_network_send_routine() { //if (use_uiuc_network) - //uiuc_network(2); + // uiuc_network(2); } //end uiuc_wrapper.cpp