// initialize actuator property nodes
static void bind_act_nodes() {
act_timestamp_node = fgGetNode("/actuators/actuator/time-stamp", true);
act_aileron_node = fgGetNode("/actuators/actuator/channel", 0, true);
act_elevator_node = fgGetNode("/actuators/actuator/channel", 1, true);
act_throttle_node = fgGetNode("/actuators/actuator/channel", 2, true);
act_rudder_node = fgGetNode("/actuators/actuator/channel", 3, true);
act_channel5_node = fgGetNode("/actuators/actuator/channel", 4, true);
act_channel6_node = fgGetNode("/actuators/actuator/channel", 5, true);
act_channel7_node = fgGetNode("/actuators/actuator/channel", 6, true);
act_channel8_node = fgGetNode("/actuators/actuator/channel", 7, true);
}
void AirData_close() {
// traverse configured modules
SGPropertyNode *toplevel = fgGetNode("/config/sensors/air-data-group", true);
for ( int i = 0; i < toplevel->nChildren(); ++i ) {
SGPropertyNode *section = toplevel->getChild(i);
string name = section->getName();
if ( name == "airdata" ) {
string source = section->getChild("source", 0, true)->getStringValue();
string basename = "/sensors/";
basename += section->getDisplayName();
// printf("i = %d name = %s source = %s %s\n",
// i, name.c_str(), source.c_str(), basename.c_str());
if ( source == "null" ) {
// do nothing
} else if ( source == "fgfs" ) {
// nop
} else if ( source == "ardupilot" ) {
// nop
#ifdef ENABLE_MNAV_SENSOR
} else if ( source == "mnav" ) {
// nop
#endif
} else {
printf("Unknown air data source = '%s' in config file\n",
source.c_str());
}
}
}
}
void Actuators_close() {
// traverse configured modules
SGPropertyNode *toplevel = fgGetNode("/config/actuators", true);
for ( int i = 0; i < toplevel->nChildren(); ++i ) {
SGPropertyNode *section = toplevel->getChild(i);
string name = section->getName();
if ( name == "actuator" ) {
string module = section->getChild("module", 0, true)->getStringValue();
bool enabled = section->getChild("enable", 0, true)->getBoolValue();
if ( !enabled ) {
continue;
}
if ( module == "null" ) {
// do nothing
} else if ( module == "ardupilot" ) {
ardupilot_close();
} else if ( module == "fgfs" ) {
fgfs_act_close();
#ifdef ENABLE_MNAV_SENSOR
} else if ( module == "mnav" ) {
// do nothing
#endif // ENABLE_MNAV_SENSOR
} else {
printf("Unknown actuator = '%s' in config file\n",
module.c_str());
}
}
}
}
// initialize mnav gps output property nodes
static void bind_airdata_output( string rootname ) {
// "/sensors/air-data"
outputroot = fgGetNode( rootname.c_str(), true );
airdata_timestamp_node = outputroot->getChild("time-stamp", 0, true);
airdata_altitude_node = outputroot->getChild("altitude-m", 0, true);
airdata_airspeed_node = outputroot->getChild("airspeed-kt", 0, true);
}
// initialize mnav gps output property nodes
static void bind_gps_output( string rootname ) {
outputroot = fgGetNode( rootname.c_str(), true );
gps_timestamp_node = outputroot->getChild("time-stamp", 0, true);
gps_lat_node = outputroot->getChild("latitude-deg", 0, true);
gps_lon_node = outputroot->getChild("longitude-deg", 0, true);
gps_alt_node = outputroot->getChild("altitude-m", 0, true);
gps_ve_node = outputroot->getChild("ve-ms", 0, true);
gps_vn_node = outputroot->getChild("vn-ms", 0, true);
gps_vd_node = outputroot->getChild("vd-ms", 0, true);
gps_unix_sec_node = outputroot->getChild("unix-time-sec", 0, true);
}
// initialize mnav imu output property nodes
static void bind_imu_output( string rootname ) {
outputroot = fgGetNode( rootname.c_str(), true );
imu_timestamp_node = outputroot->getChild("time-stamp", 0, true);
imu_p_node = outputroot->getChild("p-rad_sec", 0, true);
imu_q_node = outputroot->getChild("q-rad_sec", 0, true);
imu_r_node = outputroot->getChild("r-rad_sec", 0, true);
imu_ax_node = outputroot->getChild("ax-mps_sec", 0, true);
imu_ay_node = outputroot->getChild("ay-mps_sec", 0, true);
imu_az_node = outputroot->getChild("az-mps_sec", 0, true);
imu_hx_node = outputroot->getChild("hx", 0, true);
imu_hy_node = outputroot->getChild("hy", 0, true);
imu_hz_node = outputroot->getChild("hz", 0, true);
}
bool health_init() {
loadavg_init();
//sgbatmon_init();
ap_roll = fgGetNode("/autopilot/internal/target-roll-deg", true);
ap_hdg = fgGetNode( "/autopilot/settings/true-heading-deg", true );
ap_pitch = fgGetNode( "/autopilot/settings/target-pitch-deg", true );
ap_climb = fgGetNode("/autopilot/internal/target-climb-rate-fps", true);
ap_altitude = fgGetNode( "/autopilot/settings/target-altitude-ft", true );
ground_ref = fgGetNode( "/position/ground-altitude-pressure-m", true );
ap_agl = fgGetNode( "/autopilot/settings/target-agl-ft", true );
// set initial values
healthpacket.command_sequence = 0;
healthpacket.target_waypoint = 0;
return true;
}
void control_init() {
// initialize the autopilot class and build the structures from the
// configuration file values
ap.init();
ap.build();
// initialize the flight control output property nodes
aileron_out_node = fgGetNode("/controls/flight/aileron", true);
elevator_out_node = fgGetNode("/controls/flight/elevator", true);
throttle_out_node = fgGetNode("/engines/engine[0]/throttle", true);
rudder_out_node = fgGetNode("/controls/flight/rudder", true);
ap_target = fgGetNode("/autopilot/settings/target-roll-deg", true);
elevon_mix = fgGetNode("/config/autopilot/elevon-mixing", true);
}
bool loadavg_init() {
system_load_avg = fgGetNode("/status/system-load-avg", true);
return true;
}
void Actuator_init() {
debug6a.set_name("debug6a act update and output");
debug6b.set_name("debug6b act console logging");
// bind properties
output_aileron_node = fgGetNode("/controls/flight/aileron", true);
output_elevator_node = fgGetNode("/controls/flight/elevator", true);
output_elevator_damp_node = fgGetNode("/controls/flight/elevator-damp", true);
output_throttle_node = fgGetNode("/controls/engine/throttle", true);
output_rudder_node = fgGetNode("/controls/flight/rudder", true);
act_elevon_mix_node = fgGetNode("/config/actuators/elevon-mixing", true);
agl_alt_ft_node = fgGetNode("/position/altitude-agl-ft", true);
act_timestamp_node = fgGetNode("/actuators/actuator/time-stamp", true);
act_aileron_node = fgGetNode("/actuators/actuator/channel", 0, true);
act_elevator_node = fgGetNode("/actuators/actuator/channel", 1, true);
act_throttle_node = fgGetNode("/actuators/actuator/channel", 2, true);
act_rudder_node = fgGetNode("/actuators/actuator/channel", 3, true);
act_channel5_node = fgGetNode("/actuators/actuator/channel", 4, true);
act_channel6_node = fgGetNode("/actuators/actuator/channel", 5, true);
act_channel7_node = fgGetNode("/actuators/actuator/channel", 6, true);
act_channel8_node = fgGetNode("/actuators/actuator/channel", 7, true);
// initialize comm nodes
act_console_skip = fgGetNode("/config/remote-link/actuator-skip", true);
act_logging_skip = fgGetNode("/config/logging/actuator-skip", true);
// throttle safety
throttle_safety_prop_node
= fgGetNode("/config/actuators/throttle-safety/prop", true);
if ( (string)throttle_safety_prop_node->getStringValue() != (string)"" ) {
throttle_safety_val_node
= fgGetNode(throttle_safety_prop_node->getStringValue(), true);
}
throttle_safety_min_node
= fgGetNode("/config/actuators/throttle-safety/min-value", true);
// master autopilot switch
ap_master_switch_node = fgGetNode("/autopilot/master-switch", true);
fcs_mode_node = fgGetNode("/config/fcs/mode", true);
// default to ap on unless pilot inputs turn it off (so we can run
// with no pilot inputs connected)
ap_master_switch_node->setBoolValue( true );
// traverse configured modules
SGPropertyNode *toplevel = fgGetNode("/config/actuators", true);
for ( int i = 0; i < toplevel->nChildren(); ++i ) {
SGPropertyNode *section = toplevel->getChild(i);
string name = section->getName();
if ( name == "actuator" ) {
string module = section->getChild("module", 0, true)->getStringValue();
bool enabled = section->getChild("enable", 0, true)->getBoolValue();
if ( !enabled ) {
continue;
}
printf("i = %d name = %s module = %s\n",
i, name.c_str(), module.c_str());
if ( module == "null" ) {
// do nothing
} else if ( module == "ardupilot" ) {
ardupilot_init( section );
} else if ( module == "fgfs" ) {
fgfs_act_init( section );
#ifdef ENABLE_MNAV_SENSOR
} else if ( module == "mnav" ) {
mnav_act_init();
#endif // ENABLE_MNAV_SENSOR
} else {
printf("Unknown actuator = '%s' in config file\n",
module.c_str());
}
}
}
}
bool Actuator_update() {
debug6a.start();
// time stamp for logging
act_timestamp_node->setDoubleValue( get_Time() );
if ( ap_master_switch_node->getBoolValue() ) {
set_actuator_values_ap();
} else {
set_actuator_values_pilot();
}
// traverse configured modules
SGPropertyNode *toplevel = fgGetNode("/config/actuators", true);
for ( int i = 0; i < toplevel->nChildren(); ++i ) {
SGPropertyNode *section = toplevel->getChild(i);
string name = section->getName();
if ( name == "actuator" ) {
string module = section->getChild("module", 0, true)->getStringValue();
bool enabled = section->getChild("enable", 0, true)->getBoolValue();
if ( !enabled ) {
continue;
}
if ( module == "null" ) {
// do nothing
} else if ( module == "ardupilot" ) {
ardupilot_update();
} else if ( module == "fgfs" ) {
fgfs_act_update();
#ifdef ENABLE_MNAV_SENSOR
} else if ( module == "mnav" ) {
mnav_send_short_servo_cmd( /* &servo_out */ );
#endif // ENABLE_MNAV_SENSOR
} else {
printf("Unknown actuator = '%s' in config file\n",
module.c_str());
}
}
}
debug6a.stop();
debug6b.start();
if ( remote_link_on || log_to_file ) {
// actuators
uint8_t buf[256];
int size = packetizer->packetize_actuator( buf );
if ( remote_link_on ) {
remote_link_actuator( buf, size, act_console_skip->getIntValue() );
}
if ( log_to_file ) {
log_actuator( buf, size, act_logging_skip->getIntValue() );
}
}
debug6b.stop();
return true;
}
void AirData_init() {
debug2b1.set_name("debug2b1 airdata update");
debug2b2.set_name("debug2b2 airdata console link");
// initialize air data property nodes
airdata_timestamp_node = fgGetNode("/sensors/air-data/time-stamp", true);
airdata_altitude_node = fgGetNode("/sensors/air-data/altitude-m", true);
airdata_airspeed_node = fgGetNode("/sensors/air-data/airspeed-kt", true);
// input property nodes
filter_timestamp_node = fgGetNode("/filters/filter/time-stamp", true);
filter_alt_node = fgGetNode("/position/altitude-m", true);
// filtered/computed output property nodes
altitude_filt_node = fgGetNode("/position/altitude-pressure-m", true);
airspeed_filt_node = fgGetNode("/velocity/airspeed-kt", true);
true_alt_ft_node = fgGetNode("/position/altitude-true-combined-ft",true);
agl_alt_ft_node = fgGetNode("/position/altitude-pressure-agl-ft", true);
pressure_error_m_node
= fgGetNode("/position/pressure-error-m", true);
vert_fps_node
= fgGetNode("/velocity/pressure-vertical-speed-fps",true);
forward_accel_node = fgGetNode("/acceleration/airspeed-ktps",true);
ground_alt_press_m_node
= fgGetNode("/position/ground-altitude-pressure-m", true);
// initialize comm nodes
airdata_console_skip = fgGetNode("/config/remote-link/airdata-skip", true);
airdata_logging_skip = fgGetNode("/config/logging/airdata-skip", true);
// traverse configured modules
SGPropertyNode *toplevel = fgGetNode("/config/sensors/air-data-group", true);
for ( int i = 0; i < toplevel->nChildren(); ++i ) {
SGPropertyNode *section = toplevel->getChild(i);
string name = section->getName();
if ( name == "air-data" ) {
string source = section->getChild("source", 0, true)->getStringValue();
bool enabled = section->getChild("enable", 0, true)->getBoolValue();
if ( !enabled ) {
continue;
}
string basename = "/sensors/";
basename += section->getDisplayName();
printf("i = %d name = %s source = %s %s\n",
i, name.c_str(), source.c_str(), basename.c_str());
if ( source == "null" ) {
// do nothing
} else if ( source == "ardupilot" ) {
ardupilot_airdata_init( basename );
} else if ( source == "fgfs" ) {
fgfs_airdata_init( basename );
#ifdef ENABLE_MNAV_SENSOR
} else if ( source == "mnav" ) {
mnav_airdata_init( basename );
#endif // ENABLE_MNAV_SENSOR
} else {
printf("Unknown air data source = '%s' in config file\n",
source.c_str());
}
}
}
}
static void bind_properties() {
ap_master_switch_node = fgGetNode("/autopilot/master-switch", true);
fcs_mode_node = fgGetNode("/config/fcs/mode", true);
// ap_heading_mode_node = fgGetNode("/autopilot/heading-mode", true);
// ap_roll_mode_node = fgGetNode("/autopilot/roll-mode", true);
// ap_yaw_mode_node = fgGetNode("/autopilot/yaw-mode", true);
// ap_altitude_mode_node = fgGetNode("/autopilot/altitude-mode", true);
// ap_speed_mode_node = fgGetNode("/autopilot/speed-mode", true);
// ap_pitch_mode_node = fgGetNode("/autopilot/pitch-mode", true);
heading_lock_node = fgGetNode("/autopilot/locks/heading", true);
roll_lock_node = fgGetNode("/autopilot/locks/roll", true);
yaw_lock_node = fgGetNode("/autopilot/locks/yaw", true);
altitude_lock_node = fgGetNode("/autopilot/locks/altitude", true);
speed_lock_node = fgGetNode("/autopilot/locks/speed", true);
pitch_lock_node = fgGetNode("/autopilot/locks/pitch", true);
pointing_lock_node = fgGetNode("/autopilot/locks/pointing", true);
lookat_mode_node = fgGetNode("/pointing/lookat-mode", true);
ned_n_node = fgGetNode("/pointing/vector/north", true);
ned_e_node = fgGetNode("/pointing/vector/east", true);
ned_d_node = fgGetNode("/pointing/vector/down", true);
roll_deg_node = fgGetNode("/orientation/roll-deg", true);
pitch_deg_node = fgGetNode("/orientation/pitch-deg", true);
cur_speed_node = fgGetNode("/velocity/airspeed-kt", true);
initial_speed_node = fgGetNode("/config/fcs/initial-speed-kt", true);
target_roll_deg_node
= fgGetNode("/autopilot/settings/target-roll-deg", true);
target_pitch_base_deg_node
= fgGetNode("/autopilot/settings/target-pitch-base-deg", true);
target_speed_node = fgGetNode("/autopilot/settings/target-speed-kt", true);
}
bool AirData_update() {
debug2b1.start();
air_prof.start();
bool fresh_data = false;
// traverse configured modules
SGPropertyNode *toplevel = fgGetNode("/config/sensors/air-data-group", true);
for ( int i = 0; i < toplevel->nChildren(); ++i ) {
SGPropertyNode *section = toplevel->getChild(i);
string name = section->getName();
if ( name == "air-data" ) {
string source = section->getChild("source", 0, true)->getStringValue();
bool enabled = section->getChild("enable", 0, true)->getBoolValue();
if ( !enabled ) {
continue;
}
string basename = "/sensors/";
basename += section->getDisplayName();
// printf("i = %d name = %s source = %s %s\n",
// i, name.c_str(), source.c_str(), basename.c_str());
if ( source == "null" ) {
// do nothing
} else if ( source == "ardupilot" ) {
fresh_data = ardupilot_airdata_update();
} else if ( source == "fgfs" ) {
fresh_data = fgfs_airdata_update();
#ifdef ENABLE_MNAV_SENSOR
} else if ( source == "mnav" ) {
fresh_data = mnav_get_airdata();
#endif // ENABLE_MNAV_SENSOR
} else {
printf("Unknown air data source = '%s' in config file\n",
source.c_str());
}
}
}
debug2b1.stop();
debug2b2.start();
if ( fresh_data ) {
update_pressure_helpers();
if ( remote_link_on || log_to_file ) {
uint8_t buf[256];
int size = packetizer->packetize_airdata( buf );
if ( remote_link_on ) {
// printf("sending filter packet\n");
remote_link_airdata( buf, size,
airdata_console_skip->getIntValue() );
}
if ( log_to_file ) {
log_airdata( buf, size, airdata_logging_skip->getIntValue() );
}
}
}
debug2b2.stop();
air_prof.stop();
return fresh_data;
}
static void init_props() {
props_inited = true;
// initialize imu property nodes
imu_timestamp_node = fgGetNode("/sensors/imu/time-stamp");
imu_p_node = fgGetNode("/sensors/imu/p-rad_sec", true);
imu_q_node = fgGetNode("/sensors/imu/q-rad_sec", true);
imu_r_node = fgGetNode("/sensors/imu/r-rad_sec", true);
imu_ax_node = fgGetNode("/sensors/imu/ax-mps_sec", true);
imu_ay_node = fgGetNode("/sensors/imu/ay-mps_sec", true);
imu_az_node = fgGetNode("/sensors/imu/az-mps_sec", true);
imu_hx_node = fgGetNode("/sensors/imu/hx", true);
imu_hy_node = fgGetNode("/sensors/imu/hy", true);
imu_hz_node = fgGetNode("/sensors/imu/hz", true);
// initialize air data nodes
airdata_altitude_node = fgGetNode("/sensors/air-data/altitude-m", true);
airdata_airspeed_node = fgGetNode("/sensors/air-data/airspeed-kt", true);
// initialize gps property nodes
gps_timestamp_node = fgGetNode("/sensors/gps/time-stamp", true);
gps_lat_node = fgGetNode("/sensors/gps/latitude-deg", true);
gps_lon_node = fgGetNode("/sensors/gps/longitude-deg", true);
gps_alt_node = fgGetNode("/sensors/gps/altitude-m", true);
gps_ve_node = fgGetNode("/sensors/gps/ve-ms", true);
gps_vn_node = fgGetNode("/sensors/gps/vn-ms", true);
gps_vd_node = fgGetNode("/sensors/gps/vd-ms", true);
gps_track_node = fgGetNode("/sensors/gps/groundtrack-deg", true);
gps_unix_sec_node = fgGetNode("/sensors/gps/unix-time-sec", true);
// initialize filter property nodes
filter_theta_node = fgGetNode("/orientation/pitch-deg", true);
filter_phi_node = fgGetNode("/orientation/roll-deg", true);
filter_psi_node = fgGetNode("/orientation/heading-deg", true);
filter_lat_node = fgGetNode("/position/latitude-deg", true);
filter_lon_node = fgGetNode("/position/longitude-deg", true);
filter_alt_node = fgGetNode("/position/altitude-m", true);
filter_vn_node = fgGetNode("/velocity/vn-ms", true);
filter_ve_node = fgGetNode("/velocity/ve-ms", true);
filter_vd_node = fgGetNode("/velocity/vd-ms", true);
filter_status_node = fgGetNode("/health/navigation", true);
// initialize actuator property nodes
act_aileron_node = fgGetNode("/actuators/actuator/channel", 0, true);
act_elevator_node = fgGetNode("/actuators/actuator/channel", 1, true);
act_throttle_node = fgGetNode("/actuators/actuator/channel", 2, true);
act_rudder_node = fgGetNode("/actuators/actuator/channel", 3, true);
act_channel5_node = fgGetNode("/actuators/actuator/channel", 4, true);
// initialize pilot property nodes
pilot_aileron_node = fgGetNode("/sensors/pilot/aileron", true);
pilot_elevator_node = fgGetNode("/sensors/pilot/elevator", true);
pilot_throttle_node = fgGetNode("/sensors/pilot/throttle", true);
pilot_rudder_node = fgGetNode("/sensors/pilot/rudder", true);
pilot_channel5_node = fgGetNode("/sensors/pilot/manual", true);
// initialize health/status property nodes
link_seq_num = fgGetNode("/status/remote-link-sequence-num", true);
target_waypoint = fgGetNode( "/autopilot/route-mgr/target-waypoint-idx",
true );
system_load_avg = fgGetNode("/status/system-load-avg", true);
}
void uiuc_gear()
{
/*
* Aircraft specific initializations and data goes here
*/
static DATA percent_brake[MAX_GEAR] = /* percent applied braking */
{ 0., 0., 0., 0.,
0., 0., 0., 0.,
0., 0., 0., 0.,
0., 0., 0., 0. }; /* 0 = none, 1 = full */
static DATA caster_angle_rad[MAX_GEAR] = /* steerable tires - in */
{ 0., 0., 0., 0.,
0., 0., 0., 0.,
0., 0., 0., 0.,
0., 0., 0., 0. }; /* radians, +CW */
/*
* End of aircraft specific code
*/
/*
* Constants & coefficients for tyres on tarmac - ref [1]
*/
/* skid function looks like:
*
* mu ^
* |
* max_mu | +
* | /|
* sliding_mu | / +------
* | /
* | /
* +--+------------------------>
* | | | sideward V
* 0 bkout skid
* V V
*/
static int it_rolls[MAX_GEAR] =
{ 1, 1, 1, 0,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0 };
static DATA sliding_mu[MAX_GEAR] =
{ 0.5, 0.5, 0.5, 0.3,
0.3, 0.3, 0.3, 0.3,
0.3, 0.3, 0.3, 0.3,
0.3, 0.3, 0.3, 0.3 };
static DATA max_brake_mu[MAX_GEAR] =
{ 0.0, 0.6, 0.6, 0.0,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0 };
static DATA max_mu = 0.8;
static DATA bkout_v = 0.1;
static DATA skid_v = 1.0;
/*
* Local data variables
*/
DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/
// DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
// DATA altitude_local_v[3]; /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
// DATA altitude_body_v[3]; /*altitude vector in body (X,Y,Z)*/
DATA temp3a[3];
// DATA temp3b[3];
DATA tempF[3];
DATA tempM[3];
DATA reaction_normal_force; /* wheel normal (to rwy) force */
DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
DATA forward_mu, sideward_mu; /* friction coefficients */
DATA beta_mu; /* breakout friction slope */
DATA forward_wheel_force, sideward_wheel_force;
int i; /* per wheel loop counter */
/*
* Execution starts here
*/
beta_mu = max_mu/(skid_v-bkout_v);
clear3( F_gear_v ); /* Initialize sum of forces... */
clear3( M_gear_v ); /* ...and moments */
/*
* Put aircraft specific executable code here
*/
percent_brake[1] = Brake_pct[0];
percent_brake[2] = Brake_pct[1];
caster_angle_rad[0] =
(0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
for (i=0;i<MAX_GEAR;i++) /* Loop for each wheel */
{
// Execute only if the gear has been defined
if (!gear_model[i])
{
// do nothing
continue;
}
else
{
/*========================================*/
/* Calculate wheel position w.r.t. runway */
/*========================================*/
/* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
/* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
sub3( D_gear_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
/* then converting to local (North-East-Down) axes... */
multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
/* Runway axes correction - third element is Altitude, not (-)Z... */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
/* Add wheel offset to cg location in local axes */
add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
/* remove Runway axes correction so right hand rule applies */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
/*============================*/
/* Calculate wheel velocities */
/*============================*/
/* contribution due to angular rates */
cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
/* transform into local axes */
multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
/* plus contribution due to cg velocities */
add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
clear3(f_wheel_local_v);
reaction_normal_force=0;
#if 0
static const SGPropertyNode * gear_wow
= fgGetNode("/gear/gear[0]/wow", false);
static const SGPropertyNode * gear_wow1
= fgGetNode("/gear/gear[1]/wow", false);
static const SGPropertyNode * gear_wow2
= fgGetNode("/gear/gear[2]/wow", false);
#endif
fgSetBool("/gear/gear[0]/wow", false);
fgSetBool("/gear/gear[1]/wow", false);
fgSetBool("/gear/gear[2]/wow", false);
if( HEIGHT_AGL_WHEEL < 0. )
/*the wheel is underground -- which implies ground contact
so calculate reaction forces */
{
//set the property - weight on wheels
// if (i==0)
// {
// fgSetBool("/gear/gear[0]/wow", true);
// }
// if (i==1)
// {
// fgSetBool("/gear/gear[1]/wow", true);
// }
// if (i==2)
// {
// fgSetBool("/gear/gear[2]/wow", true);
// }
/*===========================================*/
/* Calculate forces & moments for this wheel */
/*===========================================*/
/* Add any anticipation, or frame lead/prediction, here... */
/* no lead used at present */
/* Calculate sideward and forward velocities of the wheel
in the runway plane */
cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
+ v_wheel_local_v[1]*sin_wheel_hdg_angle;
v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
- v_wheel_local_v[0]*sin_wheel_hdg_angle;
/* Calculate normal load force (simple spring constant) */
reaction_normal_force = 0.;
reaction_normal_force = kgear[i]*d_wheel_rwy_local_v[2]
- v_wheel_local_v[2]*cgear[i];
/* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
if (reaction_normal_force > 0.) reaction_normal_force = 0.;
/* to prevent damping component from swamping spring component */
/* Calculate friction coefficients */
if(it_rolls[i])
{
forward_mu = (max_brake_mu[i] - muGear[i])*percent_brake[i] + muGear[i];
abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
sideward_mu = sliding_mu[i];
if (abs_v_wheel_sideward < skid_v)
sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
}
else
{
forward_mu=sliding_mu[i];
sideward_mu=sliding_mu[i];
}
/* Calculate foreward and sideward reaction forces */
forward_wheel_force = forward_mu*reaction_normal_force;
sideward_wheel_force = sideward_mu*reaction_normal_force;
if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
/* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
*/
/* Rotate into local (N-E-D) axes */
f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
- sideward_wheel_force*sin_wheel_hdg_angle;
f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
+ sideward_wheel_force*cos_wheel_hdg_angle;
f_wheel_local_v[2] = reaction_normal_force;
/* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
/* Calculate moments from force and offsets in body axes */
cross3( d_wheel_cg_body_v, tempF, tempM );
/* Sum forces and moments across all wheels */
if (tempF[0] != 0.0 || tempF[1] != 0.0 || tempF[2] != 0.0) {
fgSetBool("/gear/gear[1]/wow", true);
}
add3( tempF, F_gear_v, F_gear_v );
add3( tempM, M_gear_v, M_gear_v );
}
}
/* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
/*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
}
}
void control_update(short flight_mode)
{
// make a quick exit if we are disabled
if ( !autopilot_active ) {
return;
}
// reset the autopilot if requested
if ( autopilot_reinit ) {
control_reset();
autopilot_reinit = false;
}
// optional: use channel #6 to change the autopilot target value
// double min_value = -35.0;
// double max_value = 35.0;
// double tgt_value = (max_value - min_value) *
// ((double)servo_in.chn[5] / 65535.0) + min_value;
// ap_target->setFloatValue( tgt_value );
// update the autopilot stages
ap.update( 0.04 ); // dt = 1/25
/* printf("%.2f %.2f\n", aileron_out_node->getFloatValue(),
elevator_out_node->getFloatValue()); */
static SGPropertyNode *vert_speed_fps
= fgGetNode("/velocities/vertical-speed-fps", true);
static SGPropertyNode *true_alt
= fgGetNode("/position/altitude-ft", true);
/* printf("%.1f %.2f %.2f\n",
true_alt->getFloatValue(),
vert_speed_fps->getFloatValue(),
elevator_out_node->getFloatValue()); */
// initialize the servo command array to central values so we don't
// inherit junk
for ( int i = 0; i < 8; ++i ) {
servo_out.chn[i] = 32768;
}
if ( elevon_mix->getBoolValue() ) {
// elevon mixing mode
//aileron
servo_out.chn[0] = 32768
+ (int16_t)(aileron_out_node->getFloatValue() * 32768)
+ (int16_t)(elevator_out_node->getFloatValue() * 32768);
//elevator
servo_out.chn[1] = 32768
+ (int16_t)(aileron_out_node->getFloatValue() * 32768)
- (int16_t)(elevator_out_node->getFloatValue() * 32768);
} else {
// conventional airframe mode
//aileron
servo_out.chn[0] = 32768
+ (int16_t)(aileron_out_node->getFloatValue() * 32768);
//elevator
servo_out.chn[1] = 32768
+ (int16_t)(elevator_out_node->getFloatValue() * 32768);
}
//throttle
servo_out.chn[2] = 32768
+ (uint16_t)(throttle_out_node->getFloatValue() * 32768);
// rudder
servo_out.chn[3] = 32768
+ (int16_t)(rudder_out_node->getFloatValue() * 32768);
// time stamp the packet for logging
servo_out.time = get_Time();
// send commanded servo positions to the MNAV
send_short_servo_cmd();
}
