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());
}
}
}
}
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());
}
}
}
}
// initialize gpsd output property nodes
static void bind_output( string rootname ) {
SGPropertyNode *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);
gps_device_name = outputroot->getChild("device-name", 0, true);
gps_satellites = outputroot->getChild("satellites", 0, true);
gps_nmode = outputroot->getChild("nmea-mode", 0, 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());
}
}
}
}
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;
}