void Common_Tool::init_config()
{
if (config_filename == NULL) {
abort();
}
struct stat buf;
if (stat(config_filename, &buf) == -1) {
if (errno == ENOENT) {
if (! streq(default_config_filename, config_filename)) {
la_log(LOG_CRIT, "Config file (%s) does not exist", config_filename);
exit(1);
}
} else {
la_log(LOG_CRIT, "Failed to stat (%s): %s\n", config_filename, strerror(errno));
exit(1);
}
}
_config = new INIReader(config_filename);
if (_config == NULL) {
la_log(LOG_CRIT, "Failed to create config from (%s)\n", config_filename);
exit(1);
}
if (_config == NULL) {
_config = new INIReader("/dev/null");
}
}
void LogAnalyzer::run_live_analysis()
{
reader = new MAVLink_Reader(config());
_client = new Telem_Forwarder_Client(_client_recv_buf, sizeof(_client_recv_buf));
_client->configure(config());
writer = new MAVLink_Writer(config());
if (writer == NULL) {
la_log(LOG_ERR, "Failed to create writer from (%s)\n", config_filename);
exit(1);
}
Heart *heart= new Heart(writer);
if (heart != NULL) {
reader->add_message_handler(heart, "Heart");
} else {
la_log(LOG_INFO, "Failed to create heart");
}
create_vehicle_from_commandline_arguments();
if (_vehicle == NULL) {
_vehicle = new AnalyzerVehicle::Base();
}
Analyze *analyze = create_analyze();
Analyzing_MAVLink_Message_Handler *handler = new Analyzing_MAVLink_Message_Handler(analyze, _vehicle);
reader->add_message_handler(handler, "Analyze");
select_loop();
_vehicle = NULL; // leak this memory
}
void DataFlash_Logger_Program::run()
{
init_config();
if (! debug_mode) {
la_log_syslog_open();
}
la_log(LOG_INFO, "dataflash_logger starting: built " __DATE__ " " __TIME__);
signal(SIGHUP, ::sighup_handler);
reader = new MAVLink_Reader(config());
if (reader == NULL) {
la_log(LOG_ERR, "Failed to create reader from (%s)\n", config_filename);
exit(1);
}
if (serial_port) {
client = new Telem_Serial(_client_recv_buf, sizeof(_client_recv_buf));
} else {
client = new Telem_Forwarder_Client(_client_recv_buf, sizeof(_client_recv_buf));
}
client->configure(config());
client->init();
_writer = new MAVLink_Writer(config());
if (_writer == NULL) {
la_log(LOG_ERR, "Failed to create writer from (%s)\n", config_filename);
exit(1);
}
_writer->add_client(client);
// instantiate message handlers:
DataFlash_Logger *dataflash_logger = new DataFlash_Logger(_writer);
if (dataflash_logger != NULL) {
reader->add_message_handler(dataflash_logger, "DataFlash_Logger");
} else {
la_log(LOG_INFO, "Failed to create dataflash logger");
}
Heart *heart= new Heart(_writer);
if (heart != NULL) {
reader->add_message_handler(heart, "Heart");
} else {
la_log(LOG_INFO, "Failed to create heart");
}
return select_loop();
}
bool Format_Reader::add_message_handler(Message_Handler *handler,
const char *handler_name)
{
if (MAX_MESSAGE_HANDLERS - next_message_handler < 2) {
la_log(LOG_INFO, "Insufficient message handler slots (MAX=%d) (next=%d)?!", MAX_MESSAGE_HANDLERS, next_message_handler);
return false;
}
if (!handler->configure(_config)) {
la_log(LOG_INFO, "Failed to configure (%s)", handler_name);
return false;
}
message_handler[next_message_handler++] = handler;
return true;
}
void Common_Tool::check_fds_are_empty_after_select(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, uint8_t nfds)
{
for (uint8_t i=0; i<nfds; i++) {
if (FD_ISSET(i, &fds_read)) {
la_log(LOG_ERR, "fds_read not empty");
break;
}
if (FD_ISSET(i, &fds_write)) {
la_log(LOG_ERR, "fds_write not empty");
break;
}
if (FD_ISSET(i, &fds_err)) {
la_log(LOG_ERR, "fds_err not empty");
break;
}
}
}
void Common_Tool::select_loop()
{
fd_set fds_read;
fd_set fds_write;
fd_set fds_err;
uint8_t nfds;
while (1) {
if (_sighup_received) {
sighup_received_tophalf();
_sighup_received = false;
}
/* Wait for a packet, or time out if no packets arrive so we always
periodically log status and check for new destinations. Downlink
packets are on the order of 100/sec, so the timeout is such that
we don't expect timeouts unless solo stops sending packets. We
almost always get a packet with a 200 msec timeout, but not with
a 100 msec timeout. (Timeouts don't really matter though.) */
struct timeval timeout;
FD_ZERO(&fds_read);
FD_ZERO(&fds_write);
FD_ZERO(&fds_err);
nfds = 0;
pack_select_fds(fds_read, fds_write, fds_err, nfds);
timeout.tv_sec = 0;
timeout.tv_usec = select_timeout_us();
int res = select(nfds, &fds_read, &fds_write, &fds_err, &timeout);
if (res < 0) {
unsigned skipped = 0;
la_log(LOG_ERR, "[%u] select: %s", skipped, strerror(errno));
/* this sleep is to avoid soaking the CPU if select starts
returning immediately for some reason */
/* previous code was not checking errfds; we are now, so
perhaps this usleep can go away -pb20150730 */
usleep(10000);
continue;
}
if (res == 0) {
// select timeout
}
handle_select_fds(fds_read, fds_write, fds_err, nfds);
check_fds_are_empty_after_select(fds_read, fds_write, fds_err, nfds);
do_idle_callbacks();
} /* while (1) */
}
void Analyze::configure_analyzer(INIReader *config, Analyzer *analyzer)
{
if (_use_names_to_run &&
!_names_to_run[analyzer->name()]) {
return;
}
if (analyzer->configure(config)) {
_analyzers.push_back(analyzer);
} else {
la_log(LOG_INFO, "Failed to configure (%s)", analyzer->name().c_str());
}
analyzer->set_pure_output(pure_output());
}
void LogAnalyzer::create_vehicle_from_commandline_arguments()
{
if (_model_string != NULL) {
if (strieq(_model_string,"copter")) {
_vehicle = new AnalyzerVehicle::Copter();
// _analyze->set_vehicle_copter();
if (_frame_string != NULL) {
((AnalyzerVehicle::Copter*)_vehicle)->set_frame(_frame_string);
}
} else if (strieq(_model_string,"plane")) {
_vehicle = new AnalyzerVehicle::Plane();
// } else if (streq(model_string,"rover")) {
// model = new AnalyzerVehicle::Rover();
} else {
la_log(LOG_ERR, "Unknown model type (%s)", _model_string);
exit(1);
}
_vehicle->set_vehicletype_is_forced(true);
} else if (_frame_string != NULL) {
la_log(LOG_ERR, "Can not specify frame type without specifying model type");
exit(1);
}
}
Analyze *LogAnalyzer::create_analyze()
{
Analyze *analyze = new Analyze(_vehicle);
if (analyze == NULL) {
la_log(LOG_ERR, "Failed to create analyze");
abort();
}
analyze->set_output_style(output_style);
if (_analyzer_names_to_run.size()) {
analyze->set_analyzer_names_to_run(_analyzer_names_to_run);
}
analyze->set_pure_output(_pure_output);
analyze->instantiate_analyzers(config());
return analyze;
}
void Analyze::instantiate_analyzers(INIReader *config)
{
Analyzer_Any_Parameters_Seen *analyzer_any_parameters_seen = new Analyzer_Any_Parameters_Seen(vehicle,_data_sources);
if (analyzer_any_parameters_seen != NULL) {
configure_analyzer(config, analyzer_any_parameters_seen);
} else {
la_log(LOG_INFO, "Failed to create analyzer_any_parameters_seen");
}
Analyzer_Arming_Checks *analyzer_arming_checks = new Analyzer_Arming_Checks(vehicle,_data_sources);
if (analyzer_arming_checks != NULL) {
configure_analyzer(config, analyzer_arming_checks);
} else {
la_log(LOG_INFO, "Failed to create analyzer_arming_checks");
}
analyzer_altitude_estimate_divergence = new Analyzer_Altitude_Estimate_Divergence(vehicle,_data_sources);
if (analyzer_altitude_estimate_divergence != NULL) {
configure_analyzer(config, analyzer_altitude_estimate_divergence);
} else {
la_log(LOG_INFO, "Failed to create analyzer_altitude_estimate_divergence");
}
Analyzer_Attitude_Estimate_Divergence *analyzer_attitude_estimate_divergence = new Analyzer_Attitude_Estimate_Divergence(vehicle,_data_sources);
if (analyzer_attitude_estimate_divergence != NULL) {
configure_analyzer(config, analyzer_attitude_estimate_divergence);
} else {
la_log(LOG_INFO, "Failed to create analyzer_attitude_estimate_divergence");
}
{
Analyzer_Compass_Offsets *analyzer_compass_offsets = new Analyzer_Compass_Offsets(vehicle,_data_sources, "");
if (analyzer_compass_offsets != NULL) {
configure_analyzer(config, analyzer_compass_offsets);
} else {
la_log(LOG_INFO, "Failed to create analyzer_compass_offsets");
}
}
{
Analyzer_Compass_Vector_Length *analyzer_compass_vector_length = new Analyzer_Compass_Vector_Length(vehicle,_data_sources);
if (analyzer_compass_vector_length != NULL) {
configure_analyzer(config, analyzer_compass_vector_length);
} else {
la_log(LOG_INFO, "Failed to create analyzer_compass_vector_length1");
}
}
{
Analyzer_Compass_Offsets *analyzer_compass_offsets = new Analyzer_Compass_Offsets(vehicle,_data_sources, "2");
if (analyzer_compass_offsets != NULL) {
configure_analyzer(config, analyzer_compass_offsets);
} else {
la_log(LOG_INFO, "Failed to create analyzer_compass_offsets");
}
}
{
Analyzer_Compass_Offsets *analyzer_compass_offsets = new Analyzer_Compass_Offsets(vehicle,_data_sources, "3");
if (analyzer_compass_offsets != NULL) {
configure_analyzer(config, analyzer_compass_offsets);
} else {
la_log(LOG_INFO, "Failed to create analyzer_compass_offsets");
}
}
Analyzer_Ever_Armed *analyzer_ever_armed = new Analyzer_Ever_Armed(vehicle,_data_sources);
if (analyzer_ever_armed != NULL) {
configure_analyzer(config, analyzer_ever_armed);
} else {
la_log(LOG_INFO, "Failed to create analyzer_ever_armed");
}
analyzer_ever_flew = new Analyzer_Ever_Flew(vehicle,_data_sources);
if (analyzer_ever_flew != NULL) {
configure_analyzer(config, analyzer_ever_flew);
} else {
la_log(LOG_INFO, "Failed to create analyzer_ever_flew");
}
Analyzer_Good_EKF *analyzer_good_ekf = new Analyzer_Good_EKF(vehicle,_data_sources);
if (analyzer_good_ekf != NULL) {
configure_analyzer(config, analyzer_good_ekf);
} else {
la_log(LOG_INFO, "Failed to create analyzer_good_ekf");
}
Analyzer_GPS_Fix *analyzer_gps_fix = new Analyzer_GPS_Fix(vehicle,_data_sources);
if (analyzer_gps_fix != NULL) {
configure_analyzer(config, analyzer_gps_fix);
} else {
la_log(LOG_INFO, "Failed to create analyzer_gps_fix");
}
{
Analyzer_Gyro_Drift *analyzer_gyro_drift = new Analyzer_Gyro_Drift(vehicle,_data_sources);
if (analyzer_gyro_drift != NULL) {
configure_analyzer(config, analyzer_gyro_drift);
} else {
la_log(LOG_INFO, "Failed to create analyzer_gyro_drift");
}
}
Analyzer_Attitude_Control *analyzer_attitude_control = new Analyzer_Attitude_Control(vehicle,_data_sources);
if (analyzer_attitude_control != NULL) {
configure_analyzer(config, analyzer_attitude_control);
} else {
la_log(LOG_INFO, "Failed to create analyzer_attitude_control");
}
Analyzer_Autopilot *analyzer_autopilot = new Analyzer_Autopilot(vehicle,_data_sources);
if (analyzer_autopilot != NULL) {
configure_analyzer(config, analyzer_autopilot);
} else {
la_log(LOG_INFO, "Failed to create analyzer_autopilot");
}
Analyzer_Battery *analyzer_battery = new Analyzer_Battery(vehicle,_data_sources);
if (analyzer_battery != NULL) {
configure_analyzer(config, analyzer_battery);
} else {
la_log(LOG_INFO, "Failed to create analyzer_battery");
}
Analyzer_Brownout *analyzer_brownout = new Analyzer_Brownout(vehicle,_data_sources);
if (analyzer_brownout != NULL) {
configure_analyzer(config, analyzer_brownout);
} else {
la_log(LOG_INFO, "Failed to create analyzer_brownout");
}
analyzer_position_estimate_divergence = new Analyzer_Position_Estimate_Divergence(vehicle,_data_sources);
if (analyzer_position_estimate_divergence != NULL) {
configure_analyzer(config, analyzer_position_estimate_divergence);
} else {
la_log(LOG_INFO, "Failed to create analyzer_position_estimate_divergence");
}
Analyzer_NotCrashed *analyzer_notcrashed = new Analyzer_NotCrashed(vehicle,_data_sources);
if (analyzer_notcrashed != NULL) {
configure_analyzer(config, analyzer_notcrashed);
} else {
la_log(LOG_INFO, "Failed to create analyzer_not_crashed");
}
Analyzer_Sensor_Health *analyzer_sensor_health = new Analyzer_Sensor_Health(vehicle,_data_sources);
if (analyzer_sensor_health != NULL) {
configure_analyzer(config, analyzer_sensor_health);
} else {
la_log(LOG_INFO, "Failed to create analyzer_sensor_health");
}
Analyzer_Vehicle_Definition *analyzer_vehicle_definition = new Analyzer_Vehicle_Definition(vehicle,_data_sources);
if (analyzer_vehicle_definition != NULL) {
configure_analyzer(config, analyzer_vehicle_definition);
} else {
la_log(LOG_INFO, "Failed to create analyzer_vehicle_definition");
}
analyzer_velocity_estimate_divergence = new Analyzer_Velocity_Estimate_Divergence(vehicle,_data_sources);
if (analyzer_velocity_estimate_divergence != NULL) {
configure_analyzer(config, analyzer_velocity_estimate_divergence);
} else {
la_log(LOG_INFO, "Failed to create analyzer_velocity_estimate_divergence");
}
}
